C++ tf::Stamped类代码示例

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);
    */
}

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);*/

}

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);

}

// compute nav fn (using wavefront assumption)
void getNavFn(double* cost_map, tf::StampedTransform &robot_to_map_transform, double* nav_fn) {
  Point robot_pt(robot_to_map_transform.getOrigin().getX() / MAP_RESOLUTION,robot_to_map_transform.getOrigin().getY() / MAP_RESOLUTION);
  Point goal_pt(goal_pose.getOrigin().getX() / MAP_RESOLUTION, goal_pose.getOrigin().getY() / MAP_RESOLUTION);
  ROS_INFO("[getNavFn]\trobot_pt: (%d,%d), goal_pt(%d,%d)",robot_pt.x,robot_pt.y,goal_pt.x,goal_pt.y);

  // setup init map
  bool init_set_map[MAP_CELLS];
  for (int i=0; i<MAP_CELLS;i++) {
    init_set_map[i] = false;
  }
  setVal(init_set_map,goal_pt.x,goal_pt.y,true);

  //updateMapDijkstra(nav_fn, init_set_map, goal_pt, cost_map, INT_MAX);
  LPN2(nav_fn, cost_map, goal_pt, robot_pt);
}

  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);
    }
  }

void publishMarker(const tf::Stamped<tf::Point>& target) {

	//create marker object
	visualization_msgs::Marker marker;


	uint32_t shape = visualization_msgs::Marker::SPHERE;

	// Set the frame ID and timestamp.
	marker.header.frame_id = target.frame_id_;
	marker.header.stamp = ros::Time::now();

	marker.ns = "head_target";
	marker.id = 0;

	// Set the marker type.
	marker.type = shape;

	// Set the marker action.
	marker.action = visualization_msgs::Marker::ADD;

	// Set the pose of the marker.
	marker.pose.position.x = target.getX();
	marker.pose.position.y = target.getY();
	marker.pose.position.z = target.getZ();
	marker.pose.orientation.x = 0.0;
	marker.pose.orientation.y = 0.0;
	marker.pose.orientation.z = 0.0;
	marker.pose.orientation.w = 1.0;

	// Set the scale of the marker -- 1x1x1 here means 1m on a side
	marker.scale.x = 0.08;
	marker.scale.y = 0.08;
	marker.scale.z = 0.08;

	// Set the color -- be sure to set alpha to something non-zero!
	marker.color.r = 1.0f;
	marker.color.g = 0.0f;
	marker.color.b = 1.0f;
	marker.color.a = 0.6;

	marker.lifetime = ros::Duration();

	// Publish the marker
	marker_pub_.publish(marker);

}

 void prunePlan(const tf::Stamped<tf::Pose>& global_pose, std::vector<geometry_msgs::PoseStamped>& plan, std::vector<geometry_msgs::PoseStamped>& global_plan){
   ROS_ASSERT(global_plan.size() >= plan.size());
   std::vector<geometry_msgs::PoseStamped>::iterator it = plan.begin();
   std::vector<geometry_msgs::PoseStamped>::iterator global_it = global_plan.begin();
   while(it != plan.end()){
     const geometry_msgs::PoseStamped& w = *it;
     // Fixed error bound of 2 meters for now. Can reduce to a portion of the map size or based on the resolution
     double x_diff = global_pose.getOrigin().x() - w.pose.position.x;
     double y_diff = global_pose.getOrigin().y() - w.pose.position.y;
     double distance_sq = x_diff * x_diff + y_diff * y_diff;
     if(distance_sq < 1){
       ROS_DEBUG("Nearest waypoint to <%f, %f> is <%f, %f>\n", global_pose.getOrigin().x(), global_pose.getOrigin().y(), w.pose.position.x, w.pose.position.y);
       break;
     }
     it = plan.erase(it);
     global_it = global_plan.erase(global_it);
   }
 }

int main(int argc, char** argv) {
  // init node
  ros::init(argc, argv, "planner_simple");    
  ros::NodeHandle nh;
  
  // set up subscribers, publishers and listeners
  ros::Subscriber goal_sub = nh.subscribe("/goal", 1000, goalCallback);
  ros::Subscriber map_sub = nh.subscribe("/map", 1000, mapCallback);
  //ros::Subscriber obstacle_map_sub = nh.subscribe("/obstacle_map", 1000, mapCallback);
  marker_pub = nh.advertise<visualization_msgs::Marker>( "visualization_marker", 0 );
  map_pub = nh.advertise<assn2::NavGrid>( "nav_grid", 0 );
  tf::TransformListener listener;  
  listener.waitForTransform("/map", "/base_link", ros::Time(), ros::Duration(1.5));
  
  // set initial goal to be the robot's pose
  try {
    tf::StampedTransform map_base_link_transform; //lookup transform only works for this type
    listener.lookupTransform("/map", "/base_link", ros::Time(0), map_base_link_transform);
    goal_pose.setOrigin(map_base_link_transform.getOrigin());
    goal_pose.setRotation(map_base_link_transform.getRotation());
  } catch (tf::TransformException ex) {
    ROS_ERROR("%s",ex.what());
  }
  
  ros::Rate rate(10);
  while (ros::ok()) {
    // populates the goal_pose and global_map if a new message has been published
    ros::spinOnce(); 

    double cost_map[MAP_CELLS];
    getCostFn(cost_map);
    visualizeHeatMap(cost_map,"intrinsic_cost_map");

    double nav_fn[MAP_CELLS];
    tf::StampedTransform robot_to_map_transform; // the pose of the robot in the map frame
    tf_util::getTransform(listener,"/base_link","/map",robot_to_map_transform);
    getNavFn(cost_map,robot_to_map_transform, nav_fn);
    visualizeHeatMap(nav_fn,"nav_fn");

    publishMap(nav_fn);
    rate.sleep();
  }
}

