diff --git a/deep_tools/camera_sync/CMakeLists.txt b/deep_tools/camera_sync/CMakeLists.txt index cf79aab..574bb7f 100644 --- a/deep_tools/camera_sync/CMakeLists.txt +++ b/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) @@ -30,7 +29,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 @@ -41,7 +39,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 @@ -59,9 +56,6 @@ 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;$\n") - - -# Install executables and libraries install(TARGETS multi_camera_sync_component ARCHIVE DESTINATION lib @@ -69,24 +63,18 @@ install(TARGETS 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() diff --git a/deep_tools/camera_sync/config/multi_camera_sync_params.yaml b/deep_tools/camera_sync/config/multi_camera_sync_params.yaml index 593d675..5e10f0c 100644 --- a/deep_tools/camera_sync/config/multi_camera_sync_params.yaml +++ b/deep_tools/camera_sync/config/multi_camera_sync_params.yaml @@ -24,14 +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: ["/front/image_compressed", "/back/image_compressed", "/port/image_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", "back", "port"] + 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 diff --git a/deep_tools/camera_sync/include/camera_sync/multi_camera_sync_node.hpp b/deep_tools/camera_sync/include/camera_sync/multi_camera_sync_node.hpp index 8a77696..c4ba067 100644 --- a/deep_tools/camera_sync/include/camera_sync/multi_camera_sync_node.hpp +++ b/deep_tools/camera_sync/include/camera_sync/multi_camera_sync_node.hpp @@ -16,16 +16,14 @@ #define CAMERA_SYNC__MULTI_CAMERA_SYNC_NODE_HPP_ #include -#include -#include -#include #include +#include #include +#include #include #include -#include #include #include #include @@ -48,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 @@ -88,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; - using CompressedImageSubscriber = message_filters::Subscriber; -#else - using ImageSubscriber = message_filters::Subscriber; - using CompressedImageSubscriber = message_filters::Subscriber; -#endif - - // Sync policies for raw images - using ImageSyncPolicy2 = message_filters::sync_policies::ApproximateTime; - using ImageSyncPolicy3 = message_filters::sync_policies::ApproximateTime; - using ImageSyncPolicy4 = message_filters::sync_policies::ApproximateTime; - using ImageSyncPolicy5 = - message_filters::sync_policies::ApproximateTime; - using ImageSyncPolicy6 = - message_filters::sync_policies::ApproximateTime; - - // Sync policies for compressed images - using CompressedSyncPolicy2 = message_filters::sync_policies::ApproximateTime; - using CompressedSyncPolicy3 = - message_filters::sync_policies::ApproximateTime; - using CompressedSyncPolicy4 = message_filters::sync_policies:: - ApproximateTime; - using CompressedSyncPolicy5 = message_filters::sync_policies:: - ApproximateTime; - using CompressedSyncPolicy6 = message_filters::sync_policies::ApproximateTime< - CompressedImageMsg, - CompressedImageMsg, - CompressedImageMsg, - CompressedImageMsg, - CompressedImageMsg, - CompressedImageMsg>; + using ImageSubscriber = rclcpp::Subscription; + using CompressedImageSubscriber = rclcpp::Subscription; /** * @brief Initialize the node parameters @@ -141,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 @@ -205,31 +118,13 @@ class MultiCameraSyncNode : public rclcpp::Node // Parameters std::vector camera_topics_; std::vector camera_names_; - std::vector image_array; bool use_compressed_; double sync_tolerance_ms_; int queue_size_; - bool publish_sync_info_; // Subscribers - std::vector> image_subscribers_; - std::vector> compressed_subscribers_; - - // Synchronizers for raw images - std::unique_ptr> sync2_raw_; - std::unique_ptr> sync3_raw_; - std::unique_ptr> sync4_raw_; - std::unique_ptr> sync5_raw_; - std::unique_ptr> sync6_raw_; - - // Synchronizers for compressed images - std::unique_ptr> sync2_compressed_; - std::unique_ptr> sync3_compressed_; - std::unique_ptr> sync4_compressed_; - std::unique_ptr> sync5_compressed_; - std::unique_ptr> sync6_compressed_; - - rclcpp::Publisher::SharedPtr sync_info_pub_; + std::vector> image_subscribers_; + std::vector> compressed_subscribers_; // Publishers for multi-image messages rclcpp::Publisher::SharedPtr multi_image_raw_pub_; @@ -239,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 buffer; + std::shared_ptr mutex; + + ImageBuffer() + : mutex(std::make_shared()) + {} + }; + + struct CompressedImageBuffer + { + std::map buffer; + std::shared_ptr mutex; + + CompressedImageBuffer() + : mutex(std::make_shared()) + {} + }; + + std::vector raw_image_buffers_; + std::vector 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 diff --git a/deep_tools/camera_sync/package.xml b/deep_tools/camera_sync/package.xml index d789129..2cfa39d 100644 --- a/deep_tools/camera_sync/package.xml +++ b/deep_tools/camera_sync/package.xml @@ -2,8 +2,8 @@ camera_sync 0.1.0 - ROS2 CLI tool and nodes for ONNX model management and multi-camera synchronization - WATonomous + ROS2 node for multi-camera synchronization + Lereljic Apache 2.0 ament_cmake @@ -16,6 +16,7 @@ message_filters image_transport deep_msgs + class_loader diff --git a/deep_tools/camera_sync/src/multi_camera_sync_node.cpp b/deep_tools/camera_sync/src/multi_camera_sync_node.cpp index 0186146..2e59adc 100644 --- a/deep_tools/camera_sync/src/multi_camera_sync_node.cpp +++ b/deep_tools/camera_sync/src/multi_camera_sync_node.cpp @@ -20,6 +20,7 @@ #include #include #include +#include #include #include @@ -66,18 +67,16 @@ void MultiCameraSyncNode::initializeParameters() this->declare_parameter("use_compressed", false); this->declare_parameter("sync_tolerance_ms", 50.0); this->declare_parameter("queue_size", 10); - this->declare_parameter("publish_sync_info", true); camera_topics_ = this->get_parameter("camera_topics").as_string_array(); camera_names_ = this->get_parameter("camera_names").as_string_array(); use_compressed_ = this->get_parameter("use_compressed").as_bool(); sync_tolerance_ms_ = this->get_parameter("sync_tolerance_ms").as_double(); queue_size_ = this->get_parameter("queue_size").as_int(); - publish_sync_info_ = this->get_parameter("publish_sync_info").as_bool(); // Validate - if (camera_topics_.empty() || camera_topics_.size() < 2 || camera_topics_.size() > 6) { - throw std::runtime_error("Need 2-6 camera topics"); + if (camera_topics_.empty() || camera_topics_.size() < 2 || camera_topics_.size() > 12) { + throw std::runtime_error("Need 2-12 camera topics"); } // Auto-generate names if not provided @@ -107,377 +106,52 @@ void MultiCameraSyncNode::setupSynchronization() } else { multi_image_raw_pub_ = this->create_publisher("~/multi_image_raw", 10); } - - if (publish_sync_info_) { - sync_info_pub_ = this->create_publisher("~/sync_info", 10); - } } void MultiCameraSyncNode::setupRawSync(size_t num_cameras) { - // Create subscribers - image_subscribers_.reserve(num_cameras); + // Initialize buffers for all cameras + raw_image_buffers_.resize(num_cameras); + + // Create subscribers with manual synchronization callback for (size_t i = 0; i < num_cameras; ++i) { - image_subscribers_.emplace_back(std::make_unique(this, camera_topics_[i])); + auto callback = [this, i](const ImageMsg::ConstSharedPtr & msg) { this->handleRawImageCallback(i, msg); }; + image_subscribers_.push_back(this->create_subscription(camera_topics_[i], queue_size_, callback)); } - // Create synchronizer based on number of cameras - switch (num_cameras) { - case 2: - sync2_raw_ = std::make_unique>( - ImageSyncPolicy2(queue_size_), *image_subscribers_[0], *image_subscribers_[1]); - sync2_raw_->setAgePenalty(sync_tolerance_ms_ / 1000.0); - sync2_raw_->registerCallback( - std::bind(&MultiCameraSyncNode::syncCallback2Raw, this, std::placeholders::_1, std::placeholders::_2)); - break; - case 3: - sync3_raw_ = std::make_unique>( - ImageSyncPolicy3(queue_size_), *image_subscribers_[0], *image_subscribers_[1], *image_subscribers_[2]); - sync3_raw_->setAgePenalty(sync_tolerance_ms_ / 1000.0); - sync3_raw_->registerCallback(std::bind( - &MultiCameraSyncNode::syncCallback3Raw, - this, - std::placeholders::_1, - std::placeholders::_2, - std::placeholders::_3)); - break; - case 4: - sync4_raw_ = std::make_unique>( - ImageSyncPolicy4(queue_size_), - *image_subscribers_[0], - *image_subscribers_[1], - *image_subscribers_[2], - *image_subscribers_[3]); - sync4_raw_->setAgePenalty(sync_tolerance_ms_ / 1000.0); - sync4_raw_->registerCallback(std::bind( - &MultiCameraSyncNode::syncCallback4Raw, - this, - std::placeholders::_1, - std::placeholders::_2, - std::placeholders::_3, - std::placeholders::_4)); - break; - case 5: - sync5_raw_ = std::make_unique>( - ImageSyncPolicy5(queue_size_), - *image_subscribers_[0], - *image_subscribers_[1], - *image_subscribers_[2], - *image_subscribers_[3], - *image_subscribers_[4]); - sync5_raw_->setAgePenalty(sync_tolerance_ms_ / 1000.0); - sync5_raw_->registerCallback(std::bind( - &MultiCameraSyncNode::syncCallback5Raw, - this, - std::placeholders::_1, - std::placeholders::_2, - std::placeholders::_3, - std::placeholders::_4, - std::placeholders::_5)); - break; - case 6: - sync6_raw_ = std::make_unique>( - ImageSyncPolicy6(queue_size_), - *image_subscribers_[0], - *image_subscribers_[1], - *image_subscribers_[2], - *image_subscribers_[3], - *image_subscribers_[4], - *image_subscribers_[5]); - sync6_raw_->setAgePenalty(sync_tolerance_ms_ / 1000.0); - sync6_raw_->registerCallback(std::bind( - &MultiCameraSyncNode::syncCallback6Raw, - this, - std::placeholders::_1, - std::placeholders::_2, - std::placeholders::_3, - std::placeholders::_4, - std::placeholders::_5, - std::placeholders::_6)); - break; - } + RCLCPP_INFO(this->get_logger(), "Set up raw image synchronization for %zu cameras", num_cameras); } void MultiCameraSyncNode::setupCompressedSync(size_t num_cameras) { - // Create subscribers - compressed_subscribers_.reserve(num_cameras); + // Initialize buffers for all cameras + compressed_image_buffers_.resize(num_cameras); + + // Create subscribers with manual synchronization callback for (size_t i = 0; i < num_cameras; ++i) { - compressed_subscribers_.emplace_back(std::make_unique(this, camera_topics_[i])); + auto callback = [this, i](const CompressedImageMsg::ConstSharedPtr & msg) { + this->handleCompressedImageCallback(i, msg); + }; + compressed_subscribers_.push_back( + this->create_subscription(camera_topics_[i], queue_size_, callback)); } - // Create synchronizer based on number of cameras - switch (num_cameras) { - case 2: - sync2_compressed_ = std::make_unique>( - CompressedSyncPolicy2(queue_size_), *compressed_subscribers_[0], *compressed_subscribers_[1]); - sync2_compressed_->setAgePenalty(sync_tolerance_ms_ / 1000.0); - sync2_compressed_->registerCallback( - std::bind(&MultiCameraSyncNode::syncCallback2Compressed, this, std::placeholders::_1, std::placeholders::_2)); - break; - case 3: - sync3_compressed_ = std::make_unique>( - CompressedSyncPolicy3(queue_size_), - *compressed_subscribers_[0], - *compressed_subscribers_[1], - *compressed_subscribers_[2]); - sync3_compressed_->setAgePenalty(sync_tolerance_ms_ / 1000.0); - sync3_compressed_->registerCallback(std::bind( - &MultiCameraSyncNode::syncCallback3Compressed, - this, - std::placeholders::_1, - std::placeholders::_2, - std::placeholders::_3)); - break; - case 4: - sync4_compressed_ = std::make_unique>( - CompressedSyncPolicy4(queue_size_), - *compressed_subscribers_[0], - *compressed_subscribers_[1], - *compressed_subscribers_[2], - *compressed_subscribers_[3]); - sync4_compressed_->setAgePenalty(sync_tolerance_ms_ / 1000.0); - sync4_compressed_->registerCallback(std::bind( - &MultiCameraSyncNode::syncCallback4Compressed, - this, - std::placeholders::_1, - std::placeholders::_2, - std::placeholders::_3, - std::placeholders::_4)); - break; - case 5: - sync5_compressed_ = std::make_unique>( - CompressedSyncPolicy5(queue_size_), - *compressed_subscribers_[0], - *compressed_subscribers_[1], - *compressed_subscribers_[2], - *compressed_subscribers_[3], - *compressed_subscribers_[4]); - sync5_compressed_->setAgePenalty(sync_tolerance_ms_ / 1000.0); - sync5_compressed_->registerCallback(std::bind( - &MultiCameraSyncNode::syncCallback5Compressed, - this, - std::placeholders::_1, - std::placeholders::_2, - std::placeholders::_3, - std::placeholders::_4, - std::placeholders::_5)); - break; - case 6: - sync6_compressed_ = std::make_unique>( - CompressedSyncPolicy6(queue_size_), - *compressed_subscribers_[0], - *compressed_subscribers_[1], - *compressed_subscribers_[2], - *compressed_subscribers_[3], - *compressed_subscribers_[4], - *compressed_subscribers_[5]); - sync6_compressed_->setAgePenalty(sync_tolerance_ms_ / 1000.0); - sync6_compressed_->registerCallback(std::bind( - &MultiCameraSyncNode::syncCallback6Compressed, - this, - std::placeholders::_1, - std::placeholders::_2, - std::placeholders::_3, - std::placeholders::_4, - std::placeholders::_5, - std::placeholders::_6)); - break; - } + RCLCPP_INFO(this->get_logger(), "Set up compressed image synchronization for %zu cameras", num_cameras); } -// Helper function to create multi-image messages +// Helper function to create multi-image messages (optimized with move semantics) template -MultiMsgT createMultiImageMessage(const std::vector & images) +MultiMsgT createMultiImageMessage(std::vector images) { MultiMsgT msg; msg.header.stamp = rclcpp::Clock().now(); msg.images.reserve(images.size()); - for (const auto & img : images) { - msg.images.push_back(*img); // Deep copy onto new synced message + for (auto & img : images) { + msg.images.push_back(std::move(*img)); // Move instead of copy } return msg; } -// Raw image callbacks -void MultiCameraSyncNode::syncCallback2Raw(const ImageMsg::ConstSharedPtr & img1, const ImageMsg::ConstSharedPtr & img2) -{ - std::vector timestamps = {rclcpp::Time(img1->header.stamp), rclcpp::Time(img2->header.stamp)}; - processSynchronizedImages(timestamps); - - // Create and publish multi-image message - std::vector images = {img1, img2}; - auto raw_msg = createMultiImageMessage(images); - multi_image_raw_pub_->publish(raw_msg); -} - -void MultiCameraSyncNode::syncCallback3Raw( - const ImageMsg::ConstSharedPtr & img1, const ImageMsg::ConstSharedPtr & img2, const ImageMsg::ConstSharedPtr & img3) -{ - std::vector timestamps = { - rclcpp::Time(img1->header.stamp), rclcpp::Time(img2->header.stamp), rclcpp::Time(img3->header.stamp)}; - processSynchronizedImages(timestamps); - - // Create and publish multi-image message - std::vector images = {img1, img2, img3}; - auto raw_msg = createMultiImageMessage(images); - multi_image_raw_pub_->publish(raw_msg); -} - -void MultiCameraSyncNode::syncCallback4Raw( - const ImageMsg::ConstSharedPtr & img1, - const ImageMsg::ConstSharedPtr & img2, - const ImageMsg::ConstSharedPtr & img3, - const ImageMsg::ConstSharedPtr & img4) -{ - std::vector timestamps = { - rclcpp::Time(img1->header.stamp), - rclcpp::Time(img2->header.stamp), - rclcpp::Time(img3->header.stamp), - rclcpp::Time(img4->header.stamp)}; - processSynchronizedImages(timestamps); - - // Create and publish multi-image message - std::vector images = {img1, img2, img3, img4}; - auto raw_msg = createMultiImageMessage(images); - multi_image_raw_pub_->publish(raw_msg); -} - -void MultiCameraSyncNode::syncCallback5Raw( - const ImageMsg::ConstSharedPtr & img1, - const ImageMsg::ConstSharedPtr & img2, - const ImageMsg::ConstSharedPtr & img3, - const ImageMsg::ConstSharedPtr & img4, - const ImageMsg::ConstSharedPtr & img5) -{ - std::vector timestamps = { - rclcpp::Time(img1->header.stamp), - rclcpp::Time(img2->header.stamp), - rclcpp::Time(img3->header.stamp), - rclcpp::Time(img4->header.stamp), - rclcpp::Time(img5->header.stamp)}; - processSynchronizedImages(timestamps); - - // Create and publish multi-image message - std::vector images = {img1, img2, img3, img4, img5}; - auto raw_msg = createMultiImageMessage(images); - multi_image_raw_pub_->publish(raw_msg); -} - -void MultiCameraSyncNode::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) -{ - std::vector timestamps = { - rclcpp::Time(img1->header.stamp), - rclcpp::Time(img2->header.stamp), - rclcpp::Time(img3->header.stamp), - rclcpp::Time(img4->header.stamp), - rclcpp::Time(img5->header.stamp), - rclcpp::Time(img6->header.stamp)}; - processSynchronizedImages(timestamps); - - // Create and publish multi-image message - std::vector images = {img1, img2, img3, img4, img5, img6}; - auto raw_msg = createMultiImageMessage(images); - multi_image_raw_pub_->publish(raw_msg); -} - -// Compressed image callbacks -void MultiCameraSyncNode::syncCallback2Compressed( - const CompressedImageMsg::ConstSharedPtr & img1, const CompressedImageMsg::ConstSharedPtr & img2) -{ - std::vector timestamps = {rclcpp::Time(img1->header.stamp), rclcpp::Time(img2->header.stamp)}; - processSynchronizedImages(timestamps); - - // Create and publish multi-image message - std::vector images = {img1, img2}; - auto compressed_msg = createMultiImageMessage(images); - multi_image_compressed_pub_->publish(compressed_msg); -} - -void MultiCameraSyncNode::syncCallback3Compressed( - const CompressedImageMsg::ConstSharedPtr & img1, - const CompressedImageMsg::ConstSharedPtr & img2, - const CompressedImageMsg::ConstSharedPtr & img3) -{ - std::vector timestamps = { - rclcpp::Time(img1->header.stamp), rclcpp::Time(img2->header.stamp), rclcpp::Time(img3->header.stamp)}; - processSynchronizedImages(timestamps); - - // Create and publish multi-image message - std::vector images = {img1, img2, img3}; - auto compressed_msg = createMultiImageMessage(images); - multi_image_compressed_pub_->publish(compressed_msg); -} - -void MultiCameraSyncNode::syncCallback4Compressed( - const CompressedImageMsg::ConstSharedPtr & img1, - const CompressedImageMsg::ConstSharedPtr & img2, - const CompressedImageMsg::ConstSharedPtr & img3, - const CompressedImageMsg::ConstSharedPtr & img4) -{ - std::vector timestamps = { - rclcpp::Time(img1->header.stamp), - rclcpp::Time(img2->header.stamp), - rclcpp::Time(img3->header.stamp), - rclcpp::Time(img4->header.stamp)}; - processSynchronizedImages(timestamps); - - // Create and publish multi-image message - std::vector images = {img1, img2, img3, img4}; - auto compressed_msg = createMultiImageMessage(images); - multi_image_compressed_pub_->publish(compressed_msg); -} - -void MultiCameraSyncNode::syncCallback5Compressed( - const CompressedImageMsg::ConstSharedPtr & img1, - const CompressedImageMsg::ConstSharedPtr & img2, - const CompressedImageMsg::ConstSharedPtr & img3, - const CompressedImageMsg::ConstSharedPtr & img4, - const CompressedImageMsg::ConstSharedPtr & img5) -{ - std::vector timestamps = { - rclcpp::Time(img1->header.stamp), - rclcpp::Time(img2->header.stamp), - rclcpp::Time(img3->header.stamp), - rclcpp::Time(img4->header.stamp), - rclcpp::Time(img5->header.stamp)}; - processSynchronizedImages(timestamps); - - // Create and publish multi-image message - std::vector images = {img1, img2, img3, img4, img5}; - auto compressed_msg = createMultiImageMessage(images); - multi_image_compressed_pub_->publish(compressed_msg); -} - -void MultiCameraSyncNode::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) -{ - std::vector timestamps = { - rclcpp::Time(img1->header.stamp), - rclcpp::Time(img2->header.stamp), - rclcpp::Time(img3->header.stamp), - rclcpp::Time(img4->header.stamp), - rclcpp::Time(img5->header.stamp), - rclcpp::Time(img6->header.stamp)}; - processSynchronizedImages(timestamps); - - // Create and publish multi-image message - std::vector images = {img1, img2, img3, img4, img5, img6}; - auto compressed_msg = createMultiImageMessage(images); - multi_image_compressed_pub_->publish(compressed_msg); -} - // Unified processing method that works with timestamps void MultiCameraSyncNode::processSynchronizedImages(const std::vector & timestamps) { @@ -561,26 +235,17 @@ rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn MultiC { RCLCPP_INFO(this->get_logger(), "Cleaning up Multi-Camera Sync Node"); - // Reset subscribers and synchronizers + // Reset subscribers image_subscribers_.clear(); compressed_subscribers_.clear(); - sync2_raw_.reset(); - sync3_raw_.reset(); - sync4_raw_.reset(); - sync5_raw_.reset(); - sync6_raw_.reset(); - - sync2_compressed_.reset(); - sync3_compressed_.reset(); - sync4_compressed_.reset(); - sync5_compressed_.reset(); - sync6_compressed_.reset(); - // Reset publishers multi_image_raw_pub_.reset(); multi_image_compressed_pub_.reset(); - sync_info_pub_.reset(); + + // Clear buffers + raw_image_buffers_.clear(); + compressed_image_buffers_.clear(); return rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn::SUCCESS; } @@ -595,6 +260,283 @@ rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn MultiC } #endif +void MultiCameraSyncNode::handleRawImageCallback(size_t camera_idx, const ImageMsg::ConstSharedPtr & msg) +{ + if (camera_idx >= raw_image_buffers_.size()) { + RCLCPP_WARN( + this->get_logger(), "Camera index %zu out of bounds (size: %zu)", camera_idx, raw_image_buffers_.size()); + return; + } + + { + std::lock_guard lock(*raw_image_buffers_[camera_idx].mutex); + uint64_t msg_time_ns = msg->header.stamp.sec * 1000000000ULL + msg->header.stamp.nanosec; + raw_image_buffers_[camera_idx].buffer[msg_time_ns] = msg; + + RCLCPP_DEBUG( + this->get_logger(), + "Camera %zu: received image at time %lu (buffer size: %zu)", + camera_idx, + msg_time_ns, + raw_image_buffers_[camera_idx].buffer.size()); + + // Clean old messages outside tolerance window + auto it = raw_image_buffers_[camera_idx].buffer.begin(); + while (it != raw_image_buffers_[camera_idx].buffer.end()) { + int64_t time_diff_ns = static_cast(msg_time_ns) - static_cast(it->first); + if (time_diff_ns > static_cast(sync_tolerance_ms_ * 2 * 1e6)) { + it = raw_image_buffers_[camera_idx].buffer.erase(it); + } else { + ++it; + } + } + } + + tryPublishSyncedRawImages(); +} + +void MultiCameraSyncNode::handleCompressedImageCallback( + size_t camera_idx, const CompressedImageMsg::ConstSharedPtr & msg) +{ + if (camera_idx >= compressed_image_buffers_.size()) { + RCLCPP_WARN( + this->get_logger(), "Camera index %zu out of bounds (size: %zu)", camera_idx, compressed_image_buffers_.size()); + return; + } + + { + std::lock_guard lock(*compressed_image_buffers_[camera_idx].mutex); + uint64_t msg_time_ns = msg->header.stamp.sec * 1000000000ULL + msg->header.stamp.nanosec; + compressed_image_buffers_[camera_idx].buffer[msg_time_ns] = msg; + + RCLCPP_DEBUG( + this->get_logger(), + "Camera %zu: received compressed image at time %lu (buffer size: %zu)", + camera_idx, + msg_time_ns, + compressed_image_buffers_[camera_idx].buffer.size()); + + // Clean old messages outside tolerance window + auto it = compressed_image_buffers_[camera_idx].buffer.begin(); + while (it != compressed_image_buffers_[camera_idx].buffer.end()) { + int64_t time_diff_ns = static_cast(msg_time_ns) - static_cast(it->first); + if (time_diff_ns > static_cast(sync_tolerance_ms_ * 2 * 1e6)) { + it = compressed_image_buffers_[camera_idx].buffer.erase(it); + } else { + ++it; + } + } + } + + tryPublishSyncedCompressedImages(); +} + +void MultiCameraSyncNode::tryPublishSyncedRawImages() +{ + std::vector synced_images; + std::vector timestamps; + uint64_t sync_time_ns; + + // Lock all buffers + for (auto & buffer : raw_image_buffers_) { + buffer.mutex->lock(); + } + + if (raw_image_buffers_.empty() || raw_image_buffers_[0].buffer.empty()) { + RCLCPP_DEBUG(this->get_logger(), "No images in buffer yet"); + for (auto & buffer : raw_image_buffers_) { + buffer.mutex->unlock(); + } + return; + } + + // Use the most recent timestamp from camera 0 as reference + sync_time_ns = raw_image_buffers_[0].buffer.rbegin()->first; + + RCLCPP_DEBUG(this->get_logger(), "Trying to sync with reference time %lu", sync_time_ns); + + // Log buffer sizes + for (size_t i = 0; i < raw_image_buffers_.size(); ++i) { + RCLCPP_DEBUG(this->get_logger(), " Buffer %zu size: %zu", i, raw_image_buffers_[i].buffer.size()); + } + + bool all_found = true; + for (size_t i = 0; i < raw_image_buffers_.size(); ++i) { + auto & buffer = raw_image_buffers_[i].buffer; + int64_t tolerance_ns = static_cast(sync_tolerance_ms_ * 1e6); + int64_t lower_bound_ns = static_cast(sync_time_ns) - tolerance_ns; + int64_t upper_bound_ns = static_cast(sync_time_ns) + tolerance_ns; + + // Find the closest timestamp within tolerance window + auto it = buffer.lower_bound(lower_bound_ns >= 0 ? static_cast(lower_bound_ns) : 0); + + ImageMsg::ConstSharedPtr best_match = nullptr; + int64_t best_time_diff = INT64_MAX; + + // Search for the closest match within the tolerance window + while (it != buffer.end() && static_cast(it->first) <= upper_bound_ns) { + int64_t time_diff_ns = std::abs(static_cast(sync_time_ns) - static_cast(it->first)); + if (time_diff_ns < best_time_diff) { + best_time_diff = time_diff_ns; + best_match = it->second; + } + ++it; + } + + if (best_match != nullptr && best_time_diff <= tolerance_ns) { + synced_images.push_back(best_match); + // Find the timestamp key for logging + for (auto & pair : buffer) { + if (pair.second == best_match) { + uint32_t sec = pair.first / 1000000000ULL; + uint32_t nanosec = pair.first % 1000000000ULL; + timestamps.push_back(rclcpp::Time(sec, nanosec)); + RCLCPP_DEBUG( + this->get_logger(), + " Camera %zu: found match (time diff: %ld ns)", + i, + static_cast(sync_time_ns) - static_cast(pair.first)); + break; + } + } + } else { + RCLCPP_DEBUG( + this->get_logger(), + " Camera %zu: no match in tolerance window (best diff: %ld ns, tolerance: %ld ns)", + i, + best_time_diff, + tolerance_ns); + all_found = false; + break; + } + } + + // Unlock all buffers + for (auto & buffer : raw_image_buffers_) { + buffer.mutex->unlock(); + } + + if (!all_found || synced_images.size() != raw_image_buffers_.size()) { + RCLCPP_DEBUG( + this->get_logger(), + "Failed to sync: all_found=%d, synced_size=%zu, total_cameras=%zu", + all_found, + synced_images.size(), + raw_image_buffers_.size()); + return; + } + + // Publish synced images + RCLCPP_DEBUG(this->get_logger(), "Publishing synced raw images (sync count: %ld)", ++sync_count_); + processSynchronizedImages(timestamps); + auto raw_msg = createMultiImageMessage(synced_images); + multi_image_raw_pub_->publish(raw_msg); +} + +void MultiCameraSyncNode::tryPublishSyncedCompressedImages() +{ + std::vector synced_images; + std::vector timestamps; + uint64_t sync_time_ns; + + // Lock all buffers + for (auto & buffer : compressed_image_buffers_) { + buffer.mutex->lock(); + } + + if (compressed_image_buffers_.empty() || compressed_image_buffers_[0].buffer.empty()) { + RCLCPP_DEBUG(this->get_logger(), "No compressed images in buffer yet"); + for (auto & buffer : compressed_image_buffers_) { + buffer.mutex->unlock(); + } + return; + } + + // Use the most recent timestamp from camera 0 as reference + sync_time_ns = compressed_image_buffers_[0].buffer.rbegin()->first; + + RCLCPP_DEBUG(this->get_logger(), "Trying to sync compressed with reference time %lu", sync_time_ns); + + // Log buffer sizes + for (size_t i = 0; i < compressed_image_buffers_.size(); ++i) { + RCLCPP_DEBUG( + this->get_logger(), " Compressed buffer %zu size: %zu", i, compressed_image_buffers_[i].buffer.size()); + } + + bool all_found = true; + for (size_t i = 0; i < compressed_image_buffers_.size(); ++i) { + auto & buffer = compressed_image_buffers_[i].buffer; + int64_t tolerance_ns = static_cast(sync_tolerance_ms_ * 1e6); + int64_t lower_bound_ns = static_cast(sync_time_ns) - tolerance_ns; + int64_t upper_bound_ns = static_cast(sync_time_ns) + tolerance_ns; + + // Find the closest timestamp within tolerance window + auto it = buffer.lower_bound(lower_bound_ns >= 0 ? static_cast(lower_bound_ns) : 0); + + CompressedImageMsg::ConstSharedPtr best_match = nullptr; + int64_t best_time_diff = INT64_MAX; + + // Search for the closest match within the tolerance window + while (it != buffer.end() && static_cast(it->first) <= upper_bound_ns) { + int64_t time_diff_ns = std::abs(static_cast(sync_time_ns) - static_cast(it->first)); + if (time_diff_ns < best_time_diff) { + best_time_diff = time_diff_ns; + best_match = it->second; + } + ++it; + } + + if (best_match != nullptr && best_time_diff <= tolerance_ns) { + synced_images.push_back(best_match); + // Find the timestamp key for logging + for (auto & pair : buffer) { + if (pair.second == best_match) { + uint32_t sec = pair.first / 1000000000ULL; + uint32_t nanosec = pair.first % 1000000000ULL; + timestamps.push_back(rclcpp::Time(sec, nanosec)); + RCLCPP_DEBUG( + this->get_logger(), + " Camera %zu: found match (time diff: %ld ns)", + i, + static_cast(sync_time_ns) - static_cast(pair.first)); + break; + } + } + } else { + RCLCPP_DEBUG( + this->get_logger(), + " Camera %zu: no match in tolerance window (best diff: %ld ns, tolerance: %ld ns)", + i, + best_time_diff, + tolerance_ns); + all_found = false; + break; + } + } + + // Unlock all buffers + for (auto & buffer : compressed_image_buffers_) { + buffer.mutex->unlock(); + } + + if (!all_found || synced_images.size() != compressed_image_buffers_.size()) { + RCLCPP_DEBUG( + this->get_logger(), + "Failed to sync compressed: all_found=%d, synced_size=%zu, total_cameras=%zu", + all_found, + synced_images.size(), + compressed_image_buffers_.size()); + return; + } + + // Publish synced images + RCLCPP_DEBUG(this->get_logger(), "Publishing synced compressed images (sync count: %ld)", ++sync_count_); + processSynchronizedImages(timestamps); + auto compressed_msg = + createMultiImageMessage(synced_images); + multi_image_compressed_pub_->publish(compressed_msg); +} + } // namespace camera_sync // Register the component