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

这篇具有很好参考价值的文章主要介绍了多个Livox雷达点云合成及使用ROS发布。希望对大家有所帮助。如果存在错误或未考虑完全的地方,请大家不吝赐教,您也可以点击"举报违法"按钮提交疑问。

项目场景:

因为单个Livox avia的FOV只有70°,无法覆盖车前方的所有范围,所以用了三个Livox avia以实现180°前方位覆盖。但由于三个雷达均是独自采集,所以需要对每个雷达采集的各帧点云进行合并,用于建图。以下工作均建立于已经知道各雷达之间的外参。
ros发布点云,自动驾驶,机器人,人工智能


问题描述

由于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文章来源地址https://www.toymoban.com/news/detail-544317.html

到了这里,关于多个Livox雷达点云合成及使用ROS发布的文章就介绍完了。如果您还想了解更多内容,请在右上角搜索TOY模板网以前的文章或继续浏览下面的相关文章,希望大家以后多多支持TOY模板网!

本文来自互联网用户投稿,该文观点仅代表作者本人,不代表本站立场。本站仅提供信息存储空间服务,不拥有所有权,不承担相关法律责任。如若转载,请注明出处: 如若内容造成侵权/违法违规/事实不符,请点击违法举报进行投诉反馈,一经查实,立即删除!

领支付宝红包 赞助服务器费用

相关文章

  • ORB_SLAM3配置及修改——将图像、点云用ROS消息发布(基于无人机仿真)

            本文有点长,可以根据目录跳转到想看的部分。因为仿真和应用环境不同,可能例程的运行方式(输入话题等)有所不同,但第三部分有关ORB_SLAM3相机仿真标定、第四部分有关ORB_SLAM3源码修改的部分是通用的。 目录 一、仿真环境配置 1.双系统安装 ① 工具准备 ②

    2024年04月10日
    浏览(49)
  • (02)Cartographer源码无死角解析-(78) ROS数据发布→2D点云数据、tf、机器人tracking frame轨迹发布

    讲解关于slam一系列文章汇总链接:史上最全slam从零开始,针对于本栏目讲解(02)Cartographer源码无死角解析-链接如下: (02)Cartographer源码无死角解析- (00)目录_最新无死角讲解:https://blog.csdn.net/weixin_43013761/article/details/127350885   文末正下方中心提供了本人 联系方式, 点击本人照片

    2024年02月12日
    浏览(43)
  • ROS小车——雷达的使用与SLAM建图(4)

    启动激光雷达并查看数据,建图并避障导航 roslaunch robot_navigation lidar.launch在终端输入启动雷达,雷达开始旋转, 并打印scan话题,rostopic echo/scan,在虚拟机中 roslaunch robot_navigation lidar_rviz.launch 查看图形化的雷达数据。 roslaunch robot_navigation robot_slam_lidar.launch在终端输入启动sl

    2023年04月21日
    浏览(48)
  • ubuntu20.04 ROS 环境下使用velodyne激光雷达

    系统版本:ubuntu 20.04 ROS版本:noetic 激光雷达型号:velodyne VLP-16 网线接 PC/工控机 端口 将PC/工控机的网口配置为: ipv4,方式设置为手动 ip地址、掩码以及网关设置成下图 查看当前话题 其中 /velodyne_points 就是 激光雷达发布的点云消息。 终端输入rviz Fiex Frame 写入 velodyne 添加

    2024年02月08日
    浏览(58)
  • 基于ros和openpcdet使用自己的雷达进行实时三维目标检测

    参考博主hello689的教程,文中主要介绍了对于kitti的三维目标检测,本文对代码进行修改,添加旋转坐标轴的代码,以适配自己的雷达,可以参考这个博主的流程,再看本文对旋转参数的修改。 3.1 ros.py代码修改 3.2 pointpillar.launch代码修改 3.3 pointpillar.rviz代码修改 3.4 ros.py订阅话

    2024年01月23日
    浏览(38)
  • 【ROS2指南-19】使用Launch启动/监控多个节点

    ROS 2 中的启动系统负责帮助用户描述他们系统的配置,然后按照描述执行。系统的配置包括要运行的程序、运行它们的位置、传递给它们的参数以及 ROS 特定约定,这些约定通过为它们提供不同的配置,使得在整个系统中重用组件变得容易。它还负责监视已启动流程的状态,

    2023年04月15日
    浏览(67)
  • 【6. 激光雷达接入ROS】

    欢迎大家阅读2345VOR的博客【6. 激光雷达接入ROS】🥳🥳🥳 2345VOR鹏鹏主页: 已获得CSDN《嵌入式领域优质创作者》称号👻👻👻,座右铭:脚踏实地,仰望星空🛹🛹🛹 本文章属于《Ubuntu学习》和《ROS机器人学习》 :围绕Ubuntu系统基本配置及相关命令行学习记录!机器人操作

    2024年02月06日
    浏览(45)
  • 相机雷达时间同步(基于ROS)

    ubuntu20.04 noetic usb_cam 速腾Robosense 16线 宏基暗影骑士笔记本 软同步:订阅相机和雷达原始数据,然后进行时间同步,最后将同步后的数据发布出去,通过rosbag record进行录制 同步前的话题: /rslidar_packets /usb_cam/image_raw 同步后的话题: /sync/img /sync/lidar 1) 在工作空间src目录下创建

    2024年02月15日
    浏览(53)
  • ROS学习笔记(四)---使用 VScode 启动launch文件运行多个节点

    ROS学习笔记文章目录 01. ROS学习笔记(一)—Linux安装VScode 02. ROS学习笔记(二)—使用 VScode 开发 ROS 的Python程序(简例) 03. ROS学习笔记(三)—好用的终端Terminator 一、什么是launch文件 虽然说Terminator终端是能够比较方便直观的看运行的节点,但有时候节点一多,输入的rosrun指令也会

    2024年02月09日
    浏览(40)
  • 超维空间S2无人机使用说明书——11、使用3维激光雷达实现ROS无人机的精准定位

    一、视频演示 视频演示: 3D雷达定位效果展示 二、源码连接 后续补充 三、启动雷达节点,确保雷达发布数据 未出现红色报错,表明程序运行正常 launch文件详解

    2024年01月23日
    浏览(47)

觉得文章有用就打赏一下文章作者

支付宝扫一扫打赏

博客赞助

微信扫一扫打赏

请作者喝杯咖啡吧~博客赞助

支付宝扫一扫领取红包,优惠每天领

二维码1

领取红包

二维码2

领红包