1. 技术文章
  2. tf之 MessageFilter 与 tf::MessageFilter理解与应用

tf之 MessageFilter 与 tf::MessageFilter理解与应用

由 回眸只為那壹抹淺笑 提交于 2020-01-03 03:58:02

Table of Contents

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进行坐标变换

Public Member Functions

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.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);

 

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
}

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 
};

 

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;
};

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

 

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

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

tf2_ros::BufferInterface　Public Member Functions
virtual bool  canTransform (const std::string &target_frame, const std::string &source_frame, const ros::Time &time, const ros::Duration timeout, std::string *errstr=NULL) const =0
  Test if a transform is possible. 
virtual bool  canTransform (const std::string &target_frame, const ros::Time &target_time, const std::string &source_frame, const ros::Time &source_time, const std::string &fixed_frame, const ros::Duration timeout, std::string *errstr=NULL) const =0
  Test if a transform is possible. 
virtual 
geometry_msgs::TransformStamped 		 lookupTransform (const std::string &target_frame, const std::string &source_frame, const ros::Time &time, const ros::Duration timeout) const =0
  Get the transform between two frames by frame ID. 
virtual 
geometry_msgs::TransformStamped 		 lookupTransform (const std::string &target_frame, const ros::Time &target_time, const std::string &source_frame, const ros::Time &source_time, const std::string &fixed_frame, const ros::Duration timeout) const =0
  Get the transform between two frames by frame ID assuming fixed frame. 
template<class T >
T &  transform (const T &in, T &out, const std::string &target_frame, ros::Duration timeout=ros::Duration(0.0)) const
  Transform an input into the target frame. This function is templated and can take as input any valid mathematical object that tf knows how to apply a transform to, by way of the templated math conversions interface. For example, the template type could be a Transform, Pose, Vector, or Quaternion message type (as defined in geometry_msgs). 
template<class T >
T  transform (const T &in, const std::string &target_frame, ros::Duration timeout=ros::Duration(0.0)) const
  Transform an input into the target frame. This function is templated and can take as input any valid mathematical object that tf knows how to apply a transform to, by way of the templated math conversions interface. For example, the template type could be a Transform, Pose, Vector, or Quaternion message type (as defined in geometry_msgs). 
template<class A , class B >
B &  transform (const A &in, B &out, const std::string &target_frame, ros::Duration timeout=ros::Duration(0.0)) const
  Transform an input into the target frame and convert to a specified output type. It is templated on two types: the type of the input object and the type of the transformed output. For example, the template types could be Transform, Pose, Vector, or Quaternion messages type (as defined in geometry_msgs). The function will calculate the transformation and then convert the result into the specified output type. Compilation will fail if a known conversion does not exist bewteen the two template parameters. 
template<class T >
T &  transform (const T &in, T &out, const std::string &target_frame, const ros::Time &target_time, const std::string &fixed_frame, ros::Duration timeout=ros::Duration(0.0)) const
  Transform an input into the target frame (advanced). This function is templated and can take as input any valid mathematical object that tf knows how to apply a transform to, by way of the templated math conversions interface. For example, the template type could be a Transform, Pose, Vector, or Quaternion message type (as defined in geometry_msgs). This function follows the advanced API, which allows transforming between different time points, and specifying a fixed frame that does not varying in time. 
template<class T >
T  transform (const T &in, const std::string &target_frame, const ros::Time &target_time, const std::string &fixed_frame, ros::Duration timeout=ros::Duration(0.0)) const
  Transform an input into the target frame (advanced). This function is templated and can take as input any valid mathematical object that tf knows how to apply a transform to, by way of the templated math conversions interface. For example, the template type could be a Transform, Pose, Vector, or Quaternion message type (as defined in geometry_msgs). This function follows the advanced API, which allows transforming between different time points, and specifying a fixed frame that does not varying in time. 
template<class A , class B >
B &  transform (const A &in, B &out, const std::string &target_frame, const ros::Time &target_time, const std::string &fixed_frame, ros::Duration timeout=ros::Duration(0.0)) const
  Transform an input into the target frame and convert to a specified output type (advanced). It is templated on two types: the type of the input object and the type of the transformed output. For example, the template type could be a Transform, Pose, Vector, or Quaternion message type (as defined in geometry_msgs). The function will calculate the transformation and then convert the result into the specified output type. Compilation will fail if a known conversion does not exist bewteen the two template parameters. This function follows the advanced API, which allows transforming between different time points, and specifying a fixed frame that does not varying in time. 

 

 

references

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

 

 
标签
ros
sub
科技新闻
易学教程内所有资源均来自网络或用户发布的内容，如有违反法律规定的内容欢迎反馈！
该文章没有解决你所遇到的问题?点击提问,说说你的问题,让更多的人一起探讨吧!