Add a camera synchronization node (#13)

* Adding Test Infra (#9)

* adds test package

* documentation on the node

* pre-commit

* ci failing

* Initial MultiCamera node

* Added Readme

* Added yaml launch files and yaml config

* Fix to pass pre-commit

* Added lifecycle support for ROS Rolling
- This is required since compressed image subs only work with lifecycle in rolling
- Additional fixed package test errors

* Added developing markdown and removed main from camera_sync component

---------

Co-authored-by: Eddy Zhou <71026430+Edwardius@users.noreply.github.com>

Author: lucasreljic

deep_msgs/CMakeLists.txt

@@ -0,0 +1,35 @@
+# Copyright (c) 2025-present WATonomous. All rights reserved.
+#
+# 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.
+cmake_minimum_required(VERSION 3.8)
+project(deep_msgs)
+
+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(std_msgs REQUIRED)
+find_package(sensor_msgs REQUIRED)
+find_package(rosidl_default_generators REQUIRED)
+
+rosidl_generate_interfaces(${PROJECT_NAME}
+  "msg/MultiImage.msg"
+  "msg/MultiImageRaw.msg"
+  DEPENDENCIES std_msgs sensor_msgs
+)
+
+
+ament_package()

deep_msgs/msg/MultiImage.msg

@@ -0,0 +1,5 @@
+# MultiImage.msg
+# A message that carries multiple compressed images together
+
+std_msgs/Header header
+sensor_msgs/CompressedImage[] images

deep_msgs/msg/MultiImageRaw.msg

@@ -0,0 +1,3 @@
+# MultiImageRaw.msg
+std_msgs/Header header
+sensor_msgs/Image[] images

deep_msgs/package.xml

@@ -0,0 +1,19 @@
+<?xml version="1.0"?>
+<package format="3">
+  <name>deep_msgs</name>
+  <version>0.0.0</version>
+  <description>Package for deep ros specific msgs</description>
+  <maintainer email="lucas.reljic@gmail.com">lucas</maintainer>
+  <license>Apache 2.0</license>
+
+  <buildtool_depend>ament_cmake</buildtool_depend>
+  <depend>std_msgs</depend>
+  <depend>sensor_msgs</depend>
+  <build_depend>rosidl_default_generators</build_depend>
+  <exec_depend>rosidl_default_runtime</exec_depend>
+  <member_of_group>rosidl_interface_packages</member_of_group>
+  <export>
+    <build_type>ament_cmake</build_type>
+  </export>
+
+</package>

deep_test/CMakeLists.txt

@@ -11,7 +11,6 @@
 # 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.22)
 project(deep_test)
 

deep_tools/camera_sync/CMakeLists.txt

@@ -0,0 +1,92 @@
+# Copyright (c) 2025-present WATonomous. All rights reserved.
+#
+# 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.
+cmake_minimum_required(VERSION 3.8)
+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)
+find_package(rclcpp_components REQUIRED)
+find_package(lifecycle_msgs REQUIRED)
+find_package(sensor_msgs REQUIRED)
+find_package(cv_bridge REQUIRED)
+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
+get_filename_component(ROS_DISTRO_NAME "$ENV{ROS_DISTRO}" NAME)
+if(ROS_DISTRO_NAME STREQUAL "rolling") # Currently only Rolling supports lifecycle nodes with compressed image subs
+  add_definitions(-DUSE_LIFECYCLE_NODE=1)
+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
+  rclcpp_lifecycle::rclcpp_lifecycle
+  rclcpp_components::component_manager
+  lifecycle_msgs::lifecycle_msgs__rosidl_typesupport_cpp
+  sensor_msgs::sensor_msgs__rosidl_typesupport_cpp
+  cv_bridge::cv_bridge
+  message_filters::message_filters
+  image_transport::image_transport
+  deep_msgs::deep_msgs__rosidl_typesupport_cpp
+)
+
+# Register the 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/DEVELOPING.md

@@ -0,0 +1,27 @@
+# Camera Sync Development Notes
+
+## Known Issues & Limitations
+
+### 1. Lifecycle Node Incompatibility (ROS Humble, Jazzy and Earlier)
+**Problem**: Lifecycle nodes cannot subscribe to compressed images in ROS Humble and earlier distributions.
+- **Root Cause**: Missing lifecycle support in `image_transport` for compressed image subscriptions
+- **Solution**: There is a Macro used throughout the code which specifies if it is with the lifecycle node or not. This is crucial to keep in mind when developing as breaking changes can occur across ROS distros if it's not compatible with a Lifecycle Node or vice versa. It is best to test with both versions to ensure compatibility.
+
+The Macro:
+
+```cpp
+#if ROS_DISTRO_NAME STREQUAL "rolling"
+  add_definitions(-DUSE_LIFECYCLE_NODE=1)
+#else
+  add_definitions(-DUSE_LIFECYCLE_NODE=0)  // Disable for Humble/earlier
+#endif
+```
+
+### 2. Message Filters Synchronization Limitation
+**Problem**: `message_filters::Synchronizer` requires compile-time known number of topics.
+- **Limitation**: Cannot dynamically sync N cameras - must handle 2-6 cameras with separate synchronizers
+- **Implementation**: Switch statement creates different sync policies for each camera count
+
+## Architecture Notes
+- **Component-only**: No standalone executable - use component loading only which is ideal to allow for IPC (zero-copy)
+- **Dual Mode**: Supports both raw and compressed image synchronization

