官方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的全部內容，希望文章能夠幫你解決所遇到的問題。

如果覺得生活随笔網站內容還不錯，歡迎將生活随笔推薦給好友。