让我知道 这样可以 直接转换点的坐标 通过监听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