点云数据读取
读取点云数据有三种方法:
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】多消息同步回调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 Referencehttp://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关系获取和点云坐标系的转换,保姆级教程https://zhuanlan.zhihu.com/p/373598208