update subscriber

ssct_camera_intrinsic_calib/include/ssct_camera_intrinsic_calib/calibration_node.hpp

@@ -32,12 +32,6 @@ namespace ssct_camera_intrinsic_calib
 
 class CalibrationNode
 {
-  struct SensorData
-  {
-    double time;
-    cv::Mat image;
-  };
-
 public:
   explicit CalibrationNode(const rclcpp::NodeOptions & options = rclcpp::NodeOptions());
   ~CalibrationNode();
@@ -71,7 +65,7 @@ class CalibrationNode
   std::unique_ptr<std::thread> run_thread_;
   bool exit_{false};
   // data
-  std::deque<SensorData> sensor_data_buffer_;
+  std::deque<ssct_common::ImageData> sensor_data_buffer_;
   std::shared_ptr<ssct_common::CalibParamManager> calib_param_manager_;
   uint8_t state_;
   bool success_{false};

ssct_camera_intrinsic_calib/src/calibration_node.cpp

@@ -103,14 +103,11 @@ CalibrationNode::~CalibrationNode()
 
 void CalibrationNode::read_data()
 {
-  std::deque<ssct_common::ImageSubscriber::MsgData> msg_buffer;
-  image_sub_->read(msg_buffer);
+  std::deque<ssct_common::ImageData> msg_buffer;
+  image_sub_->read(sensor_data_buffer_);
   for (auto & msg : msg_buffer) {
     if (sensor_data_buffer_.empty() || msg.time - sensor_data_buffer_.back().time > 0.5) {
-      SensorData data;
-      data.time = msg.time;
-      data.image = msg.image;
-      sensor_data_buffer_.push_back(data);
+      sensor_data_buffer_.push_back(msg);
     }
   }
 }

ssct_common/include/ssct_common/sensor_data/image_data.hpp

@@ -0,0 +1,27 @@
+// Copyright 2025 Gezp (https://github.com/gezp).
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#pragma once
+
+#include "opencv2/opencv.hpp"
+
+namespace ssct_common
+{
+struct ImageData
+{
+  double time {0};
+  cv::Mat image;
+};
+
+}  // namespace ssct_common

ssct_common/include/ssct_common/sensor_data/imu_data.hpp

@@ -20,7 +20,7 @@ namespace ssct_common
 {
 struct ImuData
 {
-  double time = 0.0;
+  double time{0};
   Eigen::Vector3d angular_velocity;
   Eigen::Vector3d linear_acceleration;
 };

ssct_common/include/ssct_common/subscriber/image_subscriber.hpp

@@ -17,38 +17,37 @@
 #include <deque>
 #include <mutex>
 #include <string>
+#include <map>
 
 #include "opencv2/opencv.hpp"
 
 #include "rclcpp/rclcpp.hpp"
 #include "sensor_msgs/msg/image.hpp"
 #include "sensor_msgs/msg/compressed_image.hpp"
 
+#include "ssct_common/sensor_data/image_data.hpp"
+
 namespace ssct_common
 {
 
 class ImageSubscriber
 {
 public:
-  struct MsgData
-  {
-    double time;
-    cv::Mat image;
-  };
   ImageSubscriber(
     rclcpp::Node::SharedPtr node, std::string topic_name, size_t buffer_size,
     bool enable_compressed = false);
 
   const char * get_topic_name();
-  void read(std::deque<MsgData> & output);
+  void read(std::deque<ImageData> & output);
+  void read(std::map<double, ImageData> & output);
   void clear();
 
 private:
   rclcpp::Node::SharedPtr node_;
   bool enable_compressed_;
   rclcpp::Subscription<sensor_msgs::msg::Image>::SharedPtr subscriber_;
   rclcpp::Subscription<sensor_msgs::msg::CompressedImage>::SharedPtr compressed_subscriber_;
-  std::deque<MsgData> buffer_;
+  std::deque<ImageData> buffer_;
   size_t buffer_size_;
   std::mutex buffer_mutex_;
 };

ssct_common/include/ssct_common/subscriber/imu_subscriber.hpp

@@ -28,12 +28,11 @@ class ImuSubscriber
 public:
   ImuSubscriber(rclcpp::Node::SharedPtr node, std::string topic_name, size_t buff_size);
   ImuSubscriber() = default;
+
+  const char * get_topic_name();
   void read(std::deque<ImuData> & output);
   void clear();
 
-private:
-  void msg_callback(const sensor_msgs::msg::Imu::SharedPtr imu_msg_ptr);
-
 private:
   rclcpp::Node::SharedPtr node_;
   rclcpp::Subscription<sensor_msgs::msg::Imu>::SharedPtr subscriber_;

ssct_common/src/subscriber/image_subscriber.cpp

@@ -19,8 +19,6 @@
 namespace ssct_common
 {
 
-using MsgData = ImageSubscriber::MsgData;
-
 ImageSubscriber::ImageSubscriber(
   rclcpp::Node::SharedPtr node, std::string topic_name, size_t buffer_size, bool enable_compressed)
 {
@@ -29,7 +27,7 @@ ImageSubscriber::ImageSubscriber(
   enable_compressed_ = enable_compressed;
   if (enable_compressed) {
     auto msg_callback = [this](const sensor_msgs::msg::CompressedImage::SharedPtr msg) {
-        MsgData data;
+        ImageData data;
         data.time = rclcpp::Time(msg->header.stamp).seconds();
         data.image = cv_bridge::toCvCopy(*msg)->image;
         buffer_mutex_.lock();
@@ -43,7 +41,7 @@ ImageSubscriber::ImageSubscriber(
       topic_name, buffer_size, msg_callback);
   } else {
     auto msg_callback = [this](const sensor_msgs::msg::Image::SharedPtr msg) {
-        MsgData data;
+        ImageData data;
         data.time = rclcpp::Time(msg->header.stamp).seconds();
         data.image = cv_bridge::toCvCopy(*msg)->image;
         buffer_mutex_.lock();
@@ -66,7 +64,8 @@ const char * ImageSubscriber::get_topic_name()
     return subscriber_->get_topic_name();
   }
 }
-void ImageSubscriber::read(std::deque<MsgData> & output)
+
+void ImageSubscriber::read(std::deque<ImageData> & output)
 {
   buffer_mutex_.lock();
   if (buffer_.size() > 0) {
@@ -76,6 +75,18 @@ void ImageSubscriber::read(std::deque<MsgData> & output)
   buffer_mutex_.unlock();
 }
 
+void ImageSubscriber::read(std::map<double, ImageData> & output)
+{
+  buffer_mutex_.lock();
+  if (buffer_.size() > 0) {
+    for (auto & msg : buffer_) {
+      output.insert({msg.time, msg});
+    }
+    buffer_.clear();
+  }
+  buffer_mutex_.unlock();
+}
+
 void ImageSubscriber::clear()
 {
   buffer_mutex_.lock();

ssct_common/src/subscriber/imu_subscriber.cpp

@@ -19,23 +19,26 @@ namespace ssct_common
 ImuSubscriber::ImuSubscriber(rclcpp::Node::SharedPtr node, std::string topic_name, size_t buff_size)
 : node_(node)
 {
-  subscriber_ = node_->create_subscription<sensor_msgs::msg::Imu>(
-    topic_name, buff_size, std::bind(&ImuSubscriber::msg_callback, this, std::placeholders::_1));
+  auto msg_callback = [this](const sensor_msgs::msg::Imu::SharedPtr msg) {
+      ImuData data;
+      data.time = rclcpp::Time(msg->header.stamp).seconds();
+      data.angular_velocity[0] = msg->angular_velocity.x;
+      data.angular_velocity[1] = msg->angular_velocity.y;
+      data.angular_velocity[2] = msg->angular_velocity.z;
+      data.linear_acceleration[0] = msg->linear_acceleration.x;
+      data.linear_acceleration[1] = msg->linear_acceleration.y;
+      data.linear_acceleration[2] = msg->linear_acceleration.z;
+      buffer_mutex_.lock();
+      buffer_.push_back(data);
+      buffer_mutex_.unlock();
+    };
+  subscriber_ =
+    node_->create_subscription<sensor_msgs::msg::Imu>(topic_name, buff_size, msg_callback);
 }
 
-void ImuSubscriber::msg_callback(const sensor_msgs::msg::Imu::SharedPtr imu_msg_ptr)
+const char * ImuSubscriber::get_topic_name()
 {
-  ImuData data;
-  data.time = rclcpp::Time(imu_msg_ptr->header.stamp).seconds();
-  data.angular_velocity[0] = imu_msg_ptr->angular_velocity.x;
-  data.angular_velocity[1] = imu_msg_ptr->angular_velocity.y;
-  data.angular_velocity[2] = imu_msg_ptr->angular_velocity.z;
-  data.linear_acceleration[0] = imu_msg_ptr->linear_acceleration.x;
-  data.linear_acceleration[1] = imu_msg_ptr->linear_acceleration.y;
-  data.linear_acceleration[2] = imu_msg_ptr->linear_acceleration.z;
-  buffer_mutex_.lock();
-  buffer_.push_back(data);
-  buffer_mutex_.unlock();
+  return subscriber_->get_topic_name();
 }
 
 void ImuSubscriber::read(std::deque<ImuData> & output)

ssct_lidar_cam_manual_calib/include/ssct_lidar_cam_manual_calib/calibration_node.hpp

@@ -37,7 +37,7 @@ class CalibrationNode
 {
   struct SensorData
   {
-    double time{-1};
+    double time;
     cv::Mat image;
     pcl::PointCloud<pcl::PointXYZI>::Ptr pointcloud;
   };
@@ -80,11 +80,9 @@ class CalibrationNode
   std::unique_ptr<std::thread> run_thread_;
   bool exit_{false};
   // sensor data
-  std::deque<ssct_common::CloudSubscriber<pcl::PointXYZI>::MsgData> pointcloud_msgs_;
-  std::map<double, cv::Mat> image_buffer_;
+  std::deque<ssct_common::CloudSubscriber<pcl::PointXYZI>::MsgData> pointcloud_buffer_;
+  std::map<double, ssct_common::ImageData> image_buffer_;
   std::deque<SensorData> sensor_data_buffer_;
-  double last_sensor_time_;
-  SensorData current_sensor_data_;
   // camera intrinsic data
   std::vector<double> camera_intrinsic_;
   // extrinsic data

ssct_lidar_cam_manual_calib/src/calibration_node.cpp

@@ -131,71 +131,65 @@ bool CalibrationNode::get_nearest_image(double time, double & image_timestamp, c
   if (cur == image_buffer_.end()) {
     // the last
     image_timestamp = image_buffer_.rbegin()->first;
-    image = image_buffer_.rbegin()->second;
+    image = image_buffer_.rbegin()->second.image;
   } else if (cur == image_buffer_.begin()) {
     // the first
     image_timestamp = cur->first;
-    image = cur->second;
+    image = cur->second.image;
   } else {
     // choose between prev and cur
     auto prev = std::prev(cur);
     if (fabs(prev->first - time) < fabs(cur->first - time)) {
       image_timestamp = prev->first;
-      image = prev->second;
+      image = prev->second.image;
     } else {
       image_timestamp = cur->first;
-      image = cur->second;
+      image = cur->second.image;
     }
   }
   return true;
 }
 
 bool CalibrationNode::read_data()
 {
-  pointcloud_sub_->read(pointcloud_msgs_);
-  std::deque<ssct_common::ImageSubscriber::MsgData> image_msgs;
-  image_sub_->read(image_msgs);
-  for (auto & msg : image_msgs) {
-    image_buffer_.insert({msg.time, msg.image});
-    if (image_buffer_.size() > max_image_buffer_size_) {
-      image_buffer_.erase(image_buffer_.begin());
-    }
+  pointcloud_sub_->read(pointcloud_buffer_);
+  image_sub_->read(image_buffer_);
+  while (image_buffer_.size() > max_image_buffer_size_) {
+    image_buffer_.erase(image_buffer_.begin());
   }
-  if (pointcloud_msgs_.empty() || image_buffer_.empty()) {
+  if (pointcloud_buffer_.empty() || image_buffer_.empty()) {
     return false;
   }
-  if (pointcloud_msgs_.front().time > image_buffer_.rbegin()->first) {
+  if (pointcloud_buffer_.front().time > image_buffer_.rbegin()->first) {
     return false;
   }
   double image_timestamp;
   cv::Mat image;
-  while (!pointcloud_msgs_.empty()) {
-    auto & pointcloud_msg = pointcloud_msgs_.front();
-    if (pointcloud_msg.time - last_sensor_time_ < 0) {
-      pointcloud_msgs_.pop_front();
-      continue;
-    }
-    // get nearest image
-    if (get_nearest_image(pointcloud_msgs_.front().time, image_timestamp, image)) {
-      if (fabs(pointcloud_msgs_.front().time - image_timestamp) < 0.05) {
-        current_sensor_data_.time = pointcloud_msgs_.front().time;
-        current_sensor_data_.pointcloud = pointcloud_msgs_.front().pointcloud;
-        current_sensor_data_.image = image;
-        last_sensor_time_ = current_sensor_data_.time;
-        pointcloud_msgs_.pop_front();
-        return true;
+  while (!pointcloud_buffer_.empty()) {
+    auto & pointcloud_msg = pointcloud_buffer_.front();
+    if (sensor_data_buffer_.empty() || pointcloud_msg.time > sensor_data_buffer_.back().time) {
+      // get nearest image
+      if (get_nearest_image(pointcloud_buffer_.front().time, image_timestamp, image)) {
+        if (fabs(pointcloud_buffer_.front().time - image_timestamp) < 0.05) {
+          SensorData data;
+          data.time = pointcloud_buffer_.front().time;
+          data.pointcloud = pointcloud_buffer_.front().pointcloud;
+          data.image = image;
+          sensor_data_buffer_.push_back(data);
+        }
       }
     }
+    pointcloud_buffer_.pop_front();
   }
-  return false;
+  return sensor_data_buffer_.size() > 0;
 }
 
 void CalibrationNode::clear_data()
 {
   pointcloud_sub_->clear();
   image_sub_->clear();
   image_buffer_.clear();
-  current_sensor_data_.time = -1;
+  sensor_data_buffer_.clear();
 }
 
 bool CalibrationNode::update_calib_param_manager(const std::string & key, float value)
@@ -294,26 +288,25 @@ bool CalibrationNode::run()
   }
   // calibration flow
   if (state_ == ssct_interfaces::msg::CalibrationStatus::CALIBRATING) {
-    if (current_sensor_data_.time < 0) {
-      if (!read_data()) {
-        return false;
+    if (sensor_data_buffer_.size() == 0) {
+      read_data();
+      if (sensor_data_buffer_.size() > 0) {
+        update_status_msg(state_, "finished to collect data.", 50);
       }
-      update_status_msg(state_, "finished to collect data.", 50);
     } else {
       // project pointcloud
-      cv::Mat img = current_sensor_data_.image.clone();
-      auto ok = lidar_projector_->project(
-        *current_sensor_data_.pointcloud, camera_intrinsic_, T_camera_lidar_, img);
-      debug_image_pub_->publish(img, current_sensor_data_.time);
+      auto & data = sensor_data_buffer_.back();
+      cv::Mat img = data.image.clone();
+      auto ok =
+        lidar_projector_->project(*data.pointcloud, camera_intrinsic_, T_camera_lidar_, img);
+      debug_image_pub_->publish(img, data.time);
       if (ok) {
         state_ = ssct_interfaces::msg::CalibrationStatus::SUCCESSED;
       } else {
         state_ = ssct_interfaces::msg::CalibrationStatus::FAILED;
       }
       update_status_msg(state_, "finished to project pointcloud.", 100);
     }
-  } else {
-    return false;
   }
   return true;
 }