ROS2(一)- tf之 MessageFilter 与 tf::MessageFilter理解与应用_ros2 message filter

1 MessageFilter

1.1 主要用法之——消息的订阅与回调

1.2 主要用法之——时间同步

1.3 主要用法之——时间顺序的回调

2 tf::MessageFilter

2.1 示例AMCL

2.2 wiki教程

3 tf2_ros::MessageFilter

3.1 wiki教程

4 tf2_ros之使用tf进行坐标变换

4.1 tf::Transformer Class Reference

4.2 tf2_ros::BufferInterface Class Reference

references

---

1 MessageFilter

http://wiki.ros.org/message_filters

消息过滤器message_filters类似一个消息缓存，当消息到达消息过滤器的时候，可能并不会立即输出，而是在稍后的时间点里满足一定条件下输出。
举个例子，比如时间同步器，它接收来自多个源的不同类型的消息，并且仅当它们在具有相同时间戳的每个源上接收到消息时才输出它们，也就是起到了一个消息同步输出的效果。

1.1 主要用法之——消息的订阅与回调

message_filters::Subscriber<std_msgs::UInt32> sub(nh, "my_topic", 1);
sub.registerCallback(myCallback);

is the equivalent of:

ros::Subscriber sub = nh.subscribe("my_topic", 1, myCallback);

1.2 主要用法之——时间同步

TimeSynchronizer筛选器通过包含在其header中的时间戳同步进入的通道，并以采用相同数量通道的单个回调的形式输出它们。 C ++实现最多可以同步9个通道。

Example (C++)

Suppose you are writing a ROS node that needs to process data from two time synchronized topics. Your program will probably look something like this:

#include <message_filters/subscriber.h>
#include <message_filters/time_synchronizer.h>
#include <sensor_msgs/Image.h>
#include <sensor_msgs/CameraInfo.h>

using namespace sensor_msgs;
using namespace message_filters;

void callback(const ImageConstPtr& image, const CameraInfoConstPtr& cam_info)
{
  // Solve all of perception here...
}

int main(int argc, char** argv)
{
  ros::init(argc, argv, "vision_node");

  ros::NodeHandle nh;

  message_filters::Subscriber<Image> image_sub(nh, "image", 1);
  message_filters::Subscriber<CameraInfo> info_sub(nh, "camera_info", 1);
  TimeSynchronizer<Image, CameraInfo> sync(image_sub, info_sub, 10);
  sync.registerCallback(boost::bind(&callback, _1, _2));

  ros::spin();

  return 0;
}
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28

1.3 主要用法之——时间顺序的回调

TimeSequencer过滤器确保将根据消息头的时间戳按时间顺序调用消息。 TimeSequencer具有特定的延迟，该延迟指定了在传递消息之前将消息排队的时间。 在消息的时间戳少于指定的延迟之前，永远不会调用消息的回调。 但是，对于所有至少经过延迟才过时的消息，将调用它们的回调并保证其按时间顺序排列。 如果消息是在消息已调用回调之前的某个时间到达的，则会将其丢弃。

Example (C++)

C ++版本需要延迟和更新速率。 更新速率确定定序器将在其队列中检查准备通过的消息的频率。 最后一个参数是开始丢弃之前要排队的消息数。

message_filters::Subscriber<std_msgs::String> sub(nh, "my_topic", 1);
message_filters::TimeSequencer<std_msgs::String> seq(sub, ros::Duration(0.1), ros::Duration(0.01), 10);
seq.registerCallback(myCallback);
1
2

2 tf::MessageFilter

tf::MessageFilter可以订阅任何的ROS消息，然后将其缓存，直到这些消息可以转换到目标坐标系，然后进行相应的处理（进入回调函数）。

tf::MessageFilter的初始化需要message_filters::Subscriber，tf转换，目标坐标系，等待时间。当message_filters::Subscriber的消息能够由tf转换到目标坐标系时，调用回调函数，回调函数由tf::MessageFilter::registerCallback()进行注册。

tf::MessageFilter< M > Class Template Reference:

http://docs.ros.org/diamondback/api/tf/html/c++/classtf_1_1MessageFilter.html

2.1 示例AMCL

tf_ = new TransformListenerWrapper();
message_filters::Subscriber<sensor_msgs::LaserScan>* laser_scan_sub_;
tf::MessageFilter<sensor_msgs::LaserScan>* laser_scan_filter_;
 
laser_scan_sub_ = new message_filters::Subscriber<sensor_msgs::LaserScan>(nh_, scan_topic_, 100);
laser_scan_filter_ = new tf::MessageFilter<sensor_msgs::LaserScan>(*laser_scan_sub_, 
                                                        *tf_, 
                                                        odom_frame_id_, 
                                                        100);
 
laser_scan_filter_->registerCallback(boost::bind(&AmclNode::laserReceived, this, _1));
 
