官方livox_driver驱动livox雷达发出的点云topic有两种,一种是大疆览沃定制的格式CustomMsg格式,另一种是将CustomMsg格式
转换过的pointcloud2格式,参见
Livox雷达驱动程序发布点云格式CustomMsg、PointCloud2、pcl::PointXYZI、pcl::PointXYZINormal解析
现在将转换这部分的代码提取出来,方便 随时使用
- 创建ros功能包
mkdir -p livox_repub/src
cd livox_repub/src
catkin_init_workspace
cd ..
catkin_make
- livox_repub.cpp
cd src
mkdir livox_repub
cd 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 COMPONENTS
geometry_msgs
nav_msgs
sensor_msgs
roscpp
rospy
std_msgs
pcl_ros
tf
livox_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_msgs
DEPENDS 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 ;
pt.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_make
source 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
本文内容由网友自发贡献,版权归原作者所有,本站不承担相应法律责任。如您发现有涉嫌抄袭侵权的内容,请联系:hwhale#tublm.com(使用前将#替换为@)