多个Livox雷达点云合成及使用ROS发布

冯宇祺

已于 2023-02-03 20:06:18 修改

阅读量2k

点赞数 6

文章标签：自动驾驶机器人人工智能

于 2023-02-03 20:00:25 首次发布

本文链接：https://blog.csdn.net/weixin_42856109/article/details/128871821

版权

项目场景：

因为单个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.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