merge main

Author: brianzheng206

deep_tools/camera_sync/CMakeLists.txt

@@ -11,14 +11,13 @@
 # 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.
-cmake_minimum_required(VERSION 3.8)
+cmake_minimum_required(VERSION 3.22)
 project(camera_sync)
 
 if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang")
   add_compile_options(-Wall -Wextra -Wpedantic)
 endif()
 
-# find dependencies
 find_package(ament_cmake REQUIRED)
 find_package(rclcpp REQUIRED)
 find_package(rclcpp_lifecycle REQUIRED)
@@ -29,7 +28,6 @@ find_package(message_filters REQUIRED)
 find_package(image_transport REQUIRED)
 find_package(deep_msgs REQUIRED)
 
-# Create include directory
 include_directories(include)
 
 # Define lifecycle node support based on ROS distro
@@ -40,7 +38,6 @@ else()
   add_definitions(-DUSE_LIFECYCLE_NODE=0)
 endif()
 
-# Add the multi-camera sync library for component
 add_library(multi_camera_sync_component SHARED src/multi_camera_sync_node.cpp)
 target_link_libraries(multi_camera_sync_component
   rclcpp::rclcpp
@@ -57,34 +54,25 @@ target_link_libraries(multi_camera_sync_component
 rclcpp_components_register_nodes(multi_camera_sync_component "camera_sync::MultiCameraSyncNode")
 set(node_plugins "${node_plugins}camera_sync::MultiCameraSyncNode;$<TARGET_FILE:multi_camera_sync_component>\n")
 
-
-
-# Install executables and libraries
 install(TARGETS
   multi_camera_sync_component
   ARCHIVE DESTINATION lib
   LIBRARY DESTINATION lib
   RUNTIME DESTINATION bin
 )
 
-
-
-# Install include directory
 install(DIRECTORY include/
   DESTINATION include
 )
 
-# Install launch files if any
 install(DIRECTORY launch/
   DESTINATION share/${PROJECT_NAME}/launch
   FILES_MATCHING PATTERN "*.py" PATTERN "*.yaml" PATTERN "*.md"
 )
 
-# Install config files
 install(DIRECTORY config/
   DESTINATION share/${PROJECT_NAME}/config
   FILES_MATCHING PATTERN "*.yaml"
 )
 
-
 ament_package()

deep_tools/camera_sync/config/multi_camera_sync_params.yaml

@@ -24,21 +24,38 @@ multi_camera_sync:
     # Camera topics to synchronize
     # For raw images, use topics like: ["/camera1/image_raw", "/camera2/image_raw"]
     # For compressed images, use topics like: ["/camera1/image_raw/compressed", "/camera2/image_raw/compressed"]
-    camera_topics: [
-      "/CAM_FRONT/image_rect_compressed",
-      "/CAM_FRONT_LEFT/image_rect_compressed",
-      "/CAM_FRONT_RIGHT/image_rect_compressed",
-      "/CAM_BACK/image_rect_compressed",
-      "/CAM_BACK_LEFT/image_rect_compressed",
-      "/CAM_BACK_RIGHT/image_rect_compressed"
-    ]
+    camera_topics:
+      - "/camera_lower_sw/image_rect"
+      - "/camera_lower_ne/image_rect"
+      - "/camera_lower_se/image_rect"
+      - "/camera_lower_nw/image_rect"
+      - "/camera_pano_ww/image_rect"
+      - "/camera_pano_nn/image_rect"
+      - "/camera_pano_ee/image_rect"
+      - "/camera_pano_se/image_rect"
+      - "/camera_pano_ne/image_rect"
+      - "/camera_pano_nw/image_rect"
+      - "/camera_pano_ss/image_rect"
+      - "/camera_pano_sw/image_rect"
 
     # Optional names for the cameras (if not provided, will auto-generate camera_1, camera_2, etc.)
-    camera_names: ["front", "front_left", "front_right", "back", "back_left", "back_right"]
+    camera_names:
+      - "camera_lower_sw"
+      - "camera_lower_ne"
+      - "camera_lower_se"
+      - "camera_lower_nw"
+      - "camera_pano_ww"
+      - "camera_pano_nn"
+      - "camera_pano_ee"
+      - "camera_pano_se"
+      - "camera_pano_ne"
+      - "camera_pano_nw"
+      - "camera_pano_ss"
+      - "camera_pano_sw"
 
     # Whether to use compressed images (sensor_msgs/CompressedImage) instead of raw (sensor_msgs/Image)
     # Set to true if your camera topics publish compressed images
-    use_compressed: true
+    use_compressed: false
 
     # Maximum time difference in milliseconds for message synchronization
     # Larger values allow more tolerance but may reduce temporal accuracy

deep_tools/camera_sync/include/camera_sync/multi_camera_sync_node.hpp

@@ -15,16 +15,15 @@
 #ifndef CAMERA_SYNC__MULTI_CAMERA_SYNC_NODE_HPP_
 #define CAMERA_SYNC__MULTI_CAMERA_SYNC_NODE_HPP_
 
-#include <message_filters/subscriber.h>
-#include <message_filters/sync_policies/approximate_time.h>
-#include <message_filters/synchronizer.h>
+#include <cv_bridge/cv_bridge.h>
 
 #include <chrono>
+#include <map>
 #include <memory>
+#include <mutex>
 #include <string>
 #include <vector>
 
-#include <image_transport/image_transport.hpp>
 #include <rclcpp/rclcpp.hpp>
 #include <sensor_msgs/msg/compressed_image.hpp>
 #include <sensor_msgs/msg/image.hpp>
@@ -47,11 +46,11 @@ namespace camera_sync
 {
 
 /**
- * @brief Node that synchronizes N camera image messages using message filters
+ * @brief Node that synchronizes N camera image messages using manual timestamp-based buffering
  *
  * This node can handle both raw images (sensor_msgs/Image) and compressed images
  * (sensor_msgs/CompressedImage) and synchronize them based on their timestamps.
- * Supports 2-6 cameras with a compact implementation.
+ * Supports 2-12 cameras with a unified manual synchronization implementation.
  */
 #if USE_LIFECYCLE_NODE
 class MultiCameraSyncNode : public rclcpp_lifecycle::LifecycleNode
@@ -87,38 +86,8 @@ class MultiCameraSyncNode : public rclcpp::Node
   using CompressedImageMsg = sensor_msgs::msg::CompressedImage;
 
   // Subscriber types
-#if USE_LIFECYCLE_NODE
-  using ImageSubscriber = message_filters::Subscriber<ImageMsg, rclcpp_lifecycle::LifecycleNode>;
-  using CompressedImageSubscriber = message_filters::Subscriber<CompressedImageMsg, rclcpp_lifecycle::LifecycleNode>;
-#else
-  using ImageSubscriber = message_filters::Subscriber<ImageMsg>;
-  using CompressedImageSubscriber = message_filters::Subscriber<CompressedImageMsg>;
-#endif
-
-  // Sync policies for raw images
-  using ImageSyncPolicy2 = message_filters::sync_policies::ApproximateTime<ImageMsg, ImageMsg>;
-  using ImageSyncPolicy3 = message_filters::sync_policies::ApproximateTime<ImageMsg, ImageMsg, ImageMsg>;
-  using ImageSyncPolicy4 = message_filters::sync_policies::ApproximateTime<ImageMsg, ImageMsg, ImageMsg, ImageMsg>;
-  using ImageSyncPolicy5 =
-    message_filters::sync_policies::ApproximateTime<ImageMsg, ImageMsg, ImageMsg, ImageMsg, ImageMsg>;
-  using ImageSyncPolicy6 =
-    message_filters::sync_policies::ApproximateTime<ImageMsg, ImageMsg, ImageMsg, ImageMsg, ImageMsg, ImageMsg>;
-
-  // Sync policies for compressed images
-  using CompressedSyncPolicy2 = message_filters::sync_policies::ApproximateTime<CompressedImageMsg, CompressedImageMsg>;
-  using CompressedSyncPolicy3 =
-    message_filters::sync_policies::ApproximateTime<CompressedImageMsg, CompressedImageMsg, CompressedImageMsg>;
-  using CompressedSyncPolicy4 = message_filters::sync_policies::
-    ApproximateTime<CompressedImageMsg, CompressedImageMsg, CompressedImageMsg, CompressedImageMsg>;
-  using CompressedSyncPolicy5 = message_filters::sync_policies::
-    ApproximateTime<CompressedImageMsg, CompressedImageMsg, CompressedImageMsg, CompressedImageMsg, CompressedImageMsg>;
-  using CompressedSyncPolicy6 = message_filters::sync_policies::ApproximateTime<
-    CompressedImageMsg,
-    CompressedImageMsg,
-    CompressedImageMsg,
-    CompressedImageMsg,
-    CompressedImageMsg,
-    CompressedImageMsg>;
+  using ImageSubscriber = rclcpp::Subscription<ImageMsg>;
+  using CompressedImageSubscriber = rclcpp::Subscription<CompressedImageMsg>;
 
   /**
    * @brief Initialize the node parameters
@@ -140,61 +109,6 @@ class MultiCameraSyncNode : public rclcpp::Node
    */
   void setupCompressedSync(size_t num_cameras);
 
-  /**
-   * @brief Callback functions for synchronized raw images
-   */
-  void syncCallback2Raw(const ImageMsg::ConstSharedPtr & img1, const ImageMsg::ConstSharedPtr & img2);
-  void syncCallback3Raw(
-    const ImageMsg::ConstSharedPtr & img1,
-    const ImageMsg::ConstSharedPtr & img2,
-    const ImageMsg::ConstSharedPtr & img3);
-  void syncCallback4Raw(
-    const ImageMsg::ConstSharedPtr & img1,
-    const ImageMsg::ConstSharedPtr & img2,
-    const ImageMsg::ConstSharedPtr & img3,
-    const ImageMsg::ConstSharedPtr & img4);
-  void syncCallback5Raw(
-    const ImageMsg::ConstSharedPtr & img1,
-    const ImageMsg::ConstSharedPtr & img2,
-    const ImageMsg::ConstSharedPtr & img3,
-    const ImageMsg::ConstSharedPtr & img4,
-    const ImageMsg::ConstSharedPtr & img5);
-  void syncCallback6Raw(
-    const ImageMsg::ConstSharedPtr & img1,
-    const ImageMsg::ConstSharedPtr & img2,
-    const ImageMsg::ConstSharedPtr & img3,
-    const ImageMsg::ConstSharedPtr & img4,
-    const ImageMsg::ConstSharedPtr & img5,
-    const ImageMsg::ConstSharedPtr & img6);
-
-  /**
-   * @brief Callback functions for synchronized compressed images
-   */
-  void syncCallback2Compressed(
-    const CompressedImageMsg::ConstSharedPtr & img1, const CompressedImageMsg::ConstSharedPtr & img2);
-  void syncCallback3Compressed(
-    const CompressedImageMsg::ConstSharedPtr & img1,
-    const CompressedImageMsg::ConstSharedPtr & img2,
-    const CompressedImageMsg::ConstSharedPtr & img3);
-  void syncCallback4Compressed(
-    const CompressedImageMsg::ConstSharedPtr & img1,
-    const CompressedImageMsg::ConstSharedPtr & img2,
-    const CompressedImageMsg::ConstSharedPtr & img3,
-    const CompressedImageMsg::ConstSharedPtr & img4);
-  void syncCallback5Compressed(
-    const CompressedImageMsg::ConstSharedPtr & img1,
-    const CompressedImageMsg::ConstSharedPtr & img2,
-    const CompressedImageMsg::ConstSharedPtr & img3,
-    const CompressedImageMsg::ConstSharedPtr & img4,
-    const CompressedImageMsg::ConstSharedPtr & img5);
-  void syncCallback6Compressed(
-    const CompressedImageMsg::ConstSharedPtr & img1,
-    const CompressedImageMsg::ConstSharedPtr & img2,
-    const CompressedImageMsg::ConstSharedPtr & img3,
-    const CompressedImageMsg::ConstSharedPtr & img4,
-    const CompressedImageMsg::ConstSharedPtr & img5,
-    const CompressedImageMsg::ConstSharedPtr & img6);
-
   /**
    * @brief Process synchronized images (statistics and custom logic)
    * @param timestamps Vector of synchronized timestamps from all cameras
@@ -204,31 +118,13 @@ class MultiCameraSyncNode : public rclcpp::Node
   // Parameters
   std::vector<std::string> camera_topics_;
   std::vector<std::string> camera_names_;
-  std::vector<sensor_msgs::msg::Image::ConstSharedPtr> image_array;
   bool use_compressed_;
   double sync_tolerance_ms_;
   int queue_size_;
-  bool publish_sync_info_;
 
   // Subscribers
-  std::vector<std::unique_ptr<ImageSubscriber>> image_subscribers_;
-  std::vector<std::unique_ptr<CompressedImageSubscriber>> compressed_subscribers_;
-
-  // Synchronizers for raw images
-  std::unique_ptr<message_filters::Synchronizer<ImageSyncPolicy2>> sync2_raw_;
-  std::unique_ptr<message_filters::Synchronizer<ImageSyncPolicy3>> sync3_raw_;
-  std::unique_ptr<message_filters::Synchronizer<ImageSyncPolicy4>> sync4_raw_;
-  std::unique_ptr<message_filters::Synchronizer<ImageSyncPolicy5>> sync5_raw_;
-  std::unique_ptr<message_filters::Synchronizer<ImageSyncPolicy6>> sync6_raw_;
-
-  // Synchronizers for compressed images
-  std::unique_ptr<message_filters::Synchronizer<CompressedSyncPolicy2>> sync2_compressed_;
-  std::unique_ptr<message_filters::Synchronizer<CompressedSyncPolicy3>> sync3_compressed_;
-  std::unique_ptr<message_filters::Synchronizer<CompressedSyncPolicy4>> sync4_compressed_;
-  std::unique_ptr<message_filters::Synchronizer<CompressedSyncPolicy5>> sync5_compressed_;
-  std::unique_ptr<message_filters::Synchronizer<CompressedSyncPolicy6>> sync6_compressed_;
-
-  rclcpp::Publisher<sensor_msgs::msg::Image>::SharedPtr sync_info_pub_;
+  std::vector<std::shared_ptr<ImageSubscriber>> image_subscribers_;
+  std::vector<std::shared_ptr<CompressedImageSubscriber>> compressed_subscribers_;
 
   // Publishers for multi-image messages
   rclcpp::Publisher<deep_msgs::msg::MultiImage>::SharedPtr multi_image_raw_pub_;
@@ -238,6 +134,34 @@ class MultiCameraSyncNode : public rclcpp::Node
   int64_t sync_count_;
   rclcpp::Time last_sync_time_;
   std::chrono::steady_clock::time_point start_time_;
+
+  struct ImageBuffer
+  {
+    std::map<uint64_t, ImageMsg::ConstSharedPtr> buffer;
+    std::shared_ptr<std::mutex> mutex;
+
+    ImageBuffer()
+    : mutex(std::make_shared<std::mutex>())
+    {}
+  };
+
+  struct CompressedImageBuffer
+  {
+    std::map<uint64_t, CompressedImageMsg::ConstSharedPtr> buffer;
+    std::shared_ptr<std::mutex> mutex;
+
+    CompressedImageBuffer()
+    : mutex(std::make_shared<std::mutex>())
+    {}
+  };
+
+  std::vector<ImageBuffer> raw_image_buffers_;
+  std::vector<CompressedImageBuffer> compressed_image_buffers_;
+
+  void handleRawImageCallback(size_t camera_idx, const ImageMsg::ConstSharedPtr & msg);
+  void handleCompressedImageCallback(size_t camera_idx, const CompressedImageMsg::ConstSharedPtr & msg);
+  void tryPublishSyncedRawImages();
+  void tryPublishSyncedCompressedImages();
 };
 
 }  // namespace camera_sync

deep_tools/camera_sync/package.xml

@@ -2,8 +2,8 @@
 <package format="3">
   <name>camera_sync</name>
   <version>0.1.0</version>
-  <description>ROS2 CLI tool and nodes for ONNX model management and multi-camera synchronization</description>
-  <maintainer email="hello@watonomous.ca">WATonomous</maintainer>
+  <description>ROS2 node for multi-camera synchronization</description>
+  <maintainer email="lereljic@watonomous.ca">Lereljic</maintainer>
   <license>Apache 2.0</license>
 
   <buildtool_depend>ament_cmake</buildtool_depend>
@@ -15,6 +15,7 @@
   <depend>message_filters</depend>
   <depend>image_transport</depend>
   <depend>deep_msgs</depend>
+  <depend>class_loader</depend>