项目场景:
因为单个Livox avia的FOV只有70°,无法覆盖车前方的所有范围,所以用了三个Livox avia以实现180°前方位覆盖。但由于三个雷达均是独自采集,所以需要对每个雷达采集的各帧点云进行合并,用于建图。以下工作均建立于已经知道各雷达之间的外参。
问题描述
由于Fast-LIO输入的是Livox自定义的Msg,所以需要先订阅每个雷达的topic,将其格式转换成PointCloud2格式,在该格式下对三个雷达的点云进行拼接,最后将拼接后的点云转回Livox自定义的CustomMsg即可输入给Fast-LIO,代码如下所示
#include <ros/ros.h>
#include <livox_ros_driver/CustomMsg.h>
#include <sensor_msgs/PointCloud2.h>
#include <pcl_conversions/pcl_conversions.h>
#include <pcl/common/transforms.h>
#include <pcl/point_cloud.h>
#include <pcl/point_types.h>
#include <pcl/io/io.h>
#include <message_filters/subscriber.h>
#include <message_filters/synchronizer.h>
#include <message_filters/sync_policies/approximate_time.h>
#include <livox_ros_driver/CustomMsg.h>
#include <iostream>
typedef pcl::PointXYZINormal PointType;
typedef pcl::PointCloud<PointType> PointCloudXYZI;
class SubscribeAndPublish
{
public:
SubscribeAndPublish(){
pub = node.advertise<livox_ros_driver::CustomMsg>("/livox/lidar", 10);
sub_mid = new message_filters::Subscriber<livox_ros_driver::CustomMsg>(node, "/livox/lidar_3JEDK5D0011F371", 200000);
sub_left = new message_filters::Subscriber<livox_ros_driver::CustomMsg>(node, "/livox/lidar_3JEDK620011H571", 200000);
sub_right = new message_filters::Subscriber<livox_ros_driver::CustomMsg>(node, "/livox/lidar_3JEDK6G00134651", 200000);
typedef message_filters::sync_policies::ApproximateTime<livox_ros_driver::CustomMsg,
livox_ros_driver::CustomMsg,
livox_ros_driver::CustomMsg> syncPolicy;
message_filters::Synchronizer<syncPolicy> sync(syncPolicy(9), *sub_mid, *sub_left, *sub_right);
sync.registerCallback(boost::bind(&SubscribeAndPublish::callBack, this, _1, _2, _3));
ros::spin();
}
void callBack(const livox_ros_driver::CustomMsgConstPtr& lidar_mid,
const livox_ros_driver::CustomMsgConstPtr& lidar_left,
const livox_ros_driver::CustomMsgConstPtr& lidar_right){
PointCloudXYZI pointCloud_mid;
PointCloudXYZI pointCloud_left;
PointCloudXYZI pointCloud_left_out;
PointCloudXYZI pointCloud_right;
PointCloudXYZI pointCloud_right_out;
PointCloudXYZI finalPointCloud;
convert2PointCloud2(lidar_mid, pointCloud_mid);
convert2PointCloud2(lidar_left, pointCloud_left);
convert2PointCloud2(lidar_right, pointCloud_right);
Eigen::Matrix4f transform2Left = Eigen::Matrix4f::Identity();
transform2Left(0, 0) = 0.6380962;
transform2Left(0, 1) = -0.7699567;
transform2Left(0, 2) = 0;
transform2Left(0, 3) = -0.05;
transform2Left(1, 0) = 0.7699567;
transform2Left(1, 1) = 0.6380962;
transform2Left(1, 2) = 0;
transform2Left(1, 3) = 0.15;
transform2Left(2, 0) = 0;
transform2Left(2, 1) = 0;
transform2Left(2, 2) = 1;
transform2Left(2, 3) = 0;
Eigen::Matrix4f transform2Right = Eigen::Matrix4f::Identity();
transform2Right(0, 0) = 0.6461240;
transform2Right(0, 1) = 0.7632325;
transform2Right(0, 2) = 0;
transform2Right(0, 3) = -0.05;
transform2Right(1, 0) = -0.7632325;
transform2Right(1, 1) = 0.6461240;
transform2Right(1, 2) = 0;
transform2Right(1, 3) = -0.15;
transform2Right(2, 0) = 0;
transform2Right(2, 1) = 0;
transform2Right(2, 2) = 1;
transform2Right(2, 3) = 0;
pcl::transformPointCloud(pointCloud_left, pointCloud_left_out, transform2Left);
pcl::transformPointCloud(pointCloud_right, pointCloud_right_out, transform2Right);
finalPointCloud = pointCloud_mid + pointCloud_left_out ;
finalPointCloud = finalPointCloud + pointCloud_right_out;
livox_ros_driver::CustomMsg finalMsg;
finalMsg.header = lidar_mid->header;
finalMsg.timebase = lidar_mid->timebase;
finalMsg.point_num = finalPointCloud.size();
finalMsg.lidar_id = lidar_mid->lidar_id;
for(unsigned int i = 0; i < finalMsg.point_num; i++)
{
livox_ros_driver::CustomPoint p;
p.x = finalPointCloud[i].x;
p.y = finalPointCloud[i].y;
p.z = finalPointCloud[i].z;
p.reflectivity = finalPointCloud[i].intensity;
p.offset_time = finalPointCloud[i].curvature * float(1000000);
finalMsg.points.push_back(p);
}
pub.publish(finalMsg);
}
void convert2PointCloud2(const livox_ros_driver::CustomMsgConstPtr& lidarMsg, PointCloudXYZI& pclPointCloud ){
for(unsigned int i = 0; i < lidarMsg->point_num; i++)
{
PointType point;
point.x = lidarMsg->points[i].x;
point.y = lidarMsg->points[i].y;
point.z = lidarMsg->points[i].z;
point.intensity = lidarMsg->points[i].reflectivity;
point.curvature = lidarMsg->points[i].offset_time / float(1000000);
pclPointCloud.push_back(point);
}
}
private:
ros::NodeHandle node;
ros::Publisher pub;
message_filters::Subscriber<livox_ros_driver::CustomMsg>* sub_mid;
message_filters::Subscriber<livox_ros_driver::CustomMsg>* sub_right;
message_filters::Subscriber<livox_ros_driver::CustomMsg>* sub_left;
};
int main(int argc, char* argv[]){
ros::init(argc, argv, "pointCloudMerge");
SubscribeAndPublish sap;
return 0;
}
代码分析:
上述代码实现了在同一个节点中订阅多个topic并发布topic的功能。由于订阅了多个topic,所以需要对不同的topoic进行时间同步,时间同步用到了ROS中的ApproximateTime
的功能,通过message_filters::Synchronizer
统一接收多个主题,只有在所有的topic都有相同的时间戳时,才会产生一个同步结果的回调函数,在回调函数里处理同步时间后的数据。还需要注意的是由于作用域的问题,在main
函数中使用ros::spin()
会使得回调函数得不到调用,如果跟我一样使用类来完成在一个节点中订阅和发布的话,则需要跟Subscriber
在同一个代码块中才能够成功唤起回调函数。使用message_filters
需要在cmakelist和package.xml中更改相应的代码以便在编译的时候能找到相关依赖。文章来源:https://www.toymoban.com/news/detail-544317.html
参考网站
https://www.cnblogs.com/long5683/p/13223458.html
https://blog.csdn.net/u013172864/article/details/106614962
http://zhaoxuhui.top/blog/2019/10/20/ros-note-7.html
https://blog.csdn.net/Laney_Midory/article/details/120040285
https://blog.csdn.net/qq_37683987/article/details/96432874文章来源地址https://www.toymoban.com/news/detail-544317.html
到了这里,关于多个Livox雷达点云合成及使用ROS发布的文章就介绍完了。如果您还想了解更多内容,请在右上角搜索TOY模板网以前的文章或继续浏览下面的相关文章,希望大家以后多多支持TOY模板网!