void SlamGMapping::writeOdomMsgtoFile(tf::Stamped<tf::Transform>& odom_pose){

	tf::Quaternion quat = odom_pose.getRotation();


	fwrite(&quat,sizeof(tf::Quaternion),1,odomfile);
	fwrite(&odom_pose.getOrigin(),sizeof(tf::Vector3),1,odomfile);

	/*
		tfScalar x =  odom_pose.getRotation().getX();
		tfScalar y =  odom_pose.getRotation().getY();
		tfScalar z =  odom_pose.getRotation().getZ();
		tfScalar w =  odom_pose.getRotation().getW();


	   fwrite(&x,sizeof(tfScalar),1,odomfile);
	   fwrite(&y,sizeof(tfScalar),1,odomfile);
	   fwrite(&z,sizeof(tfScalar),1,odomfile);
	   fwrite(&w,sizeof(tfScalar),1,odomfile);

	   x = odom_pose.getOrigin().getX();
	   y = odom_pose.getOrigin().getY();
	   z = odom_pose.getOrigin().getZ();

	   fwrite(&x,sizeof(tfScalar),1,odomfile);
	   fwrite(&y,sizeof(tfScalar),1,odomfile);
	   fwrite(&z,sizeof(tfScalar),1,odomfile);*/

}
void publishTransform(tf::Stamped<tf::Pose> _odom2map)
{
    /*
    double x, y, th;
    x = odom2map.getOrigin()[0];
    y = odom2map.getOrigin()[1];
    th = tf::getYaw(odom2map.getRotation());
    ROS_INFO("x = %f y = %f th = %f",
        (x), (y), (th));
        * */
    tf::Transform tfom = tf::Transform(tf::Quaternion(_odom2map.getRotation()),
                                       tf::Point(_odom2map.getOrigin()));
    tf::StampedTransform map_trans_stamped(tfom.inverse(), current_time, mapFrame, odomFrame);
    tf_broadcaster->sendTransform(map_trans_stamped);

    /*
     //first, we'll publish the transform over tf
    geometry_msgs::TransformStamped map_trans;
    mtx.lock();
    map_trans.header.stamp = current_time;
    mtx.unlock();
    map_trans.header.frame_id = mapFrame;
    map_trans.child_frame_id = odomFrame;


    map_trans.transform.translation.x = coords.x;
    map_trans.transform.translation.y = coords.y;
    map_trans.transform.translation.z = 0.0;
    geometry_msgs::Quaternion quat = tf::createQuaternionMsgFromYaw(coords.th);
    map_trans.transform.rotation = quat;

    //send the transform
    tf_broadcaster->sendTransform(map_trans);
    */
}
Beispiel #3
0
double ExploreFrontier::getOrientationChange(const Frontier& frontier, const tf::Stamped<tf::Pose>& robot_pose){
    double robot_yaw = tf::getYaw(robot_pose.getRotation());
    double robot_atan2 = atan2(robot_pose.getOrigin().y() + sin(robot_yaw), robot_pose.getOrigin().x() + cos(robot_yaw));
    double frontier_atan2 = atan2(frontier.pose.position.x, frontier.pose.position.y);
    double orientation_change = robot_atan2 - frontier_atan2;
    ROS_DEBUG("Orientation change: %.3f", orientation_change * (180.0 / M_PI));
    return orientation_change;
}
void SlamGMapping::writePoseStamped(tf::Stamped<tf::Pose> & pose_stamped){

	//fwrite(&pose_stamped.frame_id_,sizeof();
	tf::Quaternion quat = pose_stamped.getRotation();

	fwrite(&quat,sizeof(tf::Quaternion),1,posestampedfile);
	fwrite(&pose_stamped.getOrigin(),sizeof(tf::Vector3),1,posestampedfile);

}
 double getGoalOrientationAngleDifference(const tf::Stamped<tf::Pose>& global_pose, double goal_th) {
   double yaw = tf::getYaw(global_pose.getRotation());
   return angles::shortest_angular_distance(yaw, goal_th);
 }
