From 844dd85fe8006c4562ede5f765aa277f7a2d109e Mon Sep 17 00:00:00 2001 From: Livox-SDK Date: Mon, 8 Apr 2019 21:48:12 +0800 Subject: [PATCH] feature:add offset time for every point fix:fix directory tree bug in the license file change:update custom msg format in README.md --- README.md | 18 ++++++------- livox_ros_driver/livox_hub.cpp | 37 +++++++++++++++++++++----- livox_ros_driver/livox_lidar.cpp | 39 +++++++++++++++++++++++----- livox_ros_driver/msg/CustomMsg.msg | 1 - livox_ros_driver/msg/CustomPoint.msg | 1 + 5 files changed, 73 insertions(+), 23 deletions(-) diff --git a/README.md b/README.md index 308b8b2..3c53ac6 100644 --- a/README.md +++ b/README.md @@ -1,12 +1,12 @@ -# livox ros driver +# Livox ROS Driver ### Run livox ros driver livox_ros_driver is a new ros package under the Livox-SDK/Livox-SDK-ROS/src directory, which is designed to gradually become the standard driver package for livox devices in the ros environment. The driver offers users a wealth of options: -1. Publish pointcloud2 format point cloud and automatically load rviz； +1. Publish pointcloud2 format point cloud and automatically load rviz. -for example： +for example: ``` roslaunch livox_ros_driver livox_lidar_rviz.launch bd_list:="broadcast_code1&broadcast_code2&broadcast_code3" @@ -18,9 +18,9 @@ or roslaunch livox_ros_driver livox_hub_rviz.launch bd_list:="hub_broadcast_code" ``` -2. Publish pointcloud2 format point cloud only； +2. Publish pointcloud2 format point cloud only. -for example： +for example: ``` roslaunch livox_ros_driver livox_lidar.launch bd_list:="broadcast_code1&broadcast_code2&broadcast_code3" @@ -34,9 +34,9 @@ roslaunch livox_ros_driver livox_hub.launch bd_list:="hub_broadcast_code" -3. Publish livox custom format point cloud； +3. Publish livox custom format point cloud. -for example： +for example: ``` roslaunch livox_ros_driver livox_lidar_msg.launch bd_list:="broadcast_code1&broadcast_code2&broadcast_code3" @@ -48,12 +48,11 @@ or roslaunch livox_ros_driver livox_hub_msg.launch bd_list:="hub_broadcast_code" ``` -livox custom msg format： +livox custom msg format: ``` Header header # ROS standard message header uint64 timebase # The time of first point -uint32 timestep # Time interval between adjacent point clouds uint32 point_num # Total number of pointclouds uint8 lidar_id # Lidar device id number uint8[3] rsvd # Reserved use @@ -61,6 +60,7 @@ CustomPoint[] points # Pointcloud data ``` pointcloud format: ``` +uint32 offset_time # offset time relative to the base time float32 x # X axis, unit:m float32 y # Y axis, unit:m float32 z # Z axis, unit:m diff --git a/livox_ros_driver/livox_hub.cpp b/livox_ros_driver/livox_hub.cpp index 70a1ec6..b069f3d 100755 --- a/livox_ros_driver/livox_hub.cpp +++ b/livox_ros_driver/livox_hub.cpp @@ -49,6 +49,7 @@ const uint64_t kPacketTimeGap = 1000000; // 1ms = 1000000ns const uint64_t kMaxPacketTimeGap = 1700000; // the threshold of packet continuous +const uint64_t kDeviceDisconnectThreshold = 1000000000; // the threshold of device disconect const uint64_t kNsPerSecond = 1000000000; // 1s = 1000000000ns const uint32_t kPublishIntervalMs = 50; // unit:ms @@ -156,12 +157,20 @@ void InitStoragePacketPool(void) { } } -uint32_t QueuePop(StoragePacketQueue* queue, StoragePacket* storage_packet) { +static void QueueProPop(StoragePacketQueue* queue, StoragePacket* storage_packet) { uint32_t mask = queue->mask; uint32_t rd_idx = queue->rd_idx & mask; memcpy(storage_packet, queue->storage_packet[rd_idx], sizeof(StoragePacket)); +} + +static void QueuePopUpdate(StoragePacketQueue* queue) { queue->rd_idx++; +} + +uint32_t QueuePop(StoragePacketQueue* queue, StoragePacket* storage_packet) { + QueueProPop(queue, storage_packet); + QueuePopUpdate(queue); return 1; } @@ -236,6 +245,15 @@ static uint64_t GetStoragePacketTimestamp(StoragePacket* packet) { } } +static uint32_t GetPointInterval(uint32_t device_type) { + if ((kDeviceTypeLidarTele == device_type) || \ + (kDeviceTypeLidarHorizon == device_type)) { + return 4167; // 4167 ns + } else { + return 10000; // ns + } +} + /* for pointcloud convert process */ static uint32_t PublishPointcloud2(StoragePacketQueue* queue, uint32_t packet_num, \ uint8_t handle) { @@ -251,7 +269,7 @@ static uint32_t PublishPointcloud2(StoragePacketQueue* queue, uint32_t packet_nu StoragePacket storage_packet; while (published_packet < packet_num) { - QueuePop(queue, &storage_packet); + QueueProPop(queue, &storage_packet); LivoxEthPacket* raw_packet = reinterpret_cast(storage_packet.raw_data); LivoxRawPoint* raw_points = reinterpret_cast(raw_packet->data); @@ -278,6 +296,7 @@ static uint32_t PublishPointcloud2(StoragePacketQueue* queue, uint32_t packet_nu cloud->points.push_back(point); } + QueuePopUpdate(queue); last_timestamp = timestamp; ++published_packet; } @@ -294,6 +313,8 @@ static uint32_t PublishCustomPointcloud(StoragePacketQueue* queue, uint32_t pack uint64_t timestamp = 0; uint64_t last_timestamp = 0; uint32_t published_packet = 0; + uint32_t point_interval = GetPointInterval(lidars[handle].info.type); + uint32_t packet_offset_time = 0; // ns /* init livox custom msg */ livox_ros_driver::CustomMsg livox_msg; @@ -303,30 +324,33 @@ static uint32_t PublishCustomPointcloud(StoragePacketQueue* queue, uint32_t pack ++msg_seq; livox_msg.header.stamp = ros::Time::now(); livox_msg.timebase = 0; - livox_msg.timestep = 10000; // 10us = 10^4ns; livox_msg.point_num = 0; livox_msg.lidar_id = handle; StoragePacket storage_packet; while (published_packet < packet_num) { - QueuePop(queue, &storage_packet); + QueueProPop(queue, &storage_packet); LivoxEthPacket* raw_packet = reinterpret_cast(storage_packet.raw_data); LivoxRawPoint* raw_points = reinterpret_cast(raw_packet->data); timestamp = GetStoragePacketTimestamp(&storage_packet); if (published_packet && \ - ((timestamp - last_timestamp) > kMaxPacketTimeGap)) { + ((timestamp - last_timestamp) > kDeviceDisconnectThreshold)) { ROS_INFO("packet loss : %ld", timestamp); break; } if (!livox_msg.timebase) { livox_msg.timebase = timestamp; // to us - ROS_DEBUG("[%d]:%ld", handle, livox_msg.timebase); + packet_offset_time = 0; // first packet + ROS_DEBUG("[%d]:%ld %d", handle, livox_msg.timebase, point_interval); + } else { + packet_offset_time = (uint32_t)(timestamp - livox_msg.timebase); } livox_msg.point_num += storage_packet.point_num; for (uint32_t i = 0; i < storage_packet.point_num; i++) { livox_ros_driver::CustomPoint point; + point.offset_time = packet_offset_time + i*point_interval; point.x = raw_points->x/1000.0f; point.y = raw_points->y/1000.0f; point.z = raw_points->z/1000.0f; @@ -336,6 +360,7 @@ static uint32_t PublishCustomPointcloud(StoragePacketQueue* queue, uint32_t pack livox_msg.points.push_back(point); } + QueuePopUpdate(queue); last_timestamp = timestamp; ++published_packet; } diff --git a/livox_ros_driver/livox_lidar.cpp b/livox_ros_driver/livox_lidar.cpp index fe95664..79abcd9 100755 --- a/livox_ros_driver/livox_lidar.cpp +++ b/livox_ros_driver/livox_lidar.cpp @@ -49,6 +49,7 @@ const uint64_t kPacketTimeGap = 1000000; // 1ms = 1000000ns const uint64_t kMaxPacketTimeGap = 1700000; // the threshold of packet continuous +const uint64_t kDeviceDisconnectThreshold = 1000000000; // the threshold of device disconect const uint64_t kNsPerSecond = 1000000000; // 1s = 1000000000ns const uint32_t kPublishIntervalMs = 50; // unit:ms @@ -154,12 +155,20 @@ void InitStoragePacketPool(void) { } } -uint32_t QueuePop(StoragePacketQueue* queue, StoragePacket* storage_packet) { +static void QueueProPop(StoragePacketQueue* queue, StoragePacket* storage_packet) { uint32_t mask = queue->mask; uint32_t rd_idx = queue->rd_idx & mask; memcpy(storage_packet, queue->storage_packet[rd_idx], sizeof(StoragePacket)); +} + +static void QueuePopUpdate(StoragePacketQueue* queue) { queue->rd_idx++; +} + +uint32_t QueuePop(StoragePacketQueue* queue, StoragePacket* storage_packet) { + QueueProPop(queue, storage_packet); + QueuePopUpdate(queue); return 1; } @@ -231,6 +240,15 @@ static uint64_t GetStoragePacketTimestamp(StoragePacket* packet) { } } +static uint32_t GetPointInterval(uint32_t device_type) { + if ((kDeviceTypeLidarTele == device_type) || \ + (kDeviceTypeLidarHorizon == device_type)) { + return 4167; // 4167 ns + } else { + return 10000; // ns + } +} + /* for pointcloud convert process */ static uint32_t PublishPointcloud2(StoragePacketQueue* queue, uint32_t packet_num, \ uint8_t handle) { @@ -246,7 +264,7 @@ static uint32_t PublishPointcloud2(StoragePacketQueue* queue, uint32_t packet_nu StoragePacket storage_packet; while (published_packet < packet_num) { - QueuePop(queue, &storage_packet); + QueueProPop(queue, &storage_packet); LivoxEthPacket* raw_packet = reinterpret_cast(storage_packet.raw_data); LivoxRawPoint* raw_points = reinterpret_cast(raw_packet->data); @@ -273,6 +291,7 @@ static uint32_t PublishPointcloud2(StoragePacketQueue* queue, uint32_t packet_nu cloud->points.push_back(point); } + QueuePopUpdate(queue); last_timestamp = timestamp; ++published_packet; } @@ -289,6 +308,8 @@ static uint32_t PublishCustomPointcloud(StoragePacketQueue* queue, uint32_t pack uint64_t timestamp = 0; uint64_t last_timestamp = 0; uint32_t published_packet = 0; + uint32_t point_interval = GetPointInterval(lidars[handle].info.type); + uint32_t packet_offset_time = 0; // ns /* init livox custom msg */ livox_ros_driver::CustomMsg livox_msg; @@ -298,30 +319,33 @@ static uint32_t PublishCustomPointcloud(StoragePacketQueue* queue, uint32_t pack ++msg_seq; livox_msg.header.stamp = ros::Time::now(); livox_msg.timebase = 0; - livox_msg.timestep = 10000; // 10us = 10^4ns; livox_msg.point_num = 0; livox_msg.lidar_id = handle; StoragePacket storage_packet; while (published_packet < packet_num) { - QueuePop(queue, &storage_packet); + QueueProPop(queue, &storage_packet); LivoxEthPacket* raw_packet = reinterpret_cast(storage_packet.raw_data); LivoxRawPoint* raw_points = reinterpret_cast(raw_packet->data); timestamp = GetStoragePacketTimestamp(&storage_packet); if (published_packet && \ - ((timestamp - last_timestamp) > kMaxPacketTimeGap)) { + ((timestamp - last_timestamp) > kDeviceDisconnectThreshold)) { ROS_INFO("packet loss : %ld", timestamp); break; } if (!livox_msg.timebase) { livox_msg.timebase = timestamp; // to us - ROS_DEBUG("[%d]:%ld", handle, livox_msg.timebase); - } + packet_offset_time = 0; // first packet + ROS_DEBUG("[%d]:%ld %d", handle, livox_msg.timebase, point_interval); + } else { + packet_offset_time = (uint32_t)(timestamp - livox_msg.timebase); + } livox_msg.point_num += storage_packet.point_num; for (uint32_t i = 0; i < storage_packet.point_num; i++) { livox_ros_driver::CustomPoint point; + point.offset_time = packet_offset_time + i*point_interval; point.x = raw_points->x/1000.0f; point.y = raw_points->y/1000.0f; point.z = raw_points->z/1000.0f; @@ -331,6 +355,7 @@ static uint32_t PublishCustomPointcloud(StoragePacketQueue* queue, uint32_t pack livox_msg.points.push_back(point); } + QueuePopUpdate(queue); last_timestamp = timestamp; ++published_packet; } diff --git a/livox_ros_driver/msg/CustomMsg.msg b/livox_ros_driver/msg/CustomMsg.msg index 20006e2..10e0c28 100644 --- a/livox_ros_driver/msg/CustomMsg.msg +++ b/livox_ros_driver/msg/CustomMsg.msg @@ -2,7 +2,6 @@ Header header # ROS standard message header uint64 timebase # The time of first point -uint32 timestep # Time interval between adjacent point clouds uint32 point_num # Total number of pointclouds uint8 lidar_id # Lidar device id number uint8[3] rsvd # Reserved use diff --git a/livox_ros_driver/msg/CustomPoint.msg b/livox_ros_driver/msg/CustomPoint.msg index 8d3ca3a..644736d 100644 --- a/livox_ros_driver/msg/CustomPoint.msg +++ b/livox_ros_driver/msg/CustomPoint.msg @@ -1,5 +1,6 @@ # Livox costom pointcloud format. +uint32 offset_time # offset time relative to the base time float32 x # X axis, unit:m float32 y # Y axis, unit:m float32 z # Z axis, unit:m