ROS学习笔记（十三）：Using Stamped datatypes with tf::MessageFilter

本教程介绍了如何在tf中使用传感器数据。 传感器数据的一些实际示例是：

• 单声道和立体声相机
• 激光扫描

假设创建了一个新的乌龟，名为turtle3，并且没有很好的里程表，但是有一个高架摄像机跟踪其位置并将其发布为与世界框架有关的geometry_msgs / PointStamped消息。
乌龟1想要知道将turtle3与自身进行比较的地方。

为此，turtle1必须聆听正在发布turtle3姿势的主题，等到准备好转换为所需的帧后再进行操作。 为了简化操作，tf :: MessageFilter类非常有用。 tf :: MessageFilter将使用标头对任何ros消息进行订阅，并将其缓存，直到可以将其转换为目标帧为止。

1.Setup

roslaunch turtle_tf turtle_tf_sensor.launch 

接着会出现：

这将使乌龟1自行驱动，乌龟3自行移动。 有一个主题turtle_pose_stamped，其中包含geometry_msgs / PoseStamped数据类型，说明了turtle3在世界框架中的位置。

为了可靠地在turtle1的帧中获取流数据，我们将使用以下代码：

切换行号显示

   1 #include "ros/ros.h"
   2 #include "tf/transform_listener.h"
   3 #include "tf/message_filter.h"
   4 #include "message_filters/subscriber.h"
   5 
   6 class PoseDrawer
   7 {
   8 public:
   9   PoseDrawer() : tf_(),  target_frame_("turtle1")
  10   {
  11     point_sub_.subscribe(n_, "turtle_point_stamped", 10);
  12     tf_filter_ = new tf::MessageFilter<geometry_msgs::PointStamped>(point_sub_, tf_, target_frame_, 10);
  13     tf_filter_->registerCallback( boost::bind(&PoseDrawer::msgCallback, this, _1) );
  14   } ;
  15 
  16 private:
  17   message_filters::Subscriber<geometry_msgs::PointStamped> point_sub_;
  18   tf::TransformListener tf_;
  19   tf::MessageFilter<geometry_msgs::PointStamped> * tf_filter_;
  20   ros::NodeHandle n_;
  21   std::string target_frame_;
  22 
  23   //  Callback to register with tf::MessageFilter to be called when transforms are available
  24   void msgCallback(const boost::shared_ptr<const geometry_msgs::PointStamped>& point_ptr) 
  25   {
  26     geometry_msgs::PointStamped point_out;
  27     try 
  28     {
  29       tf_.transformPoint(target_frame_, *point_ptr, point_out);
  30       
  31       printf("point of turtle 3 in frame of turtle 1 Position(x:%f y:%f z:%f)\n", 
  32              point_out.point.x,
  33              point_out.point.y,
  34              point_out.point.z);
  35     }
  36     catch (tf::TransformException &ex) 
  37     {
  38       printf ("Failure %s\n", ex.what()); //Print exception which was caught
  39     }
  40   };
  41 
  42 };
  43 
  44 
  45 int main(int argc, char ** argv)
  46 {
  47   ros::init(argc, argv, "pose_drawer"); //Init ROS
  48   PoseDrawer pd; //Construct class
  49   ros::spin(); // Run until interupted 
  50 };

2.Explanation

2.1Includes
您必须包括tf包中的tf :: MessageFilter标头。 以及以前使用的tf和ROS标头。

   1 #include "ros/ros.h"
   2 #include "tf/transform_listener.h"
   3 #include "tf/message_filter.h"
   4 #include "message_filters/subscriber.h"
   5 

2.2Persistent Data
必须有tf :: TransformListener tf :: MessageFilter的持久实例

切换行号显示

  16 private:
  17   message_filters::Subscriber<geometry_msgs::PointStamped> point_sub_;
  18   tf::TransformListener tf_;
  19   tf::MessageFilter<geometry_msgs::PointStamped> * tf_filter_;
  20   ros::NodeHandle n_;
  21   std::string target_frame_;

2.3Constructor
在启动ros message_filters :: Subscriber时，必须使用该主题进行初始化。 并且tf :: MessageFilter必须使用该Subscriber对象进行初始化。 MessageFilter构造函数中的其他注释参数是target_frame和回调函数。 目标框架是将确保canTransform成功的框架。 回调函数是在数据准备好后将被调用的函数。

 切换行号显示

   9   PoseDrawer() : tf_(),  target_frame_("turtle1")
  10   {
  11     point_sub_.subscribe(n_, "turtle_point_stamped", 10);
  12     tf_filter_ = new tf::MessageFilter<geometry_msgs::PointStamped>(point_sub_, tf_, target_frame_, 10);
  13     tf_filter_->registerCallback( boost::bind(&PoseDrawer::msgCallback, this, _1) );
  14   } ;

2.4Callback Method
数据准备好后，只需调用transformPose并打印到教程的屏幕即可。

  24   void msgCallback(const boost::shared_ptr<const geometry_msgs::PointStamped>& point_ptr) 
  25   {
  26     geometry_msgs::PointStamped point_out;
  27     try 
  28     {
  29       tf_.transformPoint(target_frame_, *point_ptr, point_out);
  30       
  31       printf("point of turtle 3 in frame of turtle 1 Position(x:%f y:%f z:%f)\n", 
  32              point_out.point.x,
  33              point_out.point.y,
  34              point_out.point.z);
  35     }
  36     catch (tf::TransformException &ex) 
  37     {
  38       printf ("Failure %s\n", ex.what()); //Print exception which was caught
  39     }
  40   };
  41 
  42 };

3.Result

point of turtle 3 in frame of turtle 1 Position(x:-0.603264 y:4.276489 z:0.000000)
point of turtle 3 in frame of turtle 1 Position(x:-0.598923 y:4.291888 z:0.000000)
point of turtle 3 in frame of turtle 1 Position(x:-0.594828 y:4.307356 z:0.000000)
point of turtle 3 in frame of turtle 1 Position(x:-0.590981 y:4.322886 z:0.000000)