Beispiel #6
0
                    const std::vector<geometry_msgs::PoseStamped>& global_plan,
                    const costmap_2d::Costmap2D& costmap __attribute__ ( (unused)),
                    const std::string& global_frame,
                    tf::Stamped<tf::Pose>& global_pose,
                    const nav_msgs::Odometry& base_odom,
                    double rot_stopped_vel, double trans_stopped_vel,
                    double xy_goal_tolerance, double yaw_goal_tolerance)
{

    //we assume the global goal is the last point in the global plan
    tf::Stamped<tf::Pose> goal_pose;
    getGoalPose (tf, global_plan, global_frame, goal_pose);

    double goal_x = goal_pose.getOrigin().getX();
    double goal_y = goal_pose.getOrigin().getY();
    double goal_th = tf::getYaw (goal_pose.getRotation());

    //check to see if we've reached the goal position
    if (getGoalPositionDistance (global_pose, goal_x, goal_y) <= xy_goal_tolerance)
    {
        //check to see if the goal orientation has been reached
        if (fabs (getGoalOrientationAngleDifference (global_pose, goal_th)) <= yaw_goal_tolerance)
        {
            //make sure that we're actually stopped before returning success
            if (stopped (base_odom, rot_stopped_vel, trans_stopped_vel))
                return true;
        }
    }

    return false;
}
Beispiel #7
0
  /*
   * given the current state of the robot, find a good trajectory
   */
  base_local_planner::Trajectory EDWAPlanner::findBestPath(
      tf::Stamped<tf::Pose> global_pose,
      tf::Stamped<tf::Pose> global_vel,
      tf::Stamped<tf::Pose>& drive_velocities,
      std::vector<geometry_msgs::Point> footprint_spec) {

    obstacle_costs_.setFootprint(footprint_spec);

    //make sure that our configuration doesn't change mid-run
    boost::mutex::scoped_lock l(configuration_mutex_);

    Eigen::Vector3f pos(global_pose.getOrigin().getX(), global_pose.getOrigin().getY(), tf::getYaw(global_pose.getRotation()));
    Eigen::Vector3f vel(global_vel.getOrigin().getX(), global_vel.getOrigin().getY(), tf::getYaw(global_vel.getRotation()));
    geometry_msgs::PoseStamped goal_pose = global_plan_.back();
    Eigen::Vector3f goal(goal_pose.pose.position.x, goal_pose.pose.position.y, tf::getYaw(goal_pose.pose.orientation));
    base_local_planner::LocalPlannerLimits limits = planner_util_->getCurrentLimits();

    // prepare cost functions and generators for this run
    generator_.initialise(pos,
        vel,
        goal,
        &limits,
        vsamples_);

    energy_costs_.setLastSpeeds(global_vel.getOrigin().getX(), global_vel.getOrigin().getY(), tf::getYaw(global_vel.getRotation()));

    result_traj_.cost_ = -7;
    // find best trajectory by sampling and scoring the samples
    std::vector<base_local_planner::Trajectory> all_explored;
    scored_sampling_planner_.findBestTrajectory(result_traj_, &all_explored);

    if(publish_traj_pc_)
    {
        base_local_planner::MapGridCostPoint pt;
        traj_cloud_->points.clear();
        traj_cloud_->width = 0;
        traj_cloud_->height = 0;
        std_msgs::Header header;
        pcl_conversions::fromPCL(traj_cloud_->header, header);
        header.stamp = ros::Time::now();
        traj_cloud_->header = pcl_conversions::toPCL(header);
        for(std::vector<base_local_planner::Trajectory>::iterator t=all_explored.begin(); t != all_explored.end(); ++t)
        {
            if(t->cost_<0)
                continue;
            // Fill out the plan
            for(unsigned int i = 0; i < t->getPointsSize(); ++i) {
                double p_x, p_y, p_th;
                t->getPoint(i, p_x, p_y, p_th);
                pt.x=p_x;
                pt.y=p_y;
                pt.z=0;
                pt.path_cost=p_th;
                pt.total_cost=t->cost_;
                traj_cloud_->push_back(pt);
            }
        }
        traj_cloud_pub_.publish(*traj_cloud_);
    }

    // verbose publishing of point clouds
    if (publish_cost_grid_pc_) {
      //we'll publish the visualization of the costs to rviz before returning our best trajectory
      map_viz_.publishCostCloud(planner_util_->getCostmap());
    }

    // debrief stateful scoring functions
    oscillation_costs_.updateOscillationFlags(pos, &result_traj_, planner_util_->getCurrentLimits().min_trans_vel);

    //if we don't have a legal trajectory, we'll just command zero
    if (result_traj_.cost_ < 0) {
      drive_velocities.setIdentity();
    } else {
      tf::Vector3 start(result_traj_.xv_, result_traj_.yv_, 0);
      drive_velocities.setOrigin(start);
      tf::Matrix3x3 matrix;
      matrix.setRotation(tf::createQuaternionFromYaw(result_traj_.thetav_));
      drive_velocities.setBasis(matrix);
    }

    return result_traj_;
  }
