diff --git a/CHANGELOG.md b/CHANGELOG.md index 8f150f35..f8be0f1c 100644 --- a/CHANGELOG.md +++ b/CHANGELOG.md @@ -2,19 +2,27 @@ All notable changes to this project will be documented in this file. -## [1.1.3] - 2023-03-13 +## [1.2.0] +### Added +- Revise the frame segmentation logic. +- (Notice!!!) Add Timestamp to each point in Livox pointcloud2 (PointXYZRTLT) format. The PointXYZRTL format has been updated to PointXYZRTLT format. Compatibility needs to be considered. +### Fixed +- Improve support for gPTP and GPS synchronizations. + +--- +## [1.1.3] ### Fixed -- Improve performance when running in ROS2 Humble; +- Improve performance when running in ROS2 Humble. --- -## [1.1.2] - 2023-02-15 +## [1.1.2] ### Changed -- Change publish frequency range to [0.5Hz, 10 Hz]; +- Change publish frequency range to [0.5Hz, 10 Hz]. ### Fixed -- Fix a high CPU-usage problem; +- Fix a high CPU-usage problem. --- -## [1.1.1] - 2023-01-09 +## [1.1.1] ### Added - Offer valid line-number info in the point cloud data of MID-360 Lidar. - Enable IMU by default. @@ -22,7 +30,7 @@ All notable changes to this project will be documented in this file. - Update the README slightly. --- -## [1.0.0] - 2022-12-12 +## [1.0.0] ### Added - Support Mid-360 Lidar. - Support for Ubuntu 22.04 ROS2 humble. diff --git a/README.md b/README.md index 5a6e5293..a625d835 100644 --- a/README.md +++ b/README.md @@ -129,7 +129,7 @@ All internal parameters of Livox_ros_driver2 are in the launch file. Below are d | ------------ | ------------------------------------------------------------ | ------- | | publish_freq | Set the frequency of point cloud publish
Floating-point data type, recommended values 5.0, 10.0, 20.0, 50.0, etc. The maximum publish frequency is 100.0 Hz.| 10.0 | | multi_topic | If the LiDAR device has an independent topic to publish pointcloud data
0 -- All LiDAR devices use the same topic to publish pointcloud data
1 -- Each LiDAR device has its own topic to publish point cloud data | 0 | -| xfer_format | Set pointcloud format
0 -- Livox pointcloud2(PointXYZRTL) pointcloud format
1 -- Livox customized pointcloud format
2 -- Standard pointcloud2 (pcl :: PointXYZI) pointcloud format in the PCL library (just for ROS) | 0 | +| xfer_format | Set pointcloud format
0 -- Livox pointcloud2(PointXYZRTLT) pointcloud format
1 -- Livox customized pointcloud format
2 -- Standard pointcloud2 (pcl :: PointXYZI) pointcloud format in the PCL library (just for ROS) | 0 | **Note :** @@ -137,38 +137,42 @@ All internal parameters of Livox_ros_driver2 are in the launch file. Below are d     ***Livox_ros_driver2 pointcloud data detailed description :*** -1. Livox pointcloud2 (PointXYZRTL) point cloud format, as follows : +1. Livox pointcloud2 (PointXYZRTLT) point cloud format, as follows : ```c float32 x # X axis, unit:m float32 y # Y axis, unit:m float32 z # Z axis, unit:m float32 intensity # the value is reflectivity, 0.0~255.0 -uint8 tag # livox tag -uint8 line # laser number in lidar +uint8 tag # livox tag +uint8 line # laser number in lidar +float64 timestamp # Timestamp of point ``` + **Note :** + + The number of points in the frame may be different, but each point provides a timestamp. 2. Livox customized data package format, as follows : ```c -std_msgs/Header header # ROS standard message header -uint64 timebase # The time of first point -uint32 point_num # Total number of pointclouds -uint8 lidar_id # Lidar device id number -uint8[3] rsvd # Reserved use -CustomPoint[] points # Pointcloud data +std_msgs/Header header # ROS standard message header +uint64 timebase # The time of first point +uint32 point_num # Total number of pointclouds +uint8 lidar_id # Lidar device id number +uint8[3] rsvd # Reserved use +CustomPoint[] points # Pointcloud data ```     Customized Point Cloud (CustomPoint) format in the above customized data package : ```c -uint32 offset_time # offset time relative to the base time +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 -uint8 reflectivity # reflectivity, 0~255 -uint8 tag # livox tag -uint8 line # laser number in lidar +uint8 reflectivity # reflectivity, 0~255 +uint8 tag # livox tag +uint8 line # laser number in lidar ``` 3. The standard pointcloud2 (pcl :: PointXYZI) format in the PCL library (only ROS can publish): diff --git a/src/comm/comm.h b/src/comm/comm.h index 705560f4..492b39e1 100644 --- a/src/comm/comm.h +++ b/src/comm/comm.h @@ -63,6 +63,8 @@ const int64_t kMaxPacketTimeGap = 1700000; /**< the threshold of device disconect */ const int64_t kDeviceDisconnectThreshold = 1000000000; const uint32_t kNsPerSecond = 1000000000; /**< 1s = 1000000000ns */ +const uint32_t kNsTolerantFrameTimeDeviation = 1000000; /**< 1ms = 1000000ns */ +const uint32_t kRatioOfMsToNs = 1000000; /**< 1ms = 1000000ns */ const int kPathStrMinSize = 4; /**< Must more than 4 char */ const int kPathStrMaxSize = 256; /**< Must less than 256 char */ @@ -92,11 +94,8 @@ typedef enum { /** Timestamp sync mode define. */ typedef enum { kTimestampTypeNoSync = 0, /**< No sync signal mode. */ - kTimestampTypePtp = 1, /**< 1588v2.0 PTP sync mode. */ - kTimestampTypeRsvd = 2, /**< Reserved use. */ - kTimestampTypePpsGps = 3, /**< pps+gps sync mode. */ - kTimestampTypePps = 4, /**< pps only sync mode. */ - kTimestampTypeUnknown = 5 /**< Unknown mode. */ + kTimestampTypeGptpOrPtp = 1, /**< gPTP or PTP sync mode */ + kTimestampTypeGps = 2 /**< GPS sync mode. */ } TimestampType; /** Lidar connect state */ @@ -155,7 +154,8 @@ typedef struct { float reflectivity; /**< Reflectivity */ uint8_t tag; /**< Livox point tag */ uint8_t line; /**< Laser line id */ -} LivoxPointXyzrtl; + double timestamp; /**< Timestamp of point*/ +} LivoxPointXyzrtlt; typedef struct { float x; @@ -175,7 +175,7 @@ typedef struct { } PointPacket; typedef struct { - uint64_t base_time {}; + uint64_t base_time[kMaxSourceLidar] {}; uint8_t lidar_num {}; PointPacket lidar_point[kMaxSourceLidar] {}; } PointFrame; diff --git a/src/comm/pub_handler.cpp b/src/comm/pub_handler.cpp index caa2f970..bffc61f6 100644 --- a/src/comm/pub_handler.cpp +++ b/src/comm/pub_handler.cpp @@ -24,6 +24,7 @@ #include "pub_handler.h" +#include #include #include #include @@ -61,6 +62,8 @@ void PubHandler::RequestExit() { void PubHandler::SetPointCloudConfig(const double publish_freq) { publish_interval_ = (kNsPerSecond / (publish_freq * 10)) * 10; + publish_interval_tolerance_ = publish_interval_ - kNsTolerantFrameTimeDeviation; + publish_interval_ms_ = publish_interval_ / kRatioOfMsToNs; if (!point_process_thread_) { point_process_thread_ = std::make_shared(&PubHandler::RawDataProcess, this); } @@ -129,8 +132,8 @@ void PubHandler::OnLivoxLidarPointCloudCallback(uint32_t handle, const uint8_t d packet.data_type = data->data_type; packet.point_num = data->dot_num; packet.point_interval = data->time_interval * 100 / data->dot_num; //ns - packet.time_stamp = GetDirectEthPacketTimestamp(data->time_type, - data->timestamp, sizeof(data->timestamp)); + packet.time_stamp = GetEthPacketTimestamp(data->time_type, + data->timestamp, sizeof(data->timestamp)); uint32_t length = data->length - sizeof(LivoxLidarEthernetPacket) + 1; packet.raw_data.insert(packet.raw_data.end(), data->data, data->data + length); { @@ -143,25 +146,35 @@ void PubHandler::OnLivoxLidarPointCloudCallback(uint32_t handle, const uint8_t d } void PubHandler::PublishPointCloud() { - frame_.base_time = std::numeric_limits::max(); - frame_.lidar_num = 0; + //publish point + if (points_callback_) { + points_callback_(&frame_, pub_client_data_); + } + return; +} + +void PubHandler::CheckTimer() { + uint8_t lidar_number = lidar_process_handlers_.size(); - // Calculate Base Time for (auto &process_handler : lidar_process_handlers_) { - uint64_t base_time = process_handler.second->GetLidarBaseTime(); - if (base_time != 0 && base_time < frame_.base_time) { - frame_.base_time = base_time; + uint64_t recent_time_ms = process_handler.second->GetRecentTimeStamp() / kRatioOfMsToNs; + if ((recent_time_ms % publish_interval_ms_ != 0) || recent_time_ms == 0) { + continue; } - } - // Get Lidar Point - for (auto &process_handler : lidar_process_handlers_) { - process_handler.second->SetLidarOffsetTime(frame_.base_time); + + uint64_t diff = process_handler.second->GetRecentTimeStamp() - process_handler.second->GetLidarBaseTime(); + if (diff < publish_interval_tolerance_) { + continue; + } + uint32_t id = process_handler.first; points_[id].clear(); process_handler.second->GetLidarPointClouds(points_[id]); if (points_[id].empty()) { continue; } + + frame_.base_time[frame_.lidar_num] = process_handler.second->GetLidarBaseTime(); PointPacket& lidar_point = frame_.lidar_point[frame_.lidar_num]; lidar_point.lidar_type = LidarProtoType::kLivoxLidarType; // TODO: lidar_point.handle = id; @@ -170,27 +183,11 @@ void PubHandler::PublishPointCloud() { lidar_point.points = points_[id].data(); frame_.lidar_num++; } - //publish point - if (points_callback_) { - points_callback_(&frame_, pub_client_data_); - } - return; -} -void PubHandler::CheckTimer() { - auto now_time = std::chrono::high_resolution_clock::now(); - //First Set - if (last_pub_time_ == std::chrono::high_resolution_clock::time_point()) { - last_pub_time_ = now_time; - return; - } else if (now_time - last_pub_time_ < - std::chrono::nanoseconds(publish_interval_)) { - return; + if (frame_.lidar_num == lidar_number) { + PublishPointCloud(); + frame_.lidar_num = 0; } - last_pub_time_ += std::chrono::nanoseconds(publish_interval_); - - PublishPointCloud(); - return; } @@ -200,7 +197,7 @@ void PubHandler::RawDataProcess() { { std::unique_lock lock(packet_mutex_); if (raw_packet_queue_.empty()) { - packet_condition_.wait_for(lock, std::chrono::milliseconds(500)) ; + packet_condition_.wait_for(lock, std::chrono::milliseconds(500)); if (raw_packet_queue_.empty()) { continue; } @@ -208,7 +205,7 @@ void PubHandler::RawDataProcess() { raw_data = raw_packet_queue_.front(); raw_packet_queue_.pop_front(); } - + // 这里是来一个包判断一个雷达 uint32_t id = 0; GetLidarId(raw_data.lidar_type, raw_data.handle, id); if (lidar_process_handlers_.find(id) == lidar_process_handlers_.end()) { @@ -235,37 +232,12 @@ uint64_t PubHandler::GetEthPacketTimestamp(uint8_t timestamp_type, uint8_t* time LdsStamp time; memcpy(time.stamp_bytes, time_stamp, size); - if (timestamp_type == kTimestampTypePtp) { + if (timestamp_type == kTimestampTypeNoSync || + timestamp_type == kTimestampTypeGptpOrPtp || + timestamp_type == kTimestampTypeGps) { return time.stamp; - } else if (timestamp_type == kTimestampTypePpsGps) { - struct tm time_utc; - time_utc.tm_isdst = 0; - time_utc.tm_year = time_stamp[0] + 100; // map 2000 to 1990 - time_utc.tm_mon = time_stamp[1] - 1; // map 1~12 to 0~11 - time_utc.tm_mday = time_stamp[2]; - time_utc.tm_hour = time_stamp[3]; - time_utc.tm_min = 0; - time_utc.tm_sec = 0; - -#ifdef WIN32 - uint64_t time_epoch = _mkgmtime(&time_utc); -#else - uint64_t time_epoch = timegm(&time_utc); // no timezone -#endif - time_epoch = time_epoch * 1000000 + time.stamp_word.high; // to us - time_epoch = time_epoch * 1000; // to ns - - return time_epoch; } - return std::chrono::high_resolution_clock::now().time_since_epoch().count(); -} -inline uint64_t PubHandler::GetDirectEthPacketTimestamp(uint8_t timestamp_type, uint8_t* time_stamp, uint8_t size) { - LdsStamp time; - memcpy(time.stamp_bytes, time_stamp, size); - if (timestamp_type == kTimestampTypePtp) { - return time.stamp; - } return std::chrono::high_resolution_clock::now().time_since_epoch().count(); } @@ -280,24 +252,25 @@ uint64_t LidarPubHandler::GetLidarBaseTime() { return points_clouds_.at(0).offset_time; } -void LidarPubHandler::SetLidarOffsetTime(uint64_t base_time) { - for (auto& point_cloud : points_clouds_) { - point_cloud.offset_time -= base_time; - } -} - void LidarPubHandler::GetLidarPointClouds(std::vector& points_clouds) { std::lock_guard lock(mutex_); points_clouds.swap(points_clouds_); } +uint64_t LidarPubHandler::GetRecentTimeStamp() { + if (points_clouds_.empty()) { + return 0; + } + return points_clouds_.back().offset_time; +} + uint32_t LidarPubHandler::GetLidarPointCloudsSize() { std::lock_guard lock(mutex_); return points_clouds_.size(); } +//convert to standard format and extrinsic compensate void LidarPubHandler::PointCloudProcess(RawPacket & pkt) { - //convert to standard format and extrinsic compensate if (pkt.lidar_type == LidarProtoType::kLivoxLidarType) { LivoxLidarPointCloudProcess(pkt); } else { diff --git a/src/comm/pub_handler.h b/src/comm/pub_handler.h index 2824b76a..eb50df3a 100644 --- a/src/comm/pub_handler.h +++ b/src/comm/pub_handler.h @@ -50,10 +50,9 @@ class LidarPubHandler { void SetLidarsExtParam(LidarExtParameter param); void GetLidarPointClouds(std::vector& points_clouds); + uint64_t GetRecentTimeStamp(); uint32_t GetLidarPointCloudsSize(); - uint64_t GetLidarBaseTime(); - void SetLidarOffsetTime(uint64_t base_time); private: void LivoxLidarPointCloudProcess(RawPacket & pkt); @@ -108,7 +107,6 @@ class PubHandler { static bool GetLidarId(LidarProtoType lidar_type, uint32_t handle, uint32_t& id); static uint64_t GetEthPacketTimestamp(uint8_t timestamp_type, uint8_t* time_stamp, uint8_t size); - static uint64_t GetDirectEthPacketTimestamp(uint8_t timestamp_type, uint8_t* time_stamp, uint8_t size); PointCloudsCallback points_callback_; void* pub_client_data_ = nullptr; @@ -121,7 +119,9 @@ class PubHandler { std::deque raw_packet_queue_; //pub config - uint64_t publish_interval_ = 100000000; //100 ms + uint64_t publish_interval_ = 100000000; //100 ns + uint64_t publish_interval_tolerance_ = 100000000; //100 ns + uint64_t publish_interval_ms_ = 100; //100 ms TimePoint last_pub_time_; std::map> lidar_process_handlers_; diff --git a/src/lddc.cpp b/src/lddc.cpp index cf4c55d8..be47ac7c 100644 --- a/src/lddc.cpp +++ b/src/lddc.cpp @@ -28,6 +28,7 @@ #include #include +#include #include #include @@ -262,7 +263,7 @@ void Lddc::InitPointcloud2MsgHeader(PointCloud2& cloud) { cloud.header.frame_id.assign(frame_id_); cloud.height = 1; cloud.width = 0; - cloud.fields.resize(6); + cloud.fields.resize(7); cloud.fields[0].offset = 0; cloud.fields[0].name = "x"; cloud.fields[0].count = 1; @@ -287,13 +288,17 @@ void Lddc::InitPointcloud2MsgHeader(PointCloud2& cloud) { cloud.fields[5].name = "line"; cloud.fields[5].count = 1; cloud.fields[5].datatype = PointField::UINT8; - cloud.point_step = sizeof(LivoxPointXyzrtl); + cloud.fields[6].offset = 18; + cloud.fields[6].name = "timestamp"; + cloud.fields[6].count = 1; + cloud.fields[6].datatype = PointField::FLOAT64; + cloud.point_step = sizeof(LivoxPointXyzrtlt); } void Lddc::InitPointcloud2Msg(const StoragePacket& pkg, PointCloud2& cloud, uint64_t& timestamp) { InitPointcloud2MsgHeader(cloud); - cloud.point_step = sizeof(LivoxPointXyzrtl); + cloud.point_step = sizeof(LivoxPointXyzrtlt); cloud.width = pkg.points_num; cloud.row_step = cloud.width * cloud.point_step; @@ -311,20 +316,20 @@ void Lddc::InitPointcloud2Msg(const StoragePacket& pkg, PointCloud2& cloud, uint cloud.header.stamp = rclcpp::Time(timestamp); #endif - std::vector points; + std::vector points; for (size_t i = 0; i < pkg.points_num; ++i) { - LivoxPointXyzrtl point; + LivoxPointXyzrtlt point; point.x = pkg.points[i].x; point.y = pkg.points[i].y; point.z = pkg.points[i].z; point.reflectivity = pkg.points[i].intensity; point.tag = pkg.points[i].tag; point.line = pkg.points[i].line; - + point.timestamp = static_cast(pkg.points[i].offset_time); points.push_back(std::move(point)); } - cloud.data.resize(pkg.points_num * sizeof(LivoxPointXyzrtl)); - memcpy(cloud.data.data(), points.data(), pkg.points_num * sizeof(LivoxPointXyzrtl)); + cloud.data.resize(pkg.points_num * sizeof(LivoxPointXyzrtlt)); + memcpy(cloud.data.data(), points.data(), pkg.points_num * sizeof(LivoxPointXyzrtlt)); } void Lddc::PublishPointcloud2Data(const uint8_t index, const uint64_t timestamp, const PointCloud2& cloud) { diff --git a/src/lddc.h b/src/lddc.h index 7e75281f..6d146d13 100644 --- a/src/lddc.h +++ b/src/lddc.h @@ -120,8 +120,8 @@ class Lddc final { void InitImuMsg(const ImuData& imu_data, ImuMsg& imu_msg, uint64_t& timestamp); - void FillPointsToPclMsg(PointCloud& pcl_msg, LivoxPointXyzrtl* src_point, uint32_t num); - void FillPointsToCustomMsg(CustomMsg& livox_msg, LivoxPointXyzrtl* src_point, uint32_t num, + void FillPointsToPclMsg(PointCloud& pcl_msg, LivoxPointXyzrtlt* src_point, uint32_t num); + void FillPointsToCustomMsg(CustomMsg& livox_msg, LivoxPointXyzrtlt* src_point, uint32_t num, uint32_t offset_time, uint32_t point_interval, uint32_t echo_num); #ifdef BUILDING_ROS2 diff --git a/src/lds.cpp b/src/lds.cpp index 5b8c5267..ca984628 100644 --- a/src/lds.cpp +++ b/src/lds.cpp @@ -131,7 +131,7 @@ void Lds::StorageLvxPointData(PointFrame* frame) { for (uint i = 0; i < lidar_number; ++i) { PointPacket& lidar_point = frame->lidar_point[i]; - uint64_t base_time = frame->base_time; + uint64_t base_time = frame->base_time[i]; uint8_t index = 0; int8_t ret = cache_index_.LvxGetIndex(lidar_point.lidar_type, lidar_point.handle, index); if (ret != 0) { @@ -155,7 +155,7 @@ void Lds::StoragePointData(PointFrame* frame) { PointPacket& lidar_point = frame->lidar_point[i]; //printf("StoragePointData, lidar_type:%u, point_num:%lu.\n", lidar_point.lidar_type, lidar_point.points_num); - uint64_t base_time = frame->base_time; + uint64_t base_time = frame->base_time[i]; uint8_t index = 0; int8_t ret = cache_index_.GetIndex(lidar_point.lidar_type, lidar_point.handle, index);