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

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 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 
#include 
#include 
#include 

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_sub(nh, "image", 1);
  message_filters::Subscriber info_sub(nh, "camera_info", 1);
  TimeSynchronizer 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 sub(nh, "my_topic", 1);
message_filters::TimeSequencer 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* laser_scan_sub_;
tf::MessageFilter* laser_scan_filter_;
 
laser_scan_sub_ = new message_filters::Subscriber(nh_, scan_topic_, 100);
laser_scan_filter_ = new tf::MessageFilter(*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(point_sub_, tf_, target_frame_, 10);
    tf_filter_->registerCallback( boost::bind(&PoseDrawer::msgCallback, this, _1) );
  } ;

private:
  message_filters::Subscriber point_sub_;
  tf::TransformListener tf_;
  tf::MessageFilter * 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& 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 point_sub_;
  tf2_ros::MessageFilter 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进行坐标变换

4.1 tf::Transformer Class Reference

Public Member Functions
boost::signals2::connection	addTransformsChangedListener (boost::function< void(void)> callback)
	Add a callback that happens when a new transform has arrived.
std::string	allFramesAsDot (double current_time=0) const
	A way to see what frames have been cached Useful for debugging.
std::string	allFramesAsString () const
	A way to see what frames have been cached Useful for debugging.
bool	canTransform (const std::string &target_frame, const std::string &source_frame, const ros::Time &time, std::string *error_msg=NULL) const
	Test if a transform is possible.
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, std::string *error_msg=NULL) const
	Test if a transform is possible.
void	chainAsVector (const std::string &target_frame, ros::Time target_time, const std::string &source_frame, ros::Time source_time, const std::string &fixed_frame, std::vector< std::string > &output) const
	Debugging function that will print the spanning chain of transforms. Possible exceptions tf::LookupException, tf::ConnectivityException, tf::MaxDepthException.
void	clear ()
	Clear all data.
bool	frameExists (const std::string &frame_id_str) const
	Check if a frame exists in the tree.
ros::Duration	getCacheLength ()
	Get the duration over which this transformer will cache.
void	getFrameStrings (std::vector< std::string > &ids) const
	A way to get a std::vector of available frame ids.
int	getLatestCommonTime (const std::string &source_frame, const std::string &target_frame, ros::Time &time, std::string *error_string) const
	Return the latest rostime which is common across the spanning set zero if fails to cross.
bool	getParent (const std::string &frame_id, ros::Time time, std::string &parent) const
	Fill the parent of a frame.
std::string	getTFPrefix () const
	Get the tf_prefix this is running with.
bool	isUsingDedicatedThread ()
void	lookupTransform (const std::string &target_frame, const std::string &source_frame, const ros::Time &time, StampedTransform &transform) const
	Get the transform between two frames by frame ID.
void	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, StampedTransform &transform) const
	Get the transform between two frames by frame ID assuming fixed frame.
void	lookupTwist (const std::string &tracking_frame, const std::string &observation_frame, const std::string &reference_frame, const tf::Point &reference_point, const std::string &reference_point_frame, const ros::Time &time, const ros::Duration &averaging_interval, geometry_msgs::Twist &twist) const
	Lookup the twist of the tracking_frame with respect to the observation frame in the reference_frame using the reference point.
void	lookupTwist (const std::string &tracking_frame, const std::string &observation_frame, const ros::Time &time, const ros::Duration &averaging_interval, geometry_msgs::Twist &twist) const
	lookup the twist of the tracking frame with respect to the observational frame
void	removeTransformsChangedListener (boost::signals2::connection c)
void	setExtrapolationLimit (const ros::Duration &distance)
	Set the distance which tf is allow to extrapolate.
bool	setTransform (const StampedTransform &transform, const std::string &authority="default_authority")
	Add transform information to the tf data structure.
void	setUsingDedicatedThread (bool value)
	Transformer (bool interpolating=true, ros::Duration cache_time_=ros::Duration(DEFAULT_CACHE_TIME))
void	transformPoint (const std::string &target_frame, const Stamped< tf::Point > &stamped_in, Stamped< tf::Point > &stamped_out) const
	Transform a Stamped Point into the target frame This can throw anything a lookupTransform can throw as well as tf::InvalidArgument.
void	transformPoint (const std::string &target_frame, const ros::Time &target_time, const Stamped< tf::Point > &stamped_in, const std::string &fixed_frame, Stamped< tf::Point > &stamped_out) const
	Transform a Stamped Point into the target frame This can throw anything a lookupTransform can throw as well as tf::InvalidArgument.
void	transformPose (const std::string &target_frame, const Stamped< tf::Pose > &stamped_in, Stamped< tf::Pose > &stamped_out) const
	Transform a Stamped Pose into the target frame This can throw anything a lookupTransform can throw as well as tf::InvalidArgument.
void	transformPose (const std::string &target_frame, const ros::Time &target_time, const Stamped< tf::Pose > &stamped_in, const std::string &fixed_frame, Stamped< tf::Pose > &stamped_out) const
	Transform a Stamped Pose into the target frame This can throw anything a lookupTransform can throw as well as tf::InvalidArgument.
void	transformQuaternion (const std::string &target_frame, const Stamped< tf::Quaternion > &stamped_in, Stamped< tf::Quaternion > &stamped_out) const
	Transform a Stamped Quaternion into the target frame This can throw anything a lookupTransform can throw as well as tf::InvalidArgument.
void	transformQuaternion (const std::string &target_frame, const ros::Time &target_time, const Stamped< tf::Quaternion > &stamped_in, const std::string &fixed_frame, Stamped< tf::Quaternion > &stamped_out) const
	Transform a Stamped Quaternion into the target frame This can throw anything a lookupTransform can throw as well as tf::InvalidArgument.
void	transformVector (const std::string &target_frame, const Stamped< tf::Vector3 > &stamped_in, Stamped< tf::Vector3 > &stamped_out) const
	Transform a Stamped Vector3 into the target frame This can throw anything a lookupTransform can throw as well as tf::InvalidArgument.
void	transformVector (const std::string &target_frame, const ros::Time &target_time, const Stamped< tf::Vector3 > &stamped_in, const std::string &fixed_frame, Stamped< tf::Vector3 > &stamped_out) const
	Transform a Stamped Vector3 into the target frame This can throw anything a lookupTransform can throw as well as tf::InvalidArgument.
bool	waitForTransform (const std::string &target_frame, const std::string &source_frame, const ros::Time &time, const ros::Duration &timeout, const ros::Duration&polling_sleep_duration=ros::Duration(0.01), std::string *error_msg=NULL) const
	Block until a transform is possible or it times out.
bool	waitForTransform (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 ros::Duration &polling_sleep_duration=ros::Duration(0.01), std::string *error_msg=NULL) const
	Block until a transform is possible or it times out.
virtual	~Transformer (void)

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

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