Beispiel #8
0
  void EDWAPlanner::updatePlanAndLocalCosts(
      tf::Stamped<tf::Pose> global_pose,
      const std::vector<geometry_msgs::PoseStamped>& new_plan) {
    global_plan_.resize(new_plan.size());
    for (unsigned int i = 0; i < new_plan.size(); ++i) {
      global_plan_[i] = new_plan[i];
    }

    // costs for going away from path
    path_costs_.setTargetPoses(global_plan_);

    // costs for not going towards the local goal as much as possible
    goal_costs_.setTargetPoses(global_plan_);

    // alignment costs
    geometry_msgs::PoseStamped goal_pose = global_plan_.back();

    Eigen::Vector3f pos(global_pose.getOrigin().getX(), global_pose.getOrigin().getY(), tf::getYaw(global_pose.getRotation()));
    double sq_dist =
        (pos[0] - goal_pose.pose.position.x) * (pos[0] - goal_pose.pose.position.x) +
        (pos[1] - goal_pose.pose.position.y) * (pos[1] - goal_pose.pose.position.y);

    // we want the robot nose to be drawn to its final position
    // (before robot turns towards goal orientation), not the end of the
    // path for the robot center. Choosing the final position after
    // turning towards goal orientation causes instability when the
    // robot needs to make a 180 degree turn at the end
    std::vector<geometry_msgs::PoseStamped> front_global_plan = global_plan_;
    double angle_to_goal = atan2(goal_pose.pose.position.y - pos[1], goal_pose.pose.position.x - pos[0]);
    front_global_plan.back().pose.position.x = front_global_plan.back().pose.position.x +
      forward_point_distance_ * cos(angle_to_goal);
    front_global_plan.back().pose.position.y = front_global_plan.back().pose.position.y + forward_point_distance_ *
      sin(angle_to_goal);

    goal_front_costs_.setTargetPoses(front_global_plan);
    
    // keeping the nose on the path
    if (sq_dist > forward_point_distance_ * forward_point_distance_ * cheat_factor_) {
      double resolution = planner_util_->getCostmap()->getResolution();
      alignment_costs_.setScale(resolution * pdist_scale_ * 0.5);
      // costs for robot being aligned with path (nose on path, not ju
      alignment_costs_.setTargetPoses(global_plan_);
    } else {
      // once we are close to goal, trying to keep the nose close to anything destabilizes behavior.
      alignment_costs_.setScale(0.0);
    }
  }
