mirrors/livox_ros_driver
1
0
Fork 0
mirror of https://github.com/Livox-SDK/livox_ros_driver.git synced 2023-04-06 15:49:55 +08:00
Code Issues Packages Projects Releases Wiki Activity

feature:manually populate the PointCloud2 format and remvoe the pcl dependencies

Browse Source
This commit is contained in:
Livox-SDK
2019-04-21 19:52:47 +08:00
parent 97d132377f
commit 6cc7847fc6
4 changed files with 111 additions and 51 deletions
Download Patch File Download Diff File Expand all files Collapse all files

6
.gitignore vendored
View File

@@ -1,8 +1,6 @@
# ignore dir # ignore dir or file
build/ ./CMakeLists.txt
devel/
.catkin_workspace
#whitelist #whitelist

12
livox_ros_driver/CMakeLists.txt
View File

@@ -26,7 +26,7 @@ find_package(Boost REQUIRED)
find_package(Boost REQUIRED COMPONENTS thread) find_package(Boost REQUIRED COMPONENTS thread)
## get pointcloud package ## get pointcloud package
find_package(PCL REQUIRED) #find_package(PCL REQUIRED)
## System dependencies are found with CMake's conventions ## System dependencies are found with CMake's conventions
# find_package(Boost REQUIRED COMPONENTS system) # find_package(Boost REQUIRED COMPONENTS system)
@@ -134,7 +134,7 @@ catkin_package(
include_directories( include_directories(
# include # include
${catkin_INCLUDE_DIRS} ${catkin_INCLUDE_DIRS}
${PCL_INCLUDE_DIRS} # ${PCL_INCLUDE_DIRS}
) )
## Declare a C++ library ## Declare a C++ library
@@ -176,8 +176,8 @@ include_directories(
## PCL library ## PCL library
link_directories(${PCL_LIBRARY_DIRS}) #link_directories(${PCL_LIBRARY_DIRS})
add_definitions(${PCL_DEFINITIONS}) #add_definitions(${PCL_DEFINITIONS})
add_executable(${PROJECT_LIDAR}_node add_executable(${PROJECT_LIDAR}_node
${PROJECT_LIDAR_SRC}) ${PROJECT_LIDAR_SRC})
@@ -196,7 +196,7 @@ add_executable(${PROJECT_LIDAR}_node
target_link_libraries(${PROJECT_LIDAR}_node target_link_libraries(${PROJECT_LIDAR}_node
livox_sdk_static.a livox_sdk_static.a
${APR_LIBRARIES} ${APR_LIBRARIES}
${PCL_LIBRARIES} # ${PCL_LIBRARIES}
${catkin_LIBRARIES} ${catkin_LIBRARIES}
${Boost_LIBRARIES} ${Boost_LIBRARIES}
-lrt -lrt
@@ -209,7 +209,7 @@ add_executable(${PROJECT_HUB}_node
target_link_libraries(${PROJECT_HUB}_node target_link_libraries(${PROJECT_HUB}_node
livox_sdk_static.a livox_sdk_static.a
${APR_LIBRARIES} ${APR_LIBRARIES}
${PCL_LIBRARIES} # ${PCL_LIBRARIES}
${catkin_LIBRARIES} ${catkin_LIBRARIES}
${Boost_LIBRARIES} ${Boost_LIBRARIES}
-lrt -lrt

71
livox_ros_driver/livox_hub.cpp
View File

@@ -36,7 +36,7 @@
#include "livox_sdk.h" #include "livox_sdk.h"
#include <ros/ros.h> #include <ros/ros.h>
#include <pcl_ros/point_cloud.h> #include <sensor_msgs/PointCloud2.h>
#include <livox_ros_driver/CustomPoint.h> #include <livox_ros_driver/CustomPoint.h>
#include <livox_ros_driver/CustomMsg.h> #include <livox_ros_driver/CustomMsg.h>
@@ -57,8 +57,6 @@ const uint32_t kPublishIntervalMs = 50; // unit:ms
#define BD_ARGV_POS (1) #define BD_ARGV_POS (1)
#define COMMANDLINE_BD_SIZE (15) #define COMMANDLINE_BD_SIZE (15)
typedef pcl::PointCloud<pcl::PointXYZI> PointCloud;
#pragma pack(1) #pragma pack(1)
typedef struct { typedef struct {
uint64_t time_rcv; // used for pps sync only mode uint64_t time_rcv; // used for pps sync only mode
@@ -257,16 +255,42 @@ static uint32_t GetPointInterval(uint32_t device_type) {
/* for pointcloud convert process */ /* for pointcloud convert process */
static uint32_t PublishPointcloud2(StoragePacketQueue* queue, uint32_t packet_num, \ static uint32_t PublishPointcloud2(StoragePacketQueue* queue, uint32_t packet_num, \
uint8_t handle) { uint8_t handle) {
uint64_t timestamp = 0; uint64_t timestamp = 0;
uint64_t last_timestamp = 0; uint64_t last_timestamp = 0;
uint32_t published_packet = 0; uint32_t published_packet = 0;
/* init point cloud data struct */ uint32_t point_num = 0;
PointCloud::Ptr cloud (new PointCloud); uint32_t kPointXYZISize = 16;
sensor_msgs::PointCloud2 cloud;
cloud->header.frame_id = "livox_frame"; // init ROS standard message header
cloud->height = 1; cloud.header.frame_id = "livox_frame";
cloud->width = 0; cloud.height = 1;
cloud.width = 0;
// init ROS standard fields
cloud.fields.resize(4);
cloud.fields[0].offset = 0;
cloud.fields[0].name = "x";
cloud.fields[0].count = 1;
cloud.fields[0].datatype = sensor_msgs::PointField::FLOAT32;
cloud.fields[1].offset = 4;
cloud.fields[1].name = "y";
cloud.fields[1].count = 1;
cloud.fields[1].datatype = sensor_msgs::PointField::FLOAT32;
cloud.fields[2].offset = 8;
cloud.fields[2].name = "z";
cloud.fields[2].count = 1;
cloud.fields[2].datatype = sensor_msgs::PointField::FLOAT32;
cloud.fields[3].offset = 12;
cloud.fields[3].name = "intensity";
cloud.fields[3].count = 1;
cloud.fields[3].datatype = sensor_msgs::PointField::FLOAT32;
// add pointcloud
cloud.data.resize(packet_num * kMaxPointPerEthPacket * kPointXYZISize);
cloud.point_step = kPointXYZISize;
uint8_t *point_base = cloud.data.data();
StoragePacket storage_packet; StoragePacket storage_packet;
while (published_packet < packet_num) { while (published_packet < packet_num) {
QueueProPop(queue, &storage_packet); QueueProPop(queue, &storage_packet);
@@ -280,27 +304,34 @@ static uint32_t PublishPointcloud2(StoragePacketQueue* queue, uint32_t packet_nu
break; break;
} }
if (!cloud->width) { if (!cloud.width) {
cloud->header.stamp = timestamp / 1000; // to us cloud.header.stamp = ros::Time(timestamp/1000000000.0); // to ros time stamp
ROS_DEBUG("[%d]:%ld us", handle, cloud->header.stamp); ROS_DEBUG("[%d]:%ld us", handle, timestamp);
} }
cloud->width += storage_packet.point_num; cloud.width += storage_packet.point_num;
for (uint32_t i = 0; i < storage_packet.point_num; i++) { for (uint32_t i = 0; i < storage_packet.point_num; i++) {
pcl::PointXYZI point; *((float*)(point_base + 0)) = raw_points->x/1000.0f;
point.x = raw_points->x/1000.0f; *((float*)(point_base + 4)) = raw_points->y/1000.0f;
point.y = raw_points->y/1000.0f; *((float*)(point_base + 8)) = raw_points->z/1000.0f;
point.z = raw_points->z/1000.0f; *((float*)(point_base + 12)) = (float) raw_points->reflectivity;
point.intensity = (float) raw_points->reflectivity;
++raw_points; ++raw_points;
cloud->points.push_back(point); ++point_num;
point_base += kPointXYZISize;
} }
QueuePopUpdate(queue); QueuePopUpdate(queue);
last_timestamp = timestamp; last_timestamp = timestamp;
++published_packet; ++published_packet;
} }
//ROS_INFO("%d", cloud->width); //ROS_INFO("%d", cloud.width);
cloud.row_step = cloud.width * cloud.point_step;
cloud.is_bigendian = false;
cloud.is_dense = true;
// adjust the real size
cloud.data.resize(cloud.row_step);
ROS_INFO("%ld", cloud.data.capacity());
cloud_pub.publish(cloud); cloud_pub.publish(cloud);
return published_packet; return published_packet;
@@ -665,7 +696,7 @@ int main(int argc, char **argv) {
int msg_type; int msg_type;
livox_node.getParam("livox_msg_type", msg_type); livox_node.getParam("livox_msg_type", msg_type);
if (kPointCloud2Msg == msg_type) { if (kPointCloud2Msg == msg_type) {
cloud_pub = livox_node.advertise<PointCloud>("livox/hub", kMaxStoragePackets); cloud_pub = livox_node.advertise<sensor_msgs::PointCloud2>("livox/hub", kMaxStoragePackets);
ROS_INFO("Publish PointCloud2"); ROS_INFO("Publish PointCloud2");
} else { } else {
cloud_pub = livox_node.advertise<livox_ros_driver::CustomMsg>("livox/hub", \ cloud_pub = livox_node.advertise<livox_ros_driver::CustomMsg>("livox/hub", \

71
livox_ros_driver/livox_lidar.cpp
View File

@@ -36,7 +36,7 @@
#include "livox_sdk.h" #include "livox_sdk.h"
#include <ros/ros.h> #include <ros/ros.h>
#include <pcl_ros/point_cloud.h> #include <sensor_msgs/PointCloud2.h>
#include <livox_ros_driver/CustomPoint.h> #include <livox_ros_driver/CustomPoint.h>
#include <livox_ros_driver/CustomMsg.h> #include <livox_ros_driver/CustomMsg.h>
@@ -57,8 +57,6 @@ const uint32_t kPublishIntervalMs = 50; // unit:ms
#define BD_ARGV_POS (1) #define BD_ARGV_POS (1)
#define COMMANDLINE_BD_SIZE (15) #define COMMANDLINE_BD_SIZE (15)
typedef pcl::PointCloud<pcl::PointXYZI> PointCloud;
#pragma pack(1) #pragma pack(1)
typedef struct { typedef struct {
uint64_t time_rcv; // used for pps sync only mode uint64_t time_rcv; // used for pps sync only mode
@@ -252,16 +250,42 @@ static uint32_t GetPointInterval(uint32_t device_type) {
/* for pointcloud convert process */ /* for pointcloud convert process */
static uint32_t PublishPointcloud2(StoragePacketQueue* queue, uint32_t packet_num, \ static uint32_t PublishPointcloud2(StoragePacketQueue* queue, uint32_t packet_num, \
uint8_t handle) { uint8_t handle) {
uint64_t timestamp = 0; uint64_t timestamp = 0;
uint64_t last_timestamp = 0; uint64_t last_timestamp = 0;
uint32_t published_packet = 0; uint32_t published_packet = 0;
/* init point cloud data struct */ uint32_t point_num = 0;
PointCloud::Ptr cloud (new PointCloud); uint32_t kPointXYZISize = 16;
sensor_msgs::PointCloud2 cloud;
cloud->header.frame_id = "livox_frame"; // init ROS standard message header
cloud->height = 1; cloud.header.frame_id = "livox_frame";
cloud->width = 0; cloud.height = 1;
cloud.width = 0;
// init ROS standard fields
cloud.fields.resize(4);
cloud.fields[0].offset = 0;
cloud.fields[0].name = "x";
cloud.fields[0].count = 1;
cloud.fields[0].datatype = sensor_msgs::PointField::FLOAT32;
cloud.fields[1].offset = 4;
cloud.fields[1].name = "y";
cloud.fields[1].count = 1;
cloud.fields[1].datatype = sensor_msgs::PointField::FLOAT32;
cloud.fields[2].offset = 8;
cloud.fields[2].name = "z";
cloud.fields[2].count = 1;
cloud.fields[2].datatype = sensor_msgs::PointField::FLOAT32;
cloud.fields[3].offset = 12;
cloud.fields[3].name = "intensity";
cloud.fields[3].count = 1;
cloud.fields[3].datatype = sensor_msgs::PointField::FLOAT32;
// add pointcloud
cloud.data.resize(packet_num * kMaxPointPerEthPacket * kPointXYZISize);
cloud.point_step = kPointXYZISize;
uint8_t *point_base = cloud.data.data();
StoragePacket storage_packet; StoragePacket storage_packet;
while (published_packet < packet_num) { while (published_packet < packet_num) {
QueueProPop(queue, &storage_packet); QueueProPop(queue, &storage_packet);
@@ -275,27 +299,34 @@ static uint32_t PublishPointcloud2(StoragePacketQueue* queue, uint32_t packet_nu
break; break;
} }
if (!cloud->width) { if (!cloud.width) {
cloud->header.stamp = timestamp / 1000; // to us cloud.header.stamp = ros::Time(timestamp/1000000000.0); // to ros time stamp
ROS_DEBUG("[%d]:%ld us", handle, cloud->header.stamp); ROS_DEBUG("[%d]:%ld us", handle, timestamp);
} }
cloud->width += storage_packet.point_num; cloud.width += storage_packet.point_num;
for (uint32_t i = 0; i < storage_packet.point_num; i++) { for (uint32_t i = 0; i < storage_packet.point_num; i++) {
pcl::PointXYZI point; *((float*)(point_base + 0)) = raw_points->x/1000.0f;
point.x = raw_points->x/1000.0f; *((float*)(point_base + 4)) = raw_points->y/1000.0f;
point.y = raw_points->y/1000.0f; *((float*)(point_base + 8)) = raw_points->z/1000.0f;
point.z = raw_points->z/1000.0f; *((float*)(point_base + 12)) = (float) raw_points->reflectivity;
point.intensity = (float) raw_points->reflectivity;
++raw_points; ++raw_points;
cloud->points.push_back(point); ++point_num;
point_base += kPointXYZISize;
} }
QueuePopUpdate(queue); QueuePopUpdate(queue);
last_timestamp = timestamp; last_timestamp = timestamp;
++published_packet; ++published_packet;
} }
//ROS_INFO("%d", cloud->width); //ROS_INFO("%d", cloud.width);
cloud.row_step = cloud.width * cloud.point_step;
cloud.is_bigendian = false;
cloud.is_dense = true;
// adjust the real size
cloud.data.resize(cloud.row_step);
ROS_INFO("%ld", cloud.data.capacity());
cloud_pub.publish(cloud); cloud_pub.publish(cloud);
return published_packet; return published_packet;
@@ -620,7 +651,7 @@ int main(int argc, char **argv) {
int msg_type; int msg_type;
livox_node.getParam("livox_msg_type", msg_type); livox_node.getParam("livox_msg_type", msg_type);
if (kPointCloud2Msg == msg_type) { if (kPointCloud2Msg == msg_type) {
cloud_pub = livox_node.advertise<PointCloud>("livox/lidar", kMaxStoragePackets); cloud_pub = livox_node.advertise<sensor_msgs::PointCloud2>("livox/lidar", kMaxStoragePackets);
ROS_INFO("Publish PointCloud2"); ROS_INFO("Publish PointCloud2");
} else { } else {
cloud_pub = livox_node.advertise<livox_ros_driver::CustomMsg>("livox/lidar", \ cloud_pub = livox_node.advertise<livox_ros_driver::CustomMsg>("livox/lidar", \