Sync with system time

pylon_ros2_camera_component/include/pylon_ros2_camera_node.hpp

@@ -1815,7 +1815,10 @@ class PylonROS2CameraNode : public rclcpp::Node
   rclcpp_action::Server<GrabBlazeDataAction>::SharedPtr grab_blaze_data_as_;
 
   // spinning thread
-  rclcpp::TimerBase::SharedPtr timer_;
+  void spinLoop();
+  std::thread spin_thread_;
+  std::atomic<bool> keep_spinning_;
+
   // mutex
   std::recursive_mutex grab_mutex_;
 

pylon_ros2_camera_component/src/pylon_ros2_camera_node.cpp

@@ -74,10 +74,8 @@ PylonROS2CameraNode::PylonROS2CameraNode(const rclcpp::NodeOptions& options)
     return;
 
   // starting spinning thread
-  RCLCPP_INFO_STREAM(LOGGER, "Start image grabbing if node connects to topic with a spinning rate of: " << this->frameRate() << " Hz");
-  timer_ = this->create_wall_timer(
-            std::chrono::duration<double>(1. / this->frameRate()),
-            std::bind(&PylonROS2CameraNode::spin, this));
+  RCLCPP_INFO_STREAM(LOGGER, "Start image grabbing if node connects to topic with a frame rate of: " << this->frameRate() << " Hz");
+  this->spin_thread_ = std::thread(&PylonROS2CameraNode::spinLoop, this);
 }
 
 PylonROS2CameraNode::~PylonROS2CameraNode()
@@ -99,6 +97,30 @@ PylonROS2CameraNode::~PylonROS2CameraNode()
     delete this->pinhole_model_;
     this->pinhole_model_ = nullptr;
   }
+
+  this->keep_spinning_ = false;
+  if (this->spin_thread_.joinable()) {
+    this->spin_thread_.join();
+  }
+}
+
+void PylonROS2CameraNode::spinLoop()
+{
+  double frame_step = 1.0 / this->frameRate();
+  while (this->keep_spinning_ && rclcpp::ok())
+  {
+    // Wait for the next frame step at a fixed system time, 
+    // to allow a simple synchronization with other cameras
+    double now_time = rclcpp::Clock().now().seconds();
+    double tdiff = std::fmod(now_time, frame_step);
+    if (tdiff > 0){
+      double sleep_time = frame_step - tdiff;
+      std::this_thread::sleep_for(std::chrono::duration<double>(sleep_time));
+    }
+
+    // Grab an image
+    this->spin();
+  }
 }
 
 const double& PylonROS2CameraNode::frameRate() const