tf 變換 重要文章總結

讓我知道 這樣可以 直接轉換點的座標 通過監聽transform
ROS之tf空間座標變換淺析 - https://blog.csdn.net/u014587147/article/details/76377024

class TransformListener : public Transformer { 
public:
  TransformListener(ros::Duration max_cache_time = ros::Duration(DEFAULT_CACHE_TIME), bool spin_thread = true);
  TransformListener(const ros::NodeHandle& nh,
                    ros::Duration max_cache_time = ros::Duration(DEFAULT_CACHE_TIME), bool spin_thread = true);

  ~TransformListener();
...
  void transformPoint(const std::string& target_frame, const geometry_msgs::PointStamped& stamped_in, geometry_msgs::PointStamped& stamped_out) const;
 ...
}

geometry_msgs 的所有消息種類， 瞭解每個消息的內容及相互之間的差異

ROS Message Types 很全， 每個有詳細的鏈接， http://wiki.ros.org/geometry_msgs

ROS Message Types
Accel
AccelStamped
AccelWithCovariance
AccelWithCovarianceStamped
Inertia
InertiaStamped
Point
Point32
PointStamped
Polygon
PolygonStamped
Pose
Pose2D
PoseArray
PoseStamped
PoseWithCovariance
PoseWithCovarianceStamped
Quaternion
QuaternionStamped
Transform
TransformStamped
Twist
TwistStamped
TwistWithCovariance
TwistWithCovarianceStamped
Vector3
Vector3Stamped
Wrench
WrenchStamped

http://wiki.ros.org/geometry_msgs

主要的 tf 相關數據類型
ROS與C++入門教程-tf-數據類型 https://www.ncnynl.com/archives/201702/1305.html

tf::StampedTransform

tf::StampedTransform 是tf::Transforms的特例，它要求frame_id 、stamp 、child_frame_id.

** \brief The Stamped Transform datatype used by tf */
class StampedTransform : public tf::Transform
{
public:
  ros::Time stamp_; ///< The timestamp associated with this transform                                                                                                                                                                                                                                                        
  std::string frame_id_; ///< The frame_id of the coordinate frame  in which this transform is defined                                                                                                                                                                                                                       
  std::string child_frame_id_; ///< The frame_id of the coordinate frame this transform defines                                                                                                                                                                                                                              
  StampedTransform(const tf::Transform& input, const ros::Time& timestamp, const std::string & frame_id, const std::string & child_frame_id):
    tf::Transform (input), stamp_ ( timestamp ), frame_id_ (frame_id), child_frame_id_(child_frame_id){ };

  /** \brief Default constructor only to be used for preallocation */
  StampedTransform() { };

  /** \brief Set the inherited Traonsform data */
  void setData(const tf::Transform& input){*static_cast<tf::Transform*>(this) = input;};

};

有些比較好的查看函數
ROS Indigo 進階筆記 (二) tf http://makaidong.com/youngpan1101/1/156515_10690007.html

$ rosrun tf view_frames  （生成 pdf 文檔）
$ evince frames.pdf   （打開 pdf 文檔）
$ rosrun rviz rviz -d `rospack find turtle_tf`/rviz/turtle_rviz.rviz //使用 rviz 更直觀地查看三個座標系隨時間的變換
$ rosrun tf tf_echo /world /turtle1

530 void DatabaseUpdator::LaserCallback(const sensor_msgs::LaserScan::ConstPtr& scan)
531 {
532     ros::Rate rate(GET_LASER_FRE);
533     tf::StampedTransform transform;
534     tf::TransformListener listener_;
535     ROS_INFO("Receive Laser Data, Length is  [%lu]", scan->ranges.size());
536 
537     // Get info about the scan
538     float angle_min = scan->angle_min;
539     float angle_max = scan->angle_max;
540     float angle_increment = scan->angle_increment;
541     float range_min = scan->range_min;
542     float range_max = scan->range_max;
543 
544     float angle = angle_min;
545     int laser_length = sizeof(scan->ranges) / sizeof(double);
546 
547     nlohmann::json scan_points_xy_info;
548     scan_points_xy_info["scan_points"] = nlohmann::json::array();
549     nlohmann::json xycoordinate;
550 
551 
552     // get the \scan to \map transform when recieve a transform from \base_link to \map
553     try
554      {
555        listener_.waitForTransform("/map", "/base_laser_link",
556                             ros::Time::now(),  ros::Duration(3));
557 
558        listener_.lookupTransform("/map", "/base_laser_link",
559                             ros::Time(0), transform );
560       
561       }catch(tf::TransformException &e){
562        ROS_WARN("%s,%s,%d:%s\n", __FILE__, __FUNCTION__, __LINE__, e.what());
563       }
564 
565 
566 
567     // Deal with scan
568     for(std::vector<float>::const_iterator it = scan->ranges.begin();
569       it != scan->ranges.end();
570       it += 2)
571     {
572           //the angle of each laser
573           angle += 2.0 *  angle_increment;
574 
575           //Change the coordinate of the laser data
576           if(*it < range_max && *it > range_min)  // the laser data must be  in validate range
577           {
578                 // x y in local 
579                 double x_ = (*it)*cos(angle);
580                 double y_ = (*it)*sin(angle);
581 
582                 geometry_msgs::PointStamped laser_point;
583                 laser_point.header.frame_id = scan->header.frame_id;
584                 laser_point.header.stamp = ros::Time();
585                 laser_point.point.x = x_;
586                 laser_point.point.y = y_;
587                 laser_point.point.z = 0.0;
588 
589 
590                 // transform point in /scan frame to point in /map frame
591                 geometry_msgs::PointStamped base_point;
592                 base_point.header.frame_id = "map";
593                 listener_.transformPoint("/map", laser_point, base_point);
594 
595                 // Save the transformed point to queue
596                 xycoordinate["x"] = base_point.point.x;
597                 xycoordinate["y"] = base_point.point.y;
598                 scan_points_xy_info["scan_points"].push_back(xycoordinate);
599                 }
600       }
601 
602      std::string scan_points_str;
603      try{
604            scan_points_str = scan_points_xy_info.dump();
605 
606      }catch(nlohmann::json::exception& e){
607            ROS_WARN("%s,%s,%d:%s\n", __FILE__, __FUNCTION__, __LINE__, e.what());
608            return;
609      }
610     mongodb_manager_.update_db("robot_status_getscan_req", scan_points_str);
611 }
612