From 68e1474d5543bfdabc3369a6ae64686c40c6bee1 Mon Sep 17 00:00:00 2001 From: vividf Date: Fri, 24 Jan 2025 14:51:04 +0900 Subject: [PATCH 1/8] feat: initialize the new fusion algorithm Signed-off-by: vividf --- .../CMakeLists.txt | 2 + .../config/fusion_common.param.yaml | 10 +- .../fusion_collector.hpp | 106 +++++ .../fusion_matching_strategy.hpp | 141 ++++++ .../fusion_node.hpp | 111 ++--- .../fusion_types.hpp | 38 ++ .../pointpainting_fusion/node.hpp | 2 +- .../roi_cluster_fusion/node.hpp | 2 +- .../roi_detected_object_fusion/node.hpp | 2 +- .../roi_pointcloud_fusion/node.hpp | 2 +- .../segmentation_pointcloud_fusion/node.hpp | 2 +- .../utils/utils.hpp | 2 +- .../schema/fusion_common.schema.json | 81 +++- .../src/fusion_collector.cpp | 200 +++++++++ .../src/fusion_matching_strategy.cpp | 287 +++++++++++++ .../src/fusion_node.cpp | 404 +++++++++--------- .../src/pointpainting_fusion/node.cpp | 4 +- .../src/roi_cluster_fusion/node.cpp | 2 +- .../src/roi_detected_object_fusion/node.cpp | 2 +- .../src/roi_pointcloud_fusion/node.cpp | 2 +- .../segmentation_pointcloud_fusion/node.cpp | 2 +- .../src/utils/utils.cpp | 6 +- 22 files changed, 1113 insertions(+), 297 deletions(-) create mode 100644 perception/autoware_image_projection_based_fusion/include/autoware/image_projection_based_fusion/fusion_collector.hpp create mode 100644 perception/autoware_image_projection_based_fusion/include/autoware/image_projection_based_fusion/fusion_matching_strategy.hpp create mode 100644 perception/autoware_image_projection_based_fusion/include/autoware/image_projection_based_fusion/fusion_types.hpp create mode 100644 perception/autoware_image_projection_based_fusion/src/fusion_collector.cpp create mode 100644 perception/autoware_image_projection_based_fusion/src/fusion_matching_strategy.cpp diff --git a/perception/autoware_image_projection_based_fusion/CMakeLists.txt b/perception/autoware_image_projection_based_fusion/CMakeLists.txt index 7c8160d6fe1b7..5074741808018 100644 --- a/perception/autoware_image_projection_based_fusion/CMakeLists.txt +++ b/perception/autoware_image_projection_based_fusion/CMakeLists.txt @@ -17,6 +17,8 @@ endif() # Build non-CUDA dependent nodes ament_auto_add_library(${PROJECT_NAME} SHARED src/camera_projection.cpp + src/fusion_collector.cpp + src/fusion_matching_strategy.cpp src/fusion_node.cpp src/debugger.cpp src/utils/geometry.cpp diff --git a/perception/autoware_image_projection_based_fusion/config/fusion_common.param.yaml b/perception/autoware_image_projection_based_fusion/config/fusion_common.param.yaml index 86db3bad4f8f8..7a18806b2626f 100644 --- a/perception/autoware_image_projection_based_fusion/config/fusion_common.param.yaml +++ b/perception/autoware_image_projection_based_fusion/config/fusion_common.param.yaml @@ -1,8 +1,8 @@ /**: ros__parameters: - input_offset_ms: [61.67, 111.67, 45.0, 28.33, 78.33, 95.0] - timeout_ms: 70.0 - match_threshold_ms: 50.0 + rois_timestamp_offsets: [0.06167, 0.011167, 0.045, 0.02833, 0.07833, 0.095] + timeout_sec: 0.070 + # match_threshold_ms: 50.0 image_buffer_size: 15 # projection setting for each ROI whether unrectify image point_project_to_unrectified_image: [false, false, false, false, false, false] @@ -22,3 +22,7 @@ # debug parameters publish_processing_time_detail: false + matching_strategy: + type: advanced + det3d_noise_window: 0.01 + rois_timestamp_noise_window: [0.005, 0.005, 0.005, 0.005, 0.005, 0.005] diff --git a/perception/autoware_image_projection_based_fusion/include/autoware/image_projection_based_fusion/fusion_collector.hpp b/perception/autoware_image_projection_based_fusion/include/autoware/image_projection_based_fusion/fusion_collector.hpp new file mode 100644 index 0000000000000..b00d605f2f5a8 --- /dev/null +++ b/perception/autoware_image_projection_based_fusion/include/autoware/image_projection_based_fusion/fusion_collector.hpp @@ -0,0 +1,106 @@ +// Copyright 2024 TIER IV, Inc. +// +// 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 "autoware/image_projection_based_fusion/camera_projection.hpp" + +#include + +#include +#include +#include +#include +#include + +namespace autoware::image_projection_based_fusion +{ +using autoware::image_projection_based_fusion::CameraProjection; + +template +class FusionNode; + +template +struct Det2dStatus +{ + // camera index + std::size_t id = 0; + // camera projection + std::unique_ptr camera_projector_ptr; + bool project_to_unrectified_image = false; + bool approximate_camera_projection = false; +}; + +struct FusionCollectorInfoBase +{ + virtual ~FusionCollectorInfoBase() = default; +}; + +struct NaiveCollectorInfo : public FusionCollectorInfoBase +{ + double timestamp; + double threshold; + + explicit NaiveCollectorInfo(double timestamp = 0.0, double threshold = 0.0) + : timestamp(timestamp), threshold(threshold) + { + } +}; + +struct AdvancedCollectorInfo : public FusionCollectorInfoBase +{ + double timestamp; + double noise_window; + + explicit AdvancedCollectorInfo(double timestamp = 0.0, double noise_window = 0.0) + : timestamp(timestamp), noise_window(noise_window) + { + } +}; + +template +class FusionCollector +{ +public: + FusionCollector( + std::shared_ptr> && ros2_parent_node, double timeout_sec, + bool debug_mode); + bool process_msg_3d(const typename Msg3D::ConstSharedPtr msg_3d); + bool process_rois(const std::size_t & roi_id, const typename Msg2D::ConstSharedPtr det2d_msg); + void fusion_callback(); + + [[nodiscard]] bool fusion_finished() const; + + void set_info(std::shared_ptr collector_info); + [[nodiscard]] std::shared_ptr get_info() const; + // void show_debug_message(); + bool ready_to_fuse(); + bool rois_exists(const std::size_t & rois_id); + bool det3d_exists(); + +private: + std::shared_ptr> ros2_parent_node_; + rclcpp::TimerBase::SharedPtr timer_; + typename Msg3D::ConstSharedPtr det3d_msg_{nullptr}; + std::vector> det2d_list_; + std::unordered_map id_to_roi_map_; + double timeout_sec_; + double rois_number_; + bool debug_mode_; + bool fusion_finished_{false}; + std::mutex fusion_mutex_; + std::shared_ptr fusion_collector_info_; +}; + +} // namespace autoware::image_projection_based_fusion diff --git a/perception/autoware_image_projection_based_fusion/include/autoware/image_projection_based_fusion/fusion_matching_strategy.hpp b/perception/autoware_image_projection_based_fusion/include/autoware/image_projection_based_fusion/fusion_matching_strategy.hpp new file mode 100644 index 0000000000000..296f543e79356 --- /dev/null +++ b/perception/autoware_image_projection_based_fusion/include/autoware/image_projection_based_fusion/fusion_matching_strategy.hpp @@ -0,0 +1,141 @@ +// Copyright 2025 TIER IV, Inc. +// +// 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 "autoware/image_projection_based_fusion/fusion_collector.hpp" + +#include + +#include +#include +#include +#include +#include +#include + +namespace autoware::image_projection_based_fusion +{ + +template +class FusionNode; + +struct MatchingParamsBase +{ + virtual ~MatchingParamsBase() = default; +}; + +struct Det3dMatchingParams : public MatchingParamsBase +{ + double det3d_timestamp; + + explicit Det3dMatchingParams(double det3d_timestamp = 0.0) : det3d_timestamp(det3d_timestamp) {} +}; + +struct RoisMatchingParams : public MatchingParamsBase +{ + double rois_timestamp; + std::size_t rois_id; + + explicit RoisMatchingParams(double rois_timestamp = 0.0, std::size_t rois_id = 0) + : rois_timestamp(rois_timestamp), rois_id(rois_id) + { + } +}; + +template +class FusionMatchingStrategy +{ +public: + virtual ~FusionMatchingStrategy() = default; + + [[nodiscard]] virtual std::optional>> + match_rois_to_collector( + const std::list>> & fusion_collectors, + const std::shared_ptr & params) const = 0; + + [[nodiscard]] virtual std::optional>> + match_det3d_to_collector( + const std::list>> & fusion_collectors, + const std::shared_ptr & params) const = 0; + virtual void set_collector_info( + std::shared_ptr> & collector, + const std::shared_ptr & matching_params) = 0; +}; + +template +class NaiveMatchingStrategy : public FusionMatchingStrategy +{ +public: + explicit NaiveMatchingStrategy( + std::shared_ptr> && ros2_parent_node, + std::size_t rois_number); + + [[nodiscard]] std::optional>> + match_rois_to_collector( + const std::list>> & fusion_collectors, + const std::shared_ptr & params) const override; + + [[nodiscard]] std::optional>> + match_det3d_to_collector( + const std::list>> & fusion_collectors, + const std::shared_ptr & params) const override; + + void set_collector_info( + std::shared_ptr> & collector, + const std::shared_ptr & matching_params) override; + +private: + std::unordered_map id_to_offset_map_; + double threshold_; + std::shared_ptr> ros2_parent_node_; +}; + +template +class AdvancedMatchingStrategy : public FusionMatchingStrategy +{ +public: + explicit AdvancedMatchingStrategy( + std::shared_ptr> && ros2_parent_node, + std::size_t rois_number); + + [[nodiscard]] std::optional>> + match_rois_to_collector( + const std::list>> & fusion_collectors, + const std::shared_ptr & params) const override; + + [[nodiscard]] std::optional>> + match_det3d_to_collector( + const std::list>> & fusion_collectors, + const std::shared_ptr & params) const override; + void set_collector_info( + std::shared_ptr> & collector, + const std::shared_ptr & matching_params) override; + + double get_offset( + const double & det3d_timestamp, + const std::optional> & concatenated_status) const; + +private: + std::unordered_map id_to_offset_map_; + std::unordered_map id_to_noise_window_map_; + double det3d_noise_window_; + std::shared_ptr> ros2_parent_node_; +}; + +template +std::shared_ptr> parse_matching_strategy( + rclcpp::Node & node); + +} // namespace autoware::image_projection_based_fusion diff --git a/perception/autoware_image_projection_based_fusion/include/autoware/image_projection_based_fusion/fusion_node.hpp b/perception/autoware_image_projection_based_fusion/include/autoware/image_projection_based_fusion/fusion_node.hpp index 964974b96e73d..f4a0426195b77 100644 --- a/perception/autoware_image_projection_based_fusion/include/autoware/image_projection_based_fusion/fusion_node.hpp +++ b/perception/autoware_image_projection_based_fusion/include/autoware/image_projection_based_fusion/fusion_node.hpp @@ -1,4 +1,4 @@ -// Copyright 2022 TIER IV, Inc. +// Copyright 2025 TIER IV, Inc. // // Licensed under the Apache License, Version 2.0 (the "License"); // you may not use this file except in compliance with the License. @@ -12,8 +12,12 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef AUTOWARE__IMAGE_PROJECTION_BASED_FUSION__FUSION_NODE_HPP_ -#define AUTOWARE__IMAGE_PROJECTION_BASED_FUSION__FUSION_NODE_HPP_ +#pragma once + +#include "autoware/image_projection_based_fusion/fusion_collector.hpp" +#include "autoware/image_projection_based_fusion/fusion_matching_strategy.hpp" +// TODO(vivid): maybe we should move this to different node.hpp +#include "autoware/image_projection_based_fusion/fusion_types.hpp" #include #include @@ -23,6 +27,8 @@ #include #include +#include +#include #include #include #include @@ -38,44 +44,16 @@ #include #include -#include +#include +#include #include -#include +#include #include -#include +#include #include namespace autoware::image_projection_based_fusion { -using autoware_perception_msgs::msg::DetectedObject; -using autoware_perception_msgs::msg::DetectedObjects; -using sensor_msgs::msg::CameraInfo; -using sensor_msgs::msg::Image; -using PointCloudMsgType = sensor_msgs::msg::PointCloud2; -using RoiMsgType = tier4_perception_msgs::msg::DetectedObjectsWithFeature; -using ClusterMsgType = tier4_perception_msgs::msg::DetectedObjectsWithFeature; -using ClusterObjType = tier4_perception_msgs::msg::DetectedObjectWithFeature; -using tier4_perception_msgs::msg::DetectedObjectWithFeature; -using PointCloud = pcl::PointCloud; -using autoware::image_projection_based_fusion::CameraProjection; -using autoware_perception_msgs::msg::ObjectClassification; - -template -struct Det2dStatus -{ - // camera index - std::size_t id = 0; - // camera projection - std::unique_ptr camera_projector_ptr; - bool project_to_unrectified_image = false; - bool approximate_camera_projection = false; - // process flags - bool is_fused = false; - // timing - double input_offset_ms = 0.0; - // cache - std::map cached_det2d_msgs; -}; template class FusionNode : public rclcpp::Node @@ -89,57 +67,57 @@ class FusionNode : public rclcpp::Node explicit FusionNode( const std::string & node_name, const rclcpp::NodeOptions & options, int queue_size); + virtual void preprocess(Msg3D & output_msg); + virtual void fuse_on_single_image( + const Msg3D & input_msg, const Det2dStatus & det2d, const Msg2D & input_roi_msg, + Msg3D & output_msg) = 0; + void export_process(typename Msg3D::SharedPtr & output_det3d_msg); + std::optional> find_concatenation_status( + double timestamp); + private: // Common process methods - void cameraInfoCallback( + void camera_info_callback( const sensor_msgs::msg::CameraInfo::ConstSharedPtr input_camera_info_msg, const std::size_t camera_id); - // callback for timer - void timer_callback(); - void setPeriod(const int64_t new_period); - void exportProcess(); - - // 2d detection management methods - bool checkAllDet2dFused() - { - for (const auto & det2d : det2d_list_) { - if (!det2d.is_fused) { - return false; - } - } - return true; - } + void manage_collector_list(); + void manage_concatenated_status_map(const double & current_timestam); // camera projection float approx_grid_cell_w_size_; float approx_grid_cell_h_size_; + bool debug_mode_{false}; + // timer - rclcpp::TimerBase::SharedPtr timer_; - double timeout_ms_{}; - double match_threshold_ms_{}; + double timeout_sec_{}; std::vector::SharedPtr> rois_subs_; std::vector::SharedPtr> camera_info_subs_; - // cache for fusion - int64_t cached_det3d_msg_timestamp_; - typename Msg3D::SharedPtr cached_det3d_msg_ptr_; + std::unique_ptr> fusion_matching_strategy_; + std::mutex fusion_collectors_mutex_; + std::list>> fusion_collectors_; + + std::unordered_map id_to_noise_window_map_; + std::unordered_map id_to_offset_map_; + double cloud_noise_window_; + + // timestamp: (key, value) + std::unordered_map> concatenated_status_map_; protected: - void setDet2DStatus(std::size_t rois_number); + void set_det2d_status(std::size_t rois_number); // callback for main subscription - void subCallback(const typename Msg3D::ConstSharedPtr input_msg); + void sub_callback(const typename Msg3D::ConstSharedPtr det3d_msg); // callback for roi subscription - void roiCallback(const typename Msg2D::ConstSharedPtr input_roi_msg, const std::size_t roi_i); + void roi_callback(const typename Msg2D::ConstSharedPtr det2d_msg, const std::size_t roi_i); + + void diagnostic_callback(const diagnostic_msgs::msg::DiagnosticArray::SharedPtr diagnostic_msg); // Custom process methods - virtual void preprocess(Msg3D & output_msg); - virtual void fuseOnSingleImage( - const Msg3D & input_msg, const Det2dStatus & det2d, const Msg2D & input_roi_msg, - Msg3D & output_msg) = 0; virtual void postprocess(const Msg3D & processing_msg, ExportObj & output_msg); virtual void publish(const ExportObj & output_msg); @@ -152,6 +130,7 @@ class FusionNode : public rclcpp::Node // 3d detection subscription typename rclcpp::Subscription::SharedPtr det3d_sub_; + rclcpp::Subscription::SharedPtr sub_diag_; // parameters for out_of_scope filter float filter_scope_min_x_; @@ -161,6 +140,8 @@ class FusionNode : public rclcpp::Node float filter_scope_min_z_; float filter_scope_max_z_; + std::string matching_strategy_; + // output publisher typename rclcpp::Publisher::SharedPtr pub_ptr_; @@ -179,5 +160,3 @@ class FusionNode : public rclcpp::Node }; } // namespace autoware::image_projection_based_fusion - -#endif // AUTOWARE__IMAGE_PROJECTION_BASED_FUSION__FUSION_NODE_HPP_ diff --git a/perception/autoware_image_projection_based_fusion/include/autoware/image_projection_based_fusion/fusion_types.hpp b/perception/autoware_image_projection_based_fusion/include/autoware/image_projection_based_fusion/fusion_types.hpp new file mode 100644 index 0000000000000..e43e38061dee5 --- /dev/null +++ b/perception/autoware_image_projection_based_fusion/include/autoware/image_projection_based_fusion/fusion_types.hpp @@ -0,0 +1,38 @@ +// Copyright 2025 TIER IV, Inc. +// +// 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 +#include +#include +#include + +#include +#include + +namespace autoware::image_projection_based_fusion +{ +using autoware_perception_msgs::msg::DetectedObject; +using autoware_perception_msgs::msg::DetectedObjects; +using sensor_msgs::msg::CameraInfo; +using sensor_msgs::msg::Image; +using PointCloudMsgType = sensor_msgs::msg::PointCloud2; +using RoiMsgType = tier4_perception_msgs::msg::DetectedObjectsWithFeature; +using ClusterMsgType = tier4_perception_msgs::msg::DetectedObjectsWithFeature; +using ClusterObjType = tier4_perception_msgs::msg::DetectedObjectWithFeature; +using tier4_perception_msgs::msg::DetectedObjectWithFeature; +using PointCloud = pcl::PointCloud; +using autoware_perception_msgs::msg::ObjectClassification; +} // namespace autoware::image_projection_based_fusion diff --git a/perception/autoware_image_projection_based_fusion/include/autoware/image_projection_based_fusion/pointpainting_fusion/node.hpp b/perception/autoware_image_projection_based_fusion/include/autoware/image_projection_based_fusion/pointpainting_fusion/node.hpp index 26b4512e345f7..7c1b183117d35 100644 --- a/perception/autoware_image_projection_based_fusion/include/autoware/image_projection_based_fusion/pointpainting_fusion/node.hpp +++ b/perception/autoware_image_projection_based_fusion/include/autoware/image_projection_based_fusion/pointpainting_fusion/node.hpp @@ -50,7 +50,7 @@ class PointPaintingFusionNode : public FusionNode & det2d, const RoiMsgType & input_roi_msg, PointCloudMsgType & painted_pointcloud_msg) override; diff --git a/perception/autoware_image_projection_based_fusion/include/autoware/image_projection_based_fusion/roi_cluster_fusion/node.hpp b/perception/autoware_image_projection_based_fusion/include/autoware/image_projection_based_fusion/roi_cluster_fusion/node.hpp index b7bf8738b68a4..b3bc8c315b390 100644 --- a/perception/autoware_image_projection_based_fusion/include/autoware/image_projection_based_fusion/roi_cluster_fusion/node.hpp +++ b/perception/autoware_image_projection_based_fusion/include/autoware/image_projection_based_fusion/roi_cluster_fusion/node.hpp @@ -32,7 +32,7 @@ class RoiClusterFusionNode : public FusionNode & det2d, const RoiMsgType & input_roi_msg, ClusterMsgType & output_cluster_msg) override; diff --git a/perception/autoware_image_projection_based_fusion/include/autoware/image_projection_based_fusion/roi_detected_object_fusion/node.hpp b/perception/autoware_image_projection_based_fusion/include/autoware/image_projection_based_fusion/roi_detected_object_fusion/node.hpp index ba197126277a0..176d723de230c 100644 --- a/perception/autoware_image_projection_based_fusion/include/autoware/image_projection_based_fusion/roi_detected_object_fusion/node.hpp +++ b/perception/autoware_image_projection_based_fusion/include/autoware/image_projection_based_fusion/roi_detected_object_fusion/node.hpp @@ -39,7 +39,7 @@ class RoiDetectedObjectFusionNode : public FusionNode & det2d, const RoiMsgType & input_roi_msg, DetectedObjects & output_object_msg) override; diff --git a/perception/autoware_image_projection_based_fusion/include/autoware/image_projection_based_fusion/roi_pointcloud_fusion/node.hpp b/perception/autoware_image_projection_based_fusion/include/autoware/image_projection_based_fusion/roi_pointcloud_fusion/node.hpp index 77a1745faa7e5..387feadeeba79 100644 --- a/perception/autoware_image_projection_based_fusion/include/autoware/image_projection_based_fusion/roi_pointcloud_fusion/node.hpp +++ b/perception/autoware_image_projection_based_fusion/include/autoware/image_projection_based_fusion/roi_pointcloud_fusion/node.hpp @@ -34,7 +34,7 @@ class RoiPointCloudFusionNode : public FusionNode::SharedPtr point_pub_ptr_; rclcpp::Publisher::SharedPtr cluster_debug_pub_; - void fuseOnSingleImage( + void fuse_on_single_image( const PointCloudMsgType & input_pointcloud_msg, const Det2dStatus & det2d, const RoiMsgType & input_roi_msg, PointCloudMsgType & output_pointcloud_msg) override; diff --git a/perception/autoware_image_projection_based_fusion/include/autoware/image_projection_based_fusion/segmentation_pointcloud_fusion/node.hpp b/perception/autoware_image_projection_based_fusion/include/autoware/image_projection_based_fusion/segmentation_pointcloud_fusion/node.hpp index 5414f98e142cd..18b5344c8e418 100644 --- a/perception/autoware_image_projection_based_fusion/include/autoware/image_projection_based_fusion/segmentation_pointcloud_fusion/node.hpp +++ b/perception/autoware_image_projection_based_fusion/include/autoware/image_projection_based_fusion/segmentation_pointcloud_fusion/node.hpp @@ -42,7 +42,7 @@ class SegmentPointCloudFusionNode : public FusionNode & det2d, const Image & input_mask, PointCloudMsgType & output_pointcloud_msg) override; diff --git a/perception/autoware_image_projection_based_fusion/include/autoware/image_projection_based_fusion/utils/utils.hpp b/perception/autoware_image_projection_based_fusion/include/autoware/image_projection_based_fusion/utils/utils.hpp index 044a71ab57533..80f98ae64cd02 100644 --- a/perception/autoware_image_projection_based_fusion/include/autoware/image_projection_based_fusion/utils/utils.hpp +++ b/perception/autoware_image_projection_based_fusion/include/autoware/image_projection_based_fusion/utils/utils.hpp @@ -63,7 +63,7 @@ struct PointData size_t orig_index; }; -bool checkCameraInfo(const sensor_msgs::msg::CameraInfo & camera_info); +bool check_camera_info(const sensor_msgs::msg::CameraInfo & camera_info); std::optional getTransformStamped( const tf2_ros::Buffer & tf_buffer, const std::string & target_frame_id, diff --git a/perception/autoware_image_projection_based_fusion/schema/fusion_common.schema.json b/perception/autoware_image_projection_based_fusion/schema/fusion_common.schema.json index 8077f3e2f3e30..4a4f69b9f4c12 100644 --- a/perception/autoware_image_projection_based_fusion/schema/fusion_common.schema.json +++ b/perception/autoware_image_projection_based_fusion/schema/fusion_common.schema.json @@ -6,24 +6,22 @@ "fusion_common": { "type": "object", "properties": { - "input_offset_ms": { + "rois_timestamp_offsets": { "type": "array", - "description": "An array of timestamp offsets for each camera [ms].", - "default": [61.67, 111.67, 45.0, 28.33, 78.33, 95.0] + "items": { + "type": "number", + "minimum": 0.0 + }, + "description": "An array of timestamp offsets for each camera [s].", + "minItems": 2, + "default": [0.06167, 0.011167, 0.045, 0.02833, 0.07833, 0.095] }, "timeout_ms": { "type": "number", - "description": "A timeout value can be assigned within a single frame [ms].", - "default": 70.0, - "minimum": 1.0, - "maximum": 100.0 - }, - "match_threshold_ms": { - "type": "number", - "description": "A maximum threshold value to synchronize RoIs from multiple cameras [ms].", - "default": 50.0, - "minimum": 0.0, - "maximum": 100.0 + "description": "A timeout value can be assigned within a single frame [s].", + "default": 0.07, + "minimum": 0.001, + "maximum": 0.1 }, "image_buffer_size": { "type": "integer", @@ -85,6 +83,61 @@ "type": "number", "description": "The height of grid cell used in approximated projection [pixel].", "default": 1.0 + }, + "matching_strategy": { + "type": "object", + "properties": { + "type": { + "type": "string", + "enum": ["naive", "advanced"], + "default": "advanced", + "description": "Set it to `advanced` if you want to use more accurate and complicated logic for matching LiDAR and camera; otherwise, set it to `naive`." + }, + "threshold": { + "type": "number", + "description": "A maximum threshold value to synchronize RoIs from multiple cameras [s].", + "default": 0.05, + "minimum": 0.0, + "maximum": 0.1 + }, + "det3d_noise_window": { + "type": "number", + "description": "det3d noise window in seconds.", + "default": 0.001, + "minimum": 0.0 + }, + "rois_timestamp_noise_window": { + "type": "array", + "items": { + "type": "number", + "minimum": 0.0 + }, + "default": [0.005, 0.005, 0.005, 0.005, 0.005, 0.005], + "minItems": 2, + "description": "List of camera timestamp noise windows in seconds. The noise values should be specified in the same order as the input_topics." + } + }, + "required": ["type"], + "dependencies": { + "type": { + "oneOf": [ + { + "properties": { "type": { "const": "naive" } }, + "required": ["threshold"], + "not": { + "required": ["det3d_noise_window", "rois_timestamp_noise_window"] + } + }, + { + "properties": { "type": { "const": "advanced" } }, + "required": ["det3d_noise_window", "rois_timestamp_noise_window"], + "not": { + "required": ["threshold"] + } + } + ] + } + } } } } diff --git a/perception/autoware_image_projection_based_fusion/src/fusion_collector.cpp b/perception/autoware_image_projection_based_fusion/src/fusion_collector.cpp new file mode 100644 index 0000000000000..a1a105a61596d --- /dev/null +++ b/perception/autoware_image_projection_based_fusion/src/fusion_collector.cpp @@ -0,0 +1,200 @@ +// Copyright 2024 TIER IV, Inc. +// +// 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. + +#include "autoware/image_projection_based_fusion/fusion_collector.hpp" + +#include "autoware/image_projection_based_fusion/fusion_node.hpp" +#include "autoware/image_projection_based_fusion/fusion_types.hpp" + +#include + +#include +#include +#include +#include + +namespace autoware::image_projection_based_fusion +{ + +template +FusionCollector::FusionCollector( + std::shared_ptr> && ros2_parent_node, double timeout_sec, + bool debug_mode) +: ros2_parent_node_(std::move(ros2_parent_node)), timeout_sec_(timeout_sec), debug_mode_(debug_mode) +{ + const auto period_ns = std::chrono::duration_cast( + std::chrono::duration(timeout_sec_)); + + timer_ = + rclcpp::create_timer(ros2_parent_node_, ros2_parent_node_->get_clock(), period_ns, [this]() { + std::lock_guard fusion_lock(fusion_mutex_); + if (fusion_finished_) return; + fusion_callback(); + }); +} + +template +void FusionCollector::set_info( + std::shared_ptr fusion_collector_info) +{ + fusion_collector_info_ = std::move(fusion_collector_info); +} + +template +std::shared_ptr FusionCollector::get_info() const +{ + return fusion_collector_info_; +} + +template +bool FusionCollector::process_msg_3d( + const typename Msg3D::ConstSharedPtr msg_3d) +{ + std::lock_guard fusion_lock(fusion_mutex_); + if (fusion_finished_) return false; + + if (det3d_msg_ != nullptr) { + RCLCPP_WARN_STREAM_THROTTLE( + ros2_parent_node_->get_logger(), *ros2_parent_node_->get_clock(), + std::chrono::milliseconds(10000).count(), + "Pointcloud already exists in the collector. Check the timestamp of the pointcloud."); + } + + det3d_msg_ = msg_3d; + if (ready_to_fuse()) { + fusion_callback(); + } + + return true; +} + +template +bool FusionCollector::process_rois( + const std::size_t & roi_id, const typename Msg2D::ConstSharedPtr det2d_msg) +{ + std::lock_guard fusion_lock(fusion_mutex_); + if (fusion_finished_) return false; + + if (id_to_roi_map_.find(roi_id) != id_to_roi_map_.end()) { + RCLCPP_WARN_STREAM_THROTTLE( + ros2_parent_node_->get_logger(), *ros2_parent_node_->get_clock(), + std::chrono::milliseconds(10000).count(), + "ROIS '" << roi_id << "' already exists in the collector. Check the timestamp of the rois."); + } + id_to_roi_map_[roi_id] = det2d_msg; + if (ready_to_fuse()) { + fusion_callback(); + } + + return true; +} + +template +bool FusionCollector::ready_to_fuse() +{ + return id_to_roi_map_.size() == rois_number_ && det3d_msg_ != nullptr; +} + +template +bool FusionCollector::fusion_finished() const +{ + return fusion_finished_; +} + +template +void FusionCollector::fusion_callback() +{ + // if (debug_mode_) { + // show_debug_message(); + // } + + // All pointcloud and rois are received or the timer has timed out, cancel the timer and fuse + // them. + timer_->cancel(); + + typename Msg3D::SharedPtr output_det3d_msg = std::make_shared(*det3d_msg_); + ros2_parent_node_->preprocess(*output_det3d_msg); + + for (const auto & [roi_id, roi] : id_to_roi_map_) { + ros2_parent_node_->fuse_on_single_image( + *det3d_msg_, det2d_list_[roi_id], *roi, *output_det3d_msg); + } + + ros2_parent_node_->export_process(output_det3d_msg); + fusion_finished_ = true; +} + +template +bool FusionCollector::rois_exists(const std::size_t & rois_id) +{ + return id_to_roi_map_.find(rois_id) != id_to_roi_map_.end(); +} + +template +bool FusionCollector::det3d_exists() +{ + return det3d_msg_ != nullptr; +} + +// void CloudCollector::show_debug_message() +// { +// auto time_until_trigger = timer_->time_until_trigger(); +// std::stringstream log_stream; +// log_stream << std::fixed << std::setprecision(6); +// log_stream << "Collector's fusion callback time: " +// << ros2_parent_node_->get_clock()->now().seconds() << " seconds\n"; + +// if (auto advanced_info = std::dynamic_pointer_cast(collector_info_)) { +// log_stream << "Advanced strategy:\n Collector's reference time min: " +// << advanced_info->timestamp - advanced_info->noise_window +// << " to max: " << advanced_info->timestamp + advanced_info->noise_window +// << " seconds\n"; +// } else if (auto naive_info = std::dynamic_pointer_cast(collector_info_)) { +// log_stream << "Naive strategy:\n Collector's timestamp: " << naive_info->timestamp +// << " seconds\n"; +// } + +// log_stream << "Time until trigger: " << (time_until_trigger.count() / 1e9) << " seconds\n"; + +// log_stream << "Pointclouds: ["; +// std::string separator = ""; +// for (const auto & [topic, cloud] : topic_to_cloud_map_) { +// log_stream << separator; +// log_stream << "[" << topic << ", " << rclcpp::Time(cloud->header.stamp).seconds() << "]"; +// separator = ", "; +// } + +// log_stream << "]\n"; + +// RCLCPP_INFO(ros2_parent_node_->get_logger(), "%s", log_stream.str().c_str()); +// } + +// Explicit instantiation for the supported types + +// pointpainting fusion +template class FusionCollector; + +// roi cluster fusion +template class FusionCollector; + +// roi detected-object fusion +template class FusionCollector; + +// roi pointcloud fusion +template class FusionCollector; + +// segment pointcloud fusion +template class FusionCollector; + +} // namespace autoware::image_projection_based_fusion diff --git a/perception/autoware_image_projection_based_fusion/src/fusion_matching_strategy.cpp b/perception/autoware_image_projection_based_fusion/src/fusion_matching_strategy.cpp new file mode 100644 index 0000000000000..25685e661fd41 --- /dev/null +++ b/perception/autoware_image_projection_based_fusion/src/fusion_matching_strategy.cpp @@ -0,0 +1,287 @@ +// Copyright 2025 TIER IV, Inc. +// +// 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. + +#include "autoware/image_projection_based_fusion/fusion_matching_strategy.hpp" + +#include "autoware/image_projection_based_fusion/fusion_collector.hpp" +#include "autoware/image_projection_based_fusion/fusion_node.hpp" +#include "autoware/image_projection_based_fusion/fusion_types.hpp" + +#include + +#include +#include +#include +#include +#include +#include +#include + +namespace autoware::image_projection_based_fusion +{ + +template +NaiveMatchingStrategy::NaiveMatchingStrategy( + std::shared_ptr> && ros2_parent_node, std::size_t rois_number) +: ros2_parent_node_(std::move(ros2_parent_node)) +{ + auto rois_timestamp_offsets = + ros2_parent_node_->template declare_parameter>("rois_timestamp_offsets"); + threshold_ = ros2_parent_node_->template declare_parameter("matching_strategy.threshold"); + + if (rois_timestamp_offsets.size() != rois_number) { + throw std::runtime_error("The number of rois does not match the number of timestamp offsets."); + } + + for (size_t i = 0; i < rois_number; i++) { + id_to_offset_map_[i] = rois_timestamp_offsets[i]; + } + + RCLCPP_INFO( + ros2_parent_node_->get_logger(), "Utilize advanced matching strategy for fusion nodes."); +} + +template +std::optional>> +NaiveMatchingStrategy::match_rois_to_collector( + const std::list>> & fusion_collectors, + const std::shared_ptr & params) const +{ + std::optional smallest_time_difference = std::nullopt; + std::shared_ptr> closest_collector = nullptr; + + for (const auto & fusion_collector : fusion_collectors) { + if (!fusion_collector->rois_exists(params->rois_id)) { + auto info = fusion_collector->get_info(); + if (auto naive_info = std::dynamic_pointer_cast(info)) { + double time_difference = std::abs(params->rois_timestamp - naive_info->timestamp); + if ( + !smallest_time_difference || + (time_difference < smallest_time_difference && time_difference < naive_info->threshold)) { + smallest_time_difference = time_difference; + closest_collector = fusion_collector; + } + } + } + } + + if (closest_collector != nullptr) { + return closest_collector; + } + return std::nullopt; +} + +template +std::optional>> +NaiveMatchingStrategy::match_det3d_to_collector( + const std::list>> & fusion_collectors, + const std::shared_ptr & params) const +{ + std::optional smallest_time_difference = std::nullopt; + std::shared_ptr> closest_collector = nullptr; + + for (const auto & fusion_collector : fusion_collectors) { + if (!fusion_collector->det3d_exists()) { + auto info = fusion_collector->get_info(); + if (auto naive_info = std::dynamic_pointer_cast(info)) { + double time_difference = std::abs(params->det3d_timestamp - naive_info->timestamp); + if ( + !smallest_time_difference || + (time_difference < smallest_time_difference && time_difference < naive_info->threshold)) { + smallest_time_difference = time_difference; + closest_collector = fusion_collector; + } + } + } + } + + if (closest_collector != nullptr) { + return closest_collector; + } + return std::nullopt; +} + +template +void NaiveMatchingStrategy::set_collector_info( + std::shared_ptr> & collector, + const std::shared_ptr & matching_params) +{ + if ( + auto det3d_matching_params = std::dynamic_pointer_cast(matching_params)) { + auto info = + std::make_shared(det3d_matching_params->det3d_timestamp, threshold_); + collector->set_info(info); + } else if ( + auto rois_matching_params = std::dynamic_pointer_cast(matching_params)) { + auto info = std::make_shared( + rois_matching_params->rois_timestamp - id_to_offset_map_.at(rois_matching_params->rois_id), + threshold_); + collector->set_info(info); + } +} + +template +AdvancedMatchingStrategy::AdvancedMatchingStrategy( + std::shared_ptr> && ros2_parent_node, std::size_t rois_number) +: ros2_parent_node_(std::move(ros2_parent_node)) +{ + auto rois_timestamp_offsets = + ros2_parent_node_->template declare_parameter>("rois_timestamp_offsets"); + det3d_noise_window_ = + ros2_parent_node_->template declare_parameter("matching_strategy.det3d_noise_window"); + auto rois_timestamp_noise_window = + ros2_parent_node_->template declare_parameter>( + "matching_strategy.rois_timestamp_noise_window"); + + if (rois_timestamp_offsets.size() != rois_number) { + throw std::runtime_error("The number of rois does not match the number of timestamp offsets."); + } + if (rois_timestamp_noise_window.size() != rois_number) { + throw std::runtime_error( + "The number of rois does not match the number of timestamp noise window."); + } + + for (size_t i = 0; i < rois_number; i++) { + id_to_offset_map_[i] = rois_timestamp_offsets[i]; + id_to_noise_window_map_[i] = rois_timestamp_noise_window[i]; + } + + RCLCPP_INFO( + ros2_parent_node_->get_logger(), "Utilize advanced matching strategy for fusion nodes."); +} + +template +std::optional>> +AdvancedMatchingStrategy::match_rois_to_collector( + const std::list>> & fusion_collectors, + const std::shared_ptr & params) const +{ + for (const auto & fusion_collector : fusion_collectors) { + auto info = fusion_collector->get_info(); + if (auto advanced_info = std::dynamic_pointer_cast(info)) { + auto reference_timestamp_min = advanced_info->timestamp - advanced_info->noise_window; + auto reference_timestamp_max = advanced_info->timestamp + advanced_info->noise_window; + double time = params->rois_timestamp - id_to_offset_map_.at(params->rois_id); + if ( + time < reference_timestamp_max + id_to_noise_window_map_.at(params->rois_id) && + time > reference_timestamp_min - id_to_noise_window_map_.at(params->rois_id)) { + return fusion_collector; + } + } + } + return std::nullopt; +} + +template +std::optional>> +AdvancedMatchingStrategy::match_det3d_to_collector( + const std::list>> & fusion_collectors, + const std::shared_ptr & params) const +{ + auto concatenated_status = ros2_parent_node_->find_concatenation_status(params->det3d_timestamp); + + // TODO(vivid): double check this logic and fix the name + double offset = get_offset(params->det3d_timestamp, concatenated_status); + + for (const auto & fusion_collector : fusion_collectors) { + auto info = fusion_collector->get_info(); + if (auto advanced_info = std::dynamic_pointer_cast(info)) { + auto reference_timestamp_min = advanced_info->timestamp - advanced_info->noise_window; + auto reference_timestamp_max = advanced_info->timestamp + advanced_info->noise_window; + if ( + params->det3d_timestamp - offset < reference_timestamp_max + det3d_noise_window_ && + params->det3d_timestamp - offset > reference_timestamp_min - det3d_noise_window_) { + return fusion_collector; + } + } + } + return std::nullopt; +} + +template +void AdvancedMatchingStrategy::set_collector_info( + std::shared_ptr> & collector, + const std::shared_ptr & matching_params) +{ + if ( + auto det3d_matching_params = std::dynamic_pointer_cast(matching_params)) { + auto concatenated_status = + ros2_parent_node_->find_concatenation_status(det3d_matching_params->det3d_timestamp); + double offset = get_offset(det3d_matching_params->det3d_timestamp, concatenated_status); + + auto info = std::make_shared( + det3d_matching_params->det3d_timestamp - offset, det3d_noise_window_); + collector->set_info(info); + } else if ( + auto rois_matching_params = std::dynamic_pointer_cast(matching_params)) { + auto info = std::make_shared( + rois_matching_params->rois_timestamp - id_to_offset_map_[rois_matching_params->rois_id], + id_to_noise_window_map_[rois_matching_params->rois_id]); + collector->set_info(info); + } +} + +template +double AdvancedMatchingStrategy::get_offset( + const double & det3d_timestamp, + const std::optional> & concatenated_status) const +{ + double offset = 0.0; + if (concatenated_status) { + auto status_map = concatenated_status.value(); // Retrieve the inner map + if ( + status_map["cloud_concatenation_success"] == "False" && + det3d_timestamp > std::stod(status_map["reference_timestamp_max"])) { + // The defined earliest pointcloud is missed in the concatenation of pointcloud + offset = det3d_timestamp - (std::stod(status_map["reference_timestamp_min"]) + + (std::stod(status_map["reference_timestamp_max"]) - + std::stod(status_map["reference_timestamp_min"])) / + 2); + } + } + + return offset; +} + +// pointpainting fusion +template class NaiveMatchingStrategy; + +// roi cluster fusion +template class NaiveMatchingStrategy; + +// roi detected-object fusion +template class NaiveMatchingStrategy; + +// roi pointcloud fusion +template class NaiveMatchingStrategy; + +// segment pointcloud fusion +template class NaiveMatchingStrategy; + +// pointpainting fusion +template class AdvancedMatchingStrategy; + +// roi cluster fusion +template class AdvancedMatchingStrategy; + +// roi detected-object fusion +template class AdvancedMatchingStrategy; + +// roi pointcloud fusion +template class AdvancedMatchingStrategy; + +// segment pointcloud fusion +template class AdvancedMatchingStrategy; + +} // namespace autoware::image_projection_based_fusion diff --git a/perception/autoware_image_projection_based_fusion/src/fusion_node.cpp b/perception/autoware_image_projection_based_fusion/src/fusion_node.cpp index 53b18e20883bc..01472e960748b 100644 --- a/perception/autoware_image_projection_based_fusion/src/fusion_node.cpp +++ b/perception/autoware_image_projection_based_fusion/src/fusion_node.cpp @@ -1,4 +1,4 @@ -// Copyright 2022 TIER IV, Inc. +// Copyright 2025 TIER IV, Inc. // // Licensed under the Apache License, Version 2.0 (the "License"); // you may not use this file except in compliance with the License. @@ -16,11 +16,13 @@ #include "autoware/image_projection_based_fusion/fusion_node.hpp" +#include "autoware/image_projection_based_fusion/fusion_collector.hpp" +#include "autoware/image_projection_based_fusion/fusion_types.hpp" + #include #include #include -#include #include #include @@ -28,7 +30,9 @@ #include #include #include +#include #include +#include #include #ifdef ROS_DISTRO_GALACTIC @@ -40,7 +44,6 @@ #include #endif -// static int publish_counter = 0; static double processing_time_ms = 0; namespace autoware::image_projection_based_fusion @@ -53,8 +56,7 @@ FusionNode::FusionNode( : Node(node_name, options), tf_buffer_(this->get_clock()), tf_listener_(tf_buffer_) { // set rois_number - const std::size_t rois_number = - static_cast(declare_parameter("rois_number")); + const auto rois_number = static_cast(declare_parameter("rois_number")); if (rois_number < 1) { RCLCPP_ERROR( this->get_logger(), "minimum rois_number is 1. current rois_number is %zu", rois_number); @@ -66,8 +68,7 @@ FusionNode::FusionNode( } // Set parameters - match_threshold_ms_ = declare_parameter("match_threshold_ms"); - timeout_ms_ = declare_parameter("timeout_ms"); + timeout_sec_ = declare_parameter("timeout_sec"); std::vector input_rois_topics; std::vector input_camera_info_topics; @@ -89,7 +90,7 @@ FusionNode::FusionNode( camera_info_subs_.resize(rois_number); for (std::size_t roi_i = 0; roi_i < rois_number; ++roi_i) { std::function fnc = - std::bind(&FusionNode::cameraInfoCallback, this, std::placeholders::_1, roi_i); + std::bind(&FusionNode::camera_info_callback, this, std::placeholders::_1, roi_i); camera_info_subs_.at(roi_i) = this->create_subscription( input_camera_info_topics.at(roi_i), rclcpp::QoS{1}.best_effort(), fnc); } @@ -98,25 +99,19 @@ FusionNode::FusionNode( rois_subs_.resize(rois_number); for (std::size_t roi_i = 0; roi_i < rois_number; ++roi_i) { std::function roi_callback = - std::bind(&FusionNode::roiCallback, this, std::placeholders::_1, roi_i); + std::bind(&FusionNode::roi_callback, this, std::placeholders::_1, roi_i); rois_subs_.at(roi_i) = this->create_subscription( input_rois_topics.at(roi_i), rclcpp::QoS{1}.best_effort(), roi_callback); } // subscribe 3d detection std::function sub_callback = - std::bind(&FusionNode::subCallback, this, std::placeholders::_1); + std::bind(&FusionNode::sub_callback, this, std::placeholders::_1); det3d_sub_ = this->create_subscription("input", rclcpp::QoS(1).best_effort(), sub_callback); - // Set timer - const auto period_ns = std::chrono::duration_cast( - std::chrono::duration(timeout_ms_)); - timer_ = rclcpp::create_timer( - this, get_clock(), period_ns, std::bind(&FusionNode::timer_callback, this)); - // initialization on each 2d detections - setDet2DStatus(rois_number); + set_det2d_status(rois_number); // parameters for approximation grid approx_grid_cell_w_size_ = declare_parameter("approximation_grid_cell_width"); @@ -130,8 +125,23 @@ FusionNode::FusionNode( filter_scope_min_z_ = declare_parameter("filter_scope_min_z"); filter_scope_max_z_ = declare_parameter("filter_scope_max_z"); + matching_strategy_ = declare_parameter("matching_strategy.type"); + if (matching_strategy_ == "naive") { + fusion_matching_strategy_ = std::make_unique>( + std::dynamic_pointer_cast(shared_from_this()), rois_number); + } else if (matching_strategy_ == "advanced") { + fusion_matching_strategy_ = std::make_unique>( + std::dynamic_pointer_cast(shared_from_this()), rois_number); + // subscribe diagnostics + sub_diag_ = this->create_subscription( + "/diagnostics", 10, std::bind(&FusionNode::diagnostic_callback, this, std::placeholders::_1)); + } else { + throw std::runtime_error("Matching strategy must be 'advanced' or 'naive'"); + } + // debugger - if (declare_parameter("debug_mode", false)) { + debug_mode_ = declare_parameter("debug_mode"); + if (debug_mode_) { std::vector input_camera_topics; input_camera_topics.resize(rois_number); for (std::size_t roi_i = 0; roi_i < rois_number; ++roi_i) { @@ -139,7 +149,7 @@ FusionNode::FusionNode( "input/image" + std::to_string(roi_i), "/sensing/camera/camera" + std::to_string(roi_i) + "/image_rect_color"); } - std::size_t image_buffer_size = + auto image_buffer_size = static_cast(declare_parameter("image_buffer_size")); debugger_ = std::make_shared(this, rois_number, image_buffer_size, input_camera_topics); @@ -170,7 +180,7 @@ FusionNode::FusionNode( } template -void FusionNode::setDet2DStatus(std::size_t rois_number) +void FusionNode::set_det2d_status(std::size_t rois_number) { // camera offset settings std::vector input_offset_ms = declare_parameter>("input_offset_ms"); @@ -211,19 +221,19 @@ void FusionNode::setDet2DStatus(std::size_t rois_number det2d_list_.at(roi_i).project_to_unrectified_image = point_project_to_unrectified_image.at(roi_i); det2d_list_.at(roi_i).approximate_camera_projection = approx_camera_projection.at(roi_i); - det2d_list_.at(roi_i).input_offset_ms = input_offset_ms.at(roi_i); + // det2d_list_.at(roi_i).input_offset_ms = input_offset_ms.at(roi_i); } } template -void FusionNode::cameraInfoCallback( +void FusionNode::camera_info_callback( const sensor_msgs::msg::CameraInfo::ConstSharedPtr input_camera_info_msg, const std::size_t camera_id) { // create the CameraProjection when the camera info arrives for the first time // assuming the camera info does not change while the node is running auto & det2d = det2d_list_.at(camera_id); - if (!det2d.camera_projector_ptr && checkCameraInfo(*input_camera_info_msg)) { + if (!det2d.camera_projector_ptr && check_camera_info(*input_camera_info_msg)) { det2d.camera_projector_ptr = std::make_unique( *input_camera_info_msg, approx_grid_cell_w_size_, approx_grid_cell_h_size_, det2d.project_to_unrectified_image, det2d.approximate_camera_projection); @@ -232,18 +242,17 @@ void FusionNode::cameraInfoCallback( } template -void FusionNode::preprocess(Msg3D & ouput_msg __attribute__((unused))) +void FusionNode::preprocess(Msg3D & output_msg __attribute__((unused))) { // do nothing by default } template -void FusionNode::exportProcess() +void FusionNode::export_process( + typename Msg3D::SharedPtr & output_det3d_msg) { - timer_->cancel(); - ExportObj output_msg; - postprocess(*(cached_det3d_msg_ptr_), output_msg); + postprocess(*(output_det3d_msg), output_msg); publish(output_msg); // add processing time for debug @@ -252,7 +261,7 @@ void FusionNode::exportProcess() const double pipeline_latency_ms = std::chrono::duration( std::chrono::nanoseconds( - (this->get_clock()->now() - cached_det3d_msg_ptr_->header.stamp).nanoseconds())) + (this->get_clock()->now() - output_det3d_msg->header.stamp).nanoseconds())) .count(); debug_publisher_->publish( "debug/cyclic_time_ms", cyclic_time_ms); @@ -263,126 +272,77 @@ void FusionNode::exportProcess() "debug/pipeline_latency_ms", pipeline_latency_ms); processing_time_ms = 0; } - cached_det3d_msg_ptr_ = nullptr; } template -void FusionNode::subCallback( +void FusionNode::sub_callback( const typename Msg3D::ConstSharedPtr det3d_msg) { std::unique_ptr st_ptr; if (time_keeper_) st_ptr = std::make_unique(__func__, *time_keeper_); + stop_watch_ptr_->toc("processing_time", true); - if (cached_det3d_msg_ptr_ != nullptr) { - // PROCESS: if the main message is remained (and roi is not collected all) publish the main - // message may processed partially with arrived 2d rois - stop_watch_ptr_->toc("processing_time", true); - exportProcess(); + manage_collector_list(); + // protect fusion collectors list + std::unique_lock fusion_collectors_lock(fusion_collectors_mutex_); - // reset flags - for (auto & det2d : det2d_list_) { - det2d.is_fused = false; - } - } + auto det3d_timestamp = rclcpp::Time(det3d_msg->header.stamp).seconds(); + // For each callback, check whether there is a exist collector that matches this cloud + std::optional>> fusion_collector = + std::nullopt; + std::shared_ptr matching_params; + matching_params->det3d_timestamp = det3d_timestamp; - // TIMING: reset timer to the timeout time - auto period = std::chrono::duration_cast( - std::chrono::duration(timeout_ms_)); - try { - setPeriod(period.count()); - } catch (rclcpp::exceptions::RCLError & ex) { - RCLCPP_WARN_THROTTLE(get_logger(), *get_clock(), 5000, "%s", ex.what()); + if (!fusion_collectors_.empty()) { + fusion_collector = + fusion_matching_strategy_->match_det3d_to_collector(fusion_collectors_, matching_params); } - timer_->reset(); - - stop_watch_ptr_->toc("processing_time", true); - // PROCESS: preprocess the main message - typename Msg3D::SharedPtr output_msg = std::make_shared(*det3d_msg); - preprocess(*output_msg); - - // PROCESS: fuse the main message with the cached roi messages - // (please ask maintainers before parallelize this loop because debugger is not thread safe) - int64_t timestamp_nsec = - (*output_msg).header.stamp.sec * static_cast(1e9) + (*output_msg).header.stamp.nanosec; - // for loop for each roi - for (auto & det2d : det2d_list_) { - const auto roi_i = det2d.id; - - // check camera info - if (det2d.camera_projector_ptr == nullptr) { - RCLCPP_WARN_THROTTLE( - this->get_logger(), *this->get_clock(), 5000, "no camera info. id is %zu", roi_i); - continue; - } - auto & det2d_msgs = det2d.cached_det2d_msgs; - - // check if the roi is collected - if (det2d_msgs.size() == 0) continue; - - // MATCH: get the closest roi message, and remove outdated messages - int64_t min_interval = 1e9; - int64_t matched_stamp = -1; - std::list outdate_stamps; - for (const auto & [roi_stamp, value] : det2d_msgs) { - int64_t new_stamp = timestamp_nsec + det2d.input_offset_ms * static_cast(1e6); - int64_t interval = abs(static_cast(roi_stamp) - new_stamp); - - if (interval <= min_interval && interval <= match_threshold_ms_ * static_cast(1e6)) { - min_interval = interval; - matched_stamp = roi_stamp; - } else if ( - static_cast(roi_stamp) < new_stamp && - interval > match_threshold_ms_ * static_cast(1e6)) { - outdate_stamps.push_back(static_cast(roi_stamp)); - } - } - for (auto stamp : outdate_stamps) { - det2d_msgs.erase(stamp); + bool process_success = false; + if (fusion_collector.has_value()) { + auto collector = fusion_collector.value(); + if (collector) { + fusion_collectors_lock.unlock(); + process_success = fusion_collector.value()->process_msg_3d(det3d_msg); } + } - // PROCESS: if matched, fuse the main message with the roi message - if (matched_stamp != -1) { - if (debugger_) { - debugger_->clear(); - } + if (!process_success) { + auto new_fusion_collector = std::make_shared>( + std::dynamic_pointer_cast(shared_from_this()), timeout_sec_, debug_mode_); - fuseOnSingleImage(*det3d_msg, det2d, *(det2d_msgs[matched_stamp]), *output_msg); - det2d_msgs.erase(matched_stamp); - det2d.is_fused = true; - - // add timestamp interval for debug - if (debug_internal_pub_) { - double timestamp_interval_ms = (matched_stamp - timestamp_nsec) / 1e6; - debug_internal_pub_->publish( - "debug/roi" + std::to_string(roi_i) + "/timestamp_interval_ms", timestamp_interval_ms); - debug_internal_pub_->publish( - "debug/roi" + std::to_string(roi_i) + "/timestamp_interval_offset_ms", - timestamp_interval_ms - det2d.input_offset_ms); - } - } + fusion_collectors_.push_back(new_fusion_collector); + fusion_collectors_lock.unlock(); + + fusion_matching_strategy_->set_collector_info(new_fusion_collector, matching_params); + (void)new_fusion_collector->process_msg_3d(det3d_msg); } - // PROCESS: check if the fused message is ready to publish - cached_det3d_msg_timestamp_ = timestamp_nsec; - cached_det3d_msg_ptr_ = output_msg; - if (checkAllDet2dFused()) { - // if all camera fused, postprocess and publish the main message - exportProcess(); + // TODO(vivid): check the logic of clearing debugger. + if (debugger_) { + debugger_->clear(); + } - // reset flags - for (auto & det2d : det2d_list_) { - det2d.is_fused = false; - } - } else { - // if all of rois are not collected, publish the old Msg(if exists) and cache the - // current Msg - processing_time_ms = stop_watch_ptr_->toc("processing_time", true); + if (matching_strategy_ == "advanced") { + // remove outdated messages in the concatenated map + manage_concatenated_status_map(det3d_timestamp); } + + // // add timestamp interval for debug + // if (debug_internal_pub_) { + // double timestamp_interval_ms = (matched_stamp - timestamp_nsec) / 1e6; + // debug_internal_pub_->publish( + // "debug/roi" + std::to_string(roi_i) + "/timestamp_interval_ms", timestamp_interval_ms); + // debug_internal_pub_->publish( + // "debug/roi" + std::to_string(roi_i) + "/timestamp_interval_offset_ms", + // timestamp_interval_ms - det2d.input_offset_ms); + // } + + // processing_time_ms = stop_watch_ptr_->toc("processing_time", true); } template -void FusionNode::roiCallback( +void FusionNode::roi_callback( const typename Msg2D::ConstSharedPtr det2d_msg, const std::size_t roi_i) { std::unique_ptr st_ptr; @@ -390,93 +350,114 @@ void FusionNode::roiCallback( stop_watch_ptr_->toc("processing_time", true); - auto & det2d = det2d_list_.at(roi_i); - - int64_t timestamp_nsec = - (*det2d_msg).header.stamp.sec * static_cast(1e9) + (*det2d_msg).header.stamp.nanosec; - // if cached Msg exist, try to match - if (cached_det3d_msg_ptr_ != nullptr) { - int64_t new_stamp = - cached_det3d_msg_timestamp_ + det2d.input_offset_ms * static_cast(1e6); - int64_t interval = abs(timestamp_nsec - new_stamp); - - // PROCESS: if matched, fuse the main message with the roi message - if (interval < match_threshold_ms_ * static_cast(1e6) && det2d.is_fused == false) { - // check camera info - if (det2d.camera_projector_ptr == nullptr) { - RCLCPP_WARN_THROTTLE( - this->get_logger(), *this->get_clock(), 5000, "no camera info. id is %zu", roi_i); - det2d.cached_det2d_msgs[timestamp_nsec] = det2d_msg; - return; - } + manage_collector_list(); + // protect fusion collectors list + std::unique_lock fusion_collectors_lock(fusion_collectors_mutex_); - if (debugger_) { - debugger_->clear(); - } - // PROCESS: fuse the main message with the roi message - fuseOnSingleImage(*(cached_det3d_msg_ptr_), det2d, *det2d_msg, *(cached_det3d_msg_ptr_)); - det2d.is_fused = true; - - if (debug_internal_pub_) { - double timestamp_interval_ms = (timestamp_nsec - cached_det3d_msg_timestamp_) / 1e6; - debug_internal_pub_->publish( - "debug/roi" + std::to_string(roi_i) + "/timestamp_interval_ms", timestamp_interval_ms); - debug_internal_pub_->publish( - "debug/roi" + std::to_string(roi_i) + "/timestamp_interval_offset_ms", - timestamp_interval_ms - det2d.input_offset_ms); - } + auto rois_timestamp = rclcpp::Time(det2d_msg->header.stamp).seconds(); + std::shared_ptr matching_params; + matching_params->rois_timestamp = rois_timestamp; - // PROCESS: if all camera fused, postprocess and publish the main message - if (checkAllDet2dFused()) { - exportProcess(); - // reset flags - for (auto & status : det2d_list_) { - status.is_fused = false; - } - } - processing_time_ms = processing_time_ms + stop_watch_ptr_->toc("processing_time", true); - return; + // For each callback, check whether there is a exist collector that matches this cloud + std::optional>> fusion_collector = + std::nullopt; + + if (!fusion_collectors_.empty()) { + fusion_collector = + fusion_matching_strategy_->match_rois_to_collector(fusion_collectors_, matching_params); + } + + bool process_success = false; + if (fusion_collector.has_value()) { + auto collector = fusion_collector.value(); + if (collector) { + fusion_collectors_lock.unlock(); + process_success = fusion_collector.value()->process_rois(roi_i, det2d_msg); } } - // store roi msg if not matched - det2d.cached_det2d_msgs[timestamp_nsec] = det2d_msg; -} -template -void FusionNode::timer_callback() -{ - std::unique_ptr st_ptr; - if (time_keeper_) st_ptr = std::make_unique(__func__, *time_keeper_); + if (!process_success) { + auto new_fusion_collector = std::make_shared>( + std::dynamic_pointer_cast(shared_from_this()), timeout_sec_, debug_mode_); - using std::chrono_literals::operator""ms; - timer_->cancel(); + fusion_collectors_.push_back(new_fusion_collector); + fusion_collectors_lock.unlock(); - // PROCESS: if timeout, postprocess cached msg - if (cached_det3d_msg_ptr_ != nullptr) { - stop_watch_ptr_->toc("processing_time", true); - exportProcess(); + fusion_matching_strategy_->set_collector_info(new_fusion_collector, matching_params); + (void)new_fusion_collector->process_rois(roi_i, det2d_msg); } - // reset flags whether the message is fused or not - for (auto & det2d : det2d_list_) { - det2d.is_fused = false; + if (debugger_) { + debugger_->clear(); } + + // if (debug_internal_pub_) { + // double timestamp_interval_ms = (timestamp_nsec - cached_det3d_msg_timestamp_) / 1e6; + // debug_internal_pub_->publish( + // "debug/roi" + std::to_string(roi_i) + "/timestamp_interval_ms", timestamp_interval_ms); + // debug_internal_pub_->publish( + // "debug/roi" + std::to_string(roi_i) + "/timestamp_interval_offset_ms", + // timestamp_interval_ms - det2d.input_offset_ms); + // } + + // processing_time_ms = processing_time_ms + stop_watch_ptr_->toc("processing_time", true); } template -void FusionNode::setPeriod(const int64_t new_period) +void FusionNode::diagnostic_callback( + const diagnostic_msgs::msg::DiagnosticArray::SharedPtr diagnostic_msg) { - if (!timer_) { - return; - } - int64_t old_period = 0; - rcl_ret_t ret = rcl_timer_get_period(timer_->get_timer_handle().get(), &old_period); - if (ret != RCL_RET_OK) { - rclcpp::exceptions::throw_from_rcl_error(ret, "Couldn't get old period"); + for (const auto & status : diagnostic_msg->status) { + // Filter for the concatenate_and_time_sync_node diagnostic message + if (status.name == "concatenate_and_time_sync_node: concat_status") { + RCLCPP_INFO(get_logger(), "Processing concatenation status diagnostic message..."); + + // Temporary map to hold key-value pairs for this status + std::unordered_map key_value_map; + std::optional concatenate_timestamp_opt; + + for (const auto & value : status.values) { + key_value_map[value.key] = value.value; + + // If the key is the concatenated cloud timestamp, try to parse it + if (value.key == "concatenated_cloud_timestamp") { + try { + concatenate_timestamp_opt = std::stod(value.value); + } catch (const std::exception & e) { + RCLCPP_ERROR(get_logger(), "Error parsing concatenated cloud timestamp: %s", e.what()); + } + } + } + + // Ensure a valid timestamp was parsed before storing + if (concatenate_timestamp_opt.has_value()) { + concatenated_status_map_[concatenate_timestamp_opt.value()] = key_value_map; + RCLCPP_INFO( + get_logger(), "Stored concatenation status for timestamp: %.9f", + concatenate_timestamp_opt.value()); + } else { + RCLCPP_WARN( + get_logger(), "Missing or invalid concatenated cloud timestamp, status not stored."); + } + } } - ret = rcl_timer_exchange_period(timer_->get_timer_handle().get(), new_period, &old_period); - if (ret != RCL_RET_OK) { - rclcpp::exceptions::throw_from_rcl_error(ret, "Couldn't exchange_period"); +} + +template +void FusionNode::manage_concatenated_status_map( + const double & current_timestamp) +{ + // Clean up old entries from concatenated_status_map_ + double threshold = 1.0; // second + auto it = concatenated_status_map_.begin(); + while (it != concatenated_status_map_.end()) { + if (current_timestamp - it->first > threshold) { + RCLCPP_DEBUG( + get_logger(), "Removing old concatenation status for timestamp: %.9f", it->first); + it = concatenated_status_map_.erase(it); + } else { + ++it; + } } } @@ -497,6 +478,31 @@ void FusionNode::publish(const ExportObj & output_msg) pub_ptr_->publish(output_msg); } +template +void FusionNode::manage_collector_list() +{ + std::lock_guard collectors_lock(fusion_collectors_mutex_); + + for (auto it = fusion_collectors_.begin(); it != fusion_collectors_.end();) { + if ((*it)->fusion_finished()) { + it = fusion_collectors_.erase(it); // Erase and move the iterator to the next element + } else { + ++it; // Move to the next element + } + } +} + +template +std::optional> +FusionNode::find_concatenation_status(double timestamp) +{ + auto it = concatenated_status_map_.find(timestamp); + if (it != concatenated_status_map_.end()) { + return it->second; + } + return std::nullopt; +} + // Explicit instantiation for the supported types // pointpainting fusion diff --git a/perception/autoware_image_projection_based_fusion/src/pointpainting_fusion/node.cpp b/perception/autoware_image_projection_based_fusion/src/pointpainting_fusion/node.cpp index 91142612f6e3b..ad4fccdfdd964 100644 --- a/perception/autoware_image_projection_based_fusion/src/pointpainting_fusion/node.cpp +++ b/perception/autoware_image_projection_based_fusion/src/pointpainting_fusion/node.cpp @@ -157,7 +157,7 @@ PointPaintingFusionNode::PointPaintingFusionNode(const rclcpp::NodeOptions & opt // subscriber std::function sub_callback = - std::bind(&PointPaintingFusionNode::subCallback, this, std::placeholders::_1); + std::bind(&PointPaintingFusionNode::sub_callback, this, std::placeholders::_1); det3d_sub_ = this->create_subscription( "~/input/pointcloud", rclcpp::SensorDataQoS().keep_last(3), sub_callback); @@ -256,7 +256,7 @@ void PointPaintingFusionNode::preprocess(PointCloudMsgType & painted_pointcloud_ static_cast(painted_pointcloud_msg.data.size() / painted_pointcloud_msg.height); } -void PointPaintingFusionNode::fuseOnSingleImage( +void PointPaintingFusionNode::fuse_on_single_image( __attribute__((unused)) const PointCloudMsgType & input_pointcloud_msg, const Det2dStatus & det2d, const RoiMsgType & input_roi_msg, PointCloudMsgType & painted_pointcloud_msg) diff --git a/perception/autoware_image_projection_based_fusion/src/roi_cluster_fusion/node.cpp b/perception/autoware_image_projection_based_fusion/src/roi_cluster_fusion/node.cpp index 0a9d9e3391882..143370a62b687 100644 --- a/perception/autoware_image_projection_based_fusion/src/roi_cluster_fusion/node.cpp +++ b/perception/autoware_image_projection_based_fusion/src/roi_cluster_fusion/node.cpp @@ -73,7 +73,7 @@ void RoiClusterFusionNode::preprocess(ClusterMsgType & output_cluster_msg) } } -void RoiClusterFusionNode::fuseOnSingleImage( +void RoiClusterFusionNode::fuse_on_single_image( const ClusterMsgType & input_cluster_msg, const Det2dStatus & det2d, const RoiMsgType & input_roi_msg, ClusterMsgType & output_cluster_msg) { diff --git a/perception/autoware_image_projection_based_fusion/src/roi_detected_object_fusion/node.cpp b/perception/autoware_image_projection_based_fusion/src/roi_detected_object_fusion/node.cpp index 8b6a4fd2b7d10..92be8fa20905b 100644 --- a/perception/autoware_image_projection_based_fusion/src/roi_detected_object_fusion/node.cpp +++ b/perception/autoware_image_projection_based_fusion/src/roi_detected_object_fusion/node.cpp @@ -83,7 +83,7 @@ void RoiDetectedObjectFusionNode::preprocess(DetectedObjects & output_msg) ignored_object_flags_map_.insert(std::make_pair(timestamp_nsec, ignored_object_flags)); } -void RoiDetectedObjectFusionNode::fuseOnSingleImage( +void RoiDetectedObjectFusionNode::fuse_on_single_image( const DetectedObjects & input_object_msg, const Det2dStatus & det2d, const RoiMsgType & input_roi_msg, DetectedObjects & output_object_msg __attribute__((unused))) { diff --git a/perception/autoware_image_projection_based_fusion/src/roi_pointcloud_fusion/node.cpp b/perception/autoware_image_projection_based_fusion/src/roi_pointcloud_fusion/node.cpp index ed2a0cbc5d68f..c7f1704b0de3d 100644 --- a/perception/autoware_image_projection_based_fusion/src/roi_pointcloud_fusion/node.cpp +++ b/perception/autoware_image_projection_based_fusion/src/roi_pointcloud_fusion/node.cpp @@ -49,7 +49,7 @@ RoiPointCloudFusionNode::RoiPointCloudFusionNode(const rclcpp::NodeOptions & opt cluster_debug_pub_ = this->create_publisher("debug/clusters", 1); } -void RoiPointCloudFusionNode::fuseOnSingleImage( +void RoiPointCloudFusionNode::fuse_on_single_image( const PointCloudMsgType & input_pointcloud_msg, const Det2dStatus & det2d, const RoiMsgType & input_roi_msg, __attribute__((unused)) PointCloudMsgType & output_pointcloud_msg) diff --git a/perception/autoware_image_projection_based_fusion/src/segmentation_pointcloud_fusion/node.cpp b/perception/autoware_image_projection_based_fusion/src/segmentation_pointcloud_fusion/node.cpp index 7d63cac7273c6..2dc083f6e0193 100644 --- a/perception/autoware_image_projection_based_fusion/src/segmentation_pointcloud_fusion/node.cpp +++ b/perception/autoware_image_projection_based_fusion/src/segmentation_pointcloud_fusion/node.cpp @@ -62,7 +62,7 @@ void SegmentPointCloudFusionNode::preprocess(__attribute__((unused)) return; } -void SegmentPointCloudFusionNode::fuseOnSingleImage( +void SegmentPointCloudFusionNode::fuse_on_single_image( const PointCloudMsgType & input_pointcloud_msg, const Det2dStatus & det2d, [[maybe_unused]] const Image & input_mask, __attribute__((unused)) PointCloudMsgType & output_cloud) diff --git a/perception/autoware_image_projection_based_fusion/src/utils/utils.cpp b/perception/autoware_image_projection_based_fusion/src/utils/utils.cpp index 81424d4c23a34..3bc9331c5ac97 100644 --- a/perception/autoware_image_projection_based_fusion/src/utils/utils.cpp +++ b/perception/autoware_image_projection_based_fusion/src/utils/utils.cpp @@ -22,7 +22,7 @@ namespace autoware::image_projection_based_fusion { -bool checkCameraInfo(const sensor_msgs::msg::CameraInfo & camera_info) +bool check_camera_info(const sensor_msgs::msg::CameraInfo & camera_info) { const bool is_supported_model = (camera_info.distortion_model == sensor_msgs::distortion_models::PLUMB_BOB || @@ -30,7 +30,7 @@ bool checkCameraInfo(const sensor_msgs::msg::CameraInfo & camera_info) if (!is_supported_model) { RCLCPP_ERROR_STREAM( rclcpp::get_logger("image_projection_based_fusion"), - "checkCameraInfo: Unsupported distortion model: " << camera_info.distortion_model); + "check_camera_info: Unsupported distortion model: " << camera_info.distortion_model); return false; } const bool is_supported_distortion_param = @@ -38,7 +38,7 @@ bool checkCameraInfo(const sensor_msgs::msg::CameraInfo & camera_info) if (!is_supported_distortion_param) { RCLCPP_ERROR_STREAM( rclcpp::get_logger("image_projection_based_fusion"), - "checkCameraInfo: Unsupported distortion coefficients size: " << camera_info.d.size()); + "check_camera_info: Unsupported distortion coefficients size: " << camera_info.d.size()); return false; } return true; From 1d58565d95f7285b0c7c5c165659e33bc2fbfccc Mon Sep 17 00:00:00 2001 From: vividf Date: Mon, 27 Jan 2025 19:27:34 +0900 Subject: [PATCH 2/8] chore: fix constructor Signed-off-by: vividf --- .../fusion_node.hpp | 2 + .../roi_detected_object_fusion.launch.xml | 2 +- .../src/fusion_node.cpp | 100 ++++++++++-------- 3 files changed, 60 insertions(+), 44 deletions(-) diff --git a/perception/autoware_image_projection_based_fusion/include/autoware/image_projection_based_fusion/fusion_node.hpp b/perception/autoware_image_projection_based_fusion/include/autoware/image_projection_based_fusion/fusion_node.hpp index f4a0426195b77..4e942d2e4096a 100644 --- a/perception/autoware_image_projection_based_fusion/include/autoware/image_projection_based_fusion/fusion_node.hpp +++ b/perception/autoware_image_projection_based_fusion/include/autoware/image_projection_based_fusion/fusion_node.hpp @@ -81,6 +81,7 @@ class FusionNode : public rclcpp::Node const sensor_msgs::msg::CameraInfo::ConstSharedPtr input_camera_info_msg, const std::size_t camera_id); + void init_strategy(); void manage_collector_list(); void manage_concatenated_status_map(const double & current_timestam); @@ -90,6 +91,7 @@ class FusionNode : public rclcpp::Node bool debug_mode_{false}; + std::size_t rois_number_; // timer double timeout_sec_{}; diff --git a/perception/autoware_image_projection_based_fusion/launch/roi_detected_object_fusion.launch.xml b/perception/autoware_image_projection_based_fusion/launch/roi_detected_object_fusion.launch.xml index 3a2a74e84ec1e..db463e4fb367a 100644 --- a/perception/autoware_image_projection_based_fusion/launch/roi_detected_object_fusion.launch.xml +++ b/perception/autoware_image_projection_based_fusion/launch/roi_detected_object_fusion.launch.xml @@ -1,5 +1,5 @@ - + diff --git a/perception/autoware_image_projection_based_fusion/src/fusion_node.cpp b/perception/autoware_image_projection_based_fusion/src/fusion_node.cpp index 01472e960748b..6f93422a7ce85 100644 --- a/perception/autoware_image_projection_based_fusion/src/fusion_node.cpp +++ b/perception/autoware_image_projection_based_fusion/src/fusion_node.cpp @@ -56,15 +56,15 @@ FusionNode::FusionNode( : Node(node_name, options), tf_buffer_(this->get_clock()), tf_listener_(tf_buffer_) { // set rois_number - const auto rois_number = static_cast(declare_parameter("rois_number")); - if (rois_number < 1) { + rois_number_ = static_cast(declare_parameter("rois_number")); + if (rois_number_ < 1) { RCLCPP_ERROR( - this->get_logger(), "minimum rois_number is 1. current rois_number is %zu", rois_number); + this->get_logger(), "minimum rois_number_ is 1. current rois_number_ is %zu", rois_number_); } - if (rois_number > 8) { + if (rois_number_ > 8) { RCLCPP_WARN( this->get_logger(), - "Current rois_number is %zu. Large rois number may cause performance issue.", rois_number); + "Current rois_number_ is %zu. Large rois number may cause performance issue.", rois_number_); } // Set parameters @@ -73,10 +73,10 @@ FusionNode::FusionNode( std::vector input_rois_topics; std::vector input_camera_info_topics; - input_rois_topics.resize(rois_number); - input_camera_info_topics.resize(rois_number); + input_rois_topics.resize(rois_number_); + input_camera_info_topics.resize(rois_number_); - for (std::size_t roi_i = 0; roi_i < rois_number; ++roi_i) { + for (std::size_t roi_i = 0; roi_i < rois_number_; ++roi_i) { input_rois_topics.at(roi_i) = declare_parameter( "input/rois" + std::to_string(roi_i), "/perception/object_recognition/detection/rois" + std::to_string(roi_i)); @@ -87,8 +87,8 @@ FusionNode::FusionNode( } // subscribe camera info - camera_info_subs_.resize(rois_number); - for (std::size_t roi_i = 0; roi_i < rois_number; ++roi_i) { + camera_info_subs_.resize(rois_number_); + for (std::size_t roi_i = 0; roi_i < rois_number_; ++roi_i) { std::function fnc = std::bind(&FusionNode::camera_info_callback, this, std::placeholders::_1, roi_i); camera_info_subs_.at(roi_i) = this->create_subscription( @@ -96,8 +96,8 @@ FusionNode::FusionNode( } // subscribe rois - rois_subs_.resize(rois_number); - for (std::size_t roi_i = 0; roi_i < rois_number; ++roi_i) { + rois_subs_.resize(rois_number_); + for (std::size_t roi_i = 0; roi_i < rois_number_; ++roi_i) { std::function roi_callback = std::bind(&FusionNode::roi_callback, this, std::placeholders::_1, roi_i); rois_subs_.at(roi_i) = this->create_subscription( @@ -111,7 +111,7 @@ FusionNode::FusionNode( this->create_subscription("input", rclcpp::QoS(1).best_effort(), sub_callback); // initialization on each 2d detections - set_det2d_status(rois_number); + set_det2d_status(rois_number_); // parameters for approximation grid approx_grid_cell_w_size_ = declare_parameter("approximation_grid_cell_width"); @@ -126,25 +126,13 @@ FusionNode::FusionNode( filter_scope_max_z_ = declare_parameter("filter_scope_max_z"); matching_strategy_ = declare_parameter("matching_strategy.type"); - if (matching_strategy_ == "naive") { - fusion_matching_strategy_ = std::make_unique>( - std::dynamic_pointer_cast(shared_from_this()), rois_number); - } else if (matching_strategy_ == "advanced") { - fusion_matching_strategy_ = std::make_unique>( - std::dynamic_pointer_cast(shared_from_this()), rois_number); - // subscribe diagnostics - sub_diag_ = this->create_subscription( - "/diagnostics", 10, std::bind(&FusionNode::diagnostic_callback, this, std::placeholders::_1)); - } else { - throw std::runtime_error("Matching strategy must be 'advanced' or 'naive'"); - } // debugger debug_mode_ = declare_parameter("debug_mode"); if (debug_mode_) { std::vector input_camera_topics; - input_camera_topics.resize(rois_number); - for (std::size_t roi_i = 0; roi_i < rois_number; ++roi_i) { + input_camera_topics.resize(rois_number_); + for (std::size_t roi_i = 0; roi_i < rois_number_; ++roi_i) { input_camera_topics.at(roi_i) = declare_parameter( "input/image" + std::to_string(roi_i), "/sensing/camera/camera" + std::to_string(roi_i) + "/image_rect_color"); @@ -152,7 +140,7 @@ FusionNode::FusionNode( auto image_buffer_size = static_cast(declare_parameter("image_buffer_size")); debugger_ = - std::make_shared(this, rois_number, image_buffer_size, input_camera_topics); + std::make_shared(this, rois_number_, image_buffer_size, input_camera_topics); // input topic timing publisher debug_internal_pub_ = @@ -180,43 +168,61 @@ FusionNode::FusionNode( } template -void FusionNode::set_det2d_status(std::size_t rois_number) +void FusionNode::init_strategy() { - // camera offset settings - std::vector input_offset_ms = declare_parameter>("input_offset_ms"); - if (!input_offset_ms.empty() && rois_number > input_offset_ms.size()) { - throw std::runtime_error("The number of offsets does not match the number of topics."); + if (matching_strategy_ == "naive") { + fusion_matching_strategy_ = std::make_unique>( + std::dynamic_pointer_cast(shared_from_this()), rois_number_); + } else if (matching_strategy_ == "advanced") { + fusion_matching_strategy_ = std::make_unique>( + std::dynamic_pointer_cast(shared_from_this()), rois_number_); + // subscribe diagnostics + sub_diag_ = this->create_subscription( + "/diagnostics", 10, std::bind(&FusionNode::diagnostic_callback, this, std::placeholders::_1)); + } else { + throw std::runtime_error("Matching strategy must be 'advanced' or 'naive'"); } +} + +template +void FusionNode::set_det2d_status(std::size_t rois_number_) +{ + // camera offset settings + // std::vector input_offset_ms = + // declare_parameter>("input_offset_ms"); if (!input_offset_ms.empty() && + // rois_number_ > input_offset_ms.size()) { + // throw std::runtime_error("The number of offsets does not match the number of topics."); + // } // camera projection settings std::vector point_project_to_unrectified_image = declare_parameter>("point_project_to_unrectified_image"); - if (rois_number > point_project_to_unrectified_image.size()) { + if (rois_number_ > point_project_to_unrectified_image.size()) { throw std::runtime_error( "The number of point_project_to_unrectified_image does not match the number of rois " "topics."); } std::vector approx_camera_projection = declare_parameter>("approximate_camera_projection"); - if (rois_number != approx_camera_projection.size()) { + if (rois_number_ != approx_camera_projection.size()) { const std::size_t current_size = approx_camera_projection.size(); RCLCPP_WARN( get_logger(), "The number of elements in approximate_camera_projection should be the same as in " - "rois_number. " - "It has %zu elements.", - current_size); - if (current_size < rois_number) { - approx_camera_projection.resize(rois_number); - for (std::size_t i = current_size; i < rois_number; i++) { + "rois_number_. " + "It has %zu elements. But rois_number_ is %zu", + current_size, rois_number_); + if (current_size < rois_number_) { + approx_camera_projection.resize(rois_number_); + for (std::size_t i = current_size; i < rois_number_; i++) { approx_camera_projection.at(i) = true; } } } // 2d detection status initialization - det2d_list_.resize(rois_number); - for (std::size_t roi_i = 0; roi_i < rois_number; ++roi_i) { + det2d_list_.resize(rois_number_); + for (std::size_t roi_i = 0; roi_i < rois_number_; ++roi_i) { det2d_list_.at(roi_i).id = roi_i; det2d_list_.at(roi_i).project_to_unrectified_image = point_project_to_unrectified_image.at(roi_i); @@ -278,6 +284,10 @@ template void FusionNode::sub_callback( const typename Msg3D::ConstSharedPtr det3d_msg) { + if (!fusion_matching_strategy_) { + init_strategy(); + } + std::unique_ptr st_ptr; if (time_keeper_) st_ptr = std::make_unique(__func__, *time_keeper_); stop_watch_ptr_->toc("processing_time", true); @@ -345,6 +355,10 @@ template void FusionNode::roi_callback( const typename Msg2D::ConstSharedPtr det2d_msg, const std::size_t roi_i) { + if (!fusion_matching_strategy_) { + init_strategy(); + } + std::unique_ptr st_ptr; if (time_keeper_) st_ptr = std::make_unique(__func__, *time_keeper_); From 470f5355cbc241c4b3c959ed2d85b5b83d894d60 Mon Sep 17 00:00:00 2001 From: vividf Date: Wed, 29 Jan 2025 16:56:35 +0900 Subject: [PATCH 3/8] feat: add diagnostic updater Signed-off-by: vividf --- .../fusion_collector.hpp | 7 +- .../fusion_node.hpp | 26 ++- .../package.xml | 2 + .../src/fusion_collector.cpp | 114 +++++++----- .../src/fusion_matching_strategy.cpp | 16 +- .../src/fusion_node.cpp | 167 ++++++++++++++++-- .../concatenate_and_time_sync_node.cpp | 24 ++- 7 files changed, 278 insertions(+), 78 deletions(-) diff --git a/perception/autoware_image_projection_based_fusion/include/autoware/image_projection_based_fusion/fusion_collector.hpp b/perception/autoware_image_projection_based_fusion/include/autoware/image_projection_based_fusion/fusion_collector.hpp index b00d605f2f5a8..09601707d876e 100644 --- a/perception/autoware_image_projection_based_fusion/include/autoware/image_projection_based_fusion/fusion_collector.hpp +++ b/perception/autoware_image_projection_based_fusion/include/autoware/image_projection_based_fusion/fusion_collector.hpp @@ -37,7 +37,7 @@ struct Det2dStatus // camera index std::size_t id = 0; // camera projection - std::unique_ptr camera_projector_ptr; + std::shared_ptr camera_projector_ptr; bool project_to_unrectified_image = false; bool approximate_camera_projection = false; }; @@ -75,7 +75,7 @@ class FusionCollector public: FusionCollector( std::shared_ptr> && ros2_parent_node, double timeout_sec, - bool debug_mode); + const std::vector> & det2d_list, bool debug_mode); bool process_msg_3d(const typename Msg3D::ConstSharedPtr msg_3d); bool process_rois(const std::size_t & roi_id, const typename Msg2D::ConstSharedPtr det2d_msg); void fusion_callback(); @@ -88,14 +88,15 @@ class FusionCollector bool ready_to_fuse(); bool rois_exists(const std::size_t & rois_id); bool det3d_exists(); + void show_debug_message(); private: std::shared_ptr> ros2_parent_node_; rclcpp::TimerBase::SharedPtr timer_; + double timeout_sec_; typename Msg3D::ConstSharedPtr det3d_msg_{nullptr}; std::vector> det2d_list_; std::unordered_map id_to_roi_map_; - double timeout_sec_; double rois_number_; bool debug_mode_; bool fusion_finished_{false}; diff --git a/perception/autoware_image_projection_based_fusion/include/autoware/image_projection_based_fusion/fusion_node.hpp b/perception/autoware_image_projection_based_fusion/include/autoware/image_projection_based_fusion/fusion_node.hpp index 4e942d2e4096a..168282b43ade8 100644 --- a/perception/autoware_image_projection_based_fusion/include/autoware/image_projection_based_fusion/fusion_node.hpp +++ b/perception/autoware_image_projection_based_fusion/include/autoware/image_projection_based_fusion/fusion_node.hpp @@ -24,10 +24,11 @@ #include #include #include +#include #include #include -#include +#include #include #include #include @@ -71,9 +72,15 @@ class FusionNode : public rclcpp::Node virtual void fuse_on_single_image( const Msg3D & input_msg, const Det2dStatus & det2d, const Msg2D & input_roi_msg, Msg3D & output_msg) = 0; - void export_process(typename Msg3D::SharedPtr & output_det3d_msg); + void export_process( + typename Msg3D::SharedPtr & output_det3d_msg, + std::unordered_map id_to_stamp_map, + std::shared_ptr collector_info); std::optional> find_concatenation_status( double timestamp); + void show_diagnostic_message( + std::unordered_map id_to_stamp_map, + std::shared_ptr collector_info); private: // Common process methods @@ -85,6 +92,9 @@ class FusionNode : public rclcpp::Node void manage_collector_list(); void manage_concatenated_status_map(const double & current_timestam); + static std::string format_timestamp(double timestamp); + void check_fusion_status(diagnostic_updater::DiagnosticStatusWrapper & stat); + // camera projection float approx_grid_cell_w_size_; float approx_grid_cell_h_size_; @@ -109,6 +119,18 @@ class FusionNode : public rclcpp::Node // timestamp: (key, value) std::unordered_map> concatenated_status_map_; + diagnostic_updater::Updater diagnostic_updater_{this}; + std::shared_ptr diagnostic_collector_info_; + std::unordered_map diagnostic_id_to_stamp_map_; + + double current_output_msg_timestamp_{0.0}; + double latest_output_msg_timestamp_{0.0}; + double rosbag_length_{10.0}; + bool publish_previous_but_late_output_msg_{false}; + bool drop_previous_but_late_output_msg_{false}; + bool publish_output_msg_{false}; + bool det3d_fused_{true}; + protected: void set_det2d_status(std::size_t rois_number); diff --git a/perception/autoware_image_projection_based_fusion/package.xml b/perception/autoware_image_projection_based_fusion/package.xml index fa6217f589627..0ec9cb31d0d61 100644 --- a/perception/autoware_image_projection_based_fusion/package.xml +++ b/perception/autoware_image_projection_based_fusion/package.xml @@ -24,6 +24,8 @@ autoware_point_types autoware_universe_utils cv_bridge + diagnostic_msgs + diagnostic_updater image_geometry image_transport message_filters diff --git a/perception/autoware_image_projection_based_fusion/src/fusion_collector.cpp b/perception/autoware_image_projection_based_fusion/src/fusion_collector.cpp index a1a105a61596d..fee132c2ac22a 100644 --- a/perception/autoware_image_projection_based_fusion/src/fusion_collector.cpp +++ b/perception/autoware_image_projection_based_fusion/src/fusion_collector.cpp @@ -1,4 +1,4 @@ -// Copyright 2024 TIER IV, Inc. +// Copyright 2025 TIER IV, Inc. // // Licensed under the Apache License, Version 2.0 (the "License"); // you may not use this file except in compliance with the License. @@ -23,6 +23,7 @@ #include #include #include +#include namespace autoware::image_projection_based_fusion { @@ -30,8 +31,11 @@ namespace autoware::image_projection_based_fusion template FusionCollector::FusionCollector( std::shared_ptr> && ros2_parent_node, double timeout_sec, - bool debug_mode) -: ros2_parent_node_(std::move(ros2_parent_node)), timeout_sec_(timeout_sec), debug_mode_(debug_mode) + const std::vector> & det2d_list, bool debug_mode) +: ros2_parent_node_(std::move(ros2_parent_node)), + timeout_sec_(timeout_sec), + det2d_list_(det2d_list), + debug_mode_(debug_mode) { const auto period_ns = std::chrono::duration_cast( std::chrono::duration(timeout_sec_)); @@ -115,23 +119,44 @@ bool FusionCollector::fusion_finished() const template void FusionCollector::fusion_callback() { - // if (debug_mode_) { - // show_debug_message(); - // } + if (debug_mode_) { + show_debug_message(); + } // All pointcloud and rois are received or the timer has timed out, cancel the timer and fuse // them. timer_->cancel(); + std::unordered_map id_to_stamp_map; + for (const auto & [roi_id, roi_msg] : id_to_roi_map_) { + id_to_stamp_map[roi_id] = rclcpp::Time(roi_msg->header.stamp).seconds(); + } + + if (!det3d_msg_) { + RCLCPP_WARN( + ros2_parent_node_->get_logger(), + "The Det3d message is not in the fusion collector, so the fusion process will be skipped."); + fusion_finished_ = true; + // TODO(vivid): call another functino to show the message on diagnostic. + ros2_parent_node_->show_diagnostic_message(id_to_stamp_map, fusion_collector_info_); + return; + } + typename Msg3D::SharedPtr output_det3d_msg = std::make_shared(*det3d_msg_); ros2_parent_node_->preprocess(*output_det3d_msg); - for (const auto & [roi_id, roi] : id_to_roi_map_) { + for (const auto & [roi_id, roi_msg] : id_to_roi_map_) { + if (det2d_list_[roi_id].camera_projector_ptr == nullptr) { + RCLCPP_WARN_THROTTLE( + ros2_parent_node_->get_logger(), *ros2_parent_node_->get_clock(), 5000, + "no camera info. id is %zu", roi_id); + continue; + } ros2_parent_node_->fuse_on_single_image( - *det3d_msg_, det2d_list_[roi_id], *roi, *output_det3d_msg); + *det3d_msg_, det2d_list_[roi_id], *roi_msg, *output_det3d_msg); } - ros2_parent_node_->export_process(output_det3d_msg); + ros2_parent_node_->export_process(output_det3d_msg, id_to_stamp_map, fusion_collector_info_); fusion_finished_ = true; } @@ -147,38 +172,45 @@ bool FusionCollector::det3d_exists() return det3d_msg_ != nullptr; } -// void CloudCollector::show_debug_message() -// { -// auto time_until_trigger = timer_->time_until_trigger(); -// std::stringstream log_stream; -// log_stream << std::fixed << std::setprecision(6); -// log_stream << "Collector's fusion callback time: " -// << ros2_parent_node_->get_clock()->now().seconds() << " seconds\n"; - -// if (auto advanced_info = std::dynamic_pointer_cast(collector_info_)) { -// log_stream << "Advanced strategy:\n Collector's reference time min: " -// << advanced_info->timestamp - advanced_info->noise_window -// << " to max: " << advanced_info->timestamp + advanced_info->noise_window -// << " seconds\n"; -// } else if (auto naive_info = std::dynamic_pointer_cast(collector_info_)) { -// log_stream << "Naive strategy:\n Collector's timestamp: " << naive_info->timestamp -// << " seconds\n"; -// } - -// log_stream << "Time until trigger: " << (time_until_trigger.count() / 1e9) << " seconds\n"; - -// log_stream << "Pointclouds: ["; -// std::string separator = ""; -// for (const auto & [topic, cloud] : topic_to_cloud_map_) { -// log_stream << separator; -// log_stream << "[" << topic << ", " << rclcpp::Time(cloud->header.stamp).seconds() << "]"; -// separator = ", "; -// } - -// log_stream << "]\n"; - -// RCLCPP_INFO(ros2_parent_node_->get_logger(), "%s", log_stream.str().c_str()); -// } +template +void FusionCollector::show_debug_message() +{ + auto time_until_trigger = timer_->time_until_trigger(); + std::stringstream log_stream; + log_stream << std::fixed << std::setprecision(6); + log_stream << "Collector's fusion callback time: " + << ros2_parent_node_->get_clock()->now().seconds() << " seconds\n"; + + if ( + auto advanced_info = std::dynamic_pointer_cast(fusion_collector_info_)) { + log_stream << "Advanced strategy:\n Fusion collector's reference time min: " + << advanced_info->timestamp - advanced_info->noise_window + << " to max: " << advanced_info->timestamp + advanced_info->noise_window + << " seconds\n"; + } else if ( + auto naive_info = std::dynamic_pointer_cast(fusion_collector_info_)) { + log_stream << "Naive strategy:\n Fusino collector's timestamp: " << naive_info->timestamp + << " seconds\n"; + } + + log_stream << "Time until trigger: " << (time_until_trigger.count() / 1e9) << " seconds\n"; + if (det3d_msg_) { + log_stream << "Det3d msg: [" << rclcpp::Time(det3d_msg_->header.stamp).seconds() << "]\n"; + } else { + log_stream << "Det3d msg: [Is empty]\n"; + } + log_stream << "ROIs: ["; + std::string separator = ""; + for (const auto & [id, rois] : id_to_roi_map_) { + log_stream << separator; + log_stream << "[rois " << id << ", " << rclcpp::Time(rois->header.stamp).seconds() << "]"; + separator = ", "; + } + + log_stream << "]\n"; + + RCLCPP_INFO(ros2_parent_node_->get_logger(), "%s", log_stream.str().c_str()); +} // Explicit instantiation for the supported types diff --git a/perception/autoware_image_projection_based_fusion/src/fusion_matching_strategy.cpp b/perception/autoware_image_projection_based_fusion/src/fusion_matching_strategy.cpp index 25685e661fd41..4a8c38281ee62 100644 --- a/perception/autoware_image_projection_based_fusion/src/fusion_matching_strategy.cpp +++ b/perception/autoware_image_projection_based_fusion/src/fusion_matching_strategy.cpp @@ -48,8 +48,7 @@ NaiveMatchingStrategy::NaiveMatchingStrategy( id_to_offset_map_[i] = rois_timestamp_offsets[i]; } - RCLCPP_INFO( - ros2_parent_node_->get_logger(), "Utilize advanced matching strategy for fusion nodes."); + RCLCPP_INFO(ros2_parent_node_->get_logger(), "Utilize naive matching strategy for fusion nodes."); } template @@ -67,8 +66,9 @@ NaiveMatchingStrategy::match_rois_to_collector( if (auto naive_info = std::dynamic_pointer_cast(info)) { double time_difference = std::abs(params->rois_timestamp - naive_info->timestamp); if ( - !smallest_time_difference || - (time_difference < smallest_time_difference && time_difference < naive_info->threshold)) { + !smallest_time_difference.has_value() || + (time_difference < smallest_time_difference.value() && + time_difference < naive_info->threshold)) { smallest_time_difference = time_difference; closest_collector = fusion_collector; } @@ -97,8 +97,9 @@ NaiveMatchingStrategy::match_det3d_to_collector( if (auto naive_info = std::dynamic_pointer_cast(info)) { double time_difference = std::abs(params->det3d_timestamp - naive_info->timestamp); if ( - !smallest_time_difference || - (time_difference < smallest_time_difference && time_difference < naive_info->threshold)) { + !smallest_time_difference.has_value() || + (time_difference < smallest_time_difference.value() && + time_difference < naive_info->threshold)) { smallest_time_difference = time_difference; closest_collector = fusion_collector; } @@ -226,7 +227,7 @@ void AdvancedMatchingStrategy::set_collector_info( } else if ( auto rois_matching_params = std::dynamic_pointer_cast(matching_params)) { auto info = std::make_shared( - rois_matching_params->rois_timestamp - id_to_offset_map_[rois_matching_params->rois_id], + rois_matching_params->rois_timestamp - id_to_offset_map_.at(rois_matching_params->rois_id), id_to_noise_window_map_[rois_matching_params->rois_id]); collector->set_info(info); } @@ -251,6 +252,7 @@ double AdvancedMatchingStrategy::get_offset( } } + // std::cout << "Offset: " << offset << std::endl; return offset; } diff --git a/perception/autoware_image_projection_based_fusion/src/fusion_node.cpp b/perception/autoware_image_projection_based_fusion/src/fusion_node.cpp index 6f93422a7ce85..2c672503db389 100644 --- a/perception/autoware_image_projection_based_fusion/src/fusion_node.cpp +++ b/perception/autoware_image_projection_based_fusion/src/fusion_node.cpp @@ -12,11 +12,11 @@ // See the License for the specific language governing permissions and // limitations under the License. +#include #define EIGEN_MPL2_ONLY -#include "autoware/image_projection_based_fusion/fusion_node.hpp" - #include "autoware/image_projection_based_fusion/fusion_collector.hpp" +#include "autoware/image_projection_based_fusion/fusion_node.hpp" #include "autoware/image_projection_based_fusion/fusion_types.hpp" #include @@ -33,6 +33,7 @@ #include #include #include +#include #include #ifdef ROS_DISTRO_GALACTIC @@ -125,6 +126,9 @@ FusionNode::FusionNode( filter_scope_min_z_ = declare_parameter("filter_scope_min_z"); filter_scope_max_z_ = declare_parameter("filter_scope_max_z"); + rosbag_length_ = declare_parameter("rosbag_length"); + publish_previous_but_late_output_msg_ = + declare_parameter("publish_previous_but_late_output_msg"); matching_strategy_ = declare_parameter("matching_strategy.type"); // debugger @@ -165,6 +169,10 @@ FusionNode::FusionNode( stop_watch_ptr_->tic("cyclic_time"); stop_watch_ptr_->tic("processing_time"); } + + // Diagnostic Updater + diagnostic_updater_.setHardwareID(node_name + "_checker"); + diagnostic_updater_.add(node_name + "fusion_status", this, &FusionNode::check_fusion_status); } template @@ -187,13 +195,6 @@ void FusionNode::init_strategy() template void FusionNode::set_det2d_status(std::size_t rois_number_) { - // camera offset settings - // std::vector input_offset_ms = - // declare_parameter>("input_offset_ms"); if (!input_offset_ms.empty() && - // rois_number_ > input_offset_ms.size()) { - // throw std::runtime_error("The number of offsets does not match the number of topics."); - // } - // camera projection settings std::vector point_project_to_unrectified_image = declare_parameter>("point_project_to_unrectified_image"); @@ -227,7 +228,6 @@ void FusionNode::set_det2d_status(std::size_t rois_numb det2d_list_.at(roi_i).project_to_unrectified_image = point_project_to_unrectified_image.at(roi_i); det2d_list_.at(roi_i).approximate_camera_projection = approx_camera_projection.at(roi_i); - // det2d_list_.at(roi_i).input_offset_ms = input_offset_ms.at(roi_i); } } @@ -255,11 +255,35 @@ void FusionNode::preprocess(Msg3D & output_msg __attrib template void FusionNode::export_process( - typename Msg3D::SharedPtr & output_det3d_msg) + typename Msg3D::SharedPtr & output_det3d_msg, + std::unordered_map id_to_stamp_map, + std::shared_ptr collector_info) { ExportObj output_msg; postprocess(*(output_det3d_msg), output_msg); - publish(output_msg); + + current_output_msg_timestamp_ = rclcpp::Time(output_msg.header.stamp).seconds(); + if ( + current_output_msg_timestamp_ < latest_output_msg_timestamp_ && + !publish_previous_but_late_output_msg_) { + // Publish the output msg if the rosbag replays in loop + if (latest_output_msg_timestamp_ - current_output_msg_timestamp_ > rosbag_length_) { + publish_output_msg_ = true; // Force publishing in this case + } else { + drop_previous_but_late_output_msg_ = true; // Otherwise, drop the late output msg + } + } else { + // Publish pointcloud if timestamps are valid or the condition doesn't apply + publish_output_msg_ = true; + } + + if (publish_output_msg_) { + publish(output_msg); + } + + diagnostic_collector_info_ = std::move(collector_info); + diagnostic_id_to_stamp_map_ = std::move(id_to_stamp_map); + diagnostic_updater_.force_update(); // add processing time for debug if (debug_publisher_) { @@ -300,7 +324,7 @@ void FusionNode::sub_callback( // For each callback, check whether there is a exist collector that matches this cloud std::optional>> fusion_collector = std::nullopt; - std::shared_ptr matching_params; + auto matching_params = std::make_shared(); matching_params->det3d_timestamp = det3d_timestamp; if (!fusion_collectors_.empty()) { @@ -319,7 +343,8 @@ void FusionNode::sub_callback( if (!process_success) { auto new_fusion_collector = std::make_shared>( - std::dynamic_pointer_cast(shared_from_this()), timeout_sec_, debug_mode_); + std::dynamic_pointer_cast(shared_from_this()), timeout_sec_, det2d_list_, + debug_mode_); fusion_collectors_.push_back(new_fusion_collector); fusion_collectors_lock.unlock(); @@ -369,7 +394,8 @@ void FusionNode::roi_callback( std::unique_lock fusion_collectors_lock(fusion_collectors_mutex_); auto rois_timestamp = rclcpp::Time(det2d_msg->header.stamp).seconds(); - std::shared_ptr matching_params; + auto matching_params = std::make_shared(); + matching_params->rois_id = roi_i; matching_params->rois_timestamp = rois_timestamp; // For each callback, check whether there is a exist collector that matches this cloud @@ -392,7 +418,8 @@ void FusionNode::roi_callback( if (!process_success) { auto new_fusion_collector = std::make_shared>( - std::dynamic_pointer_cast(shared_from_this()), timeout_sec_, debug_mode_); + std::dynamic_pointer_cast(shared_from_this()), timeout_sec_, det2d_list_, + debug_mode_); fusion_collectors_.push_back(new_fusion_collector); fusion_collectors_lock.unlock(); @@ -423,7 +450,7 @@ void FusionNode::diagnostic_callback( { for (const auto & status : diagnostic_msg->status) { // Filter for the concatenate_and_time_sync_node diagnostic message - if (status.name == "concatenate_and_time_sync_node: concat_status") { + if (status.name == "concatenate_data: /sensing/lidar/concatenate_data_status") { RCLCPP_INFO(get_logger(), "Processing concatenation status diagnostic message..."); // Temporary map to hold key-value pairs for this status @@ -436,6 +463,7 @@ void FusionNode::diagnostic_callback( // If the key is the concatenated cloud timestamp, try to parse it if (value.key == "concatenated_cloud_timestamp") { try { + // concatenate_timestamp_opt = std::round(std::stod(value.value) * 1000.0) / 1000.0; concatenate_timestamp_opt = std::stod(value.value); } catch (const std::exception & e) { RCLCPP_ERROR(get_logger(), "Error parsing concatenated cloud timestamp: %s", e.what()); @@ -461,6 +489,7 @@ template void FusionNode::manage_concatenated_status_map( const double & current_timestamp) { + // TOOD(vivid): there might be a better way to do this // Clean up old entries from concatenated_status_map_ double threshold = 1.0; // second auto it = concatenated_status_map_.begin(); @@ -510,13 +539,117 @@ template std::optional> FusionNode::find_concatenation_status(double timestamp) { + // timestamp = std::round(timestamp * 1000.0) / 1000.0; + // for (const auto& outer_pair : concatenated_status_map_) { + // double outer_key = outer_pair.first; + // std::cout << "Outer Key (double): " << format_timestamp(outer_key) << std::endl; + // } + auto it = concatenated_status_map_.find(timestamp); if (it != concatenated_status_map_.end()) { + // std::cout << "Found the concatenated status for timestamp: " << format_timestamp(timestamp) + // << std::endl; return it->second; } + // std::cout << "Could not find the concatenated status for timestamp: " + // << format_timestamp(timestamp) << std::endl; return std::nullopt; } +template +std::string FusionNode::format_timestamp(double timestamp) +{ + std::ostringstream oss; + oss << std::fixed << std::setprecision(9) << timestamp; + return oss.str(); +} + +template +void FusionNode::show_diagnostic_message( + std::unordered_map id_to_stamp_map, + std::shared_ptr collector_info) +{ + det3d_fused_ = false; + diagnostic_collector_info_ = std::move(collector_info); + diagnostic_id_to_stamp_map_ = std::move(id_to_stamp_map); + diagnostic_updater_.force_update(); +} + +template +void FusionNode::check_fusion_status( + diagnostic_updater::DiagnosticStatusWrapper & stat) +{ + if (publish_output_msg_ || drop_previous_but_late_output_msg_ || !det3d_fused_) { + stat.add("det3d/is_fused", det3d_fused_); + if (det3d_fused_) { + stat.add("fused_timestamp", format_timestamp(current_output_msg_timestamp_)); + } + + if ( + auto naive_info = std::dynamic_pointer_cast(diagnostic_collector_info_)) { + stat.add("first_input_arrival_timestamp", format_timestamp(naive_info->timestamp)); + } else if ( + auto advanced_info = + std::dynamic_pointer_cast(diagnostic_collector_info_)) { + stat.add( + "reference_timestamp_min", + format_timestamp(advanced_info->timestamp - advanced_info->noise_window)); + stat.add( + "reference_timestamp_max", + format_timestamp(advanced_info->timestamp + advanced_info->noise_window)); + } + + bool rois_miss = false; + + bool fusion_success = true; + for (std::size_t id = 0; id < rois_number_; ++id) { + bool input_rois_fused = true; + if (diagnostic_id_to_stamp_map_.find(id) != diagnostic_id_to_stamp_map_.end()) { + stat.add( + "rois" + std::to_string(id) + "/timestamp", + format_timestamp(diagnostic_id_to_stamp_map_[id])); + } else { + rois_miss = true; + fusion_success = false; + input_rois_fused = false; + } + stat.add("rois" + std::to_string(id) + "/is_fused", input_rois_fused); + } + + stat.add("fusion_success", fusion_success); + + int8_t level = diagnostic_msgs::msg::DiagnosticStatus::OK; + std::string message = "Fused output is published and include all rois and det3d msg"; + + if (drop_previous_but_late_output_msg_) { + if (rois_miss) { + level = diagnostic_msgs::msg::DiagnosticStatus::ERROR; + message = + "Fused output msg misses some rois and is not published because it arrived " + "too late"; + } else { + level = diagnostic_msgs::msg::DiagnosticStatus::ERROR; + message = "Fused output msg is not published as it is too late"; + } + } else { + if (rois_miss) { + level = diagnostic_msgs::msg::DiagnosticStatus::ERROR; + message = "Fused output msg is published but misses some rois"; + } + } + + stat.summary(level, message); + + publish_output_msg_ = false; + drop_previous_but_late_output_msg_ = false; + det3d_fused_ = true; + } else { + const int8_t level = diagnostic_msgs::msg::DiagnosticStatus::OK; + const std::string message = "Fusion node launch successfully, but waiting for input pointcloud"; + stat.summary(level, message); + } +} + // Explicit instantiation for the supported types // pointpainting fusion diff --git a/sensing/autoware_pointcloud_preprocessor/src/concatenate_data/concatenate_and_time_sync_node.cpp b/sensing/autoware_pointcloud_preprocessor/src/concatenate_data/concatenate_and_time_sync_node.cpp index e168781c647ed..395609d84c33d 100644 --- a/sensing/autoware_pointcloud_preprocessor/src/concatenate_data/concatenate_and_time_sync_node.cpp +++ b/sensing/autoware_pointcloud_preprocessor/src/concatenate_data/concatenate_and_time_sync_node.cpp @@ -143,9 +143,17 @@ PointCloudConcatenateDataSynchronizerComponent::PointCloudConcatenateDataSynchro params_.has_static_tf_only); // Diagnostic Updater - diagnostic_updater_.setHardwareID("concatenate_data_checker"); + std::ostringstream hardware_id_stream; + hardware_id_stream << this->get_fully_qualified_name() << "_checker"; + std::string hardware_id = hardware_id_stream.str(); + + std::ostringstream diagnositic_name_stream; + diagnositic_name_stream << this->get_fully_qualified_name() << "_status"; + std::string diagnositic_name = diagnositic_name_stream.str(); + + diagnostic_updater_.setHardwareID(hardware_id); diagnostic_updater_.add( - "concat_status", this, &PointCloudConcatenateDataSynchronizerComponent::check_concat_status); + diagnositic_name, this, &PointCloudConcatenateDataSynchronizerComponent::check_concat_status); } std::string PointCloudConcatenateDataSynchronizerComponent::replace_sync_topic_name_postfix( @@ -293,6 +301,10 @@ void PointCloudConcatenateDataSynchronizerComponent::publish_clouds( publish_pointcloud_ = true; } + diagnostic_collector_info_ = std::move(collector_info); + diagnostic_topic_to_original_stamp_map_ = concatenated_cloud_result.topic_to_original_stamp_map; + diagnostic_updater_.force_update(); + if (publish_pointcloud_) { latest_concatenate_cloud_timestamp_ = current_concatenate_cloud_timestamp_; auto concatenate_pointcloud_output = std::make_unique( @@ -321,10 +333,8 @@ void PointCloudConcatenateDataSynchronizerComponent::publish_clouds( } } - diagnostic_collector_info_ = collector_info; - - diagnostic_topic_to_original_stamp_map_ = concatenated_cloud_result.topic_to_original_stamp_map; - diagnostic_updater_.force_update(); + publish_pointcloud_ = false; + drop_previous_but_late_pointcloud_ = false; // add processing time for debug if (debug_publisher_) { @@ -429,8 +439,6 @@ void PointCloudConcatenateDataSynchronizerComponent::check_concat_status( stat.summary(level, message); - publish_pointcloud_ = false; - drop_previous_but_late_pointcloud_ = false; is_concatenated_cloud_empty_ = false; } else { const int8_t level = diagnostic_msgs::msg::DiagnosticStatus::OK; From 057e1ccd8b6951601f37954639a5be9c1e501ebf Mon Sep 17 00:00:00 2001 From: vividf Date: Thu, 30 Jan 2025 19:03:19 +0900 Subject: [PATCH 4/8] feat: add automatic offset calculation Signed-off-by: vividf --- .../fusion_matching_strategy.hpp | 22 ++-- .../launch/roi_pointcloud_fusion.launch.xml | 2 +- .../src/fusion_matching_strategy.cpp | 113 ++++++++++++++++-- .../src/fusion_node.cpp | 12 -- .../collector_matching_strategy.hpp | 2 - 5 files changed, 115 insertions(+), 36 deletions(-) diff --git a/perception/autoware_image_projection_based_fusion/include/autoware/image_projection_based_fusion/fusion_matching_strategy.hpp b/perception/autoware_image_projection_based_fusion/include/autoware/image_projection_based_fusion/fusion_matching_strategy.hpp index 296f543e79356..a8446709748a2 100644 --- a/perception/autoware_image_projection_based_fusion/include/autoware/image_projection_based_fusion/fusion_matching_strategy.hpp +++ b/perception/autoware_image_projection_based_fusion/include/autoware/image_projection_based_fusion/fusion_matching_strategy.hpp @@ -22,6 +22,7 @@ #include #include #include +#include #include #include @@ -68,7 +69,7 @@ class FusionMatchingStrategy [[nodiscard]] virtual std::optional>> match_det3d_to_collector( const std::list>> & fusion_collectors, - const std::shared_ptr & params) const = 0; + const std::shared_ptr & params) = 0; virtual void set_collector_info( std::shared_ptr> & collector, const std::shared_ptr & matching_params) = 0; @@ -90,7 +91,7 @@ class NaiveMatchingStrategy : public FusionMatchingStrategy>> match_det3d_to_collector( const std::list>> & fusion_collectors, - const std::shared_ptr & params) const override; + const std::shared_ptr & params) override; void set_collector_info( std::shared_ptr> & collector, @@ -118,24 +119,27 @@ class AdvancedMatchingStrategy : public FusionMatchingStrategy>> match_det3d_to_collector( const std::list>> & fusion_collectors, - const std::shared_ptr & params) const override; + const std::shared_ptr & params) override; void set_collector_info( std::shared_ptr> & collector, const std::shared_ptr & matching_params) override; double get_offset( const double & det3d_timestamp, - const std::optional> & concatenated_status) const; + const std::optional> & concatenated_status); + + double extract_fractional(double timestamp); + void update_fractional_timestamp_set(double new_timestamp); + double compute_offset(double input_timestamp); private: std::unordered_map id_to_offset_map_; std::unordered_map id_to_noise_window_map_; double det3d_noise_window_; std::shared_ptr> ros2_parent_node_; + std::set fractional_timestamp_set_; // Use set to store unique fractional timestamps + int success_status_counter_{0}; + static constexpr int success_threshold{100}; + bool database_created_{false}; }; - -template -std::shared_ptr> parse_matching_strategy( - rclcpp::Node & node); - } // namespace autoware::image_projection_based_fusion diff --git a/perception/autoware_image_projection_based_fusion/launch/roi_pointcloud_fusion.launch.xml b/perception/autoware_image_projection_based_fusion/launch/roi_pointcloud_fusion.launch.xml index a221af7fe44d3..4aa9595e91da1 100644 --- a/perception/autoware_image_projection_based_fusion/launch/roi_pointcloud_fusion.launch.xml +++ b/perception/autoware_image_projection_based_fusion/launch/roi_pointcloud_fusion.launch.xml @@ -16,7 +16,7 @@ - + diff --git a/perception/autoware_image_projection_based_fusion/src/fusion_matching_strategy.cpp b/perception/autoware_image_projection_based_fusion/src/fusion_matching_strategy.cpp index 4a8c38281ee62..0c7d78cc66b95 100644 --- a/perception/autoware_image_projection_based_fusion/src/fusion_matching_strategy.cpp +++ b/perception/autoware_image_projection_based_fusion/src/fusion_matching_strategy.cpp @@ -86,7 +86,7 @@ template std::optional>> NaiveMatchingStrategy::match_det3d_to_collector( const std::list>> & fusion_collectors, - const std::shared_ptr & params) const + const std::shared_ptr & params) { std::optional smallest_time_difference = std::nullopt; std::shared_ptr> closest_collector = nullptr; @@ -188,7 +188,7 @@ template std::optional>> AdvancedMatchingStrategy::match_det3d_to_collector( const std::list>> & fusion_collectors, - const std::shared_ptr & params) const + const std::shared_ptr & params) { auto concatenated_status = ros2_parent_node_->find_concatenation_status(params->det3d_timestamp); @@ -236,26 +236,115 @@ void AdvancedMatchingStrategy::set_collector_info( template double AdvancedMatchingStrategy::get_offset( const double & det3d_timestamp, - const std::optional> & concatenated_status) const + const std::optional> & concatenated_status) { double offset = 0.0; + if (concatenated_status) { - auto status_map = concatenated_status.value(); // Retrieve the inner map + const auto & status_map = concatenated_status.value(); + if ( - status_map["cloud_concatenation_success"] == "False" && - det3d_timestamp > std::stod(status_map["reference_timestamp_max"])) { - // The defined earliest pointcloud is missed in the concatenation of pointcloud - offset = det3d_timestamp - (std::stod(status_map["reference_timestamp_min"]) + - (std::stod(status_map["reference_timestamp_max"]) - - std::stod(status_map["reference_timestamp_min"])) / - 2); + status_map.find("cloud_concatenation_success") != status_map.end() && + status_map.find("reference_timestamp_min") != status_map.end() && + status_map.find("reference_timestamp_max") != status_map.end()) { + bool concatenation_success = (status_map.at("cloud_concatenation_success") == "True"); + + if ( + !concatenation_success && + det3d_timestamp > std::stod(status_map.at("reference_timestamp_max"))) { + // The defined earliest pointcloud is missed in the concatenation + double reference_min = std::stod(status_map.at("reference_timestamp_min")); + double reference_max = std::stod(status_map.at("reference_timestamp_max")); + offset = det3d_timestamp - (reference_min + (reference_max - reference_min) / 2); + } else if (!database_created_ && concatenation_success) { + // Ensure "cloud_concatenated_timestamp" key exists before accessing + if (status_map.find("concatenated_cloud_timestamp") != status_map.end()) { + double concatenated_cloud_timestamp = + std::stod(status_map.at("concatenated_cloud_timestamp")); + update_fractional_timestamp_set(concatenated_cloud_timestamp); + success_status_counter_++; + offset = 0.0; + + if (success_status_counter_ > success_threshold) { + database_created_ = true; + } + } + } + } + } else { + RCLCPP_WARN( + ros2_parent_node_->get_logger(), + "If concatenated_status is missing, find the offset using the timestamp difference"); + // If concatenated_status is missing, find the offset using the timestamp difference + if (database_created_) { + offset = compute_offset(det3d_timestamp); + RCLCPP_WARN(ros2_parent_node_->get_logger(), "Use database and find offset: %f", offset); + + } else { + offset = 0.0; } } - // std::cout << "Offset: " << offset << std::endl; return offset; } +template +double AdvancedMatchingStrategy::extract_fractional(double timestamp) +{ + return fmod(timestamp, 1.0); +} + +template +void AdvancedMatchingStrategy::update_fractional_timestamp_set( + double timestamp) +{ + double fractional_part = extract_fractional(timestamp); + + // Check if the new timestamp belongs to an existing element within noise tolerance + for (auto existing_timestamp : fractional_timestamp_set_) { + if (std::abs(fractional_part - existing_timestamp) < det3d_noise_window_ * 2) { + existing_timestamp = (existing_timestamp + fractional_part) / 2; + return; // If it belongs to an existing group, average the timestamp + } + } + + fractional_timestamp_set_.insert(fractional_part); +} + +template +double AdvancedMatchingStrategy::compute_offset(double input_timestamp) +{ + if (fractional_timestamp_set_.empty()) { + return 0.0; + } + + double fractional_part = extract_fractional(input_timestamp); + double expected_timestamp = -1.0; + + // Check if input timestamp is within an existing timestamp ± noise_tolerance + for (const auto & timestamp : fractional_timestamp_set_) { + if ( + fractional_part >= timestamp - det3d_noise_window_ && + fractional_part < timestamp + det3d_noise_window_) { + return 0.0; // If within range, offset is zero + } + } + + // Find the closest timestamp ≤ fractional_part + auto it = fractional_timestamp_set_.lower_bound(fractional_part); + if (it == fractional_timestamp_set_.end()) { + --it; + expected_timestamp = floor(input_timestamp) + *it; + } else if (it == fractional_timestamp_set_.begin()) { + // **If `new_timestamp` is smaller than all stored timestamps, use the largest timestamp** + expected_timestamp = floor(input_timestamp) - 1 + *fractional_timestamp_set_.rbegin(); + } else { + --it; + expected_timestamp = floor(input_timestamp) + *it; + } + return input_timestamp - expected_timestamp; +} + // pointpainting fusion template class NaiveMatchingStrategy; diff --git a/perception/autoware_image_projection_based_fusion/src/fusion_node.cpp b/perception/autoware_image_projection_based_fusion/src/fusion_node.cpp index 2c672503db389..f145ed25ce458 100644 --- a/perception/autoware_image_projection_based_fusion/src/fusion_node.cpp +++ b/perception/autoware_image_projection_based_fusion/src/fusion_node.cpp @@ -451,8 +451,6 @@ void FusionNode::diagnostic_callback( for (const auto & status : diagnostic_msg->status) { // Filter for the concatenate_and_time_sync_node diagnostic message if (status.name == "concatenate_data: /sensing/lidar/concatenate_data_status") { - RCLCPP_INFO(get_logger(), "Processing concatenation status diagnostic message..."); - // Temporary map to hold key-value pairs for this status std::unordered_map key_value_map; std::optional concatenate_timestamp_opt; @@ -539,20 +537,10 @@ template std::optional> FusionNode::find_concatenation_status(double timestamp) { - // timestamp = std::round(timestamp * 1000.0) / 1000.0; - // for (const auto& outer_pair : concatenated_status_map_) { - // double outer_key = outer_pair.first; - // std::cout << "Outer Key (double): " << format_timestamp(outer_key) << std::endl; - // } - auto it = concatenated_status_map_.find(timestamp); if (it != concatenated_status_map_.end()) { - // std::cout << "Found the concatenated status for timestamp: " << format_timestamp(timestamp) - // << std::endl; return it->second; } - // std::cout << "Could not find the concatenated status for timestamp: " - // << format_timestamp(timestamp) << std::endl; return std::nullopt; } diff --git a/sensing/autoware_pointcloud_preprocessor/include/autoware/pointcloud_preprocessor/concatenate_data/collector_matching_strategy.hpp b/sensing/autoware_pointcloud_preprocessor/include/autoware/pointcloud_preprocessor/concatenate_data/collector_matching_strategy.hpp index c314b24a07921..19061ed004347 100644 --- a/sensing/autoware_pointcloud_preprocessor/include/autoware/pointcloud_preprocessor/concatenate_data/collector_matching_strategy.hpp +++ b/sensing/autoware_pointcloud_preprocessor/include/autoware/pointcloud_preprocessor/concatenate_data/collector_matching_strategy.hpp @@ -76,6 +76,4 @@ class AdvancedMatchingStrategy : public CollectorMatchingStrategy std::unordered_map topic_to_noise_window_map_; }; -std::shared_ptr parse_matching_strategy(rclcpp::Node & node); - } // namespace autoware::pointcloud_preprocessor From 5280d82a758717f3e935c016db618f9614137703 Mon Sep 17 00:00:00 2001 From: vividf Date: Fri, 31 Jan 2025 14:25:31 +0900 Subject: [PATCH 5/8] chore: pass rois_number as parameters Signed-off-by: vividf --- .../fusion_collector.hpp | 4 +-- .../src/fusion_collector.cpp | 3 +- .../src/fusion_node.cpp | 35 ++++++++++++++----- 3 files changed, 31 insertions(+), 11 deletions(-) diff --git a/perception/autoware_image_projection_based_fusion/include/autoware/image_projection_based_fusion/fusion_collector.hpp b/perception/autoware_image_projection_based_fusion/include/autoware/image_projection_based_fusion/fusion_collector.hpp index 09601707d876e..e216d22b8c47c 100644 --- a/perception/autoware_image_projection_based_fusion/include/autoware/image_projection_based_fusion/fusion_collector.hpp +++ b/perception/autoware_image_projection_based_fusion/include/autoware/image_projection_based_fusion/fusion_collector.hpp @@ -75,7 +75,7 @@ class FusionCollector public: FusionCollector( std::shared_ptr> && ros2_parent_node, double timeout_sec, - const std::vector> & det2d_list, bool debug_mode); + std::size_t rois_number, const std::vector> & det2d_list, bool debug_mode); bool process_msg_3d(const typename Msg3D::ConstSharedPtr msg_3d); bool process_rois(const std::size_t & roi_id, const typename Msg2D::ConstSharedPtr det2d_msg); void fusion_callback(); @@ -94,10 +94,10 @@ class FusionCollector std::shared_ptr> ros2_parent_node_; rclcpp::TimerBase::SharedPtr timer_; double timeout_sec_; + std::size_t rois_number_; typename Msg3D::ConstSharedPtr det3d_msg_{nullptr}; std::vector> det2d_list_; std::unordered_map id_to_roi_map_; - double rois_number_; bool debug_mode_; bool fusion_finished_{false}; std::mutex fusion_mutex_; diff --git a/perception/autoware_image_projection_based_fusion/src/fusion_collector.cpp b/perception/autoware_image_projection_based_fusion/src/fusion_collector.cpp index fee132c2ac22a..1ed249986176f 100644 --- a/perception/autoware_image_projection_based_fusion/src/fusion_collector.cpp +++ b/perception/autoware_image_projection_based_fusion/src/fusion_collector.cpp @@ -31,9 +31,10 @@ namespace autoware::image_projection_based_fusion template FusionCollector::FusionCollector( std::shared_ptr> && ros2_parent_node, double timeout_sec, - const std::vector> & det2d_list, bool debug_mode) + std::size_t rois_number, const std::vector> & det2d_list, bool debug_mode) : ros2_parent_node_(std::move(ros2_parent_node)), timeout_sec_(timeout_sec), + rois_number_(rois_number), det2d_list_(det2d_list), debug_mode_(debug_mode) { diff --git a/perception/autoware_image_projection_based_fusion/src/fusion_node.cpp b/perception/autoware_image_projection_based_fusion/src/fusion_node.cpp index f145ed25ce458..28f10a32bc456 100644 --- a/perception/autoware_image_projection_based_fusion/src/fusion_node.cpp +++ b/perception/autoware_image_projection_based_fusion/src/fusion_node.cpp @@ -172,7 +172,7 @@ FusionNode::FusionNode( // Diagnostic Updater diagnostic_updater_.setHardwareID(node_name + "_checker"); - diagnostic_updater_.add(node_name + "fusion_status", this, &FusionNode::check_fusion_status); + diagnostic_updater_.add(node_name + "_status", this, &FusionNode::check_fusion_status); } template @@ -312,6 +312,14 @@ void FusionNode::sub_callback( init_strategy(); } + if (debug_mode_) { + auto arrive_time = this->get_clock()->now().seconds(); + RCLCPP_ERROR( + this->get_logger(), " det3d's timestamp: %lf arrive time: %lf seconds, latency: %lf", + rclcpp::Time(det3d_msg->header.stamp).seconds(), arrive_time, + arrive_time - rclcpp::Time(det3d_msg->header.stamp).seconds()); + } + std::unique_ptr st_ptr; if (time_keeper_) st_ptr = std::make_unique(__func__, *time_keeper_); stop_watch_ptr_->toc("processing_time", true); @@ -343,8 +351,8 @@ void FusionNode::sub_callback( if (!process_success) { auto new_fusion_collector = std::make_shared>( - std::dynamic_pointer_cast(shared_from_this()), timeout_sec_, det2d_list_, - debug_mode_); + std::dynamic_pointer_cast(shared_from_this()), timeout_sec_, rois_number_, + det2d_list_, debug_mode_); fusion_collectors_.push_back(new_fusion_collector); fusion_collectors_lock.unlock(); @@ -384,6 +392,13 @@ void FusionNode::roi_callback( init_strategy(); } + if (debug_mode_) { + auto arrive_time = this->get_clock()->now().seconds(); + RCLCPP_ERROR( + this->get_logger(), " rois %zu timestamp: %lf arrive time: %lf seconds, latency: %lf", roi_i, + rclcpp::Time(det2d_msg->header.stamp).seconds(), arrive_time, + arrive_time - rclcpp::Time(det2d_msg->header.stamp).seconds()); + } std::unique_ptr st_ptr; if (time_keeper_) st_ptr = std::make_unique(__func__, *time_keeper_); @@ -418,8 +433,8 @@ void FusionNode::roi_callback( if (!process_success) { auto new_fusion_collector = std::make_shared>( - std::dynamic_pointer_cast(shared_from_this()), timeout_sec_, det2d_list_, - debug_mode_); + std::dynamic_pointer_cast(shared_from_this()), timeout_sec_, rois_number_, + det2d_list_, debug_mode_); fusion_collectors_.push_back(new_fusion_collector); fusion_collectors_lock.unlock(); @@ -472,9 +487,9 @@ void FusionNode::diagnostic_callback( // Ensure a valid timestamp was parsed before storing if (concatenate_timestamp_opt.has_value()) { concatenated_status_map_[concatenate_timestamp_opt.value()] = key_value_map; - RCLCPP_INFO( - get_logger(), "Stored concatenation status for timestamp: %.9f", - concatenate_timestamp_opt.value()); + // RCLCPP_INFO( + // get_logger(), "Stored concatenation status for timestamp: %.9f", + // concatenate_timestamp_opt.value()); } else { RCLCPP_WARN( get_logger(), "Missing or invalid concatenated cloud timestamp, status not stored."); @@ -590,6 +605,10 @@ void FusionNode::check_fusion_status( bool rois_miss = false; bool fusion_success = true; + if (!det3d_fused_) { + fusion_success = false; + } + for (std::size_t id = 0; id < rois_number_; ++id) { bool input_rois_fused = true; if (diagnostic_id_to_stamp_map_.find(id) != diagnostic_id_to_stamp_map_.end()) { From 15f506b612a783a1696893df6ab6eb94eccedaac Mon Sep 17 00:00:00 2001 From: vividf Date: Tue, 4 Feb 2025 01:30:00 +0900 Subject: [PATCH 6/8] feat: timeout parameters for rois and msg3d Signed-off-by: vividf --- .../fusion_collector.hpp | 7 ++-- .../fusion_node.hpp | 3 +- .../src/fusion_collector.cpp | 35 +++++++++++++++++-- .../src/fusion_matching_strategy.cpp | 8 ++--- .../src/fusion_node.cpp | 15 ++++---- 5 files changed, 51 insertions(+), 17 deletions(-) diff --git a/perception/autoware_image_projection_based_fusion/include/autoware/image_projection_based_fusion/fusion_collector.hpp b/perception/autoware_image_projection_based_fusion/include/autoware/image_projection_based_fusion/fusion_collector.hpp index e216d22b8c47c..97baa05743c67 100644 --- a/perception/autoware_image_projection_based_fusion/include/autoware/image_projection_based_fusion/fusion_collector.hpp +++ b/perception/autoware_image_projection_based_fusion/include/autoware/image_projection_based_fusion/fusion_collector.hpp @@ -75,8 +75,9 @@ class FusionCollector public: FusionCollector( std::shared_ptr> && ros2_parent_node, double timeout_sec, - std::size_t rois_number, const std::vector> & det2d_list, bool debug_mode); - bool process_msg_3d(const typename Msg3D::ConstSharedPtr msg_3d); + std::size_t rois_number, const std::vector> & det2d_list, bool is_3d, + bool debug_mode); + bool process_msg_3d(const typename Msg3D::ConstSharedPtr msg_3d, double msg_3d_timeout); bool process_rois(const std::size_t & roi_id, const typename Msg2D::ConstSharedPtr det2d_msg); void fusion_callback(); @@ -89,6 +90,7 @@ class FusionCollector bool rois_exists(const std::size_t & rois_id); bool det3d_exists(); void show_debug_message(); + void set_period(const int64_t new_period); private: std::shared_ptr> ros2_parent_node_; @@ -98,6 +100,7 @@ class FusionCollector typename Msg3D::ConstSharedPtr det3d_msg_{nullptr}; std::vector> det2d_list_; std::unordered_map id_to_roi_map_; + bool is_3d_; bool debug_mode_; bool fusion_finished_{false}; std::mutex fusion_mutex_; diff --git a/perception/autoware_image_projection_based_fusion/include/autoware/image_projection_based_fusion/fusion_node.hpp b/perception/autoware_image_projection_based_fusion/include/autoware/image_projection_based_fusion/fusion_node.hpp index 168282b43ade8..ff4d52ea8f94a 100644 --- a/perception/autoware_image_projection_based_fusion/include/autoware/image_projection_based_fusion/fusion_node.hpp +++ b/perception/autoware_image_projection_based_fusion/include/autoware/image_projection_based_fusion/fusion_node.hpp @@ -103,7 +103,8 @@ class FusionNode : public rclcpp::Node std::size_t rois_number_; // timer - double timeout_sec_{}; + double msg3d_timeout_sec_{}; + double rois_timeout_sec_{}; std::vector::SharedPtr> rois_subs_; std::vector::SharedPtr> camera_info_subs_; diff --git a/perception/autoware_image_projection_based_fusion/src/fusion_collector.cpp b/perception/autoware_image_projection_based_fusion/src/fusion_collector.cpp index 1ed249986176f..c6b0c7dbc13aa 100644 --- a/perception/autoware_image_projection_based_fusion/src/fusion_collector.cpp +++ b/perception/autoware_image_projection_based_fusion/src/fusion_collector.cpp @@ -31,11 +31,13 @@ namespace autoware::image_projection_based_fusion template FusionCollector::FusionCollector( std::shared_ptr> && ros2_parent_node, double timeout_sec, - std::size_t rois_number, const std::vector> & det2d_list, bool debug_mode) + std::size_t rois_number, const std::vector> & det2d_list, bool is_3d, + bool debug_mode) : ros2_parent_node_(std::move(ros2_parent_node)), timeout_sec_(timeout_sec), rois_number_(rois_number), det2d_list_(det2d_list), + is_3d_(is_3d), debug_mode_(debug_mode) { const auto period_ns = std::chrono::duration_cast( @@ -64,7 +66,7 @@ std::shared_ptr FusionCollector bool FusionCollector::process_msg_3d( - const typename Msg3D::ConstSharedPtr msg_3d) + const typename Msg3D::ConstSharedPtr msg_3d, double msg_3d_timeout) { std::lock_guard fusion_lock(fusion_mutex_); if (fusion_finished_) return false; @@ -79,6 +81,18 @@ bool FusionCollector::process_msg_3d( det3d_msg_ = msg_3d; if (ready_to_fuse()) { fusion_callback(); + } else { + if (!is_3d_) { + const auto period_ns = std::chrono::duration_cast( + std::chrono::duration(msg_3d_timeout)); + try { + set_period(period_ns.count()); + } catch (rclcpp::exceptions::RCLError & ex) { + RCLCPP_WARN_THROTTLE( + ros2_parent_node_->get_logger(), *ros2_parent_node_->get_clock(), 5000, "%s", ex.what()); + } + timer_->reset(); + } } return true; @@ -213,6 +227,23 @@ void FusionCollector::show_debug_message() RCLCPP_INFO(ros2_parent_node_->get_logger(), "%s", log_stream.str().c_str()); } +template +void FusionCollector::set_period(const int64_t new_period) +{ + if (!timer_) { + return; + } + int64_t old_period = 0; + rcl_ret_t ret = rcl_timer_get_period(timer_->get_timer_handle().get(), &old_period); + if (ret != RCL_RET_OK) { + rclcpp::exceptions::throw_from_rcl_error(ret, "Couldn't get old period"); + } + ret = rcl_timer_exchange_period(timer_->get_timer_handle().get(), new_period, &old_period); + if (ret != RCL_RET_OK) { + rclcpp::exceptions::throw_from_rcl_error(ret, "Couldn't exchange_period"); + } +} + // Explicit instantiation for the supported types // pointpainting fusion diff --git a/perception/autoware_image_projection_based_fusion/src/fusion_matching_strategy.cpp b/perception/autoware_image_projection_based_fusion/src/fusion_matching_strategy.cpp index 0c7d78cc66b95..c661fbc0c59e9 100644 --- a/perception/autoware_image_projection_based_fusion/src/fusion_matching_strategy.cpp +++ b/perception/autoware_image_projection_based_fusion/src/fusion_matching_strategy.cpp @@ -89,7 +89,8 @@ NaiveMatchingStrategy::match_det3d_to_collector( const std::shared_ptr & params) { std::optional smallest_time_difference = std::nullopt; - std::shared_ptr> closest_collector = nullptr; + std::optional>> closest_collector = + std::nullopt; for (const auto & fusion_collector : fusion_collectors) { if (!fusion_collector->det3d_exists()) { @@ -107,10 +108,7 @@ NaiveMatchingStrategy::match_det3d_to_collector( } } - if (closest_collector != nullptr) { - return closest_collector; - } - return std::nullopt; + return closest_collector; } template diff --git a/perception/autoware_image_projection_based_fusion/src/fusion_node.cpp b/perception/autoware_image_projection_based_fusion/src/fusion_node.cpp index 28f10a32bc456..16d70098b7c89 100644 --- a/perception/autoware_image_projection_based_fusion/src/fusion_node.cpp +++ b/perception/autoware_image_projection_based_fusion/src/fusion_node.cpp @@ -69,7 +69,8 @@ FusionNode::FusionNode( } // Set parameters - timeout_sec_ = declare_parameter("timeout_sec"); + msg3d_timeout_sec_ = declare_parameter("msg3d_timeout_sec"); + rois_timeout_sec_ = declare_parameter("rois_timeout_sec"); std::vector input_rois_topics; std::vector input_camera_info_topics; @@ -345,20 +346,20 @@ void FusionNode::sub_callback( auto collector = fusion_collector.value(); if (collector) { fusion_collectors_lock.unlock(); - process_success = fusion_collector.value()->process_msg_3d(det3d_msg); + process_success = fusion_collector.value()->process_msg_3d(det3d_msg, msg3d_timeout_sec_); } } if (!process_success) { auto new_fusion_collector = std::make_shared>( - std::dynamic_pointer_cast(shared_from_this()), timeout_sec_, rois_number_, - det2d_list_, debug_mode_); + std::dynamic_pointer_cast(shared_from_this()), msg3d_timeout_sec_, rois_number_, + det2d_list_, true, debug_mode_); fusion_collectors_.push_back(new_fusion_collector); fusion_collectors_lock.unlock(); fusion_matching_strategy_->set_collector_info(new_fusion_collector, matching_params); - (void)new_fusion_collector->process_msg_3d(det3d_msg); + (void)new_fusion_collector->process_msg_3d(det3d_msg, msg3d_timeout_sec_); } // TODO(vivid): check the logic of clearing debugger. @@ -433,8 +434,8 @@ void FusionNode::roi_callback( if (!process_success) { auto new_fusion_collector = std::make_shared>( - std::dynamic_pointer_cast(shared_from_this()), timeout_sec_, rois_number_, - det2d_list_, debug_mode_); + std::dynamic_pointer_cast(shared_from_this()), rois_timeout_sec_, rois_number_, + det2d_list_, false, debug_mode_); fusion_collectors_.push_back(new_fusion_collector); fusion_collectors_lock.unlock(); From cb277efcf4f45e841bc8194242b3ccd448d744e2 Mon Sep 17 00:00:00 2001 From: vividf Date: Wed, 5 Feb 2025 14:29:36 +0900 Subject: [PATCH 7/8] chore: clean code Signed-off-by: vividf --- .../fusion_collector.hpp | 20 +- .../fusion_matching_strategy.hpp | 24 +- .../fusion_node.hpp | 18 +- .../src/fusion_collector.cpp | 42 +-- .../src/fusion_matching_strategy.cpp | 242 ++++++++----- .../src/fusion_node.cpp | 342 +++++++++--------- .../src/pointpainting_fusion/node.cpp | 2 +- 7 files changed, 373 insertions(+), 317 deletions(-) diff --git a/perception/autoware_image_projection_based_fusion/include/autoware/image_projection_based_fusion/fusion_collector.hpp b/perception/autoware_image_projection_based_fusion/include/autoware/image_projection_based_fusion/fusion_collector.hpp index 97baa05743c67..ebdb8715d4092 100644 --- a/perception/autoware_image_projection_based_fusion/include/autoware/image_projection_based_fusion/fusion_collector.hpp +++ b/perception/autoware_image_projection_based_fusion/include/autoware/image_projection_based_fusion/fusion_collector.hpp @@ -1,4 +1,4 @@ -// Copyright 2024 TIER IV, Inc. +// Copyright 2025 TIER IV, Inc. // // Licensed under the Apache License, Version 2.0 (the "License"); // you may not use this file except in compliance with the License. @@ -35,11 +35,11 @@ template struct Det2dStatus { // camera index - std::size_t id = 0; + std::size_t id{0}; // camera projection - std::shared_ptr camera_projector_ptr; - bool project_to_unrectified_image = false; - bool approximate_camera_projection = false; + std::shared_ptr camera_projector_ptr{nullptr}; + bool project_to_unrectified_image{false}; + bool approximate_camera_projection{false}; }; struct FusionCollectorInfoBase @@ -75,7 +75,7 @@ class FusionCollector public: FusionCollector( std::shared_ptr> && ros2_parent_node, double timeout_sec, - std::size_t rois_number, const std::vector> & det2d_list, bool is_3d, + std::size_t rois_number, const std::vector> & det2d_status_list, bool is_3d, bool debug_mode); bool process_msg_3d(const typename Msg3D::ConstSharedPtr msg_3d, double msg_3d_timeout); bool process_rois(const std::size_t & roi_id, const typename Msg2D::ConstSharedPtr det2d_msg); @@ -88,7 +88,7 @@ class FusionCollector // void show_debug_message(); bool ready_to_fuse(); bool rois_exists(const std::size_t & rois_id); - bool det3d_exists(); + bool msg3d_exists(); void show_debug_message(); void set_period(const int64_t new_period); @@ -97,9 +97,9 @@ class FusionCollector rclcpp::TimerBase::SharedPtr timer_; double timeout_sec_; std::size_t rois_number_; - typename Msg3D::ConstSharedPtr det3d_msg_{nullptr}; - std::vector> det2d_list_; - std::unordered_map id_to_roi_map_; + typename Msg3D::ConstSharedPtr msg3d_{nullptr}; + std::vector> det2d_status_list_; + std::unordered_map id_to_rois_map_; bool is_3d_; bool debug_mode_; bool fusion_finished_{false}; diff --git a/perception/autoware_image_projection_based_fusion/include/autoware/image_projection_based_fusion/fusion_matching_strategy.hpp b/perception/autoware_image_projection_based_fusion/include/autoware/image_projection_based_fusion/fusion_matching_strategy.hpp index a8446709748a2..87e65cd6c636a 100644 --- a/perception/autoware_image_projection_based_fusion/include/autoware/image_projection_based_fusion/fusion_matching_strategy.hpp +++ b/perception/autoware_image_projection_based_fusion/include/autoware/image_projection_based_fusion/fusion_matching_strategy.hpp @@ -37,11 +37,11 @@ struct MatchingParamsBase virtual ~MatchingParamsBase() = default; }; -struct Det3dMatchingParams : public MatchingParamsBase +struct Msg3dMatchingParams : public MatchingParamsBase { - double det3d_timestamp; + double msg3d_timestamp; - explicit Det3dMatchingParams(double det3d_timestamp = 0.0) : det3d_timestamp(det3d_timestamp) {} + explicit Msg3dMatchingParams(double msg3d_timestamp = 0.0) : msg3d_timestamp(msg3d_timestamp) {} }; struct RoisMatchingParams : public MatchingParamsBase @@ -67,9 +67,9 @@ class FusionMatchingStrategy const std::shared_ptr & params) const = 0; [[nodiscard]] virtual std::optional>> - match_det3d_to_collector( + match_msg3d_to_collector( const std::list>> & fusion_collectors, - const std::shared_ptr & params) = 0; + const std::shared_ptr & params) = 0; virtual void set_collector_info( std::shared_ptr> & collector, const std::shared_ptr & matching_params) = 0; @@ -81,7 +81,7 @@ class NaiveMatchingStrategy : public FusionMatchingStrategy> && ros2_parent_node, - std::size_t rois_number); + const std::size_t rois_number); [[nodiscard]] std::optional>> match_rois_to_collector( @@ -89,9 +89,9 @@ class NaiveMatchingStrategy : public FusionMatchingStrategy & params) const override; [[nodiscard]] std::optional>> - match_det3d_to_collector( + match_msg3d_to_collector( const std::list>> & fusion_collectors, - const std::shared_ptr & params) override; + const std::shared_ptr & params) override; void set_collector_info( std::shared_ptr> & collector, @@ -109,7 +109,7 @@ class AdvancedMatchingStrategy : public FusionMatchingStrategy> && ros2_parent_node, - std::size_t rois_number); + const std::size_t rois_number); [[nodiscard]] std::optional>> match_rois_to_collector( @@ -117,15 +117,15 @@ class AdvancedMatchingStrategy : public FusionMatchingStrategy & params) const override; [[nodiscard]] std::optional>> - match_det3d_to_collector( + match_msg3d_to_collector( const std::list>> & fusion_collectors, - const std::shared_ptr & params) override; + const std::shared_ptr & params) override; void set_collector_info( std::shared_ptr> & collector, const std::shared_ptr & matching_params) override; double get_offset( - const double & det3d_timestamp, + const double & msg3d_timestamp, const std::optional> & concatenated_status); double extract_fractional(double timestamp); diff --git a/perception/autoware_image_projection_based_fusion/include/autoware/image_projection_based_fusion/fusion_node.hpp b/perception/autoware_image_projection_based_fusion/include/autoware/image_projection_based_fusion/fusion_node.hpp index ff4d52ea8f94a..32f72fee5871b 100644 --- a/perception/autoware_image_projection_based_fusion/include/autoware/image_projection_based_fusion/fusion_node.hpp +++ b/perception/autoware_image_projection_based_fusion/include/autoware/image_projection_based_fusion/fusion_node.hpp @@ -86,11 +86,11 @@ class FusionNode : public rclcpp::Node // Common process methods void camera_info_callback( const sensor_msgs::msg::CameraInfo::ConstSharedPtr input_camera_info_msg, - const std::size_t camera_id); + const std::size_t rois_id); void init_strategy(); void manage_collector_list(); - void manage_concatenated_status_map(const double & current_timestam); + void manage_concatenated_status_map(double current_timestam); static std::string format_timestamp(double timestamp); void check_fusion_status(diagnostic_updater::DiagnosticStatusWrapper & stat); @@ -130,15 +130,15 @@ class FusionNode : public rclcpp::Node bool publish_previous_but_late_output_msg_{false}; bool drop_previous_but_late_output_msg_{false}; bool publish_output_msg_{false}; - bool det3d_fused_{true}; + bool msg3d_fused_{true}; protected: - void set_det2d_status(std::size_t rois_number); + void initialize_det2d_status(std::size_t rois_number); // callback for main subscription - void sub_callback(const typename Msg3D::ConstSharedPtr det3d_msg); - // callback for roi subscription - void roi_callback(const typename Msg2D::ConstSharedPtr det2d_msg, const std::size_t roi_i); + void sub_callback(const typename Msg3D::ConstSharedPtr msg3d); + // callback for rois subscription + void rois_callback(const typename Msg2D::ConstSharedPtr det2d_msg, const std::size_t rois_id); void diagnostic_callback(const diagnostic_msgs::msg::DiagnosticArray::SharedPtr diagnostic_msg); @@ -151,10 +151,10 @@ class FusionNode : public rclcpp::Node tf2_ros::TransformListener tf_listener_; // 2d detection management - std::vector> det2d_list_; + std::vector> det2d_status_list_; // 3d detection subscription - typename rclcpp::Subscription::SharedPtr det3d_sub_; + typename rclcpp::Subscription::SharedPtr msg3d_sub_; rclcpp::Subscription::SharedPtr sub_diag_; // parameters for out_of_scope filter diff --git a/perception/autoware_image_projection_based_fusion/src/fusion_collector.cpp b/perception/autoware_image_projection_based_fusion/src/fusion_collector.cpp index c6b0c7dbc13aa..cd5703f331625 100644 --- a/perception/autoware_image_projection_based_fusion/src/fusion_collector.cpp +++ b/perception/autoware_image_projection_based_fusion/src/fusion_collector.cpp @@ -31,12 +31,12 @@ namespace autoware::image_projection_based_fusion template FusionCollector::FusionCollector( std::shared_ptr> && ros2_parent_node, double timeout_sec, - std::size_t rois_number, const std::vector> & det2d_list, bool is_3d, + std::size_t rois_number, const std::vector> & det2d_status_list, bool is_3d, bool debug_mode) : ros2_parent_node_(std::move(ros2_parent_node)), timeout_sec_(timeout_sec), rois_number_(rois_number), - det2d_list_(det2d_list), + det2d_status_list_(det2d_status_list), is_3d_(is_3d), debug_mode_(debug_mode) { @@ -71,14 +71,14 @@ bool FusionCollector::process_msg_3d( std::lock_guard fusion_lock(fusion_mutex_); if (fusion_finished_) return false; - if (det3d_msg_ != nullptr) { + if (msg3d_ != nullptr) { RCLCPP_WARN_STREAM_THROTTLE( ros2_parent_node_->get_logger(), *ros2_parent_node_->get_clock(), std::chrono::milliseconds(10000).count(), "Pointcloud already exists in the collector. Check the timestamp of the pointcloud."); } - det3d_msg_ = msg_3d; + msg3d_ = msg_3d; if (ready_to_fuse()) { fusion_callback(); } else { @@ -100,18 +100,18 @@ bool FusionCollector::process_msg_3d( template bool FusionCollector::process_rois( - const std::size_t & roi_id, const typename Msg2D::ConstSharedPtr det2d_msg) + const std::size_t & rois_id, const typename Msg2D::ConstSharedPtr det2d_msg) { std::lock_guard fusion_lock(fusion_mutex_); if (fusion_finished_) return false; - if (id_to_roi_map_.find(roi_id) != id_to_roi_map_.end()) { + if (id_to_rois_map_.find(rois_id) != id_to_rois_map_.end()) { RCLCPP_WARN_STREAM_THROTTLE( ros2_parent_node_->get_logger(), *ros2_parent_node_->get_clock(), std::chrono::milliseconds(10000).count(), - "ROIS '" << roi_id << "' already exists in the collector. Check the timestamp of the rois."); + "ROIs '" << rois_id << "' already exists in the collector. Check the timestamp of the rois."); } - id_to_roi_map_[roi_id] = det2d_msg; + id_to_rois_map_[rois_id] = det2d_msg; if (ready_to_fuse()) { fusion_callback(); } @@ -122,7 +122,7 @@ bool FusionCollector::process_rois( template bool FusionCollector::ready_to_fuse() { - return id_to_roi_map_.size() == rois_number_ && det3d_msg_ != nullptr; + return id_to_rois_map_.size() == rois_number_ && msg3d_ != nullptr; } template @@ -143,11 +143,11 @@ void FusionCollector::fusion_callback() timer_->cancel(); std::unordered_map id_to_stamp_map; - for (const auto & [roi_id, roi_msg] : id_to_roi_map_) { + for (const auto & [roi_id, roi_msg] : id_to_rois_map_) { id_to_stamp_map[roi_id] = rclcpp::Time(roi_msg->header.stamp).seconds(); } - if (!det3d_msg_) { + if (!msg3d_) { RCLCPP_WARN( ros2_parent_node_->get_logger(), "The Det3d message is not in the fusion collector, so the fusion process will be skipped."); @@ -157,18 +157,18 @@ void FusionCollector::fusion_callback() return; } - typename Msg3D::SharedPtr output_det3d_msg = std::make_shared(*det3d_msg_); + typename Msg3D::SharedPtr output_det3d_msg = std::make_shared(*msg3d_); ros2_parent_node_->preprocess(*output_det3d_msg); - for (const auto & [roi_id, roi_msg] : id_to_roi_map_) { - if (det2d_list_[roi_id].camera_projector_ptr == nullptr) { + for (const auto & [roi_id, roi_msg] : id_to_rois_map_) { + if (det2d_status_list_[roi_id].camera_projector_ptr == nullptr) { RCLCPP_WARN_THROTTLE( ros2_parent_node_->get_logger(), *ros2_parent_node_->get_clock(), 5000, "no camera info. id is %zu", roi_id); continue; } ros2_parent_node_->fuse_on_single_image( - *det3d_msg_, det2d_list_[roi_id], *roi_msg, *output_det3d_msg); + *msg3d_, det2d_status_list_[roi_id], *roi_msg, *output_det3d_msg); } ros2_parent_node_->export_process(output_det3d_msg, id_to_stamp_map, fusion_collector_info_); @@ -178,13 +178,13 @@ void FusionCollector::fusion_callback() template bool FusionCollector::rois_exists(const std::size_t & rois_id) { - return id_to_roi_map_.find(rois_id) != id_to_roi_map_.end(); + return id_to_rois_map_.find(rois_id) != id_to_rois_map_.end(); } template -bool FusionCollector::det3d_exists() +bool FusionCollector::msg3d_exists() { - return det3d_msg_ != nullptr; + return msg3d_ != nullptr; } template @@ -209,14 +209,14 @@ void FusionCollector::show_debug_message() } log_stream << "Time until trigger: " << (time_until_trigger.count() / 1e9) << " seconds\n"; - if (det3d_msg_) { - log_stream << "Det3d msg: [" << rclcpp::Time(det3d_msg_->header.stamp).seconds() << "]\n"; + if (msg3d_) { + log_stream << "Det3d msg: [" << rclcpp::Time(msg3d_->header.stamp).seconds() << "]\n"; } else { log_stream << "Det3d msg: [Is empty]\n"; } log_stream << "ROIs: ["; std::string separator = ""; - for (const auto & [id, rois] : id_to_roi_map_) { + for (const auto & [id, rois] : id_to_rois_map_) { log_stream << separator; log_stream << "[rois " << id << ", " << rclcpp::Time(rois->header.stamp).seconds() << "]"; separator = ", "; diff --git a/perception/autoware_image_projection_based_fusion/src/fusion_matching_strategy.cpp b/perception/autoware_image_projection_based_fusion/src/fusion_matching_strategy.cpp index c661fbc0c59e9..da78bbd124823 100644 --- a/perception/autoware_image_projection_based_fusion/src/fusion_matching_strategy.cpp +++ b/perception/autoware_image_projection_based_fusion/src/fusion_matching_strategy.cpp @@ -22,6 +22,7 @@ #include #include +#include #include #include #include @@ -33,18 +34,26 @@ namespace autoware::image_projection_based_fusion template NaiveMatchingStrategy::NaiveMatchingStrategy( - std::shared_ptr> && ros2_parent_node, std::size_t rois_number) + std::shared_ptr> && ros2_parent_node, + const std::size_t rois_number) : ros2_parent_node_(std::move(ros2_parent_node)) { + if (!ros2_parent_node_) { + throw std::runtime_error("ros2_parent_node is nullptr in NaiveMatchingStrategy constructor."); + } + auto rois_timestamp_offsets = ros2_parent_node_->template declare_parameter>("rois_timestamp_offsets"); threshold_ = ros2_parent_node_->template declare_parameter("matching_strategy.threshold"); if (rois_timestamp_offsets.size() != rois_number) { - throw std::runtime_error("The number of rois does not match the number of timestamp offsets."); + throw std::runtime_error( + "Mismatch: rois_number (" + std::to_string(rois_number) + + ") does not match rois_timestamp_offsets size (" + + std::to_string(rois_timestamp_offsets.size()) + ")."); } - for (size_t i = 0; i < rois_number; i++) { + for (std::size_t i = 0; i < rois_number; i++) { id_to_offset_map_[i] = rois_timestamp_offsets[i]; } @@ -57,18 +66,16 @@ NaiveMatchingStrategy::match_rois_to_collector( const std::list>> & fusion_collectors, const std::shared_ptr & params) const { - std::optional smallest_time_difference = std::nullopt; + double smallest_time_difference = std::numeric_limits::max(); std::shared_ptr> closest_collector = nullptr; for (const auto & fusion_collector : fusion_collectors) { if (!fusion_collector->rois_exists(params->rois_id)) { - auto info = fusion_collector->get_info(); - if (auto naive_info = std::dynamic_pointer_cast(info)) { + if ( + auto naive_info = + std::dynamic_pointer_cast(fusion_collector->get_info())) { double time_difference = std::abs(params->rois_timestamp - naive_info->timestamp); - if ( - !smallest_time_difference.has_value() || - (time_difference < smallest_time_difference.value() && - time_difference < naive_info->threshold)) { + if (time_difference < smallest_time_difference && time_difference < naive_info->threshold) { smallest_time_difference = time_difference; closest_collector = fusion_collector; } @@ -76,31 +83,26 @@ NaiveMatchingStrategy::match_rois_to_collector( } } - if (closest_collector != nullptr) { - return closest_collector; - } - return std::nullopt; + return closest_collector; // Implicitly returns std::nullopt if closest_collector is nullptr } template std::optional>> -NaiveMatchingStrategy::match_det3d_to_collector( +NaiveMatchingStrategy::match_msg3d_to_collector( const std::list>> & fusion_collectors, - const std::shared_ptr & params) + const std::shared_ptr & params) { - std::optional smallest_time_difference = std::nullopt; - std::optional>> closest_collector = - std::nullopt; + double smallest_time_difference = std::numeric_limits::max(); + std::shared_ptr> closest_collector = nullptr; for (const auto & fusion_collector : fusion_collectors) { - if (!fusion_collector->det3d_exists()) { - auto info = fusion_collector->get_info(); - if (auto naive_info = std::dynamic_pointer_cast(info)) { - double time_difference = std::abs(params->det3d_timestamp - naive_info->timestamp); - if ( - !smallest_time_difference.has_value() || - (time_difference < smallest_time_difference.value() && - time_difference < naive_info->threshold)) { + if (!fusion_collector->msg3d_exists()) { + if ( + auto naive_info = + std::dynamic_pointer_cast(fusion_collector->get_info())) { + double time_difference = std::abs(params->msg3d_timestamp - naive_info->timestamp); + + if (time_difference < smallest_time_difference && time_difference < naive_info->threshold) { smallest_time_difference = time_difference; closest_collector = fusion_collector; } @@ -108,7 +110,7 @@ NaiveMatchingStrategy::match_det3d_to_collector( } } - return closest_collector; + return closest_collector; // Implicitly returns std::nullopt if closest_collector is nullptr } template @@ -116,25 +118,44 @@ void NaiveMatchingStrategy::set_collector_info( std::shared_ptr> & collector, const std::shared_ptr & matching_params) { + if (!matching_params) { + RCLCPP_ERROR(ros2_parent_node_->get_logger(), "matching_params is nullptr!"); + return; + } + if ( - auto det3d_matching_params = std::dynamic_pointer_cast(matching_params)) { + auto det3d_matching_params = std::dynamic_pointer_cast(matching_params)) { auto info = - std::make_shared(det3d_matching_params->det3d_timestamp, threshold_); + std::make_shared(det3d_matching_params->msg3d_timestamp, threshold_); collector->set_info(info); + } else if ( auto rois_matching_params = std::dynamic_pointer_cast(matching_params)) { + auto offset_it = id_to_offset_map_.find(rois_matching_params->rois_id); + if (offset_it == id_to_offset_map_.end()) { + RCLCPP_ERROR( + ros2_parent_node_->get_logger(), "Missing offset for rois_id: %zu", + rois_matching_params->rois_id); + return; + } + auto info = std::make_shared( - rois_matching_params->rois_timestamp - id_to_offset_map_.at(rois_matching_params->rois_id), - threshold_); + rois_matching_params->rois_timestamp - offset_it->second, threshold_); collector->set_info(info); } } template AdvancedMatchingStrategy::AdvancedMatchingStrategy( - std::shared_ptr> && ros2_parent_node, std::size_t rois_number) + std::shared_ptr> && ros2_parent_node, + const std::size_t rois_number) : ros2_parent_node_(std::move(ros2_parent_node)) { + if (!ros2_parent_node_) { + throw std::runtime_error( + "ros2_parent_node is nullptr in AdvancedMatchingStrategy constructor."); + } + auto rois_timestamp_offsets = ros2_parent_node_->template declare_parameter>("rois_timestamp_offsets"); det3d_noise_window_ = @@ -144,11 +165,16 @@ AdvancedMatchingStrategy::AdvancedMatchingStrategy( "matching_strategy.rois_timestamp_noise_window"); if (rois_timestamp_offsets.size() != rois_number) { - throw std::runtime_error("The number of rois does not match the number of timestamp offsets."); + throw std::runtime_error( + "Mismatch: rois_number (" + std::to_string(rois_number) + + ") does not match rois_timestamp_offsets size (" + + std::to_string(rois_timestamp_offsets.size()) + ")."); } if (rois_timestamp_noise_window.size() != rois_number) { throw std::runtime_error( - "The number of rois does not match the number of timestamp noise window."); + "Mismatch: rois_number (" + std::to_string(rois_number) + + ") does not match rois_timestamp_noise_window size (" + + std::to_string(rois_timestamp_noise_window.size()) + ")."); } for (size_t i = 0; i < rois_number; i++) { @@ -166,15 +192,29 @@ AdvancedMatchingStrategy::match_rois_to_collector( const std::list>> & fusion_collectors, const std::shared_ptr & params) const { + auto offset_it = id_to_offset_map_.find(params->rois_id); + auto noise_it = id_to_noise_window_map_.find(params->rois_id); + + if (offset_it == id_to_offset_map_.end() || noise_it == id_to_noise_window_map_.end()) { + RCLCPP_ERROR( + ros2_parent_node_->get_logger(), "Missing offset or noise window for rois_id: %zu", + params->rois_id); + return std::nullopt; + } + + double adjusted_timestamp = params->rois_timestamp - offset_it->second; + double noise_window = noise_it->second; + for (const auto & fusion_collector : fusion_collectors) { - auto info = fusion_collector->get_info(); - if (auto advanced_info = std::dynamic_pointer_cast(info)) { - auto reference_timestamp_min = advanced_info->timestamp - advanced_info->noise_window; - auto reference_timestamp_max = advanced_info->timestamp + advanced_info->noise_window; - double time = params->rois_timestamp - id_to_offset_map_.at(params->rois_id); + if ( + auto advanced_info = + std::dynamic_pointer_cast(fusion_collector->get_info())) { + double reference_timestamp_min = advanced_info->timestamp - advanced_info->noise_window; + double reference_timestamp_max = advanced_info->timestamp + advanced_info->noise_window; + if ( - time < reference_timestamp_max + id_to_noise_window_map_.at(params->rois_id) && - time > reference_timestamp_min - id_to_noise_window_map_.at(params->rois_id)) { + adjusted_timestamp < reference_timestamp_max + noise_window && + adjusted_timestamp > reference_timestamp_min - noise_window) { return fusion_collector; } } @@ -184,23 +224,25 @@ AdvancedMatchingStrategy::match_rois_to_collector( template std::optional>> -AdvancedMatchingStrategy::match_det3d_to_collector( +AdvancedMatchingStrategy::match_msg3d_to_collector( const std::list>> & fusion_collectors, - const std::shared_ptr & params) + const std::shared_ptr & params) { - auto concatenated_status = ros2_parent_node_->find_concatenation_status(params->det3d_timestamp); + auto concatenated_status = ros2_parent_node_->find_concatenation_status(params->msg3d_timestamp); - // TODO(vivid): double check this logic and fix the name - double offset = get_offset(params->det3d_timestamp, concatenated_status); + double offset = get_offset(params->msg3d_timestamp, concatenated_status); + double adjusted_timestamp = params->msg3d_timestamp - offset; for (const auto & fusion_collector : fusion_collectors) { - auto info = fusion_collector->get_info(); - if (auto advanced_info = std::dynamic_pointer_cast(info)) { - auto reference_timestamp_min = advanced_info->timestamp - advanced_info->noise_window; - auto reference_timestamp_max = advanced_info->timestamp + advanced_info->noise_window; + if ( + auto advanced_info = + std::dynamic_pointer_cast(fusion_collector->get_info())) { + double reference_timestamp_min = advanced_info->timestamp - advanced_info->noise_window; + double reference_timestamp_max = advanced_info->timestamp + advanced_info->noise_window; + if ( - params->det3d_timestamp - offset < reference_timestamp_max + det3d_noise_window_ && - params->det3d_timestamp - offset > reference_timestamp_min - det3d_noise_window_) { + adjusted_timestamp < reference_timestamp_max + det3d_noise_window_ && + adjusted_timestamp > reference_timestamp_min - det3d_noise_window_) { return fusion_collector; } } @@ -213,73 +255,89 @@ void AdvancedMatchingStrategy::set_collector_info( std::shared_ptr> & collector, const std::shared_ptr & matching_params) { + if (!matching_params) { + RCLCPP_ERROR(ros2_parent_node_->get_logger(), "matching_params is nullptr!"); + return; + } + if ( - auto det3d_matching_params = std::dynamic_pointer_cast(matching_params)) { + auto det3d_matching_params = std::dynamic_pointer_cast(matching_params)) { auto concatenated_status = - ros2_parent_node_->find_concatenation_status(det3d_matching_params->det3d_timestamp); - double offset = get_offset(det3d_matching_params->det3d_timestamp, concatenated_status); + ros2_parent_node_->find_concatenation_status(det3d_matching_params->msg3d_timestamp); + double offset = get_offset(det3d_matching_params->msg3d_timestamp, concatenated_status); auto info = std::make_shared( - det3d_matching_params->det3d_timestamp - offset, det3d_noise_window_); + det3d_matching_params->msg3d_timestamp - offset, det3d_noise_window_); collector->set_info(info); } else if ( auto rois_matching_params = std::dynamic_pointer_cast(matching_params)) { + auto offset_it = id_to_offset_map_.find(rois_matching_params->rois_id); + auto noise_it = id_to_noise_window_map_.find(rois_matching_params->rois_id); + + if (offset_it == id_to_offset_map_.end() || noise_it == id_to_noise_window_map_.end()) { + RCLCPP_ERROR( + ros2_parent_node_->get_logger(), "Missing offset or noise window for rois_id: %zu", + rois_matching_params->rois_id); + return; + } + auto info = std::make_shared( - rois_matching_params->rois_timestamp - id_to_offset_map_.at(rois_matching_params->rois_id), - id_to_noise_window_map_[rois_matching_params->rois_id]); + rois_matching_params->rois_timestamp - offset_it->second, noise_it->second); collector->set_info(info); } } template double AdvancedMatchingStrategy::get_offset( - const double & det3d_timestamp, + const double & msg3d_timestamp, const std::optional> & concatenated_status) { double offset = 0.0; - if (concatenated_status) { + if (concatenated_status.has_value()) { const auto & status_map = concatenated_status.value(); - if ( - status_map.find("cloud_concatenation_success") != status_map.end() && - status_map.find("reference_timestamp_min") != status_map.end() && - status_map.find("reference_timestamp_max") != status_map.end()) { - bool concatenation_success = (status_map.at("cloud_concatenation_success") == "True"); + // Find required keys in the map + auto concat_success_it = status_map.find("cloud_concatenation_success"); + auto ref_min_it = status_map.find("reference_timestamp_min"); + auto ref_max_it = status_map.find("reference_timestamp_max"); - if ( - !concatenation_success && - det3d_timestamp > std::stod(status_map.at("reference_timestamp_max"))) { - // The defined earliest pointcloud is missed in the concatenation - double reference_min = std::stod(status_map.at("reference_timestamp_min")); - double reference_max = std::stod(status_map.at("reference_timestamp_max")); - offset = det3d_timestamp - (reference_min + (reference_max - reference_min) / 2); - } else if (!database_created_ && concatenation_success) { - // Ensure "cloud_concatenated_timestamp" key exists before accessing - if (status_map.find("concatenated_cloud_timestamp") != status_map.end()) { - double concatenated_cloud_timestamp = - std::stod(status_map.at("concatenated_cloud_timestamp")); - update_fractional_timestamp_set(concatenated_cloud_timestamp); - success_status_counter_++; - offset = 0.0; - - if (success_status_counter_ > success_threshold) { - database_created_ = true; + if ( + concat_success_it != status_map.end() && ref_min_it != status_map.end() && + ref_max_it != status_map.end()) { + bool concatenation_success = (concat_success_it->second == "True"); + + try { + double reference_min = std::stod(ref_min_it->second); + double reference_max = std::stod(ref_max_it->second); + + if (!concatenation_success && msg3d_timestamp > reference_max) { + offset = msg3d_timestamp - (reference_min + (reference_max - reference_min) / 2); + } else if (!database_created_ && concatenation_success) { + auto concat_cloud_it = status_map.find("concatenated_cloud_timestamp"); + if (concat_cloud_it != status_map.end()) { + double concatenated_cloud_timestamp = std::stod(concat_cloud_it->second); + update_fractional_timestamp_set(concatenated_cloud_timestamp); + success_status_counter_++; + offset = 0.0; + + if (success_status_counter_ > success_threshold) { + database_created_ = true; + } } } + } catch (const std::exception & e) { + RCLCPP_ERROR(ros2_parent_node_->get_logger(), "Failed to parse timestamp: %s", e.what()); } } } else { - RCLCPP_WARN( + RCLCPP_DEBUG( ros2_parent_node_->get_logger(), - "If concatenated_status is missing, find the offset using the timestamp difference"); - // If concatenated_status is missing, find the offset using the timestamp difference - if (database_created_) { - offset = compute_offset(det3d_timestamp); - RCLCPP_WARN(ros2_parent_node_->get_logger(), "Use database and find offset: %f", offset); + "Concatenated_status is missing, finding offset using timestamp difference"); - } else { - offset = 0.0; + if (database_created_) { + offset = compute_offset(msg3d_timestamp); + RCLCPP_DEBUG(ros2_parent_node_->get_logger(), "Using database, computed offset: %f", offset); } } diff --git a/perception/autoware_image_projection_based_fusion/src/fusion_node.cpp b/perception/autoware_image_projection_based_fusion/src/fusion_node.cpp index 16d70098b7c89..1fc4dcd2d0c3a 100644 --- a/perception/autoware_image_projection_based_fusion/src/fusion_node.cpp +++ b/perception/autoware_image_projection_based_fusion/src/fusion_node.cpp @@ -59,8 +59,8 @@ FusionNode::FusionNode( // set rois_number rois_number_ = static_cast(declare_parameter("rois_number")); if (rois_number_ < 1) { - RCLCPP_ERROR( - this->get_logger(), "minimum rois_number_ is 1. current rois_number_ is %zu", rois_number_); + throw std::runtime_error( + "Minimum rois_number_ is 1. Current rois_number_ is " + std::to_string(rois_number_)); } if (rois_number_ > 8) { RCLCPP_WARN( @@ -88,32 +88,38 @@ FusionNode::FusionNode( "/sensing/camera/camera" + std::to_string(roi_i) + "/camera_info"); } - // subscribe camera info + // Subscribe to Camera Info camera_info_subs_.resize(rois_number_); - for (std::size_t roi_i = 0; roi_i < rois_number_; ++roi_i) { - std::function fnc = - std::bind(&FusionNode::camera_info_callback, this, std::placeholders::_1, roi_i); - camera_info_subs_.at(roi_i) = this->create_subscription( - input_camera_info_topics.at(roi_i), rclcpp::QoS{1}.best_effort(), fnc); + for (auto rois_id = 0u; rois_id < rois_number_; ++rois_id) { + auto topic = input_camera_info_topics.at(rois_id); + auto qos = rclcpp::QoS{1}.best_effort(); + + camera_info_subs_[rois_id] = this->create_subscription( + topic, qos, [this, rois_id](const sensor_msgs::msg::CameraInfo::ConstSharedPtr msg) { + this->camera_info_callback(msg, rois_id); + }); } - // subscribe rois + // Subscribe to ROIs rois_subs_.resize(rois_number_); - for (std::size_t roi_i = 0; roi_i < rois_number_; ++roi_i) { - std::function roi_callback = - std::bind(&FusionNode::roi_callback, this, std::placeholders::_1, roi_i); - rois_subs_.at(roi_i) = this->create_subscription( - input_rois_topics.at(roi_i), rclcpp::QoS{1}.best_effort(), roi_callback); + + for (auto rois_id = 0u; rois_id < rois_number_; ++rois_id) { + auto topic = input_rois_topics.at(rois_id); + auto qos = rclcpp::QoS{1}.best_effort(); + + rois_subs_[rois_id] = this->create_subscription( + topic, qos, [this, rois_id](const typename Msg2D::ConstSharedPtr msg) { + this->rois_callback(msg, rois_id); + }); } - // subscribe 3d detection - std::function sub_callback = - std::bind(&FusionNode::sub_callback, this, std::placeholders::_1); - det3d_sub_ = - this->create_subscription("input", rclcpp::QoS(1).best_effort(), sub_callback); + // Subscribe 3D input msg + msg3d_sub_ = this->create_subscription( + "input", rclcpp::QoS(1).best_effort(), + [this](const typename Msg3D::ConstSharedPtr msg) { this->sub_callback(msg); }); // initialization on each 2d detections - set_det2d_status(rois_number_); + initialize_det2d_status(rois_number_); // parameters for approximation grid approx_grid_cell_w_size_ = declare_parameter("approximation_grid_cell_width"); @@ -194,64 +200,63 @@ void FusionNode::init_strategy() } template -void FusionNode::set_det2d_status(std::size_t rois_number_) +void FusionNode::initialize_det2d_status(std::size_t rois_number) { - // camera projection settings + // Camera projection settings std::vector point_project_to_unrectified_image = declare_parameter>("point_project_to_unrectified_image"); - if (rois_number_ > point_project_to_unrectified_image.size()) { + if (rois_number > point_project_to_unrectified_image.size()) { throw std::runtime_error( "The number of point_project_to_unrectified_image does not match the number of rois " "topics."); } - std::vector approx_camera_projection = + std::vector approximate_camera_projection = declare_parameter>("approximate_camera_projection"); - if (rois_number_ != approx_camera_projection.size()) { - const std::size_t current_size = approx_camera_projection.size(); - RCLCPP_WARN( - get_logger(), - "The number of elements in approximate_camera_projection should be the same as in " - "rois_number_. " - "It has %zu elements. But rois_number_ is %zu", - current_size, rois_number_); - if (current_size < rois_number_) { - approx_camera_projection.resize(rois_number_); - for (std::size_t i = current_size; i < rois_number_; i++) { - approx_camera_projection.at(i) = true; - } - } + if (rois_number != approximate_camera_projection.size()) { + const std::size_t current_size = approximate_camera_projection.size(); + throw std::runtime_error( + "The number of elements in approximate_camera_projection should be the same as rois_number_. " + "It has " + + std::to_string(current_size) + " elements, but rois_number is " + + std::to_string(rois_number) + "."); } - // 2d detection status initialization - det2d_list_.resize(rois_number_); - for (std::size_t roi_i = 0; roi_i < rois_number_; ++roi_i) { - det2d_list_.at(roi_i).id = roi_i; - det2d_list_.at(roi_i).project_to_unrectified_image = - point_project_to_unrectified_image.at(roi_i); - det2d_list_.at(roi_i).approximate_camera_projection = approx_camera_projection.at(roi_i); + // 2D detection status initialization + det2d_status_list_.resize(rois_number_); + for (std::size_t rois_id = 0; rois_id < rois_number_; ++rois_id) { + det2d_status_list_.at(rois_id).id = rois_id; + det2d_status_list_.at(rois_id).project_to_unrectified_image = + point_project_to_unrectified_image.at(rois_id); + det2d_status_list_.at(rois_id).approximate_camera_projection = + approximate_camera_projection.at(rois_id); } } template void FusionNode::camera_info_callback( const sensor_msgs::msg::CameraInfo::ConstSharedPtr input_camera_info_msg, - const std::size_t camera_id) + const std::size_t rois_id) { - // create the CameraProjection when the camera info arrives for the first time - // assuming the camera info does not change while the node is running - auto & det2d = det2d_list_.at(camera_id); - if (!det2d.camera_projector_ptr && check_camera_info(*input_camera_info_msg)) { - det2d.camera_projector_ptr = std::make_unique( + if (rois_id >= det2d_status_list_.size()) { + throw std::out_of_range("rois_id " + std::to_string(rois_id) + " is out of range."); + } + + // Create the CameraProjection only when the camera info arrives for the first time. + // This assume the camera info does not change while the node is running + auto & det2d_status = det2d_status_list_.at(rois_id); + if (det2d_status.camera_projector_ptr == nullptr && check_camera_info(*input_camera_info_msg)) { + det2d_status.camera_projector_ptr = std::make_unique( *input_camera_info_msg, approx_grid_cell_w_size_, approx_grid_cell_h_size_, - det2d.project_to_unrectified_image, det2d.approximate_camera_projection); - det2d.camera_projector_ptr->initialize(); + det2d_status.project_to_unrectified_image, det2d_status.approximate_camera_projection); + det2d_status.camera_projector_ptr->initialize(); } } template -void FusionNode::preprocess(Msg3D & output_msg __attribute__((unused))) +void FusionNode::preprocess([[maybe_unused]] Msg3D & output_msg) { - // do nothing by default + // Default implementation: No preprocessing. + // This function can be overridden by derived classes if needed. } template @@ -263,37 +268,39 @@ void FusionNode::export_process( ExportObj output_msg; postprocess(*(output_det3d_msg), output_msg); + // Update timestamp current_output_msg_timestamp_ = rclcpp::Time(output_msg.header.stamp).seconds(); + + // Handle late messages during rosbag replay if ( current_output_msg_timestamp_ < latest_output_msg_timestamp_ && !publish_previous_but_late_output_msg_) { - // Publish the output msg if the rosbag replays in loop - if (latest_output_msg_timestamp_ - current_output_msg_timestamp_ > rosbag_length_) { - publish_output_msg_ = true; // Force publishing in this case - } else { - drop_previous_but_late_output_msg_ = true; // Otherwise, drop the late output msg - } + double timestamp_diff = latest_output_msg_timestamp_ - current_output_msg_timestamp_; + + publish_output_msg_ = (timestamp_diff > rosbag_length_); // Force publish if rosbag looped + drop_previous_but_late_output_msg_ = !publish_output_msg_; // Drop otherwise } else { - // Publish pointcloud if timestamps are valid or the condition doesn't apply - publish_output_msg_ = true; + publish_output_msg_ = true; // Publish normally } if (publish_output_msg_) { publish(output_msg); } + // Move collected diagnostics info diagnostic_collector_info_ = std::move(collector_info); diagnostic_id_to_stamp_map_ = std::move(id_to_stamp_map); diagnostic_updater_.force_update(); - // add processing time for debug + // Add processing time for debugging if (debug_publisher_) { - const double cyclic_time_ms = stop_watch_ptr_->toc("cyclic_time", true); - const double pipeline_latency_ms = + auto cyclic_time_ms = stop_watch_ptr_->toc("cyclic_time", true); + auto pipeline_latency_ms = std::chrono::duration( std::chrono::nanoseconds( (this->get_clock()->now() - output_det3d_msg->header.stamp).nanoseconds())) .count(); + debug_publisher_->publish( "debug/cyclic_time_ms", cyclic_time_ms); debug_publisher_->publish( @@ -301,24 +308,25 @@ void FusionNode::export_process( processing_time_ms + stop_watch_ptr_->toc("processing_time", true)); debug_publisher_->publish( "debug/pipeline_latency_ms", pipeline_latency_ms); + processing_time_ms = 0; } } template -void FusionNode::sub_callback( - const typename Msg3D::ConstSharedPtr det3d_msg) +void FusionNode::sub_callback(const typename Msg3D::ConstSharedPtr msg3d) { if (!fusion_matching_strategy_) { init_strategy(); } + // Debug logging for message latency if (debug_mode_) { auto arrive_time = this->get_clock()->now().seconds(); - RCLCPP_ERROR( - this->get_logger(), " det3d's timestamp: %lf arrive time: %lf seconds, latency: %lf", - rclcpp::Time(det3d_msg->header.stamp).seconds(), arrive_time, - arrive_time - rclcpp::Time(det3d_msg->header.stamp).seconds()); + auto msg3d_stamp = rclcpp::Time(msg3d->header.stamp).seconds(); + RCLCPP_DEBUG( + this->get_logger(), "msg3d timestamp: %lf, arrive time: %lf, latency: %lf", msg3d_stamp, + arrive_time, arrive_time - msg3d_stamp); } std::unique_ptr st_ptr; @@ -326,40 +334,39 @@ void FusionNode::sub_callback( stop_watch_ptr_->toc("processing_time", true); manage_collector_list(); - // protect fusion collectors list + + // Lock fusion collectors list std::unique_lock fusion_collectors_lock(fusion_collectors_mutex_); - auto det3d_timestamp = rclcpp::Time(det3d_msg->header.stamp).seconds(); + auto msg3d_timestamp = rclcpp::Time(msg3d->header.stamp).seconds(); // For each callback, check whether there is a exist collector that matches this cloud - std::optional>> fusion_collector = - std::nullopt; - auto matching_params = std::make_shared(); - matching_params->det3d_timestamp = det3d_timestamp; - if (!fusion_collectors_.empty()) { - fusion_collector = - fusion_matching_strategy_->match_det3d_to_collector(fusion_collectors_, matching_params); - } + auto matching_params = std::make_shared(); + matching_params->msg3d_timestamp = msg3d_timestamp; + + // Try to find an existing FusionCollector that matches this message + auto fusion_collector = + !fusion_collectors_.empty() + ? fusion_matching_strategy_->match_msg3d_to_collector(fusion_collectors_, matching_params) + : std::nullopt; bool process_success = false; - if (fusion_collector.has_value()) { - auto collector = fusion_collector.value(); - if (collector) { - fusion_collectors_lock.unlock(); - process_success = fusion_collector.value()->process_msg_3d(det3d_msg, msg3d_timeout_sec_); - } + if (fusion_collector && fusion_collector.value()) { + fusion_collectors_lock.unlock(); // Unlock before processing + process_success = fusion_collector.value()->process_msg_3d(msg3d, msg3d_timeout_sec_); } + // If no matching collector, create a new one if (!process_success) { auto new_fusion_collector = std::make_shared>( std::dynamic_pointer_cast(shared_from_this()), msg3d_timeout_sec_, rois_number_, - det2d_list_, true, debug_mode_); + det2d_status_list_, true, debug_mode_); - fusion_collectors_.push_back(new_fusion_collector); + fusion_collectors_.emplace_back(new_fusion_collector); fusion_collectors_lock.unlock(); fusion_matching_strategy_->set_collector_info(new_fusion_collector, matching_params); - (void)new_fusion_collector->process_msg_3d(det3d_msg, msg3d_timeout_sec_); + (void)new_fusion_collector->process_msg_3d(msg3d, msg3d_timeout_sec_); } // TODO(vivid): check the logic of clearing debugger. @@ -369,7 +376,7 @@ void FusionNode::sub_callback( if (matching_strategy_ == "advanced") { // remove outdated messages in the concatenated map - manage_concatenated_status_map(det3d_timestamp); + manage_concatenated_status_map(msg3d_timestamp); } // // add timestamp interval for debug @@ -386,8 +393,8 @@ void FusionNode::sub_callback( } template -void FusionNode::roi_callback( - const typename Msg2D::ConstSharedPtr det2d_msg, const std::size_t roi_i) +void FusionNode::rois_callback( + const typename Msg2D::ConstSharedPtr det2d_msg, const std::size_t rois_id) { if (!fusion_matching_strategy_) { init_strategy(); @@ -395,53 +402,49 @@ void FusionNode::roi_callback( if (debug_mode_) { auto arrive_time = this->get_clock()->now().seconds(); - RCLCPP_ERROR( - this->get_logger(), " rois %zu timestamp: %lf arrive time: %lf seconds, latency: %lf", roi_i, - rclcpp::Time(det2d_msg->header.stamp).seconds(), arrive_time, + RCLCPP_DEBUG( + this->get_logger(), " rois %zu timestamp: %lf arrive time: %lf seconds, latency: %lf", + rois_id, rclcpp::Time(det2d_msg->header.stamp).seconds(), arrive_time, arrive_time - rclcpp::Time(det2d_msg->header.stamp).seconds()); } + std::unique_ptr st_ptr; if (time_keeper_) st_ptr = std::make_unique(__func__, *time_keeper_); stop_watch_ptr_->toc("processing_time", true); manage_collector_list(); - // protect fusion collectors list + + // Lock fusion collectors list std::unique_lock fusion_collectors_lock(fusion_collectors_mutex_); auto rois_timestamp = rclcpp::Time(det2d_msg->header.stamp).seconds(); auto matching_params = std::make_shared(); - matching_params->rois_id = roi_i; + matching_params->rois_id = rois_id; matching_params->rois_timestamp = rois_timestamp; - // For each callback, check whether there is a exist collector that matches this cloud - std::optional>> fusion_collector = - std::nullopt; - - if (!fusion_collectors_.empty()) { - fusion_collector = - fusion_matching_strategy_->match_rois_to_collector(fusion_collectors_, matching_params); - } + // Try to find an existing FusionCollector that matches this ROI + auto fusion_collector = + !fusion_collectors_.empty() + ? fusion_matching_strategy_->match_rois_to_collector(fusion_collectors_, matching_params) + : std::nullopt; bool process_success = false; - if (fusion_collector.has_value()) { - auto collector = fusion_collector.value(); - if (collector) { - fusion_collectors_lock.unlock(); - process_success = fusion_collector.value()->process_rois(roi_i, det2d_msg); - } + if (fusion_collector && fusion_collector.value()) { + fusion_collectors_lock.unlock(); + process_success = fusion_collector.value()->process_rois(rois_id, det2d_msg); } if (!process_success) { auto new_fusion_collector = std::make_shared>( std::dynamic_pointer_cast(shared_from_this()), rois_timeout_sec_, rois_number_, - det2d_list_, false, debug_mode_); + det2d_status_list_, false, debug_mode_); - fusion_collectors_.push_back(new_fusion_collector); + fusion_collectors_.emplace_back(new_fusion_collector); fusion_collectors_lock.unlock(); fusion_matching_strategy_->set_collector_info(new_fusion_collector, matching_params); - (void)new_fusion_collector->process_rois(roi_i, det2d_msg); + (void)new_fusion_collector->process_rois(rois_id, det2d_msg); } if (debugger_) { @@ -466,18 +469,15 @@ void FusionNode::diagnostic_callback( { for (const auto & status : diagnostic_msg->status) { // Filter for the concatenate_and_time_sync_node diagnostic message - if (status.name == "concatenate_data: /sensing/lidar/concatenate_data_status") { - // Temporary map to hold key-value pairs for this status + if ( + status.name == std::string_view("concatenate_data: /sensing/lidar/concatenate_data_status")) { std::unordered_map key_value_map; std::optional concatenate_timestamp_opt; + // First pass: Locate concatenated_cloud_timestamp for (const auto & value : status.values) { - key_value_map[value.key] = value.value; - - // If the key is the concatenated cloud timestamp, try to parse it - if (value.key == "concatenated_cloud_timestamp") { + if (value.key == std::string_view("concatenated_cloud_timestamp")) { try { - // concatenate_timestamp_opt = std::round(std::stod(value.value) * 1000.0) / 1000.0; concatenate_timestamp_opt = std::stod(value.value); } catch (const std::exception & e) { RCLCPP_ERROR(get_logger(), "Error parsing concatenated cloud timestamp: %s", e.what()); @@ -485,30 +485,28 @@ void FusionNode::diagnostic_callback( } } - // Ensure a valid timestamp was parsed before storing + // Second pass: Fill key-value map only if timestamp was valid if (concatenate_timestamp_opt.has_value()) { - concatenated_status_map_[concatenate_timestamp_opt.value()] = key_value_map; - // RCLCPP_INFO( - // get_logger(), "Stored concatenation status for timestamp: %.9f", - // concatenate_timestamp_opt.value()); - } else { - RCLCPP_WARN( - get_logger(), "Missing or invalid concatenated cloud timestamp, status not stored."); + for (const auto & value : status.values) { + key_value_map.emplace(value.key, value.value); + } + + concatenated_status_map_.emplace( + concatenate_timestamp_opt.value(), std::move(key_value_map)); } } } } template -void FusionNode::manage_concatenated_status_map( - const double & current_timestamp) +void FusionNode::manage_concatenated_status_map(double current_timestamp) { - // TOOD(vivid): there might be a better way to do this - // Clean up old entries from concatenated_status_map_ - double threshold = 1.0; // second + constexpr double threshold_seconds = 1.0; // Define threshold as a constant + + // Remove old entries from concatenated_status_map_ auto it = concatenated_status_map_.begin(); while (it != concatenated_status_map_.end()) { - if (current_timestamp - it->first > threshold) { + if (current_timestamp - it->first > threshold_seconds) { RCLCPP_DEBUG( get_logger(), "Removing old concatenation status for timestamp: %.9f", it->first); it = concatenated_status_map_.erase(it); @@ -520,18 +518,25 @@ void FusionNode::manage_concatenated_status_map( template void FusionNode::postprocess( - const Msg3D & processing_msg __attribute__((unused)), - ExportObj & output_msg __attribute__((unused))) + [[maybe_unused]] const Msg3D & processing_msg, [[maybe_unused]] ExportObj & output_msg) { - // do nothing by default + // Default implementation: No postprocessing. + // This function can be overridden by derived classes if needed. } template void FusionNode::publish(const ExportObj & output_msg) { + if (!pub_ptr_) { + RCLCPP_WARN(get_logger(), "Publish failed: pub_ptr_ is null."); + return; + } + if (pub_ptr_->get_subscription_count() < 1) { + RCLCPP_DEBUG(get_logger(), "No subscribers, skipping publish."); return; } + pub_ptr_->publish(output_msg); } @@ -573,7 +578,7 @@ void FusionNode::show_diagnostic_message( std::unordered_map id_to_stamp_map, std::shared_ptr collector_info) { - det3d_fused_ = false; + msg3d_fused_ = false; diagnostic_collector_info_ = std::move(collector_info); diagnostic_id_to_stamp_map_ = std::move(id_to_stamp_map); diagnostic_updater_.force_update(); @@ -583,9 +588,10 @@ template void FusionNode::check_fusion_status( diagnostic_updater::DiagnosticStatusWrapper & stat) { - if (publish_output_msg_ || drop_previous_but_late_output_msg_ || !det3d_fused_) { - stat.add("det3d/is_fused", det3d_fused_); - if (det3d_fused_) { + if (publish_output_msg_ || drop_previous_but_late_output_msg_ || !msg3d_fused_) { + stat.add("msg3d/is_fused", msg3d_fused_); + + if (msg3d_fused_) { stat.add("fused_timestamp", format_timestamp(current_output_msg_timestamp_)); } @@ -604,24 +610,21 @@ void FusionNode::check_fusion_status( } bool rois_miss = false; - - bool fusion_success = true; - if (!det3d_fused_) { - fusion_success = false; - } + bool fusion_success = msg3d_fused_; for (std::size_t id = 0; id < rois_number_; ++id) { + std::string rois_prefix = "rois" + std::to_string(id); bool input_rois_fused = true; - if (diagnostic_id_to_stamp_map_.find(id) != diagnostic_id_to_stamp_map_.end()) { - stat.add( - "rois" + std::to_string(id) + "/timestamp", - format_timestamp(diagnostic_id_to_stamp_map_[id])); + + auto it = diagnostic_id_to_stamp_map_.find(id); + if (it != diagnostic_id_to_stamp_map_.end()) { + stat.add(rois_prefix + "/timestamp", format_timestamp(it->second)); } else { rois_miss = true; fusion_success = false; input_rois_fused = false; } - stat.add("rois" + std::to_string(id) + "/is_fused", input_rois_fused); + stat.add(rois_prefix + "/is_fused", input_rois_fused); } stat.add("fusion_success", fusion_success); @@ -630,31 +633,26 @@ void FusionNode::check_fusion_status( std::string message = "Fused output is published and include all rois and det3d msg"; if (drop_previous_but_late_output_msg_) { - if (rois_miss) { - level = diagnostic_msgs::msg::DiagnosticStatus::ERROR; - message = - "Fused output msg misses some rois and is not published because it arrived " - "too late"; - } else { - level = diagnostic_msgs::msg::DiagnosticStatus::ERROR; - message = "Fused output msg is not published as it is too late"; - } - } else { - if (rois_miss) { - level = diagnostic_msgs::msg::DiagnosticStatus::ERROR; - message = "Fused output msg is published but misses some rois"; - } + level = diagnostic_msgs::msg::DiagnosticStatus::ERROR; + message = + rois_miss + ? "Fused output msg misses some ROIs and is not published because it arrived too late" + : "Fused output msg is not published as it is too late"; + } else if (rois_miss) { + level = diagnostic_msgs::msg::DiagnosticStatus::ERROR; + message = "Fused output msg is published but misses some ROIs"; } stat.summary(level, message); + // Reset status flags publish_output_msg_ = false; drop_previous_but_late_output_msg_ = false; - det3d_fused_ = true; + msg3d_fused_ = true; } else { - const int8_t level = diagnostic_msgs::msg::DiagnosticStatus::OK; - const std::string message = "Fusion node launch successfully, but waiting for input pointcloud"; - stat.summary(level, message); + stat.summary( + diagnostic_msgs::msg::DiagnosticStatus::OK, + "Fusion node launched successfully, but waiting for input pointcloud"); } } diff --git a/perception/autoware_image_projection_based_fusion/src/pointpainting_fusion/node.cpp b/perception/autoware_image_projection_based_fusion/src/pointpainting_fusion/node.cpp index ad4fccdfdd964..a8fd6962a8a95 100644 --- a/perception/autoware_image_projection_based_fusion/src/pointpainting_fusion/node.cpp +++ b/perception/autoware_image_projection_based_fusion/src/pointpainting_fusion/node.cpp @@ -158,7 +158,7 @@ PointPaintingFusionNode::PointPaintingFusionNode(const rclcpp::NodeOptions & opt // subscriber std::function sub_callback = std::bind(&PointPaintingFusionNode::sub_callback, this, std::placeholders::_1); - det3d_sub_ = this->create_subscription( + msg3d_sub_ = this->create_subscription( "~/input/pointcloud", rclcpp::SensorDataQoS().keep_last(3), sub_callback); // publisher From 9661ac834365a0c50b5528bd31f286480a102771 Mon Sep 17 00:00:00 2001 From: vividf Date: Thu, 6 Feb 2025 18:51:51 +0900 Subject: [PATCH 8/8] feat: reuse collector Signed-off-by: vividf --- .../fusion_collector.hpp | 22 +-- .../fusion_matching_strategy.hpp | 8 +- .../fusion_node.hpp | 4 +- .../src/fusion_collector.cpp | 141 ++++++++++------- .../src/fusion_matching_strategy.cpp | 32 +--- .../src/fusion_node.cpp | 142 +++++++++++------- 6 files changed, 205 insertions(+), 144 deletions(-) diff --git a/perception/autoware_image_projection_based_fusion/include/autoware/image_projection_based_fusion/fusion_collector.hpp b/perception/autoware_image_projection_based_fusion/include/autoware/image_projection_based_fusion/fusion_collector.hpp index ebdb8715d4092..3c08e96b5d894 100644 --- a/perception/autoware_image_projection_based_fusion/include/autoware/image_projection_based_fusion/fusion_collector.hpp +++ b/perception/autoware_image_projection_based_fusion/include/autoware/image_projection_based_fusion/fusion_collector.hpp @@ -69,19 +69,23 @@ struct AdvancedCollectorInfo : public FusionCollectorInfoBase } }; +enum class CollectorStatus { Idle, Processing, Finished }; + template class FusionCollector { public: FusionCollector( - std::shared_ptr> && ros2_parent_node, double timeout_sec, - std::size_t rois_number, const std::vector> & det2d_status_list, bool is_3d, + std::shared_ptr> && ros2_parent_node, + std::size_t rois_number, const std::vector> & det2d_status_list, bool debug_mode); - bool process_msg_3d(const typename Msg3D::ConstSharedPtr msg_3d, double msg_3d_timeout); - bool process_rois(const std::size_t & roi_id, const typename Msg2D::ConstSharedPtr det2d_msg); + bool process_msg3d(const typename Msg3D::ConstSharedPtr msg3d, double msg3d_timeout); + bool process_rois( + const std::size_t & rois_id, const typename Msg2D::ConstSharedPtr det2d_msg, + double det2d_timeout); void fusion_callback(); - [[nodiscard]] bool fusion_finished() const; + [[nodiscard]] CollectorStatus get_status() const; void set_info(std::shared_ptr collector_info); [[nodiscard]] std::shared_ptr get_info() const; @@ -90,21 +94,21 @@ class FusionCollector bool rois_exists(const std::size_t & rois_id); bool msg3d_exists(); void show_debug_message(); - void set_period(const int64_t new_period); + void reset(); + void set_period(const std::chrono::nanoseconds period); private: std::shared_ptr> ros2_parent_node_; rclcpp::TimerBase::SharedPtr timer_; - double timeout_sec_; std::size_t rois_number_; typename Msg3D::ConstSharedPtr msg3d_{nullptr}; std::vector> det2d_status_list_; std::unordered_map id_to_rois_map_; - bool is_3d_; + bool is_first_msg3d_{false}; bool debug_mode_; - bool fusion_finished_{false}; std::mutex fusion_mutex_; std::shared_ptr fusion_collector_info_; + CollectorStatus status_; }; } // namespace autoware::image_projection_based_fusion diff --git a/perception/autoware_image_projection_based_fusion/include/autoware/image_projection_based_fusion/fusion_matching_strategy.hpp b/perception/autoware_image_projection_based_fusion/include/autoware/image_projection_based_fusion/fusion_matching_strategy.hpp index 87e65cd6c636a..a6a69bacd8aca 100644 --- a/perception/autoware_image_projection_based_fusion/include/autoware/image_projection_based_fusion/fusion_matching_strategy.hpp +++ b/perception/autoware_image_projection_based_fusion/include/autoware/image_projection_based_fusion/fusion_matching_strategy.hpp @@ -81,7 +81,7 @@ class NaiveMatchingStrategy : public FusionMatchingStrategy> && ros2_parent_node, - const std::size_t rois_number); + const std::unordered_map & id_to_offset_map); [[nodiscard]] std::optional>> match_rois_to_collector( @@ -98,9 +98,9 @@ class NaiveMatchingStrategy : public FusionMatchingStrategy & matching_params) override; private: + std::shared_ptr> ros2_parent_node_; std::unordered_map id_to_offset_map_; double threshold_; - std::shared_ptr> ros2_parent_node_; }; template @@ -109,7 +109,7 @@ class AdvancedMatchingStrategy : public FusionMatchingStrategy> && ros2_parent_node, - const std::size_t rois_number); + const std::unordered_map & id_to_offset_map); [[nodiscard]] std::optional>> match_rois_to_collector( @@ -133,10 +133,10 @@ class AdvancedMatchingStrategy : public FusionMatchingStrategy> ros2_parent_node_; std::unordered_map id_to_offset_map_; std::unordered_map id_to_noise_window_map_; double det3d_noise_window_; - std::shared_ptr> ros2_parent_node_; std::set fractional_timestamp_set_; // Use set to store unique fractional timestamps int success_status_counter_{0}; static constexpr int success_threshold{100}; diff --git a/perception/autoware_image_projection_based_fusion/include/autoware/image_projection_based_fusion/fusion_node.hpp b/perception/autoware_image_projection_based_fusion/include/autoware/image_projection_based_fusion/fusion_node.hpp index 32f72fee5871b..5774aeb2f3fb2 100644 --- a/perception/autoware_image_projection_based_fusion/include/autoware/image_projection_based_fusion/fusion_node.hpp +++ b/perception/autoware_image_projection_based_fusion/include/autoware/image_projection_based_fusion/fusion_node.hpp @@ -100,6 +100,7 @@ class FusionNode : public rclcpp::Node float approx_grid_cell_h_size_; bool debug_mode_{false}; + bool collector_debug_mode_{false}; std::size_t rois_number_; // timer @@ -113,9 +114,7 @@ class FusionNode : public rclcpp::Node std::mutex fusion_collectors_mutex_; std::list>> fusion_collectors_; - std::unordered_map id_to_noise_window_map_; std::unordered_map id_to_offset_map_; - double cloud_noise_window_; // timestamp: (key, value) std::unordered_map> concatenated_status_map_; @@ -131,6 +130,7 @@ class FusionNode : public rclcpp::Node bool drop_previous_but_late_output_msg_{false}; bool publish_output_msg_{false}; bool msg3d_fused_{true}; + static constexpr const int collectors_threshold = 20; protected: void initialize_det2d_status(std::size_t rois_number); diff --git a/perception/autoware_image_projection_based_fusion/src/fusion_collector.cpp b/perception/autoware_image_projection_based_fusion/src/fusion_collector.cpp index cd5703f331625..0be9bdf6ee43a 100644 --- a/perception/autoware_image_projection_based_fusion/src/fusion_collector.cpp +++ b/perception/autoware_image_projection_based_fusion/src/fusion_collector.cpp @@ -30,23 +30,23 @@ namespace autoware::image_projection_based_fusion template FusionCollector::FusionCollector( - std::shared_ptr> && ros2_parent_node, double timeout_sec, - std::size_t rois_number, const std::vector> & det2d_status_list, bool is_3d, - bool debug_mode) + std::shared_ptr> && ros2_parent_node, std::size_t rois_number, + const std::vector> & det2d_status_list, bool debug_mode) : ros2_parent_node_(std::move(ros2_parent_node)), - timeout_sec_(timeout_sec), rois_number_(rois_number), det2d_status_list_(det2d_status_list), - is_3d_(is_3d), debug_mode_(debug_mode) { + status_ = CollectorStatus::Idle; + + auto init_timeout_sec = 1.0; // This will be overwritten when input come const auto period_ns = std::chrono::duration_cast( - std::chrono::duration(timeout_sec_)); + std::chrono::duration(init_timeout_sec)); timer_ = rclcpp::create_timer(ros2_parent_node_, ros2_parent_node_->get_clock(), period_ns, [this]() { std::lock_guard fusion_lock(fusion_mutex_); - if (fusion_finished_) return; + if (status_ == CollectorStatus::Finished) return; fusion_callback(); }); } @@ -65,34 +65,39 @@ std::shared_ptr FusionCollector -bool FusionCollector::process_msg_3d( - const typename Msg3D::ConstSharedPtr msg_3d, double msg_3d_timeout) +bool FusionCollector::process_msg3d( + const typename Msg3D::ConstSharedPtr msg3d, double msg3d_timeout) { std::lock_guard fusion_lock(fusion_mutex_); - if (fusion_finished_) return false; + if (status_ == CollectorStatus::Finished) return false; + + if (status_ == CollectorStatus::Idle) { + // Add det3d to the collector, restart the timer + status_ = CollectorStatus::Processing; + is_first_msg3d_ = true; + const auto period_ns = std::chrono::duration_cast( + std::chrono::duration(msg3d_timeout)); + set_period(period_ns); + timer_->reset(); + } else if (status_ == CollectorStatus::Processing) { + if (msg3d_ != nullptr) { + RCLCPP_WARN_STREAM_THROTTLE( + ros2_parent_node_->get_logger(), *ros2_parent_node_->get_clock(), + std::chrono::milliseconds(10000).count(), + "Msg3d already exists in the collector. Check the timestamp of the msg3d."); + } - if (msg3d_ != nullptr) { - RCLCPP_WARN_STREAM_THROTTLE( - ros2_parent_node_->get_logger(), *ros2_parent_node_->get_clock(), - std::chrono::milliseconds(10000).count(), - "Pointcloud already exists in the collector. Check the timestamp of the pointcloud."); + if (!is_first_msg3d_) { + const auto period_ns = std::chrono::duration_cast( + std::chrono::duration(msg3d_timeout)); + set_period(period_ns); + timer_->reset(); + } } - msg3d_ = msg_3d; + msg3d_ = msg3d; if (ready_to_fuse()) { fusion_callback(); - } else { - if (!is_3d_) { - const auto period_ns = std::chrono::duration_cast( - std::chrono::duration(msg_3d_timeout)); - try { - set_period(period_ns.count()); - } catch (rclcpp::exceptions::RCLError & ex) { - RCLCPP_WARN_THROTTLE( - ros2_parent_node_->get_logger(), *ros2_parent_node_->get_clock(), 5000, "%s", ex.what()); - } - timer_->reset(); - } } return true; @@ -100,17 +105,29 @@ bool FusionCollector::process_msg_3d( template bool FusionCollector::process_rois( - const std::size_t & rois_id, const typename Msg2D::ConstSharedPtr det2d_msg) + const std::size_t & rois_id, const typename Msg2D::ConstSharedPtr det2d_msg, double det2d_timeout) { std::lock_guard fusion_lock(fusion_mutex_); - if (fusion_finished_) return false; - - if (id_to_rois_map_.find(rois_id) != id_to_rois_map_.end()) { - RCLCPP_WARN_STREAM_THROTTLE( - ros2_parent_node_->get_logger(), *ros2_parent_node_->get_clock(), - std::chrono::milliseconds(10000).count(), - "ROIs '" << rois_id << "' already exists in the collector. Check the timestamp of the rois."); + if (status_ == CollectorStatus::Finished) return false; + + if (status_ == CollectorStatus::Idle) { + // Add det2d to the collector, restart the timer + status_ = CollectorStatus::Processing; + is_first_msg3d_ = false; + const auto period_ns = std::chrono::duration_cast( + std::chrono::duration(det2d_timeout)); + set_period(period_ns); + timer_->reset(); + } else if (status_ == CollectorStatus::Processing) { + if (id_to_rois_map_.find(rois_id) != id_to_rois_map_.end()) { + RCLCPP_WARN_STREAM_THROTTLE( + ros2_parent_node_->get_logger(), *ros2_parent_node_->get_clock(), + std::chrono::milliseconds(10000).count(), + "ROIs '" << rois_id + << "' already exists in the collector. Check the timestamp of the rois."); + } } + id_to_rois_map_[rois_id] = det2d_msg; if (ready_to_fuse()) { fusion_callback(); @@ -126,9 +143,9 @@ bool FusionCollector::ready_to_fuse() } template -bool FusionCollector::fusion_finished() const +CollectorStatus FusionCollector::get_status() const { - return fusion_finished_; + return status_; } template @@ -151,7 +168,7 @@ void FusionCollector::fusion_callback() RCLCPP_WARN( ros2_parent_node_->get_logger(), "The Det3d message is not in the fusion collector, so the fusion process will be skipped."); - fusion_finished_ = true; + status_ = CollectorStatus::Finished; // TODO(vivid): call another functino to show the message on diagnostic. ros2_parent_node_->show_diagnostic_message(id_to_stamp_map, fusion_collector_info_); return; @@ -172,7 +189,7 @@ void FusionCollector::fusion_callback() } ros2_parent_node_->export_process(output_det3d_msg, id_to_stamp_map, fusion_collector_info_); - fusion_finished_ = true; + status_ = CollectorStatus::Finished; } template @@ -228,19 +245,41 @@ void FusionCollector::show_debug_message() } template -void FusionCollector::set_period(const int64_t new_period) +void FusionCollector::set_period(const std::chrono::nanoseconds period) { - if (!timer_) { - return; - } - int64_t old_period = 0; - rcl_ret_t ret = rcl_timer_get_period(timer_->get_timer_handle().get(), &old_period); - if (ret != RCL_RET_OK) { - rclcpp::exceptions::throw_from_rcl_error(ret, "Couldn't get old period"); + try { + const auto new_period = period.count(); + if (!timer_) { + return; + } + int64_t old_period = 0; + rcl_ret_t ret = rcl_timer_get_period(timer_->get_timer_handle().get(), &old_period); + if (ret != RCL_RET_OK) { + rclcpp::exceptions::throw_from_rcl_error(ret, "Couldn't get old period"); + } + ret = rcl_timer_exchange_period(timer_->get_timer_handle().get(), new_period, &old_period); + if (ret != RCL_RET_OK) { + rclcpp::exceptions::throw_from_rcl_error(ret, "Couldn't exchange_period"); + } + } catch (rclcpp::exceptions::RCLError & ex) { + RCLCPP_WARN_THROTTLE( + ros2_parent_node_->get_logger(), *ros2_parent_node_->get_clock(), 5000, "%s", ex.what()); } - ret = rcl_timer_exchange_period(timer_->get_timer_handle().get(), new_period, &old_period); - if (ret != RCL_RET_OK) { - rclcpp::exceptions::throw_from_rcl_error(ret, "Couldn't exchange_period"); +} + +template +void FusionCollector::reset() +{ + std::lock_guard lock(fusion_mutex_); + + status_ = CollectorStatus::Idle; // Reset status to Idle + id_to_rois_map_.clear(); + msg3d_ = nullptr; + fusion_collector_info_ = nullptr; + is_first_msg3d_ = false; + + if (timer_ && !timer_->is_canceled()) { + timer_->cancel(); } } diff --git a/perception/autoware_image_projection_based_fusion/src/fusion_matching_strategy.cpp b/perception/autoware_image_projection_based_fusion/src/fusion_matching_strategy.cpp index da78bbd124823..242948773cf8c 100644 --- a/perception/autoware_image_projection_based_fusion/src/fusion_matching_strategy.cpp +++ b/perception/autoware_image_projection_based_fusion/src/fusion_matching_strategy.cpp @@ -26,6 +26,7 @@ #include #include #include +#include #include #include @@ -35,28 +36,15 @@ namespace autoware::image_projection_based_fusion template NaiveMatchingStrategy::NaiveMatchingStrategy( std::shared_ptr> && ros2_parent_node, - const std::size_t rois_number) -: ros2_parent_node_(std::move(ros2_parent_node)) + const std::unordered_map & id_to_offset_map) +: ros2_parent_node_(std::move(ros2_parent_node)), id_to_offset_map_(id_to_offset_map) { if (!ros2_parent_node_) { throw std::runtime_error("ros2_parent_node is nullptr in NaiveMatchingStrategy constructor."); } - auto rois_timestamp_offsets = - ros2_parent_node_->template declare_parameter>("rois_timestamp_offsets"); threshold_ = ros2_parent_node_->template declare_parameter("matching_strategy.threshold"); - if (rois_timestamp_offsets.size() != rois_number) { - throw std::runtime_error( - "Mismatch: rois_number (" + std::to_string(rois_number) + - ") does not match rois_timestamp_offsets size (" + - std::to_string(rois_timestamp_offsets.size()) + ")."); - } - - for (std::size_t i = 0; i < rois_number; i++) { - id_to_offset_map_[i] = rois_timestamp_offsets[i]; - } - RCLCPP_INFO(ros2_parent_node_->get_logger(), "Utilize naive matching strategy for fusion nodes."); } @@ -148,28 +136,21 @@ void NaiveMatchingStrategy::set_collector_info( template AdvancedMatchingStrategy::AdvancedMatchingStrategy( std::shared_ptr> && ros2_parent_node, - const std::size_t rois_number) -: ros2_parent_node_(std::move(ros2_parent_node)) + const std::unordered_map & id_to_offset_map) +: ros2_parent_node_(std::move(ros2_parent_node)), id_to_offset_map_(id_to_offset_map) { if (!ros2_parent_node_) { throw std::runtime_error( "ros2_parent_node is nullptr in AdvancedMatchingStrategy constructor."); } - auto rois_timestamp_offsets = - ros2_parent_node_->template declare_parameter>("rois_timestamp_offsets"); det3d_noise_window_ = ros2_parent_node_->template declare_parameter("matching_strategy.det3d_noise_window"); auto rois_timestamp_noise_window = ros2_parent_node_->template declare_parameter>( "matching_strategy.rois_timestamp_noise_window"); - if (rois_timestamp_offsets.size() != rois_number) { - throw std::runtime_error( - "Mismatch: rois_number (" + std::to_string(rois_number) + - ") does not match rois_timestamp_offsets size (" + - std::to_string(rois_timestamp_offsets.size()) + ")."); - } + auto rois_number = id_to_offset_map_.size(); if (rois_timestamp_noise_window.size() != rois_number) { throw std::runtime_error( "Mismatch: rois_number (" + std::to_string(rois_number) + @@ -178,7 +159,6 @@ AdvancedMatchingStrategy::AdvancedMatchingStrategy( } for (size_t i = 0; i < rois_number; i++) { - id_to_offset_map_[i] = rois_timestamp_offsets[i]; id_to_noise_window_map_[i] = rois_timestamp_noise_window[i]; } diff --git a/perception/autoware_image_projection_based_fusion/src/fusion_node.cpp b/perception/autoware_image_projection_based_fusion/src/fusion_node.cpp index 1fc4dcd2d0c3a..da1efc9a106b4 100644 --- a/perception/autoware_image_projection_based_fusion/src/fusion_node.cpp +++ b/perception/autoware_image_projection_based_fusion/src/fusion_node.cpp @@ -72,6 +72,18 @@ FusionNode::FusionNode( msg3d_timeout_sec_ = declare_parameter("msg3d_timeout_sec"); rois_timeout_sec_ = declare_parameter("rois_timeout_sec"); + auto rois_timestamp_offsets = declare_parameter>("rois_timestamp_offsets"); + if (rois_timestamp_offsets.size() != rois_number_) { + throw std::runtime_error( + "Mismatch: rois_number (" + std::to_string(rois_number_) + + ") does not match rois_timestamp_offsets size (" + + std::to_string(rois_timestamp_offsets.size()) + ")."); + } + + for (std::size_t i = 0; i < rois_number_; i++) { + id_to_offset_map_[i] = rois_timestamp_offsets[i]; + } + std::vector input_rois_topics; std::vector input_camera_info_topics; @@ -157,6 +169,7 @@ FusionNode::FusionNode( debug_internal_pub_ = std::make_unique(this, get_name()); } + collector_debug_mode_ = declare_parameter("collector_debug_mode"); // time keeper bool use_time_keeper = declare_parameter("publish_processing_time_detail"); @@ -187,10 +200,10 @@ void FusionNode::init_strategy() { if (matching_strategy_ == "naive") { fusion_matching_strategy_ = std::make_unique>( - std::dynamic_pointer_cast(shared_from_this()), rois_number_); + std::dynamic_pointer_cast(shared_from_this()), id_to_offset_map_); } else if (matching_strategy_ == "advanced") { fusion_matching_strategy_ = std::make_unique>( - std::dynamic_pointer_cast(shared_from_this()), rois_number_); + std::dynamic_pointer_cast(shared_from_this()), id_to_offset_map_); // subscribe diagnostics sub_diag_ = this->create_subscription( "/diagnostics", 10, std::bind(&FusionNode::diagnostic_callback, this, std::placeholders::_1)); @@ -321,7 +334,7 @@ void FusionNode::sub_callback(const typename Msg3D::Con } // Debug logging for message latency - if (debug_mode_) { + if (collector_debug_mode_) { auto arrive_time = this->get_clock()->now().seconds(); auto msg3d_stamp = rclcpp::Time(msg3d->header.stamp).seconds(); RCLCPP_DEBUG( @@ -353,25 +366,33 @@ void FusionNode::sub_callback(const typename Msg3D::Con bool process_success = false; if (fusion_collector && fusion_collector.value()) { fusion_collectors_lock.unlock(); // Unlock before processing - process_success = fusion_collector.value()->process_msg_3d(msg3d, msg3d_timeout_sec_); + process_success = fusion_collector.value()->process_msg3d(msg3d, msg3d_timeout_sec_); } - // If no matching collector, create a new one + // Didn't find matched collector, create a new collector if (!process_success) { - auto new_fusion_collector = std::make_shared>( - std::dynamic_pointer_cast(shared_from_this()), msg3d_timeout_sec_, rois_number_, - det2d_status_list_, true, debug_mode_); + std::shared_ptr> selected_collector = nullptr; - fusion_collectors_.emplace_back(new_fusion_collector); - fusion_collectors_lock.unlock(); + // Reuse the collector if the status is IDLE + for (auto & collector : fusion_collectors_) { + if (collector->get_status() == CollectorStatus::Idle) { + selected_collector = collector; + break; + } + } - fusion_matching_strategy_->set_collector_info(new_fusion_collector, matching_params); - (void)new_fusion_collector->process_msg_3d(msg3d, msg3d_timeout_sec_); - } + if (!selected_collector) { + // If no idle collector exists, create a new collector + selected_collector = std::make_shared>( + std::dynamic_pointer_cast(shared_from_this()), rois_number_, det2d_status_list_, + collector_debug_mode_); - // TODO(vivid): check the logic of clearing debugger. - if (debugger_) { - debugger_->clear(); + fusion_collectors_.emplace_back(selected_collector); + } + + fusion_collectors_lock.unlock(); + fusion_matching_strategy_->set_collector_info(selected_collector, matching_params); + (void)selected_collector->process_msg3d(msg3d, msg3d_timeout_sec_); } if (matching_strategy_ == "advanced") { @@ -379,17 +400,9 @@ void FusionNode::sub_callback(const typename Msg3D::Con manage_concatenated_status_map(msg3d_timestamp); } - // // add timestamp interval for debug - // if (debug_internal_pub_) { - // double timestamp_interval_ms = (matched_stamp - timestamp_nsec) / 1e6; - // debug_internal_pub_->publish( - // "debug/roi" + std::to_string(roi_i) + "/timestamp_interval_ms", timestamp_interval_ms); - // debug_internal_pub_->publish( - // "debug/roi" + std::to_string(roi_i) + "/timestamp_interval_offset_ms", - // timestamp_interval_ms - det2d.input_offset_ms); - // } - - // processing_time_ms = stop_watch_ptr_->toc("processing_time", true); + if (debugger_) { + debugger_->clear(); + } } template @@ -400,7 +413,7 @@ void FusionNode::rois_callback( init_strategy(); } - if (debug_mode_) { + if (collector_debug_mode_) { auto arrive_time = this->get_clock()->now().seconds(); RCLCPP_DEBUG( this->get_logger(), " rois %zu timestamp: %lf arrive time: %lf seconds, latency: %lf", @@ -432,35 +445,38 @@ void FusionNode::rois_callback( bool process_success = false; if (fusion_collector && fusion_collector.value()) { fusion_collectors_lock.unlock(); - process_success = fusion_collector.value()->process_rois(rois_id, det2d_msg); + process_success = fusion_collector.value()->process_rois(rois_id, det2d_msg, rois_timeout_sec_); } + // Didn't find matched collector, create a new collector if (!process_success) { - auto new_fusion_collector = std::make_shared>( - std::dynamic_pointer_cast(shared_from_this()), rois_timeout_sec_, rois_number_, - det2d_status_list_, false, debug_mode_); + std::shared_ptr> selected_collector = nullptr; - fusion_collectors_.emplace_back(new_fusion_collector); - fusion_collectors_lock.unlock(); + // Reuse the collector if the status is IDLE + for (auto & collector : fusion_collectors_) { + if (collector->get_status() == CollectorStatus::Idle) { + selected_collector = collector; + break; + } + } - fusion_matching_strategy_->set_collector_info(new_fusion_collector, matching_params); - (void)new_fusion_collector->process_rois(rois_id, det2d_msg); + if (!selected_collector) { + // If no idle collector exists, create a new collector + selected_collector = std::make_shared>( + std::dynamic_pointer_cast(shared_from_this()), rois_number_, det2d_status_list_, + collector_debug_mode_); + + fusion_collectors_.emplace_back(selected_collector); + } + + fusion_collectors_lock.unlock(); + fusion_matching_strategy_->set_collector_info(selected_collector, matching_params); + (void)selected_collector->process_rois(rois_id, det2d_msg, rois_timeout_sec_); } if (debugger_) { debugger_->clear(); } - - // if (debug_internal_pub_) { - // double timestamp_interval_ms = (timestamp_nsec - cached_det3d_msg_timestamp_) / 1e6; - // debug_internal_pub_->publish( - // "debug/roi" + std::to_string(roi_i) + "/timestamp_interval_ms", timestamp_interval_ms); - // debug_internal_pub_->publish( - // "debug/roi" + std::to_string(roi_i) + "/timestamp_interval_offset_ms", - // timestamp_interval_ms - det2d.input_offset_ms); - // } - - // processing_time_ms = processing_time_ms + stop_watch_ptr_->toc("processing_time", true); } template @@ -545,13 +561,19 @@ void FusionNode::manage_collector_list() { std::lock_guard collectors_lock(fusion_collectors_mutex_); - for (auto it = fusion_collectors_.begin(); it != fusion_collectors_.end();) { - if ((*it)->fusion_finished()) { - it = fusion_collectors_.erase(it); // Erase and move the iterator to the next element - } else { - ++it; // Move to the next element + for (auto & collector : fusion_collectors_) { + if (collector->get_status() == CollectorStatus::Finished) { + collector->reset(); } } + + if (fusion_collectors_.size() > collectors_threshold) { + RCLCPP_WARN_STREAM_THROTTLE( + get_logger(), *get_clock(), 1000, + "The number of cloud collectors (" << fusion_collectors_.size() << ") exceeds the threshold (" + << collectors_threshold + << "), be careful if it keeps increasing."); + } } template @@ -641,6 +663,9 @@ void FusionNode::check_fusion_status( } else if (rois_miss) { level = diagnostic_msgs::msg::DiagnosticStatus::ERROR; message = "Fused output msg is published but misses some ROIs"; + } else if (!msg3d_fused_) { + level = diagnostic_msgs::msg::DiagnosticStatus::ERROR; + message = "Fused output msg is not published as msg3d is missed"; } stat.summary(level, message); @@ -654,6 +679,19 @@ void FusionNode::check_fusion_status( diagnostic_msgs::msg::DiagnosticStatus::OK, "Fusion node launched successfully, but waiting for input pointcloud"); } + + // debug + if (debug_internal_pub_) { + for (std::size_t rois_id = 0; rois_id < rois_number_; ++rois_id) { + auto rois_timestamp = diagnostic_id_to_stamp_map_[rois_id]; + auto timestamp_interval_ms = rois_timestamp - current_output_msg_timestamp_; + debug_internal_pub_->publish( + "debug/roi" + std::to_string(rois_id) + "/timestamp_interval_ms", timestamp_interval_ms); + debug_internal_pub_->publish( + "debug/roi" + std::to_string(rois_id) + "/timestamp_interval_offset_ms", + timestamp_interval_ms - id_to_offset_map_[rois_id] * 1000); + } + } } // Explicit instantiation for the supported types