博客

ROS下点云消息接收、TF关系获取和点云坐标系的转换,保姆级教程

  • Post author:
  • Post category:其他


点云数据读取

读取点云数据有三种方法:

1.读取本地的pcd文件（没用过）;

给个pcd文件读取并发布的例子，需要的操作里面都有 

//总得导入ros吧
#include <ros/ros.h>
//导入消息格式
#include <sensor_msgs/PointCloud2.h>
//导入pcl
#include <pcl_conversions/pcl_conversions.h>
#include <pcl/point_cloud.h>
#include <pcl/point_types.h>
int main(int argc, char **argv)
{
  ros::init(argc, argv, "listener");
  ros::NodeHandle nh;
  ros::Publisher pub;
  //设置发送的topic
  pub = nh.advertise<sensor_msgs::PointCloud2> ("发送的topic", 100);  
  //填写要导入的文件名
  pcl::PCLPointCloud2::Ptr cloud2(new pcl::PCLPointCloud2); 
  pcl::io::loadPCDFile ("要导入的文件", *cloud2);
  // Convert to ROS data type
  sensor_msgs::PointCloud2 output;
  pcl_conversions::fromPCL(*cloud2, output);
  //设置输出点云的坐标系
  output.header.frame_id = std::string("base_link");
  //发布
  pub.publish (output);
}

2.是从雷达读取topic（这个只要会使用雷达驱动也没问题）;

都是从topic里获取的数据，和下面的一起写

3.使用bag包;

使用bag包要注意我们要使用的是bag的时间还是系统当前的时间，如果想使用bag包的时间，要使用 

//use_sim_time设置为true，此时说明系统使用的是bag包的仿真时间
//如果设置为false，则系统使用walltime
//在使用bag之前在终端输入即可
rosparam set use_sim_time true
//发布bag的 clock time
rosbag play --clock 包名.bag

因为我们的tf关系也存储在bag中，tf对应的是仿真的时间，所以我们使用bag的时间，不然会导致tf查询的时间问题。

通过topic读取点云的数据 

//总得导入ros吧
#include <ros/ros.h> 
//导入消息格式
#include <sensor_msgs/PointCloud2.h>
//导入各种头文件，有的是后面的
#include <pcl_ros/point_cloud.h>
#include <pcl/common/transforms.h>
#include <geometry_msgs/Pose.h>
#include <message_filters/subscriber.h>
#include <message_filters/time_synchronizer.h>
#include <tf/transform_broadcaster.h>
#include <tf/message_filter.h>
#include <tf/transform_listener.h>

#include <pcl_ros/transforms.h>
#include <pcl/common/transforms.h>
#include <pcl/conversions.h>
#include <pcl_ros/impl/transforms.hpp>

int main(int argc, char **argv)
{
ros::init(argc, argv, "listener");
ros::NodeHandle n;
ros::Subscriber sub = n.subscribe<sensor_msgs::PointCloud2>("Topic名", 1000, callback);
//进入自循环，可以尽可能快的调用消息回调函数。
ros::spin();
return 0; 
}

void callback(const std_msgs::PointCloud2::ConstPtr& msg)//回调函数，当接收到 Topic名 的时候就会被调用。
{
  pcl::PointCloud<pcl::PointXYZL> pc;
  pcl::PointCloud<pcl::PointXYZL> pc_global;
  pcl::fromROSMsg(*cloud, pc);
  //剩下来的点云处理在这里实现
}

那如果不止有一个topic，而是有多个点云的消息（有多个激光雷达）该怎么办呢？TimeSynchronizer根据多个传入消息的时间戳进行同步，从而实现调用同一个回调函数的目的。看一个roswiki官网的例子： 

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

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

//最后构建多消息回调函数
void callback(const ImageConstPtr& image, const CameraInfoConstPtr& cam_info)
{
  // Solve all of perception here...
}

在此外还有Policy-Based的消息同步机制，本质与上述Time Synchronizer类似。它分为两种，ExactTime Policy 和ApproximateTime Policy，ExactTime Policy这种方法要求输入的消息的时间戳必须完全相同才调用回调函数，ApproximateTime Policy这种方法根据输入消息的时间戳进行近似匹配，不要求消息时间完全相同。具体可在下面链接中找到。




【ros】多消息同步回调


icon-default.png?t=LA92
https://www.codeleading.com/article/17212367821/

顺便记录下我写的三雷达的同步回调，学会类比，二个三个四个的回调怎么写。 

ros::NodeHandle pnh_;
pub = nh.advertise<sensor_msgs::PointCloud2>("pointclouds_concatenate", 1000);
message_filters::Subscriber<sensor_msgs::PointCloud2> pointcloud_middle(pnh_, "/my_cloud_middle", 1); // topic1 输入
message_filters::Subscriber<sensor_msgs::PointCloud2> pointcloud_left(pnh_, "/my_cloud_left", 1); // topic2 输入
message_filters::Subscriber<sensor_msgs::PointCloud2> pointcloud_right(pnh_, "/my_cloud_right", 1); // topic3 输入
sync_ = new message_filters::Synchronizer<slamSyncPolicy>(slamSyncPolicy(10), pointcloud_middle, pointcloud_left, pointcloud_right);                             // 同步
//这有this是因为我写在PerceptionComponent类里了
sync_->registerCallback(boost::bind(&PerceptionComponent::callback, this, _1, _2, _3)); 