bool getGoalPose (const tf::TransformListener& tf,
                  const std::vector<geometry_msgs::PoseStamped>& global_plan,
                  const std::string& global_frame, tf::Stamped<tf::Pose>& goal_pose)
{
    if (global_plan.empty())
    {
        ROS_ERROR ("Received plan with zero length");
        return false;
    }

    const geometry_msgs::PoseStamped& plan_goal_pose = global_plan.back();

    try
    {
        tf::StampedTransform transform;
        tf.waitForTransform (global_frame, ros::Time::now(),
                             plan_goal_pose.header.frame_id, plan_goal_pose.header.stamp,
                             plan_goal_pose.header.frame_id, ros::Duration (0.5));
        tf.lookupTransform (global_frame, ros::Time(),
                            plan_goal_pose.header.frame_id, plan_goal_pose.header.stamp,
                            plan_goal_pose.header.frame_id, transform);

        poseStampedMsgToTF (plan_goal_pose, goal_pose);
        goal_pose.setData (transform * goal_pose);
        goal_pose.stamp_ = transform.stamp_;
        goal_pose.frame_id_ = global_frame;

    }
    catch (tf::LookupException& ex)
    {
        ROS_ERROR ("No Transform available Error: %s\n", ex.what());
        return false;
    }
    catch (tf::ConnectivityException& ex)
    {
        ROS_ERROR ("Connectivity Error: %s\n", ex.what());
        return false;
    }
    catch (tf::ExtrapolationException& ex)
    {
        ROS_ERROR ("Extrapolation Error: %s\n", ex.what());

        if (global_plan.size() > 0)
            ROS_ERROR ("Global Frame: %s Plan Frame size %d: %s\n", global_frame.c_str(), (unsigned int) global_plan.size(), global_plan[0].header.frame_id.c_str());

        return false;
    }

    return true;
}

void OdometryHelperRos::getRobotVel(tf::Stamped<tf::Pose>& robot_vel) {
  // Set current velocities from odometry
  geometry_msgs::Twist global_vel;
  {
    boost::mutex::scoped_lock lock(odom_mutex_);
    global_vel.linear.x = base_odom_.twist.twist.linear.x;
    global_vel.linear.y = base_odom_.twist.twist.linear.y;
    global_vel.angular.z = base_odom_.twist.twist.angular.z;

    robot_vel.frame_id_ = base_odom_.child_frame_id;
  }
  robot_vel.setData(tf::Transform(tf::createQuaternionFromYaw(global_vel.angular.z), tf::Vector3(global_vel.linear.x, global_vel.linear.y, 0)));
  robot_vel.stamp_ = ros::Time();
}

bool
AmclNode::getOdomPose(tf::Stamped<tf::Pose>& odom_pose,
                      double& x, double& y, double& yaw,
                      const ros::Time& t, const std::string& f)
{
    // Get the robot's pose
    tf::Stamped<tf::Pose> ident (tf::Transform(tf::createIdentityQuaternion(),
                                 tf::Vector3(0,0,0)), t, f);
    try
    {
        this->tf_->transformPose(odom_frame_id_, ident, odom_pose);
    }
    catch(tf::TransformException e)
    {
        ROS_WARN("Failed to compute odom pose, skipping scan (%s)", e.what());
        return false;
    }
    x = odom_pose.getOrigin().x();
    y = odom_pose.getOrigin().y();
    double pitch,roll;
    odom_pose.getBasis().getEulerYPR(yaw, pitch, roll);

    return true;
}

  /*
   * 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
  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;
  }

bool Costmap2DROS::getRobotPose(tf::Stamped<tf::Pose>& global_pose) const
{

  global_pose.setIdentity();
  tf::Stamped < tf::Pose > robot_pose;
  robot_pose.setIdentity();
  robot_pose.frame_id_ = robot_base_frame_;
  robot_pose.stamp_ = ros::Time();
  ros::Time current_time = ros::Time::now(); // save time for checking tf delay later

  //get the global pose of the robot
  try
  {
    tf_.transformPose(global_frame_, robot_pose, global_pose);
  }
  catch (tf::LookupException& ex)
  {
    ROS_ERROR_THROTTLE(1.0, "No Transform available Error looking up robot pose: %s\n", ex.what());
    return false;
  }
  catch (tf::ConnectivityException& ex)
  {
    ROS_ERROR_THROTTLE(1.0, "Connectivity Error looking up robot pose: %s\n", ex.what());
    return false;
  }
  catch (tf::ExtrapolationException& ex)
  {
    ROS_ERROR_THROTTLE(1.0, "Extrapolation Error looking up robot pose: %s\n", ex.what());
    return false;
  }
  // check global_pose timeout
  if (current_time.toSec() - global_pose.stamp_.toSec() > transform_tolerance_)
  {
    ROS_WARN_THROTTLE(1.0,
                      "Costmap2DROS transform timeout. Current time: %.4f, global_pose stamp: %.4f, tolerance: %.4f",
                      current_time.toSec(), global_pose.stamp_.toSec(), transform_tolerance_);
    return false;
  }

  return true;
}