bool goalOrientationReached(const tf::Stamped<tf::Pose>& global_pose, double goal_th, double yaw_goal_tolerance) {
    double yaw = tf::getYaw(global_pose.getRotation());
    return fabs(angles::shortest_angular_distance(yaw, goal_th)) <= yaw_goal_tolerance;
}
  //given the current state of the robot, find a good trajectory
  base_local_planner::Trajectory DWAPlanner::findBestPath(tf::Stamped<tf::Pose> global_pose, tf::Stamped<tf::Pose> global_vel, 
      tf::Stamped<tf::Pose>& drive_velocities){

    //make sure that our configuration doesn't change mid-run
    boost::mutex::scoped_lock l(configuration_mutex_);

    //make sure to get an updated copy of the costmap before computing trajectories
    costmap_ros_->getCostmapCopy(costmap_);

    Eigen::Vector3f pos(global_pose.getOrigin().getX(), global_pose.getOrigin().getY(), tf::getYaw(global_pose.getRotation()));
    Eigen::Vector3f vel(global_vel.getOrigin().getX(), global_vel.getOrigin().getY(), tf::getYaw(global_vel.getRotation()));

    //reset the map for new operations
    map_.resetPathDist();
    front_map_.resetPathDist();

    //make sure that we update our path based on the global plan and compute costs
    map_.setPathCells(costmap_, global_plan_);

    std::vector<geometry_msgs::PoseStamped> front_global_plan = global_plan_;
    front_global_plan.back().pose.position.x = front_global_plan.back().pose.position.x + forward_point_distance_ * cos(tf::getYaw(front_global_plan.back().pose.orientation));
    front_global_plan.back().pose.position.y = front_global_plan.back().pose.position.y + forward_point_distance_ * sin(tf::getYaw(front_global_plan.back().pose.orientation));
    front_map_.setPathCells(costmap_, front_global_plan);
    ROS_DEBUG_NAMED("dwa_local_planner", "Path/Goal distance computed");

    //rollout trajectories and find the minimum cost one
    base_local_planner::Trajectory best = computeTrajectories(pos, vel);
    ROS_DEBUG_NAMED("dwa_local_planner", "Trajectories created");

    //if we don't have a legal trajectory, we'll just command zero
    if(best.cost_ < 0){
      drive_velocities.setIdentity();
    }
    else{
      tf::Vector3 start(best.xv_, best.yv_, 0);
      drive_velocities.setOrigin(start);
      tf::Matrix3x3 matrix;
      matrix.setRotation(tf::createQuaternionFromYaw(best.thetav_));
      drive_velocities.setBasis(matrix);
    }

    //we'll publish the visualization of the costs to rviz before returning our best trajectory
    map_viz_.publishCostCloud();

    return best;
  }
