大疆livox定制的格式CustomMsg格式转换pointcloud2

官方livox_driver驱动livox雷达发出的点云topic有两种，一种是大疆览沃定制的格式CustomMsg格式，另一种是将CustomMsg格式
转换过的pointcloud2格式，参见
Livox雷达驱动程序发布点云格式CustomMsg、PointCloud2、pcl::PointXYZI、pcl::PointXYZINormal解析
现在将转换这部分的代码提取出来，方便随时使用

创建ros功能包

             mkdir -p livox_repub/srccd livox_repub/srccatkin_init_workspace cd ..catkin_make

livox_repub.cpp

             cd srcmkdir livox_repubcd livox_repub

vi package.xml

<?xml version="1.0"?>
<package><name>livox_repub</name><version>0.0.0</version><description>This is a .</description><maintainer email="xxxxx@xxx.com">xxx</maintainer><license>BSD</license><buildtool_depend>catkin</buildtool_depend><build_depend>geometry_msgs</build_depend><build_depend>nav_msgs</build_depend><build_depend>roscpp</build_depend><build_depend>rospy</build_depend><build_depend>std_msgs</build_depend><build_depend>sensor_msgs</build_depend><build_depend>tf</build_depend><build_depend>pcl_ros</build_depend><build_depend>livox_ros_driver</build_depend><run_depend>geometry_msgs</run_depend><run_depend>nav_msgs</run_depend><run_depend>sensor_msgs</run_depend><run_depend>roscpp</run_depend><run_depend>rospy</run_depend><run_depend>std_msgs</run_depend><run_depend>tf</run_depend><run_depend>pcl_ros</run_depend><run_depend>livox_ros_driver</run_depend><test_depend>rostest</test_depend><test_depend>rosbag</test_depend><export></export>
</package>

vi CMakeLists.txt

cmake_minimum_required(VERSION 2.8.3)
project(livox_repub)SET(CMAKE_BUILD_TYPE "Debug")set(CMAKE_C_FLAGS "${CMAKE_C_FLAGS} -fexceptions")
set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -std=c++11 -pthread -std=c++0x -std=c++14 -fexceptions -Wno-unused-local-typedefs")find_package(OpenMP QUIET)
set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} ${OpenMP_CXX_FLAGS}")
set(CMAKE_C_FLAGS "${CMAKE_C_FLAGS}   ${OpenMP_C_FLAGS}")find_package(catkin REQUIRED COMPONENTSgeometry_msgsnav_msgssensor_msgsroscpprospystd_msgspcl_rostflivox_ros_driver)find_package(Eigen3 REQUIRED)
find_package(PCL REQUIRED)
find_package(OpenCV REQUIRED)include_directories(${catkin_INCLUDE_DIRS} ${EIGEN3_INCLUDE_DIR}${PCL_INCLUDE_DIRS})catkin_package(CATKIN_DEPENDS geometry_msgs nav_msgs roscpp rospy std_msgsDEPENDS EIGEN3 PCL OpenCV)add_executable(livox_repub livox_repub.cpp)
target_link_libraries(livox_repub ${catkin_LIBRARIES} ${PCL_LIBRARIES} ${OpenCV_LIBS})

vi livox_repub.cpp

