• 欢迎访问开心洋葱网站,在线教程,推荐使用最新版火狐浏览器和Chrome浏览器访问本网站,欢迎加入开心洋葱 QQ群
  • 为方便开心洋葱网用户,开心洋葱官网已经开启复制功能!
  • 欢迎访问开心洋葱网站,手机也能访问哦~欢迎加入开心洋葱多维思维学习平台 QQ群
  • 如果您觉得本站非常有看点,那么赶紧使用Ctrl+D 收藏开心洋葱吧~~~~~~~~~~~~~!
  • 由于近期流量激增,小站的ECS没能经的起亲们的访问,本站依然没有盈利,如果各位看如果觉着文字不错,还请看官给小站打个赏~~~~~~~~~~~~~!

ROS TF2 :通过tf2_ros::MessageFilter 将世界坐标系下的点转为期望坐标系下的点

人工智能 月照银海似蛟龙 2194次浏览 0个评论

本片博客描述:如何使用 tf2_ros::MessageFilter 处理 Stamped(时间戳) 数据类型

在 TF2 的 体系下 如何使用传感器的数据

在 实际 情况下 传感器的数据包括: 相机、激光雷达

1、情景

假设一只新的乌龟 命名为 乌龟3 , 它没有好的里程计(也就是说不指定它的具体坐标),但是现在有一个上方的相机 跟踪 它的位置并且发布 相对与 世界坐标系 下的 位置信息 ( PointStamped 类型的 topic)

乌龟1 想要知道 乌龟3 相对自己的位置

为此,turtle1必须监听正在发布turtle3位置的topic,直到准备好转换为期望坐标系下,然后进行操作。

做这件事的话,使用 tf2_ros::MessageFilter 类 非常有用

tf2_ros :: MessageFilter将使用标头对任何ros消息进行订阅,并将其缓存,直到可以将其转换为期望坐标系下为止。

2、实现代码

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

   
  

3、代码解释

=============================================================

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

首先时包含 tf2_ros::MessageFilter 的头文件 和其它头文件

===============================================================

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

   
  

类的私有变量

tf2_ros::Buffer
tf2_ros::TransformListener
tf2_ros::MessageFilter
这三个要在构造函数时初始化 使数据一直被保持

====================================================================

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

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

=======================================================================

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

tf2_ros::MessageFilter 的 回调函数

buffer_.transform(*point_ptr, point_out, target_frame_);

通过这个函数(就是tf2_ros::TransformListener的一个功能函数) 将 在 世界坐标系的 点 转化为 期望坐标系 下的点

4、 疑问

上面通过 tf2_ros::MessageFilter 的 方式 和 直接订阅 位置信息 然后 通过tf2_ros::TransformListener的transform功能函数去结算 这两者有什么区别呢?


开心洋葱 , 版权所有丨如未注明 , 均为原创丨未经授权请勿修改 , 转载请注明ROS TF2 :通过tf2_ros::MessageFilter 将世界坐标系下的点转为期望坐标系下的点
喜欢 (0)

您必须 登录 才能发表评论!

加载中……