void AmclNode::laserReceived(const sensor_msgs::LaserScanConstPtr& laser_scan){
    this->tf_->transformPose(base_frame_id_, ident, laser_pose);//这个函数的意思是，ident在base_frame_id下的表示为laser_pose
}
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15

2.2 wiki教程

http://wiki.ros.org/tf/Tutorials/Using%20Stamped%20datatypes%20with%20tf%3A%3AMessageFilter

#include "ros/ros.h"
#include "tf/transform_listener.h"
#include "tf/message_filter.h"
#include "message_filters/subscriber.h"

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

private:
  message_filters::Subscriber<geometry_msgs::PointStamped> point_sub_;
  tf::TransformListener tf_;
  tf::MessageFilter<geometry_msgs::PointStamped> * tf_filter_;
  ros::NodeHandle n_;
  std::string target_frame_;

  //  Callback to register with tf::MessageFilter to be called when transforms are available
  void msgCallback(const boost::shared_ptr<const geometry_msgs::PointStamped>& point_ptr) 
  {
    geometry_msgs::PointStamped point_out;
    try 
    {
      tf_.transformPoint(target_frame_, *point_ptr, point_out);
      
      printf("point of turtle 3 in frame of turtle 1 Position(x:%f y:%f z:%f)\n", 
             point_out.point.x,
             point_out.point.y,
             point_out.point.z);
    }
    catch (tf::TransformException &ex) 
    {
      printf ("Failure %s\n", ex.what()); //Print exception which was caught
    }
  };

};


int main(int argc, char ** argv)
{
  ros::init(argc, argv, "pose_drawer"); //Init ROS
  PoseDrawer pd; //Construct class
  ros::spin(); // Run until interupted 
};
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49

3 tf2_ros::MessageFilter

3.1 wiki教程

http://wiki.ros.org/tf2/Tutorials/Using%20stamped%20datatypes%20with%20tf2%3A%3AMessageFilter

#include "ros/ros.h"
#include "geometry_msgs/PointStamped.h"

#include "tf2_ros/transform_listener.h"
#include "tf2_ros/message_filter.h"
#include "message_filters/subscriber.h"
#include "tf2_geometry_msgs/tf2_geometry_msgs.h"

class PoseDrawer
{
public:
  PoseDrawer() :
    tf2_(buffer_),  target_frame_("turtle1"),
    tf2_filter_(point_sub_, buffer_, target_frame_, 10, 0)
  {
    point_sub_.subscribe(n_, "turtle_point_stamped", 10);
    tf2_filter_.registerCallback( boost::bind(&PoseDrawer::msgCallback, this, _1) );
  }

  //  Callback to register with tf2_ros::MessageFilter to be called when transforms are available
  void msgCallback(const geometry_msgs::PointStampedConstPtr& point_ptr) 
  {
    geometry_msgs::PointStamped point_out;
    try 
    {
      buffer_.transform(*point_ptr, point_out, target_frame_);
      
      ROS_INFO("point of turtle 3 in frame of turtle 1 Position(x:%f y:%f z:%f)\n", 
             point_out.point.x,
             point_out.point.y,
             point_out.point.z);
    }
    catch (tf2::TransformException &ex) 
    {
      ROS_WARN("Failure %s\n", ex.what()); //Print exception which was caught
    }
  }

private:
  std::string target_frame_;
  tf2_ros::Buffer buffer_;
  tf2_ros::TransformListener tf2_;
  ros::NodeHandle n_;
  message_filters::Subscriber<geometry_msgs::PointStamped> point_sub_;
  tf2_ros::MessageFilter<geometry_msgs::PointStamped> tf2_filter_;

};


int main(int argc, char ** argv)
{
  ros::init(argc, argv, "pose_drawer"); //Init ROS
  PoseDrawer pd; //Construct class
  ros::spin(); // Run until interupted 
  return 0;
};
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56

tf2_ros::TransformListener　通过　tf2_(buffer_)　对　tf2_ros::Buffer　进行初始化。

4 tf2_ros之使用tf进行坐标变换

4.1 tf::Transformer Class Reference

4.2 tf2_ros::BufferInterface Class Reference

tf2_ros::Buffer::transform() 可以直接将一个坐标系下的位姿转换到另一个坐标系的位姿。

tf2_ros::Buffer Class Reference: http://docs.ros.org/melodic/api/tf2_ros/html/c++/classtf2__ros_1_1Buffer.html

buffer_interface.h File Reference: http://docs.ros.org/jade/api/tf2_ros/html/c++/buffer__interface_8h.html

tf2_ros::BufferInterface Class Reference: http://docs.ros.org/jade/api/tf2_ros/html/c++/classtf2__ros_1_1BufferInterface.html

references

1. https://blog.csdn.net/u013834525/article/details/80222686