diff --git a/livox_ros_driver/CMakeLists.txt b/livox_ros_driver/CMakeLists.txt index f2fe02d..61e7d2c 100755 --- a/livox_ros_driver/CMakeLists.txt +++ b/livox_ros_driver/CMakeLists.txt @@ -26,7 +26,7 @@ find_package(Boost REQUIRED) find_package(Boost REQUIRED COMPONENTS thread) ## get pointcloud package -#find_package(PCL REQUIRED) +find_package(PCL REQUIRED) ## System dependencies are found with CMake's conventions # find_package(Boost REQUIRED COMPONENTS system) @@ -130,7 +130,7 @@ catkin_package(CATKIN_DEPENDS ## Your package locations should be listed before other locations include_directories( ${catkin_INCLUDE_DIRS} - #${PCL_INCLUDE_DIRS} + ${PCL_INCLUDE_DIRS} ) ## Declare a C++ library @@ -171,8 +171,8 @@ include_directories( ) ## PCL library -#link_directories(${PCL_LIBRARY_DIRS}) -#add_definitions(${PCL_DEFINITIONS}) +link_directories(${PCL_LIBRARY_DIRS}) +add_definitions(${PCL_DEFINITIONS}) ## make sure the livox_sdk_static library is installed find_library(LIVOX_SDK_LIBRARY liblivox_sdk_static.a /usr/local/lib) diff --git a/livox_ros_driver/livox_hub.cpp b/livox_ros_driver/livox_hub.cpp index 301288b..7fbdb82 100755 --- a/livox_ros_driver/livox_hub.cpp +++ b/livox_ros_driver/livox_hub.cpp @@ -41,6 +41,8 @@ #include #include #include +#include + /* const varible -------------------------------------------------------------------------------- */ /* user area */ @@ -112,6 +114,8 @@ typedef enum { /* for global publisher use */ ros::Publisher cloud_pub; +typedef pcl::PointCloud PointCloud; + /* for device connect use ----------------------------------------------------------------------- */ typedef enum { @@ -305,7 +309,7 @@ static uint32_t CalculatePacketQueueSize(uint32_t interval_ms, uint32_t device_t return queue_size; } - +#if 0 /* for pointcloud convert process */ static uint32_t PublishPointcloud2(StoragePacketQueue* queue, uint32_t packet_num, \ uint8_t handle) { @@ -317,6 +321,10 @@ static uint32_t PublishPointcloud2(StoragePacketQueue* queue, uint32_t packet_nu uint32_t kPointXYZISize = 16; sensor_msgs::PointCloud2 cloud; + if (!packet_num) { + return -1; + } + // init ROS standard message header cloud.header.frame_id = "livox_frame"; cloud.height = 1; @@ -384,8 +392,70 @@ static uint32_t PublishPointcloud2(StoragePacketQueue* queue, uint32_t packet_nu 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.data.resize(cloud.row_step); + //ROS_INFO("%ld", cloud.data.capacity()); + cloud_pub.publish(cloud); + + return published_packet; +} +#endif + +/* for pointcloud convert process */ +static uint32_t PublishPointcloud2(StoragePacketQueue* queue, uint32_t packet_num, \ + uint8_t handle) { + + uint64_t timestamp = 0; + uint64_t last_timestamp = 0; + uint32_t published_packet = 0; + uint32_t point_num = 0; + + /* init point cloud data struct */ + PointCloud::Ptr cloud (new PointCloud); + cloud->header.frame_id = "livox_frame"; + cloud->height = 1; + cloud->width = 0; + + // add pointcloud + StoragePacket storage_packet; + while (published_packet < packet_num) { + 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)) { + ROS_INFO("packet loss : %ld", timestamp); + break; + } + + if (!cloud->width) { + //cloud->header.stamp = ros::Time(timestamp/1000000000.0); // to ros time stamp + + cloud->header.stamp = timestamp/1000000000.0; // to ros time stamp + ROS_DEBUG("[%d]:%ld us", handle, timestamp); + } + cloud->width += storage_packet.point_num; + + for (uint32_t i = 0; i < storage_packet.point_num; i++) { + pcl::PointXYZI point; + point.x = raw_points->x/1000.0f; + point.y = raw_points->y/1000.0f; + point.z = raw_points->z/1000.0f; + point.intensity = (float)raw_points->reflectivity; + cloud->points.push_back(point); + + ++raw_points; + ++point_num; + } + + QueuePopUpdate(queue); + last_timestamp = timestamp; + ++published_packet; + } + + //ROS_INFO("%ld", ptr_cloud->data.capacity()); + cloud_pub.publish(cloud); return published_packet; @@ -754,11 +824,11 @@ int main(int argc, char **argv) { int msg_type; livox_node.getParam("livox_msg_type", msg_type); if (kPointCloud2Msg == msg_type) { - cloud_pub = livox_node.advertise("livox/hub", kMinEthPacketQueueSize); + cloud_pub = livox_node.advertise("livox/hub", kMaxEthPacketQueueSize); ROS_INFO("Publish PointCloud2"); } else { cloud_pub = livox_node.advertise("livox/hub", \ - kMinEthPacketQueueSize); + kMaxEthPacketQueueSize); ROS_INFO("Publish Livox Custom Msg"); } diff --git a/livox_ros_driver/livox_lidar.cpp b/livox_ros_driver/livox_lidar.cpp index 9f131a3..d635717 100755 --- a/livox_ros_driver/livox_lidar.cpp +++ b/livox_ros_driver/livox_lidar.cpp @@ -41,6 +41,8 @@ #include #include #include +#include + /* const varible -------------------------------------------------------------------------------- */ /* user area */ @@ -112,6 +114,7 @@ typedef enum { /* for global publisher use */ ros::Publisher cloud_pub; +typedef pcl::PointCloud PointCloud; /* for device connect use ----------------------------------------------------------------------- */ typedef enum { @@ -300,7 +303,7 @@ static uint32_t CalculatePacketQueueSize(uint32_t interval_ms, uint32_t device_t return queue_size; } - +#if 0 /* for pointcloud convert process */ static uint32_t PublishPointcloud2(StoragePacketQueue* queue, uint32_t packet_num, \ uint8_t handle) { @@ -312,6 +315,10 @@ static uint32_t PublishPointcloud2(StoragePacketQueue* queue, uint32_t packet_nu uint32_t kPointXYZISize = 16; sensor_msgs::PointCloud2 cloud; + if (!packet_num) { + return -1; + } + // init ROS standard message header cloud.header.frame_id = "livox_frame"; cloud.height = 1; @@ -379,12 +386,74 @@ static uint32_t PublishPointcloud2(StoragePacketQueue* queue, uint32_t packet_nu 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.data.resize(cloud.row_step); + //ROS_INFO("%ld", cloud.data.capacity()); cloud_pub.publish(cloud); return published_packet; } +#endif + +/* for pointcloud convert process */ +static uint32_t PublishPointcloud2(StoragePacketQueue* queue, uint32_t packet_num, \ + uint8_t handle) { + uint64_t timestamp = 0; + uint64_t last_timestamp = 0; + uint32_t published_packet = 0; + uint32_t point_num = 0; + + /* init point cloud data struct */ + PointCloud::Ptr cloud (new PointCloud); + cloud->header.frame_id = "livox_frame"; + cloud->height = 1; + cloud->width = 0; + + // add pointcloud + StoragePacket storage_packet; + while (published_packet < packet_num) { + 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)) { + ROS_INFO("packet loss : %ld", timestamp); + break; + } + + if (!cloud->width) { + //cloud->header.stamp = ros::Time(timestamp/1000000000.0); // to ros time stamp + + cloud->header.stamp = timestamp/1000000000.0; // to ros time stamp + ROS_DEBUG("[%d]:%ld us", handle, timestamp); + } + cloud->width += storage_packet.point_num; + + for (uint32_t i = 0; i < storage_packet.point_num; i++) { + pcl::PointXYZI point; + point.x = raw_points->x/1000.0f; + point.y = raw_points->y/1000.0f; + point.z = raw_points->z/1000.0f; + point.intensity = (float)raw_points->reflectivity; + cloud->points.push_back(point); + + ++raw_points; + ++point_num; + } + + QueuePopUpdate(queue); + last_timestamp = timestamp; + ++published_packet; + } + + //ROS_INFO("%ld", ptr_cloud->data.capacity()); + + cloud_pub.publish(cloud); + + return published_packet; +} + /* for pointcloud convert process */ static uint32_t PublishCustomPointcloud(StoragePacketQueue* queue, uint32_t packet_num,\ @@ -710,11 +779,11 @@ int main(int argc, char **argv) { livox_node.getParam("livox_msg_type", msg_type); if (kPointCloud2Msg == msg_type) { cloud_pub = livox_node.advertise("livox/lidar", \ - kMinEthPacketQueueSize); + kMaxEthPacketQueueSize); ROS_INFO("Publish PointCloud2"); } else { cloud_pub = livox_node.advertise("livox/lidar", \ - kMinEthPacketQueueSize); + kMaxEthPacketQueueSize); ROS_INFO("Publish Livox Custom Msg"); }