#include <pcl_conversions/pcl_conversions.h>
#include <sensor_msgs/PointCloud2.h>
#include "livox_ros_driver/CustomMsg.h"typedef pcl::PointXYZINormal PointType;
typedef pcl::PointCloud<PointType> PointCloudXYZI;ros::Publisher pub_pcl_out0, pub_pcl_out1;
uint64_t TO_MERGE_CNT = 1;
constexpr bool b_dbg_line = false;
std::vector<livox_ros_driver::CustomMsgConstPtr> livox_data;
void LivoxMsgCbk1(const livox_ros_driver::CustomMsgConstPtr& livox_msg_in) {livox_data.push_back(livox_msg_in);if (livox_data.size() < TO_MERGE_CNT) return;pcl::PointCloud<PointType> pcl_in;for (size_t j = 0; j < livox_data.size(); j++) {auto& livox_msg = livox_data[j];auto time_end = livox_msg->points.back().offset_time;for (unsigned int i = 0; i < livox_msg->point_num; ++i) {PointType pt;pt.x = livox_msg->points[i].x;pt.y = livox_msg->points[i].y;pt.z = livox_msg->points[i].z;float s = livox_msg->points[i].offset_time / (float)time_end;pt.intensity = livox_msg->points[i].line +livox_msg->points[i].reflectivity /10000.0 ; // The integer part is line number and the decimal part is timestamppt.curvature = s*0.1;pcl_in.push_back(pt);}}unsigned long timebase_ns = livox_data[0]->timebase;ros::Time timestamp;timestamp.fromNSec(timebase_ns);sensor_msgs::PointCloud2 pcl_ros_msg;pcl::toROSMsg(pcl_in, pcl_ros_msg);pcl_ros_msg.header.stamp.fromNSec(timebase_ns);pcl_ros_msg.header.frame_id = "/livox";pub_pcl_out1.publish(pcl_ros_msg);livox_data.clear();
}int main(int argc, char** argv) {ros::init(argc, argv, "livox_repub");ros::NodeHandle nh;ROS_INFO("start livox_repub");ros::Subscriber sub_livox_msg1 = nh.subscribe<livox_ros_driver::CustomMsg>("/livox/lidar", 100, LivoxMsgCbk1);pub_pcl_out1 = nh.advertise<sensor_msgs::PointCloud2>("/livox_pcl0", 100);ros::spin();
}

launch文件：

vi livox_repub.launch

<launch><node pkg="livox_repub" type="livox_repub" name="livox_repub" output="screen" ><remap from="/livox/lidar" to="/livox/lidar" /></node></launch>

编译运行

             cd ../../catkin_makesource devel/setup.bash roslaunch livox_repub livox_repub.launch

注意：这个包是要先订阅CustomMsg的话题/livox/lidar，然后发布PointCloud2格式的"/livox_pcl0"话题，所以不论是实时驱动livox-driver还是通过bag包发布/livox/lidar，都需要确保有/livox/lidar才能有转换结果
转换后的PointCloud2点云可以通过rviz显示，终端输

rviz

Fixed Frame设置为livox，点云设置为/livox_pcl0

ros graph

大疆livox定制的格式CustomMsg格式转换pointcloud2相关推荐

大疆Livox Mid-40 SLAM 配置问题以及解决方法总结
大疆Livox Mid-40 SLAM 配置问题以及解决方法总结介绍基于Livox三维激光雷达和开源LOAM-Livox进行定位和建图实验. 出现的问题以及解决介绍基于Livox三维激光雷达和开源 ...
大疆Livox Mid-70雷达的使用
一.前期准备 (一).使用Livox Viewer测试雷达 1.设置静态IP sudo ifconfig enx081f7152092a 192.168.1.50 查看IP是否设置成功 lacyexs ...
大疆坐标系和高德地图坐标系之间转换的问题
目前各种坐标系非常多,他们之间的转换就是个比较麻烦的问题.之前有个问题,就是大疆的坐标系在高德地图上显示发生了偏移,但是好在高德地图提供了坐标转换的API,但是新的问题来了,大疆的坐标适应了高德的坐标 ...
大疆Livox Mid-40 SLAM
原博主琨特,本人便于学习写到了自己的账号下,见谅. 介绍基于Livox三维激光雷达和开源LOAM-Livox进行定位和建图实验. 目录 1.简介 1.1 Livox Mid-40三维激光雷达 1.2 ...
大疆激光雷达livox avia 在ubuntu18.04+ROS中基本使用方法
Readme 大疆livox avia 固态激光雷达操作步骤: 1.参考本人CSDN博客lsg_dawn中收藏中的一篇关于ubuntu18.04中使用livox avia 雷达的文章 Livox Av ...
Pix4Dmapper拼接处理大疆H20T无人机热红外成像照片（生成正射影像）
相机已经有不少朋友对大疆的M300 RTK加禅思H20T混合相机这套经典组合不陌生了. 这套设备虽然比较适用于各种现场勘查调查,热成像追踪,实时查看等,但是这套系统其实也可以用于航拍正射影像,正射热红 ...
关于大疆御2行业进阶版（M2EA）热红外照片温度信息提取问题
(1)大疆dji_thermal_sdk只支持导出RAW格式的数据信息而非TIFF,RAW格式无法直接导入Pix4d Mapper等软件用于拼接,需要自己转换转换成TIFF. (2)虽然M2EA也就是 ...
激光雷达大洗牌：华为、大疆入局、初创玩家眼热，谁将率先破局？
文/智能相对论(aixdlun) 作者/leo陈 2021年,自动驾驶热潮未退去,作为"眼睛"的激光雷达同样热度不减.尤其是L2级别以上的自动驾驶功能迈入量产阶段,让激光雷达& ...
大疆半固态激光雷达Horizon的优缺点
原文链接: 大疆激光雷达,车厂为何不爱? 优点: 1.成本低,可以量产:2020 年,在当年的 CES 展会上,大疆 Livox 发布了 Horizon(浩界)和 Tele-15(泰览),前者的售价仅 ...
基于大疆智图和云端地球，发布OSGB格式三维航拍模型
一三维模型生成 1.利用大疆精灵4进行航拍 2.利用大疆智图进行三维建模,在1处选择拷贝的航拍文件夹,2处勾选三维模型. 在高级设置里面选择模型格式,这里我们选择最普遍的格式之一,OSGB格式. 结 ...

大疆livox定制的格式CustomMsg格式转换pointcloud2

大疆livox定制的格式CustomMsg格式转换pointcloud2相关推荐

最新文章

热门文章