void PerceptionComponent::callback(const sensor_msgs::PointCloud2::ConstPtr &pointcloud_middle, const sensor_msgs::PointCloud2::ConstPtr &pointcloud_left, const sensor_msgs::PointCloud2::ConstPtr &pointcloud_right) //回调中包含多个消息
{}

如过还有什么神奇的方法欢迎补充。

TF关系获取和点云坐标系的转换

按照上面说的，我们已经能获取到sensor_msgs::PointCloud2::ConstPtr格式的点云数据了，

先来看tf的读取和点云的变换，有一种方法是，图是他的文档，来自于，但不一定能打开这个连接。




transforms.h File Reference


icon-default.png?t=LA92
http://docs.ros.org/en/indigo/api/pcl_ros/html/transforms_8h.html


 

tf::TransformListener tf_listener;
pcl_ros::transformPointCloud("目标坐标系名称"，输入的点云(sensor_msgs::PointCloud2)，输出的点云(sensor_msgs::PointCloud2)，tf_listener)

这个函数还有很多的功能，看一下它的具体实现 

bool
transformPointCloud (const std::string &target_frame, const sensor_msgs::PointCloud2 &in, 
                     sensor_msgs::PointCloud2 &out, const tf::TransformListener &tf_listener)
{
  if (in.header.frame_id == target_frame)
  {
    out = in;
    return (true);
  }

  // Get the TF transform
  tf::StampedTransform transform;
  try
  {
    tf_listener.lookupTransform (target_frame, in.header.frame_id, in.header.stamp, transform);
  }
  catch (tf::LookupException &e)
  {
    ROS_ERROR ("%s", e.what ());
    return (false);
  }
  catch (tf::ExtrapolationException &e)
  {
    ROS_ERROR ("%s", e.what ());
   return (false);
  }
  // Convert the TF transform to Eigen format  Eigen::Matrix4f eigen_transform;  transformAsMatrix (transform, eigen_transform)
  transformPointCloud (eigen_transform, in, out);

  out.header.frame_id = target_frame;
  return (true);
}

我们要注意，这里可能会报错 

[ERROR] [1426895120.908823306, 1422676721.758969255]: Lookup would require extrapolation into the past.  Requested time 1422676720.347031633 but the earliest data is at time 1422676720.654202231, when looking up transform from frame [imu_link] to frame [base_link]
//就是这种
Lookup would require extrapolation into the past. 
Requested time 1422676720.347031633 
but the earliest data is at time 1422676720.654202231,

它用的是in.header.stamp,就是说它寻找的tf关系是在我们接收输入点云的时刻比如时间为100的时刻的tf关系，但有时当我们寻找时间为100时的tf关系时已经找不到了，系统存储的tf关系已经被更新了，存储的最早的可能就只有103时刻的了，这时候就会报错或者抛出异常，对于transformPointCloud而言就是返回false，虽然合乎情理，但是我就想要他的tf变换，不是100时刻的也行，105时刻的也行，这时候我们就要用ros::Time(0）获取当前时刻tf，这样不会再找不到吧，别说还真会，因为你用的是bag包的数据，tf关系对应在bag包的时间里，你用ros::Time(0）找的是系统的时间比如系统说我现在是120时刻，去bag里一看，只有70-90时刻的tf，这就找不到了，要用我上文写的使用bag包的方法，来使用bag包的时间。 

tf_listener.lookupTransform(target_frame, in.header.frame_id, in.header.stamp, transform);

那这么看pcl_ros::transformPointCloud很可能会出错，毕竟它用的是in.header.stamp，那我们肯定不乐意啊，那怎么办，自己写吧，下面是我写的。 

void transformPointCloudWithLidar(const sensor_msgs::PointCloud2::ConstPtr &in, const std::string out_id, sensor_msgs::PointCloud2::Ptr &out)
{
  //out_id就是目标坐标系，如果输入就在目标坐标系那就不用动了
  if (out_id != in->header.frame_id)
  {
     tf::StampedTransform transform;
     try
     {
       tf_listener.lookupTransform(out_id, (*in).header.frame_id, ros::Time(0), transform);
     }
     catch (tf::LookupException &e)
     {
       std::cout<<"cuole"<<std::endl;
       ROS_ERROR ("%s", e.what ());
    }
    Eigen::Matrix4f eigen_transform;
    pcl_ros::transformAsMatrix(transform, eigen_transform);
    pcl_ros::transformPointCloud(eigen_transform, *in, *out);
    (*out).header.frame_id = out_id;
  }
  else
  {
    out = boost::make_shared<sensor_msgs::PointCloud2> (*in);
  }
}

点云的tf变换有一种方法是对于多点云的输入，如果相融合成一个可以用下面的方法合并，不过点云必须在同一坐标系下。 

//设置一个来存储合并后的点云
sensor_msgs::PointCloud2::Ptr concat_cloud_ptr_(new sensor_msgs::PointCloud2());
pcl::concatenatePointCloud(*(输入点云1), *(输入点云2), *concat_cloud_ptr_);

这是我在知乎写的：




ROS下点云消息接收、TF关系获取和点云坐标系的转换,保姆级教程


icon-default.png?t=LA92
https://zhuanlan.zhihu.com/p/373598208


版权声明：本文为qq_41505897原创文章，遵循 CC 4.0 BY-SA 版权协议，转载请附上原文出处链接和本声明。