ros c++ src code

ros src code

tf 订阅转换：
  初始化，一个transform：
  tf::Transform map_to_odom_;  
  map_to_odom_(tf::Transform(tf::createQuaternionFromRPY( 0, 0, 0 ), tf::Point(0, 0, 0 )))；

  tf::TransformListener tf_;
  float max_duration_buffer = 0.2;
  tf_(ros::Duration(max_duration_buffer);
  laser_frame_ = "scan";
  // Get the laser's pose, relative to base.
  tf::Stamped<tf::Pose> ident;
  tf::Stamped<tf::Transform> laser_pose;
  ident.setIdentity();
  ident.frame_id_ = laser_frame_;
  ident.stamp_ = scan.header.stamp;
  try
  {
    tf_.transformPose("base_link", ident, laser_pose);
  }
  catch(tf::TransformException e)
  {
    ROS_WARN("Failed to compute laser pose, aborting initialization (%s)",
             e.what());
    return false;
  }
  double yaw = tf::getYaw(odom_pose.getRotation());
  double x = odom_pose.getOrigin().x();
  double y = odom_pose.getOrigin().y();


  // create a point 1m above the laser position and transform it into the laser-frame
  tf::Vector3 v;
  v.setValue(0, 0, 1 + laser_pose.getOrigin().z());
  tf::Stamped<tf::Vector3> up(v, scan.header.stamp,
                                      base_frame_);
  try
  {
    tf_.transformPoint(laser_frame_, up, up);
    ROS_DEBUG("Z-Axis in sensor frame: %.3f", up.z());
  }
  catch(tf::TransformException& e)
  {
    ROS_WARN("Unable to determine orientation of laser: %s",
             e.what());
  }


  tf2::Vector3 transform_base_link_to_map_position(pose.position.x, pose.position.y, pose.position.z);
  tf2::Quaternion transform_base_link_to_map_orientation(pose.orientation.x, pose.orientation.y, pose.orientation.z, pose.orientation.w);
  transform_base_link_to_map_orientation.normalize();
  tf2::Transform transform_base_link_to_map(transform_base_link_to_map_orientation, transform_base_link_to_map_position);

  //  tf or latest tf
  tf2::Transform transform_odom_to_base_link;
  if (pose_to_tf_publisher_->getTfCollector().lookForTransform(transform_odom_to_base_link, base_link_frame_id_, odom_frame_id_, pose_time, tf_timeout_)) {
      tf_available = true;
   } else {
      if (pose_to_tf_publisher_->getTfCollector().lookForTransform(transform_odom_to_base_link, base_link_frame_id_, odom_frame_id_, ros::Time(0), tf_timeout_)) {
      ROS_WARN_STREAM("Set new initial pose using latest TF because there is no transform from frame [" << base_link_frame_id_ << "] to frame [" << odom_frame_id_ << "] at time " << pose_time);
      tf_available = true;
   }