diff --git a/Lidar.MD b/Lidar.MD index a7bb1796..3422de54 100644 --- a/Lidar.MD +++ b/Lidar.MD @@ -55,11 +55,8 @@ rviz2 ``` 1. Open Rviz2. - 2. Add a `PointCloud2` or `LaserScan` display. - 3. For `PointCloud2`, select the `/lidar/cloud/points` topic; for `LaserScan`, select the `/lidar/scan/points` topic. - 4. Set the `Fixed Frame` to `lidar_lidar_frame` to properly align the data. * `PointCloud2` visualization example: @@ -119,6 +116,7 @@ The `lidar.launch.py` file contains default parameters for the driver. You can c - **tf_publish_rate**: TF publishing frequency. - **lidar_format**: Data format for the LiDAR. Optional values: `LIDAR_POINT`, `LIDAR_SPHERE_POINT`, `LIDAR_SCAN` - **lidar_rate**: Scan rate of the LiDAR. +- **publish_n_pkts**: Number of frames to accumulate before publishing merged point cloud. Range: 1-12000. Only effective when lidar_format is `LIDAR_POINT` or `LIDAR_SPHERE_POINT`, used to merge specified number of frames before publishing. Default value: `1` - **enable_scan_to_point**: Enable conversion of scan data to point cloud data, publishing PointCloud2 data type topics. - **repetitive_scan_mode**: Repetitive scan mode parameter. - **filter_level**: Add filter level parameter. @@ -143,14 +141,15 @@ The `lidar.launch.py` file contains default parameters for the driver. You can c ## Point Cloud Data Detailed Description -Livox PointCloud2 (PointXYZRT) point cloud format is as follows: +PointCloud2 (PointXYZITO) point cloud format is as follows: ``` float32 x # X axis, unit:m float32 y # Y axis, unit:m float32 z # Z axis, unit:m -uint8 intensity # lidar intensity +uint8 intensity # lidar intensity uint8 tag # lidar tag +uint32 offset_time # Point cloud offset time relative to topic time, in nanoseconds ``` ## 3. IMU Data @@ -178,6 +177,7 @@ ros2 topic echo /lidar/imu/sample ``` The IMU data includes: + - **linear_acceleration**: 3D acceleration data (x, y, z) in m/s² - **angular_velocity**: 3D angular velocity data (x, y, z) in rad/s - **orientation**: Quaternion orientation (not provided by hardware, set to zero) diff --git a/Lidar_CN.MD b/Lidar_CN.MD index 172c13b2..df76ec73 100644 --- a/Lidar_CN.MD +++ b/Lidar_CN.MD @@ -116,6 +116,7 @@ ros2 run orbbec_camera list_camera_profile_mode_node - **tf_publish_rate**: TF发布频率。 - **lidar_format**: 激光雷达的数据格式。可选值：`LIDAR_POINT`、`LIDAR_SPHERE_POINT`、`LIDAR_SCAN` - **lidar_rate**: 激光雷达的扫描频率。 +- **publish_n_pkts**: 多帧数据合并发布的帧数量。范围：1-12000。仅在激光雷达格式为 `LIDAR_POINT` 或 `LIDAR_SPHERE_POINT` 时生效，用于累积指定数量的帧后再发布合并的点云数据。默认值：`1` - **enable_scan_to_point**: 启用扫描数据到点云数据的转换，发布PointCloud2数据类型话题。 - **repetitive_scan_mode**: 重复扫描模式参数。 - **filter_level**: 添加过滤级别参数。 @@ -140,14 +141,15 @@ ros2 run orbbec_camera list_camera_profile_mode_node ## 点云数据详细说明 -Livox PointCloud2 (PointXYZRT) 点云格式如下： +PointCloud2 (PointXYZITO) 点云格式如下： ``` float32 x # X轴，单位：米 float32 y # Y轴，单位：米 float32 z # Z轴，单位：米 -uint8 intensity # 激光雷达强度 +uint8 intensity # 激光雷达强度 uint8 tag # 激光雷达标签 +uint32 offset_time # 点云相对话题时间的偏移量，单位纳秒 ``` ## 3. IMU数据 diff --git a/orbbec_camera/include/orbbec_camera/ob_lidar_node.h b/orbbec_camera/include/orbbec_camera/ob_lidar_node.h index e9b25e72..fbecd13a 100644 --- a/orbbec_camera/include/orbbec_camera/ob_lidar_node.h +++ b/orbbec_camera/include/orbbec_camera/ob_lidar_node.h @@ -184,6 +184,10 @@ class OBLidarNode { void publishSpherePointCloud(std::shared_ptr frame_set); + void publishMergedPointCloud(); + + void publishMergedSpherePointCloud(); + uint64_t getFrameTimestampUs(const std::shared_ptr& frame); void filterScan(sensor_msgs::msg::LaserScan& scan); @@ -268,6 +272,12 @@ class OBLidarNode { bool enable_cloud_accumulated_ = false; int cloud_accumulation_count_ = -1; + // Multi-frame publishing parameters + int publish_n_pkts_ = 1; + std::mutex frame_buffer_mutex_; + std::vector> frame_buffer_; + rclcpp::Time last_frame_timestamp_; + // IMU bool enable_imu_ = false; std::string imu_rate_ = "50hz"; diff --git a/orbbec_camera/launch/lidar.launch.py b/orbbec_camera/launch/lidar.launch.py index d8ac3786..26ea9ba7 100644 --- a/orbbec_camera/launch/lidar.launch.py +++ b/orbbec_camera/launch/lidar.launch.py @@ -96,6 +96,11 @@ def generate_launch_description(): default_value='20', description='LiDAR scan/publish rate in Hz. Supported range depends on device model.' ), + DeclareLaunchArgument( + 'publish_n_pkts', + default_value='1', + description='Number of frames to accumulate before publishing. Range: 1-12000. Used for multi-frame data merging.' + ), DeclareLaunchArgument( 'enable_scan_to_point', default_value='false', diff --git a/orbbec_camera/src/ob_lidar_node.cpp b/orbbec_camera/src/ob_lidar_node.cpp index d30c2ea5..8e849c99 100644 --- a/orbbec_camera/src/ob_lidar_node.cpp +++ b/orbbec_camera/src/ob_lidar_node.cpp @@ -166,6 +166,28 @@ void OBLidarNode::getParameters() { setAndGetNodeParameter(liner_accel_cov_, "linear_accel_cov", 0.0003); setAndGetNodeParameter(angular_vel_cov_, "angular_vel_cov", 0.02); + // Multi-frame publishing parameter - only for LIDAR_POINT and LIDAR_SPHERE_POINT formats + bool use_multi_frame = false; + for (auto stream_index : LIDAR_STREAMS) { + if (format_[stream_index] == OB_FORMAT_LIDAR_POINT || format_[stream_index] == OB_FORMAT_LIDAR_SPHERE_POINT) { + use_multi_frame = true; + break; + } + } + + if (use_multi_frame) { + setAndGetNodeParameter(publish_n_pkts_, "publish_n_pkts", 1); + if (publish_n_pkts_ < 1 || publish_n_pkts_ > 12000) { + RCLCPP_WARN_STREAM(logger_, "publish_n_pkts value " << publish_n_pkts_ + << " is out of range [1, 12000], setting to 1"); + publish_n_pkts_ = 1; + } + RCLCPP_INFO_STREAM(logger_, "Multi-frame publishing enabled: " << publish_n_pkts_ << " frames will be merged"); + } else { + publish_n_pkts_ = 1; + RCLCPP_INFO_STREAM(logger_, "Multi-frame publishing disabled for current lidar format"); + } + // Setup IMU streams if enabled if (enable_imu_) { enable_stream_[ACCEL] = true; @@ -619,14 +641,29 @@ void OBLidarNode::onNewFrameSetCallback(std::shared_ptr frame_set) publishStaticTransforms(); tf_published_ = true; } - if (format_[LIDAR] == OB_FORMAT_LIDAR_SCAN && !enable_scan_to_point_) { - publishScan(frame_set); - } else if (format_[LIDAR] == OB_FORMAT_LIDAR_SCAN && enable_scan_to_point_) { - publishScanToPoint(frame_set); - } else if (format_[LIDAR] == OB_FORMAT_LIDAR_POINT) { - publishPointCloud(frame_set); - } else if (format_[LIDAR] == OB_FORMAT_LIDAR_SPHERE_POINT) { - publishSpherePointCloud(frame_set); + + // Handle multi-frame publishing for LIDAR_POINT and LIDAR_SPHERE_POINT formats + if ((format_[LIDAR] == OB_FORMAT_LIDAR_POINT || format_[LIDAR] == OB_FORMAT_LIDAR_SPHERE_POINT)) { + std::lock_guard lock(frame_buffer_mutex_); + frame_buffer_.push_back(frame_set); + + // If we have enough frames, publish merged point cloud + if (frame_buffer_.size() >= static_cast(publish_n_pkts_)) { + if (format_[LIDAR] == OB_FORMAT_LIDAR_POINT) { + publishMergedPointCloud(); + } else if (format_[LIDAR] == OB_FORMAT_LIDAR_SPHERE_POINT) { + publishMergedSpherePointCloud(); + } + frame_buffer_.clear(); + } + } + else { + // Original single frame publishing logic + if (format_[LIDAR] == OB_FORMAT_LIDAR_SCAN && !enable_scan_to_point_) { + publishScan(frame_set); + } else if (format_[LIDAR] == OB_FORMAT_LIDAR_SCAN && enable_scan_to_point_) { + publishScanToPoint(frame_set); + } } } catch (const ob::Error &e) { RCLCPP_ERROR_STREAM(logger_, "onNewFrameSetCallback error: " << e.getMessage()); @@ -805,6 +842,181 @@ void OBLidarNode::publishSpherePointCloud(std::shared_ptr frame_se point_cloud_pub_->publish(std::move(point_cloud_msg)); } +void OBLidarNode::publishMergedPointCloud() { + if (frame_buffer_.empty()) { + return; + } + + // Calculate total point count across all frames + size_t total_point_count = 0; + std::vector> frame_data; + std::vector frame_timestamps; + + for (const auto& fs : frame_buffer_) { + auto lidar_frame = fs->getFrame(OB_FRAME_LIDAR_POINTS); + if (lidar_frame) { + auto* point_data = reinterpret_cast(lidar_frame->getData()); + auto point_count = lidar_frame->getDataSize() / sizeof(OBLiDARPoint); + frame_data.emplace_back(point_data, point_count); + frame_timestamps.push_back(getFrameTimestampUs(lidar_frame)); + total_point_count += point_count; + } + } + + if (total_point_count == 0) { + return; + } + + // Create merged point cloud message with offset_time field + auto point_cloud_msg = std::make_unique(); + sensor_msgs::PointCloud2Modifier modifier(*point_cloud_msg); + modifier.setPointCloud2Fields(6, + "x", 1, sensor_msgs::msg::PointField::FLOAT32, + "y", 1, sensor_msgs::msg::PointField::FLOAT32, + "z", 1, sensor_msgs::msg::PointField::FLOAT32, + "intensity", 1, sensor_msgs::msg::PointField::UINT8, + "tag", 1, sensor_msgs::msg::PointField::UINT8, + "offset_time", 1, sensor_msgs::msg::PointField::UINT32); + modifier.resize(total_point_count); + + // Use the timestamp of the latest frame as the header timestamp + auto timestamp = fromUsToROSTime(frame_timestamps.front()); + point_cloud_msg->header.stamp = timestamp; + point_cloud_msg->header.frame_id = frame_id_[LIDAR]; + point_cloud_msg->height = 1; + point_cloud_msg->width = total_point_count; + point_cloud_msg->is_dense = true; + point_cloud_msg->is_bigendian = false; + point_cloud_msg->row_step = point_cloud_msg->width * point_cloud_msg->point_step; + point_cloud_msg->data.resize(point_cloud_msg->height * point_cloud_msg->row_step); + + // Create iterators + sensor_msgs::PointCloud2Iterator iter_x(*point_cloud_msg, "x"); + sensor_msgs::PointCloud2Iterator iter_y(*point_cloud_msg, "y"); + sensor_msgs::PointCloud2Iterator iter_z(*point_cloud_msg, "z"); + sensor_msgs::PointCloud2Iterator iter_intensity(*point_cloud_msg, "intensity"); + sensor_msgs::PointCloud2Iterator iter_tag(*point_cloud_msg, "tag"); + sensor_msgs::PointCloud2Iterator iter_offset_time(*point_cloud_msg, "offset_time"); + + // Calculate frame time interval based on lidar rate + double frame_interval_us = 1000000.0 / static_cast(rate_int_[LIDAR]); + + // Merge all frames with per-point timestamps + for (size_t frame_idx = 0; frame_idx < frame_data.size(); ++frame_idx) { + auto [point_data, point_count] = frame_data[frame_idx]; + uint64_t frame_timestamp_us = frame_timestamps[frame_idx]; + + // Calculate time increment per point within this frame (uniform sampling) + double point_time_increment_us = frame_interval_us / static_cast(point_count); + + for (size_t i = 0; i < point_count; + ++i, ++iter_x, ++iter_y, ++iter_z, ++iter_intensity, ++iter_tag, ++iter_offset_time) { + *iter_x = static_cast(point_data[i].x / 1000.0); + *iter_y = static_cast(point_data[i].y / -1000.0); + *iter_z = static_cast(point_data[i].z / 1000.0); + *iter_intensity = point_data[i].intensity; + *iter_tag = point_data[i].tag; + + // Calculate per-point offset time in nanoseconds relative to point cloud header timestamp + double point_timestamp_us = static_cast(frame_timestamp_us) + (i * point_time_increment_us); + uint64_t header_timestamp_us = frame_timestamps[0]; // First frame timestamp + *iter_offset_time = static_cast((point_timestamp_us - static_cast(header_timestamp_us)) * 1000.0); // Convert to nanoseconds + } + } + + *point_cloud_msg = filterPointCloud(*point_cloud_msg); + point_cloud_pub_->publish(std::move(point_cloud_msg)); +} + +void OBLidarNode::publishMergedSpherePointCloud() { + if (frame_buffer_.empty()) { + return; + } + + // Calculate total point count across all frames + size_t total_point_count = 0; + std::vector> frame_data; + std::vector frame_timestamps; + + for (const auto& fs : frame_buffer_) { + auto lidar_frame = fs->getFrame(OB_FRAME_LIDAR_POINTS); + if (lidar_frame) { + auto* point_data = reinterpret_cast(lidar_frame->getData()); + auto point_count = lidar_frame->getDataSize() / sizeof(OBLiDARSpherePoint); + frame_data.emplace_back(point_data, point_count); + frame_timestamps.push_back(getFrameTimestampUs(lidar_frame)); + total_point_count += point_count; + } + } + + if (total_point_count == 0) { + return; + } + + // Create merged point cloud message with timestamp field + auto point_cloud_msg = std::make_unique(); + sensor_msgs::PointCloud2Modifier modifier(*point_cloud_msg); + modifier.setPointCloud2Fields(6, + "x", 1, sensor_msgs::msg::PointField::FLOAT32, + "y", 1, sensor_msgs::msg::PointField::FLOAT32, + "z", 1, sensor_msgs::msg::PointField::FLOAT32, + "intensity", 1, sensor_msgs::msg::PointField::UINT8, + "tag", 1, sensor_msgs::msg::PointField::UINT8, + "offset_time", 1, sensor_msgs::msg::PointField::UINT32); + modifier.resize(total_point_count); + + // Use the timestamp of the latest frame as the header timestamp + auto timestamp = fromUsToROSTime(frame_timestamps.front()); + point_cloud_msg->header.stamp = timestamp; + point_cloud_msg->header.frame_id = frame_id_[LIDAR]; + point_cloud_msg->height = 1; + point_cloud_msg->width = total_point_count; + point_cloud_msg->is_dense = true; + point_cloud_msg->is_bigendian = false; + point_cloud_msg->row_step = point_cloud_msg->width * point_cloud_msg->point_step; + point_cloud_msg->data.resize(point_cloud_msg->height * point_cloud_msg->row_step); + + // Create iterators + sensor_msgs::PointCloud2Iterator iter_x(*point_cloud_msg, "x"); + sensor_msgs::PointCloud2Iterator iter_y(*point_cloud_msg, "y"); + sensor_msgs::PointCloud2Iterator iter_z(*point_cloud_msg, "z"); + sensor_msgs::PointCloud2Iterator iter_intensity(*point_cloud_msg, "intensity"); + sensor_msgs::PointCloud2Iterator iter_tag(*point_cloud_msg, "tag"); + sensor_msgs::PointCloud2Iterator iter_offset_time(*point_cloud_msg, "offset_time"); + + // Calculate frame time interval based on lidar rate + double frame_interval_us = 1000000.0 / static_cast(rate_int_[LIDAR]); + + // Merge all frames with per-point timestamps + for (size_t frame_idx = 0; frame_idx < frame_data.size(); ++frame_idx) { + auto [sphere_point_data, point_count] = frame_data[frame_idx]; + uint64_t frame_timestamp_us = frame_timestamps[frame_idx]; + + // Convert sphere points to cartesian points + auto result_point = spherePointToPoint(sphere_point_data, point_count); + + // Calculate time increment per point within this frame (uniform sampling) + double point_time_increment_us = frame_interval_us / static_cast(point_count); + + for (size_t i = 0; i < point_count; + ++i, ++iter_x, ++iter_y, ++iter_z, ++iter_intensity, ++iter_tag, ++iter_offset_time) { + *iter_x = static_cast(result_point[i].x / 1000.0); + *iter_y = static_cast(result_point[i].y / -1000.0); + *iter_z = static_cast(result_point[i].z / 1000.0); + *iter_intensity = result_point[i].intensity; + *iter_tag = result_point[i].tag; + + // Calculate per-point offset time in nanoseconds relative to point cloud header timestamp + double point_timestamp_us = static_cast(frame_timestamp_us) + (i * point_time_increment_us); + uint64_t header_timestamp_us = frame_timestamps[0]; // First frame timestamp + *iter_offset_time = static_cast((point_timestamp_us - static_cast(header_timestamp_us)) * 1000.0); // Convert to nanoseconds + } + } + + *point_cloud_msg = filterPointCloud(*point_cloud_msg); + point_cloud_pub_->publish(std::move(point_cloud_msg)); +} + uint64_t OBLidarNode::getFrameTimestampUs(const std::shared_ptr &frame) { if (frame == nullptr) { RCLCPP_WARN(logger_, "getFrameTimestampUs: frame is nullptr, return 0");