Dajiang livox customized format custommsg format conversion pointcloud2

official livox_driver drive livox Point cloud from radar topic There are two kinds of , One is the customized format of Dajiang Lanwo CustomMsg Format , The other is to make CustomMsg Format
Transformed pointcloud2 Format , See
Livox Radar driver release point cloud format CustomMsg、PointCloud2、pcl::PointXYZI、pcl::PointXYZINormal analysis
Now extract the code that transforms this part , convenient Use at any time

1. establish ros Function pack

				mkdir -p livox_repub/src
				cd livox_repub/src
				catkin_init_workspace 
				cd ..
				catkin_make

2. 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="[email protected]">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 ; // The integer part is line number and the decimal part is timestamp
      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();
}

3. Compile operation

				cd ../../
				catkin_make
				source devel/setup.bash 
				roslaunch livox_repub livox_repub.launch

Be careful ： This package needs to be subscribed first CustomMsg The topic of /livox/lidar, And then release PointCloud2 Format "/livox_pcl0" topic of conversation , So whether it's real-time drive livox-driver Or through bag Package release /livox/lidar, All need to make sure there is /livox/lidar Only then can there be conversion results
Converted PointCloud2 Point cloud can pass rviz Show , Terminal output

rviz

Fixed Frame Set to livox, Point cloud set to /livox_pcl0

ros graph