deep_tools/camera_sync/README.md

@@ -0,0 +1,79 @@
+# Multi-Camera Synchronization Node
+
+A ROS2 node that uses message filters to time-synchronize N camera image messages, supporting both compressed and raw images.
+
+## Features
+
+- **Flexible camera count**: Supports 2-6 cameras simultaneously
+- **Image format support**: Works with both `sensor_msgs/Image` (raw) and `sensor_msgs/CompressedImage`
+- **Configurable synchronization**: Adjustable time tolerance and queue sizes
+- **Real-time monitoring**: Synchronization statistics and rate monitoring
+- **Easy integration**: Standard ROS2 node with parameter-based configuration
+
+## Parameters
+
+| Parameter | Type | Default | Description |
+|-----------|------|---------|-------------|
+| `camera_topics` | string[] | `[]` | List of camera image topics to synchronize |
+| `camera_names` | string[] | `[]` | Names for cameras (auto-generated if empty) |
+| `use_compressed` | bool | `false` | Use compressed images instead of raw RGB |
+| `sync_tolerance_ms` | double | `50.0` | Max time difference for sync (milliseconds) |
+| `queue_size` | int | `10` | Message filter queue size |
+| `publish_sync_info` | bool | `true` | Publish synchronization statistics |
+
+## Usage
+
+### Parameter usage:
+All configuration changes can be passed through CLI. Here are some example arguments:
+
+```bash
+  -p camera_topics:="['/camera1/image_raw', '/camera2/image_raw']" \
+  -p camera_names:="['left_camera', 'right_camera']" \
+  -p use_compressed:=true \
+  -p sync_tolerance_ms:=30.0
+```
+
+## How It Works
+
+The node uses ROS2 message filters with approximate time synchronization policy to match images from multiple cameras based on their timestamps. Key components:
+
+1. **Message Subscribers**: Creates subscribers for each camera topic
+2. **Synchronizer**: Uses `message_filters::sync_policies::ApproximateTime` to match messages
+3. **Callback & Publishing**: Creates MultiImage msgs with timestamp to batch the camera images together
+4. **Statistics**: Tracks synchronization rate and timing spread
+
+### Synchronization Logic
+
+- Messages are considered synchronized if their timestamps are within `sync_tolerance_ms`
+- The synchronizer maintains a queue of recent messages from each camera
+- When a valid sync set is found, all cameras' images are processed together
+- Unmatched messages are eventually dropped when they exceed the age penalty
+
+## Monitoring
+
+The node provides several ways to monitor synchronization performance:
+
+1. **Console logs**: Periodic sync rate and timing statistics
+2. **ROS2 parameters**: Runtime inspection of configuration
+3. **Debug output**: Detailed timestamp information (when debug logging enabled)
+
+Example log output:
+
+```
+[INFO] [multi_camera_sync]: Sync #500: compressed images from 4 cameras, spread: 12.3 ms, rate: 29.8 Hz
+```
+
+## Dependencies
+
+- `rclcpp` - ROS2 C++ client library
+- `sensor_msgs` - Standard sensor message types
+- `message_filters` - Time synchronization utilities
+- `cv_bridge` - OpenCV integration (for future extensions)
+- `image_transport` - Efficient image transmission
+
+## Limitations
+
+- Maximum 6 cameras supported (can be extended by adding more sync policies)
+- Uses approximate time synchronization (not exact)
+- All cameras must use the same image message type (raw or compressed)
+- Requires reasonably synchronized system clocks across camera sources

deep_tools/camera_sync/config/multi_camera_sync_params.yaml

@@ -0,0 +1,54 @@
+#  Copyright (c) 2025-present WATonomous. All rights reserved.
+#
+#  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.
+
+
+# Configuration file for multi-camera synchronization node
+#
+# This file contains the parameters for the multi_camera_sync_node.
+# You can modify these values to suit your camera setup.
+
+---
+multi_camera_sync:
+  ros__parameters:
+    # 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: ["/front/image_compressed", "/back/image_compressed", "/port/image_compressed"]
+
+    # Optional names for the cameras (if not provided, will auto-generate camera_1, camera_2, etc.)
+    camera_names: ["front", "back", "port"]
+
+    # 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
+
+    # Maximum time difference in milliseconds for message synchronization
+    # Larger values allow more tolerance but may reduce temporal accuracy
+    sync_tolerance_ms: 33.0
+
+    # Queue size for message filters
+    # Larger values can handle more variable frame rates but use more memory
+    queue_size: 10
+
+    # Whether to publish synchronization information and statistics
+    publish_sync_info: true
+
+    # IPC-specific optimizations
+    use_intra_process_comms: true
+
+    # QoS settings optimized for camera data with IPC
+    qos_depth: 5
+    qos_reliability: "reliable"  # reliable or "best_effort" for higher throughput
+    qos_durability: "volatile"
+    qos_history: "keep_last"