ROS订阅tf并转换坐标

刀么克瑟拉莫

已于 2022-07-12 16:34:38 修改

阅读量2.5k

点赞数 4

分类专栏： ROS 文章标签：自动驾驶

于 2020-09-25 10:15:39 首次发布

本文链接：https://blog.csdn.net/random_repick/article/details/108790623

版权

不要在函数中，特别是回调函数中声明TransformListener对象
TransformListener对象应该被限定为持久化，否则它的缓存将无法填充，并且几乎每个查询都将失败。出现Lookup would require extrapolation at time 1657511018.391161648, but only time 1657511018.856185462 is in the buffer之类的问题
一个常见的方法是使TransformListener对象成为一个类的成员变量

#include <ros/ros.h>
#include <tf/transform_listener.h>
#include <tf/transform_datatypes.h>
#include <geometry_msgs/Quaternion.h>
#include <nav_msgs/OccupancyGrid.h>
#include <sensor_msgs/LaserScan.h>

class Callback
{
   
public:
  tf::TransformListener listener_;
  tf::StampedTransform transform_;
  ros::Subscriber sub_;
  double last_x_;
  double last_y_;
  double last_angle_;

  Callback(ros::NodeHandle n)
  {
   
    last_x_ = 0;
    last_y_ = 0;
    last_angle_ = 0;
    sub_ = n.subscribe<sensor_msgs::LaserScan>("/scan", 1, &Callback::CB, this);
  }
  void CB(const sensor_msgs::LaserScanConstPtr &scan);
};

void Callback::CB(const sensor_msgs::LaserScanConstPtr &scan)
{
   

  try
  {
   
    // base_link to map
    listener_.waitForTransform("/map",
                               "/base_link",
                               ros::Time(0), ros::Duration(0.2));
    listener_.lookupTransform("map",
                              "base_link",
                              ros::Time(0), transform_);
  }
  catch (tf::TransformException &ex)
  {
   
    ROS_ERROR("%s", ex.what());
    return;
  }
  double dx = transform_.getOrigin().x() - last_x_;