void SlamGMapping::writeLaserPoseStamped(tf::Stamped<tf::Transform>& laser_pose){

	tf::Quaternion quat = laser_pose.getRotation();
	fwrite(&quat,sizeof(tf::Quaternion),1,laserposefile);
	fwrite(&laser_pose.getOrigin(),sizeof(tf::Vector3),1,laserposefile);
}
  /*
   * given the current state of the robot, find a good trajectory
   */
  base_local_planner::Trajectory DWAPlanner::findBestPath(
      tf::Stamped<tf::Pose> global_pose,
      tf::Stamped<tf::Pose> global_vel,
      tf::Stamped<tf::Pose>& drive_velocities,
      std::vector<geometry_msgs::Point> footprint_spec) {

    obstacle_costs_.setFootprint(footprint_spec);

    //make sure that our configuration doesn't change mid-run
    boost::mutex::scoped_lock l(configuration_mutex_);

    Eigen::Vector3f pos(global_pose.getOrigin().getX(), global_pose.getOrigin().getY(), tf::getYaw(global_pose.getRotation()));
    Eigen::Vector3f vel(global_vel.getOrigin().getX(), global_vel.getOrigin().getY(), tf::getYaw(global_vel.getRotation()));
    geometry_msgs::PoseStamped goal_pose = global_plan_.back();
    Eigen::Vector3f goal(goal_pose.pose.position.x, goal_pose.pose.position.y, tf::getYaw(goal_pose.pose.orientation));
    base_local_planner::LocalPlannerLimits limits = planner_util_->getCurrentLimits();

    // prepare cost functions and generators for this run
    generator_.initialise(pos,
        vel,
        goal,
        &limits,
        vsamples_);

    result_traj_.cost_ = -7;
    // find best trajectory by sampling and scoring the samples
    scored_sampling_planner_.findBestTrajectory(result_traj_, NULL);

    // verbose publishing of point clouds
    if (publish_cost_grid_pc_) {
      //we'll publish the visualization of the costs to rviz before returning our best trajectory
      map_viz_.publishCostCloud(*(planner_util_->getCostmap()));
    }

    // debrief stateful scoring functions
    oscillation_costs_.updateOscillationFlags(pos, &result_traj_, planner_util_->getCurrentLimits().min_trans_vel);

    //if we don't have a legal trajectory, we'll just command zero
    if (result_traj_.cost_ < 0) {
      drive_velocities.setIdentity();
    } else {
      tf::Vector3 start(result_traj_.xv_, result_traj_.yv_, 0);
      drive_velocities.setOrigin(start);
      tf::Matrix3x3 matrix;
      matrix.setRotation(tf::createQuaternionFromYaw(result_traj_.thetav_));
      drive_velocities.setBasis(matrix);
    }

    return result_traj_;
  }
  //given the current state of the robot, find a good trajectory
  Trajectory TrajectoryPlanner::findBestPath(tf::Stamped<tf::Pose> global_pose, tf::Stamped<tf::Pose> global_vel, 
      tf::Stamped<tf::Pose>& drive_velocities){

    Eigen::Vector3f pos(global_pose.getOrigin().getX(), global_pose.getOrigin().getY(), tf::getYaw(global_pose.getRotation()));
    Eigen::Vector3f vel(global_vel.getOrigin().getX(), global_vel.getOrigin().getY(), tf::getYaw(global_vel.getRotation()));

    //reset the map for new operations
    path_map_.resetPathDist();
    goal_map_.resetPathDist();

    //temporarily remove obstacles that are within the footprint of the robot
    std::vector<base_local_planner::Position2DInt> footprint_list =
        footprint_helper_.getFootprintCells(
            pos,
            footprint_spec_,
            costmap_,
            true);

    //mark cells within the initial footprint of the robot
    for (unsigned int i = 0; i < footprint_list.size(); ++i) {
      path_map_(footprint_list[i].x, footprint_list[i].y).within_robot = true;
    }

    //make sure that we update our path based on the global plan and compute costs
    path_map_.setTargetCells(costmap_, global_plan_);
    goal_map_.setLocalGoal(costmap_, global_plan_);
    ROS_DEBUG("Path/Goal distance computed");

    //rollout trajectories and find the minimum cost one
    Trajectory best = createTrajectories(pos[0], pos[1], pos[2],
        vel[0], vel[1], vel[2],
        acc_lim_x_, acc_lim_y_, acc_lim_theta_);
    ROS_DEBUG("Trajectories created");

    /*
    //If we want to print a ppm file to draw goal dist
    char buf[4096];
    sprintf(buf, "base_local_planner.ppm");
    FILE *fp = fopen(buf, "w");
    if(fp){
      fprintf(fp, "P3\n");
      fprintf(fp, "%d %d\n", map_.size_x_, map_.size_y_);
      fprintf(fp, "255\n");
      for(int j = map_.size_y_ - 1; j >= 0; --j){
        for(unsigned int i = 0; i < map_.size_x_; ++i){
          int g_dist = 255 - int(map_(i, j).goal_dist);
          int p_dist = 255 - int(map_(i, j).path_dist);
          if(g_dist < 0)
            g_dist = 0;
          if(p_dist < 0)
            p_dist = 0;
          fprintf(fp, "%d 0 %d ", g_dist, 0);
        }
        fprintf(fp, "\n");
      }
      fclose(fp);
    }
    */

    if(best.cost_ < 0){
      drive_velocities.setIdentity();
    }
    else{
      tf::Vector3 start(best.xv_, best.yv_, 0);
      drive_velocities.setOrigin(start);
      tf::Matrix3x3 matrix;
      matrix.setRotation(tf::createQuaternionFromYaw(best.thetav_));
      drive_velocities.setBasis(matrix);
    }

    return best;
  }