Add publish_n_pkts param

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)

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数据

orbbec_camera/include/orbbec_camera/ob_lidar_node.h

@@ -184,6 +184,10 @@ class OBLidarNode {
 
   void publishSpherePointCloud(std::shared_ptr<ob::FrameSet> frame_set);
 
+  void publishMergedPointCloud();
+
+  void publishMergedSpherePointCloud();
+
   uint64_t getFrameTimestampUs(const std::shared_ptr<ob::Frame>& 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<std::shared_ptr<ob::FrameSet>> frame_buffer_;
+  rclcpp::Time last_frame_timestamp_;
+
   // IMU
   bool enable_imu_ = false;
   std::string imu_rate_ = "50hz";

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',

orbbec_camera/src/ob_lidar_node.cpp

@@ -166,6 +166,28 @@ void OBLidarNode::getParameters() {
   setAndGetNodeParameter<double>(liner_accel_cov_, "linear_accel_cov", 0.0003);
   setAndGetNodeParameter<double>(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<int>(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<ob::FrameSet> frame_set)
       publishStaticTransforms();
       tf_published_ = true;
     }
-    if (format_[LIDAR] == OB_FORMAT_LIDAR_SCAN && !enable_scan_to_point_) {
+
-      publishScan(frame_set);
+    // Handle multi-frame publishing for LIDAR_POINT and LIDAR_SPHERE_POINT formats
-    } else if (format_[LIDAR] == OB_FORMAT_LIDAR_SCAN && enable_scan_to_point_) {
+    if ((format_[LIDAR] == OB_FORMAT_LIDAR_POINT || format_[LIDAR] == OB_FORMAT_LIDAR_SPHERE_POINT)) {
-      publishScanToPoint(frame_set);
+      std::lock_guard<std::mutex> lock(frame_buffer_mutex_);
-    } else if (format_[LIDAR] == OB_FORMAT_LIDAR_POINT) {
+      frame_buffer_.push_back(frame_set);
-      publishPointCloud(frame_set);
+
-    } else if (format_[LIDAR] == OB_FORMAT_LIDAR_SPHERE_POINT) {
+      // If we have enough frames, publish merged point cloud
-      publishSpherePointCloud(frame_set);
+      if (frame_buffer_.size() >= static_cast<size_t>(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<ob::FrameSet> 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<std::pair<OBLiDARPoint*, size_t>> frame_data;
+  std::vector<uint64_t> 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<OBLiDARPoint*>(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::msg::PointCloud2>();
+  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<float> iter_x(*point_cloud_msg, "x");
+  sensor_msgs::PointCloud2Iterator<float> iter_y(*point_cloud_msg, "y");
+  sensor_msgs::PointCloud2Iterator<float> iter_z(*point_cloud_msg, "z");
+  sensor_msgs::PointCloud2Iterator<uint8_t> iter_intensity(*point_cloud_msg, "intensity");
+  sensor_msgs::PointCloud2Iterator<uint8_t> iter_tag(*point_cloud_msg, "tag");
+  sensor_msgs::PointCloud2Iterator<uint32_t> iter_offset_time(*point_cloud_msg, "offset_time");
+
+  // Calculate frame time interval based on lidar rate
+  double frame_interval_us = 1000000.0 / static_cast<double>(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<double>(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<float>(point_data[i].x / 1000.0);
+      *iter_y = static_cast<float>(point_data[i].y / -1000.0);
+      *iter_z = static_cast<float>(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<double>(frame_timestamp_us) + (i * point_time_increment_us);
+      uint64_t header_timestamp_us = frame_timestamps[0]; // First frame timestamp
+      *iter_offset_time = static_cast<uint32_t>((point_timestamp_us - static_cast<double>(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<std::pair<OBLiDARSpherePoint*, size_t>> frame_data;
+  std::vector<uint64_t> 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<OBLiDARSpherePoint*>(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::msg::PointCloud2>();
+  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<float> iter_x(*point_cloud_msg, "x");
+  sensor_msgs::PointCloud2Iterator<float> iter_y(*point_cloud_msg, "y");
+  sensor_msgs::PointCloud2Iterator<float> iter_z(*point_cloud_msg, "z");
+  sensor_msgs::PointCloud2Iterator<uint8_t> iter_intensity(*point_cloud_msg, "intensity");
+  sensor_msgs::PointCloud2Iterator<uint8_t> iter_tag(*point_cloud_msg, "tag");
+  sensor_msgs::PointCloud2Iterator<uint32_t> iter_offset_time(*point_cloud_msg, "offset_time");
+
+  // Calculate frame time interval based on lidar rate
+  double frame_interval_us = 1000000.0 / static_cast<double>(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<double>(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<float>(result_point[i].x / 1000.0);
+      *iter_y = static_cast<float>(result_point[i].y / -1000.0);
+      *iter_z = static_cast<float>(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<double>(frame_timestamp_us) + (i * point_time_increment_us);
+      uint64_t header_timestamp_us = frame_timestamps[0]; // First frame timestamp
+      *iter_offset_time = static_cast<uint32_t>((point_timestamp_us - static_cast<double>(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<ob::Frame> &frame) {
   if (frame == nullptr) {
     RCLCPP_WARN(logger_, "getFrameTimestampUs: frame is nullptr, return 0");