From ae2ec9684c7114eda4335bb67169085ef85a4941 Mon Sep 17 00:00:00 2001 From: Taekjin LEE Date: Wed, 25 Dec 2024 15:25:51 +0900 Subject: [PATCH 01/33] chore: input_camera_topics_ is only for debug Signed-off-by: Taekjin LEE --- .../image_projection_based_fusion/fusion_node.hpp | 1 - .../src/fusion_node.cpp | 15 +++++++++------ 2 files changed, 9 insertions(+), 7 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 239d406650ce8..daecf0871c48e 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 @@ -118,7 +118,6 @@ class FusionNode : public rclcpp::Node double match_threshold_ms_{}; std::vector input_rois_topics_; std::vector input_camera_info_topics_; - std::vector input_camera_topics_; /** \brief A vector of subscriber. */ typename rclcpp::Subscription::SharedPtr sub_; 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 ea0904215cb86..e531e8c6919d8 100644 --- a/perception/autoware_image_projection_based_fusion/src/fusion_node.cpp +++ b/perception/autoware_image_projection_based_fusion/src/fusion_node.cpp @@ -28,6 +28,7 @@ #include #include #include +#include #ifdef ROS_DISTRO_GALACTIC #include @@ -68,7 +69,6 @@ FusionNode::FusionNode( timeout_ms_ = declare_parameter("timeout_ms"); input_rois_topics_.resize(rois_number_); - input_camera_topics_.resize(rois_number_); input_camera_info_topics_.resize(rois_number_); for (std::size_t roi_i = 0; roi_i < rois_number_; ++roi_i) { @@ -79,10 +79,6 @@ FusionNode::FusionNode( input_camera_info_topics_.at(roi_i) = declare_parameter( "input/camera_info" + std::to_string(roi_i), "/sensing/camera/camera" + std::to_string(roi_i) + "/camera_info"); - - 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"); } input_offset_ms_ = declare_parameter>("input_offset_ms"); @@ -155,10 +151,17 @@ FusionNode::FusionNode( // debugger if (declare_parameter("debug_mode", false)) { + 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.at(roi_i) = declare_parameter( + "input/image" + std::to_string(roi_i), + "/sensing/camera/camera" + std::to_string(roi_i) + "/image_rect_color"); + } std::size_t 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); } // time keeper From da3545b117298159adfef9a6029c763774c537b7 Mon Sep 17 00:00:00 2001 From: Taekjin LEE Date: Wed, 25 Dec 2024 16:28:42 +0900 Subject: [PATCH 02/33] feat: fuse main message with cached roi messages in fusion_node.cpp Signed-off-by: Taekjin LEE --- .../src/fusion_node.cpp | 120 ++++++++++-------- 1 file changed, 68 insertions(+), 52 deletions(-) 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 e531e8c6919d8..4ec4fc04bb7a6 100644 --- a/perception/autoware_image_projection_based_fusion/src/fusion_node.cpp +++ b/perception/autoware_image_projection_based_fusion/src/fusion_node.cpp @@ -226,11 +226,12 @@ void FusionNode::subCallback( if (time_keeper_) st_ptr = std::make_unique(__func__, *time_keeper_); if (cached_msg_.second != 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); timer_->cancel(); postprocess(*(cached_msg_.second)); publish(*(cached_msg_.second)); - std::fill(is_fused_.begin(), is_fused_.end(), false); // add processing time for debug if (debug_publisher_) { @@ -250,9 +251,12 @@ void FusionNode::subCallback( processing_time_ms = 0; } + // reset flags + std::fill(is_fused_.begin(), is_fused_.end(), false); cached_msg_.second = nullptr; } + // TIMING: reset timer to the timeout time std::lock_guard lock(mutex_cached_msgs_); auto period = std::chrono::duration_cast( std::chrono::duration(timeout_ms_)); @@ -265,77 +269,77 @@ void FusionNode::subCallback( stop_watch_ptr_->toc("processing_time", true); + // PROCESS: preprocess the main message typename TargetMsg3D::SharedPtr output_msg = std::make_shared(*input_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; - - // if matching rois exist, fuseOnSingle - // please ask maintainers before parallelize this loop because debugger is not thread safe + // for loop for each roi for (std::size_t roi_i = 0; roi_i < rois_number_; ++roi_i) { + // check camera info if (camera_info_map_.find(roi_i) == camera_info_map_.end()) { RCLCPP_WARN_THROTTLE( this->get_logger(), *this->get_clock(), 5000, "no camera info. id is %zu", roi_i); continue; } - - if ((cached_roi_msgs_.at(roi_i)).size() > 0) { - int64_t min_interval = 1e9; - int64_t matched_stamp = -1; - std::list outdate_stamps; - - for (const auto & [k, v] : cached_roi_msgs_.at(roi_i)) { - int64_t new_stamp = timestamp_nsec + input_offset_ms_.at(roi_i) * static_cast(1e6); - int64_t interval = abs(static_cast(k) - new_stamp); - - if ( - interval <= min_interval && interval <= match_threshold_ms_ * static_cast(1e6)) { - min_interval = interval; - matched_stamp = k; - } else if ( - static_cast(k) < new_stamp && - interval > match_threshold_ms_ * static_cast(1e6)) { - outdate_stamps.push_back(static_cast(k)); - } + auto & roi_msgs = cached_roi_msgs_.at(roi_i); + + // check if the roi is collected + if (roi_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] : roi_msgs) { + int64_t new_stamp = timestamp_nsec + input_offset_ms_.at(roi_i) * 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) { + roi_msgs.erase(stamp); + } - // remove outdated stamps - for (auto stamp : outdate_stamps) { - (cached_roi_msgs_.at(roi_i)).erase(stamp); + // PROCESS: if matched, fuse the main message with the roi message + if (matched_stamp != -1) { + if (debugger_) { + debugger_->clear(); } - // fuseOnSingle - if (matched_stamp != -1) { - if (debugger_) { - debugger_->clear(); - } - - fuseOnSingleImage( - *input_msg, roi_i, *((cached_roi_msgs_.at(roi_i))[matched_stamp]), *output_msg); - (cached_roi_msgs_.at(roi_i)).erase(matched_stamp); - is_fused_.at(roi_i) = true; + fuseOnSingleImage(*input_msg, roi_i, *(roi_msgs[matched_stamp]), *output_msg); + roi_msgs.erase(matched_stamp); + is_fused_.at(roi_i) = true; - // add timestamp interval for debug - if (debug_publisher_) { - double timestamp_interval_ms = (matched_stamp - timestamp_nsec) / 1e6; - debug_publisher_->publish( - "debug/roi" + std::to_string(roi_i) + "/timestamp_interval_ms", timestamp_interval_ms); - debug_publisher_->publish( - "debug/roi" + std::to_string(roi_i) + "/timestamp_interval_offset_ms", - timestamp_interval_ms - input_offset_ms_.at(roi_i)); - } + // add timestamp interval for debug + if (debug_publisher_) { + double timestamp_interval_ms = (matched_stamp - timestamp_nsec) / 1e6; + debug_publisher_->publish( + "debug/roi" + std::to_string(roi_i) + "/timestamp_interval_ms", timestamp_interval_ms); + debug_publisher_->publish( + "debug/roi" + std::to_string(roi_i) + "/timestamp_interval_offset_ms", + timestamp_interval_ms - input_offset_ms_.at(roi_i)); } } } - // if all camera fused, postprocess; else, publish the old Msg(if exists) and cache the current - // Msg + // PROCESS: if all camera fused, postprocess and publish the main message if (std::count(is_fused_.begin(), is_fused_.end(), true) == static_cast(rois_number_)) { timer_->cancel(); postprocess(*output_msg); publish(*output_msg); + + // reset flags std::fill(is_fused_.begin(), is_fused_.end(), false); cached_msg_.second = nullptr; @@ -350,6 +354,8 @@ void FusionNode::subCallback( processing_time_ms = 0; } } else { + // PROCESS: if all of rois are not collected, publish the old Msg(if exists) and cache the + // current Msg cached_msg_.first = timestamp_nsec; cached_msg_.second = output_msg; processing_time_ms = stop_watch_ptr_->toc("processing_time", true); @@ -373,18 +379,21 @@ void FusionNode::roiCallback( int64_t new_stamp = cached_msg_.first + input_offset_ms_.at(roi_i) * 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) && is_fused_.at(roi_i) == false) { + // check camera info if (camera_info_map_.find(roi_i) == camera_info_map_.end()) { RCLCPP_WARN_THROTTLE( this->get_logger(), *this->get_clock(), 5000, "no camera info. id is %zu", roi_i); (cached_roi_msgs_.at(roi_i))[timestamp_nsec] = input_roi_msg; return; } + if (debugger_) { debugger_->clear(); } - + // PROCESS: fuse the main message with the roi message fuseOnSingleImage(*(cached_msg_.second), roi_i, *input_roi_msg, *(cached_msg_.second)); is_fused_.at(roi_i) = true; @@ -397,12 +406,11 @@ void FusionNode::roiCallback( timestamp_interval_ms - input_offset_ms_.at(roi_i)); } + // PROCESS: if all camera fused, postprocess and publish the main message if (std::count(is_fused_.begin(), is_fused_.end(), true) == static_cast(rois_number_)) { timer_->cancel(); postprocess(*(cached_msg_.second)); publish(*(cached_msg_.second)); - std::fill(is_fused_.begin(), is_fused_.end(), false); - cached_msg_.second = nullptr; // add processing time for debug if (debug_publisher_) { @@ -414,6 +422,10 @@ void FusionNode::roiCallback( processing_time_ms + stop_watch_ptr_->toc("processing_time", true)); processing_time_ms = 0; } + + // reset flags + std::fill(is_fused_.begin(), is_fused_.end(), false); + cached_msg_.second = nullptr; } processing_time_ms = processing_time_ms + stop_watch_ptr_->toc("processing_time", true); return; @@ -439,7 +451,7 @@ void FusionNode::timer_callback() using std::chrono_literals::operator""ms; timer_->cancel(); if (mutex_cached_msgs_.try_lock()) { - // timeout, postprocess cached msg + // PROCESS: if timeout, postprocess cached msg if (cached_msg_.second != nullptr) { stop_watch_ptr_->toc("processing_time", true); @@ -457,11 +469,14 @@ void FusionNode::timer_callback() processing_time_ms = 0; } } + + // reset flags std::fill(is_fused_.begin(), is_fused_.end(), false); cached_msg_.second = nullptr; mutex_cached_msgs_.unlock(); } else { + // TIMING: retry the process after 10ms try { std::chrono::nanoseconds period = 10ms; setPeriod(period.count()); @@ -498,6 +513,7 @@ void FusionNode::publish(const TargetMsg3D & output_msg pub_ptr_->publish(output_msg); } +// explicit instantiation for the supported types template class FusionNode; template class FusionNode< DetectedObjectsWithFeature, DetectedObjectWithFeature, DetectedObjectsWithFeature>; From 4a7cce81efd94225bf147bfe882298e91ed970fc Mon Sep 17 00:00:00 2001 From: Taekjin LEE Date: Wed, 25 Dec 2024 16:56:36 +0900 Subject: [PATCH 03/33] chore: add comments on each process step, organize methods Signed-off-by: Taekjin LEE --- .../fusion_node.hpp | 25 ++++++++----------- .../src/fusion_node.cpp | 7 +++--- 2 files changed, 13 insertions(+), 19 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 daecf0871c48e..44f8340c07ba1 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 @@ -72,32 +72,27 @@ class FusionNode : public rclcpp::Node const std::string & node_name, const rclcpp::NodeOptions & options, int queue_size); protected: + // Common process methods void cameraInfoCallback( const sensor_msgs::msg::CameraInfo::ConstSharedPtr input_camera_info_msg, const std::size_t camera_id); - - virtual void preprocess(TargetMsg3D & output_msg); - - // callback for Msg subscription - virtual void subCallback(const typename TargetMsg3D::ConstSharedPtr input_msg); - + // callback for main subscription + void subCallback(const typename TargetMsg3D::ConstSharedPtr input_msg); // callback for roi subscription + void roiCallback(const typename Msg2D::ConstSharedPtr input_roi_msg, const std::size_t roi_i); + // callback for timer + void timer_callback(); + void setPeriod(const int64_t new_period); - virtual void roiCallback( - const typename Msg2D::ConstSharedPtr input_roi_msg, const std::size_t roi_i); - + // Custom process methods + virtual void preprocess(TargetMsg3D & output_msg); virtual void fuseOnSingleImage( const TargetMsg3D & input_msg, const std::size_t image_id, const Msg2D & input_roi_msg, TargetMsg3D & output_msg) = 0; - - // set args if you need virtual void postprocess(TargetMsg3D & output_msg); - virtual void publish(const TargetMsg3D & output_msg); - void timer_callback(); - void setPeriod(const int64_t new_period); - + // Members std::size_t rois_number_{1}; tf2_ros::Buffer tf_buffer_; tf2_ros::TransformListener tf_listener_; 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 4ec4fc04bb7a6..0db1131240deb 100644 --- a/perception/autoware_image_projection_based_fusion/src/fusion_node.cpp +++ b/perception/autoware_image_projection_based_fusion/src/fusion_node.cpp @@ -339,10 +339,6 @@ void FusionNode::subCallback( postprocess(*output_msg); publish(*output_msg); - // reset flags - std::fill(is_fused_.begin(), is_fused_.end(), false); - cached_msg_.second = nullptr; - // add processing time for debug if (debug_publisher_) { const double cyclic_time_ms = stop_watch_ptr_->toc("cyclic_time", true); @@ -353,6 +349,9 @@ void FusionNode::subCallback( "debug/processing_time_ms", processing_time_ms); processing_time_ms = 0; } + // reset flags + std::fill(is_fused_.begin(), is_fused_.end(), false); + cached_msg_.second = nullptr; } else { // PROCESS: if all of rois are not collected, publish the old Msg(if exists) and cache the // current Msg From eccdf9687eda752d5ef36de4b6e89dbfae03c4cd Mon Sep 17 00:00:00 2001 From: Taekjin LEE Date: Wed, 25 Dec 2024 17:23:53 +0900 Subject: [PATCH 04/33] feat: Export process method in fusion_node.cpp Export the `exportProcess()` method in `fusion_node.cpp` to handle the post-processing and publishing of the fused messages. This method cancels the timer, performs the necessary post-processing steps, publishes the output message, and resets the flags. It also adds processing time for debugging purposes. This change improves the organization and readability of the code. Signed-off-by: Taekjin LEE --- .../fusion_node.hpp | 1 + .../src/fusion_node.cpp | 118 ++++++------------ 2 files changed, 42 insertions(+), 77 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 44f8340c07ba1..72393c67477de 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 @@ -83,6 +83,7 @@ class FusionNode : public rclcpp::Node // callback for timer void timer_callback(); void setPeriod(const int64_t new_period); + void exportProcess(); // Custom process methods virtual void preprocess(TargetMsg3D & output_msg); 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 0db1131240deb..4b2f4246912c7 100644 --- a/perception/autoware_image_projection_based_fusion/src/fusion_node.cpp +++ b/perception/autoware_image_projection_based_fusion/src/fusion_node.cpp @@ -218,6 +218,37 @@ void FusionNode::preprocess(TargetMsg3D & ouput_msg // do nothing by default } +template +void FusionNode::exportProcess() +{ + timer_->cancel(); + + postprocess(*(cached_msg_.second)); + publish(*(cached_msg_.second)); + + // add processing time for debug + if (debug_publisher_) { + const double cyclic_time_ms = stop_watch_ptr_->toc("cyclic_time", true); + const double pipeline_latency_ms = + std::chrono::duration( + std::chrono::nanoseconds( + (this->get_clock()->now() - cached_msg_.second->header.stamp).nanoseconds())) + .count(); + debug_publisher_->publish( + "debug/cyclic_time_ms", cyclic_time_ms); + debug_publisher_->publish( + "debug/processing_time_ms", + processing_time_ms + stop_watch_ptr_->toc("processing_time", true)); + debug_publisher_->publish( + "debug/pipeline_latency_ms", pipeline_latency_ms); + processing_time_ms = 0; + } + + // reset flags + std::fill(is_fused_.begin(), is_fused_.end(), false); + cached_msg_.second = nullptr; +} + template void FusionNode::subCallback( const typename TargetMsg3D::ConstSharedPtr input_msg) @@ -225,39 +256,17 @@ void FusionNode::subCallback( std::unique_ptr st_ptr; if (time_keeper_) st_ptr = std::make_unique(__func__, *time_keeper_); + std::lock_guard lock(mutex_cached_msgs_); + if (cached_msg_.second != 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); - timer_->cancel(); - postprocess(*(cached_msg_.second)); - publish(*(cached_msg_.second)); - - // add processing time for debug - if (debug_publisher_) { - const double cyclic_time_ms = stop_watch_ptr_->toc("cyclic_time", true); - const double pipeline_latency_ms = - std::chrono::duration( - std::chrono::nanoseconds( - (this->get_clock()->now() - cached_msg_.second->header.stamp).nanoseconds())) - .count(); - debug_publisher_->publish( - "debug/cyclic_time_ms", cyclic_time_ms); - debug_publisher_->publish( - "debug/processing_time_ms", - processing_time_ms + stop_watch_ptr_->toc("processing_time", true)); - debug_publisher_->publish( - "debug/pipeline_latency_ms", pipeline_latency_ms); - processing_time_ms = 0; - } - // reset flags - std::fill(is_fused_.begin(), is_fused_.end(), false); - cached_msg_.second = nullptr; + exportProcess(); } // TIMING: reset timer to the timeout time - std::lock_guard lock(mutex_cached_msgs_); auto period = std::chrono::duration_cast( std::chrono::duration(timeout_ms_)); try { @@ -332,31 +341,15 @@ void FusionNode::subCallback( } } } + cached_msg_.first = timestamp_nsec; + cached_msg_.second = output_msg; // PROCESS: if all camera fused, postprocess and publish the main message if (std::count(is_fused_.begin(), is_fused_.end(), true) == static_cast(rois_number_)) { - timer_->cancel(); - postprocess(*output_msg); - publish(*output_msg); - - // add processing time for debug - if (debug_publisher_) { - const double cyclic_time_ms = stop_watch_ptr_->toc("cyclic_time", true); - processing_time_ms = stop_watch_ptr_->toc("processing_time", true); - debug_publisher_->publish( - "debug/cyclic_time_ms", cyclic_time_ms); - debug_publisher_->publish( - "debug/processing_time_ms", processing_time_ms); - processing_time_ms = 0; - } - // reset flags - std::fill(is_fused_.begin(), is_fused_.end(), false); - cached_msg_.second = nullptr; + exportProcess(); } else { // PROCESS: if all of rois are not collected, publish the old Msg(if exists) and cache the // current Msg - cached_msg_.first = timestamp_nsec; - cached_msg_.second = output_msg; processing_time_ms = stop_watch_ptr_->toc("processing_time", true); } } @@ -407,24 +400,8 @@ void FusionNode::roiCallback( // PROCESS: if all camera fused, postprocess and publish the main message if (std::count(is_fused_.begin(), is_fused_.end(), true) == static_cast(rois_number_)) { - timer_->cancel(); - postprocess(*(cached_msg_.second)); - publish(*(cached_msg_.second)); - - // add processing time for debug - if (debug_publisher_) { - const double cyclic_time_ms = stop_watch_ptr_->toc("cyclic_time", true); - debug_publisher_->publish( - "debug/cyclic_time_ms", cyclic_time_ms); - debug_publisher_->publish( - "debug/processing_time_ms", - processing_time_ms + stop_watch_ptr_->toc("processing_time", true)); - processing_time_ms = 0; - } - - // reset flags - std::fill(is_fused_.begin(), is_fused_.end(), false); - cached_msg_.second = nullptr; + std::lock_guard lock(mutex_cached_msgs_); + exportProcess(); } processing_time_ms = processing_time_ms + stop_watch_ptr_->toc("processing_time", true); return; @@ -453,23 +430,10 @@ void FusionNode::timer_callback() // PROCESS: if timeout, postprocess cached msg if (cached_msg_.second != nullptr) { stop_watch_ptr_->toc("processing_time", true); - - postprocess(*(cached_msg_.second)); - publish(*(cached_msg_.second)); - - // add processing time for debug - if (debug_publisher_) { - const double cyclic_time_ms = stop_watch_ptr_->toc("cyclic_time", true); - debug_publisher_->publish( - "debug/cyclic_time_ms", cyclic_time_ms); - debug_publisher_->publish( - "debug/processing_time_ms", - processing_time_ms + stop_watch_ptr_->toc("processing_time", true)); - processing_time_ms = 0; - } + exportProcess(); } - // reset flags + // reset flags whether the message is fused or not std::fill(is_fused_.begin(), is_fused_.end(), false); cached_msg_.second = nullptr; From 8405ae7d71636941da11863db3a409905c71ab2f Mon Sep 17 00:00:00 2001 From: Taekjin LEE Date: Wed, 25 Dec 2024 18:34:38 +0900 Subject: [PATCH 05/33] feat: Refactor fusion_node.hpp and fusion_node.cpp Refactor the `fusion_node.hpp` and `fusion_node.cpp` files to improve code organization and readability. This includes exporting the `exportProcess()` method in `fusion_node.cpp` to handle post-processing and publishing of fused messages, adding comments on each process step, organizing methods, and fusing the main message with cached ROI messages. These changes enhance the overall quality of the codebase. Signed-off-by: Taekjin LEE --- .../fusion_node.hpp | 35 ++++++++- .../src/fusion_node.cpp | 74 ++++++++++++------- 2 files changed, 81 insertions(+), 28 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 72393c67477de..ec94765b50ec7 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 @@ -59,6 +59,35 @@ using PointCloud = pcl::PointCloud; using autoware::image_projection_based_fusion::CameraProjection; using autoware_perception_msgs::msg::ObjectClassification; +template +struct Det2dManager +{ + // 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 +bool checkAllDet2dFused(const std::vector> & det2d_list) +{ + for (const auto & det2d : det2d_list) { + if (!det2d.is_fused) { + return false; + } + } + return true; +} + template class FusionNode : public rclcpp::Node { @@ -101,6 +130,8 @@ class FusionNode : public rclcpp::Node std::vector point_project_to_unrectified_image_; // camera_info + std::vector> det2d_list_; + std::map camera_info_map_; std::vector::SharedPtr> camera_info_subs_; // camera projection @@ -112,8 +143,6 @@ class FusionNode : public rclcpp::Node rclcpp::TimerBase::SharedPtr timer_; double timeout_ms_{}; double match_threshold_ms_{}; - std::vector input_rois_topics_; - std::vector input_camera_info_topics_; /** \brief A vector of subscriber. */ typename rclcpp::Subscription::SharedPtr sub_; @@ -126,7 +155,7 @@ class FusionNode : public rclcpp::Node std::vector is_fused_; std::pair cached_msg_; // first element is the timestamp in nanoseconds, second element is the message - std::vector> cached_roi_msgs_; + std::vector> cached_det2d_msgs_; std::mutex mutex_cached_msgs_; // output publisher 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 4b2f4246912c7..da3ce9f8a2734 100644 --- a/perception/autoware_image_projection_based_fusion/src/fusion_node.cpp +++ b/perception/autoware_image_projection_based_fusion/src/fusion_node.cpp @@ -68,15 +68,17 @@ FusionNode::FusionNode( match_threshold_ms_ = declare_parameter("match_threshold_ms"); timeout_ms_ = declare_parameter("timeout_ms"); - input_rois_topics_.resize(rois_number_); - input_camera_info_topics_.resize(rois_number_); + std::vector input_rois_topics; + std::vector input_camera_info_topics; + 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) { - input_rois_topics_.at(roi_i) = declare_parameter( + 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)); - input_camera_info_topics_.at(roi_i) = declare_parameter( + input_camera_info_topics.at(roi_i) = declare_parameter( "input/camera_info" + std::to_string(roi_i), "/sensing/camera/camera" + std::to_string(roi_i) + "/camera_info"); } @@ -92,18 +94,18 @@ FusionNode::FusionNode( std::function fnc = std::bind(&FusionNode::cameraInfoCallback, 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); + input_camera_info_topics.at(roi_i), rclcpp::QoS{1}.best_effort(), fnc); } // sub rois rois_subs_.resize(rois_number_); - cached_roi_msgs_.resize(rois_number_); + cached_det2d_msgs_.resize(rois_number_); is_fused_.resize(rois_number_, false); 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); rois_subs_.at(roi_i) = this->create_subscription( - input_rois_topics_.at(roi_i), rclcpp::QoS{1}.best_effort(), roi_callback); + input_rois_topics.at(roi_i), rclcpp::QoS{1}.best_effort(), roi_callback); } // subscribers @@ -121,11 +123,13 @@ FusionNode::FusionNode( timer_ = rclcpp::create_timer( this, get_clock(), period_ns, std::bind(&FusionNode::timer_callback, this)); + // camera manager initialization + det2d_list_.resize(rois_number_); + // camera projection settings camera_projectors_.resize(rois_number_); point_project_to_unrectified_image_ = declare_parameter>("point_project_to_unrectified_image"); - 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."); @@ -146,6 +150,12 @@ FusionNode::FusionNode( } } } + for (std::size_t roi_i = 0; roi_i < rois_number_; ++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); + } + approx_grid_cell_w_size_ = declare_parameter("approximation_grid_cell_width"); approx_grid_cell_h_size_ = declare_parameter("approximation_grid_cell_height"); @@ -209,6 +219,14 @@ void FusionNode::cameraInfoCallback( camera_info_map_[camera_id] = *input_camera_info_msg; } + + auto & det_2d = det2d_list_.at(camera_id); + if (!det_2d.camera_projector_ptr && checkCameraInfo(*input_camera_info_msg)) { + det_2d.camera_projector_ptr = std::make_unique( + *input_camera_info_msg, approx_grid_cell_w_size_, approx_grid_cell_h_size_, + det_2d.project_to_unrectified_image, det_2d.approximate_camera_projection); + det_2d.camera_projector_ptr->initialize(); + } } template @@ -245,7 +263,9 @@ void FusionNode::exportProcess() } // reset flags - std::fill(is_fused_.begin(), is_fused_.end(), false); + for (auto & det_2d : det2d_list_) { + det_2d.is_fused = false; + } cached_msg_.second = nullptr; } @@ -288,13 +308,14 @@ void FusionNode::subCallback( (*output_msg).header.stamp.sec * static_cast(1e9) + (*output_msg).header.stamp.nanosec; // for loop for each roi for (std::size_t roi_i = 0; roi_i < rois_number_; ++roi_i) { + auto & det_2d = det2d_list_.at(roi_i); // check camera info - if (camera_info_map_.find(roi_i) == camera_info_map_.end()) { + if (det_2d.camera_projector_ptr == nullptr) { RCLCPP_WARN_THROTTLE( this->get_logger(), *this->get_clock(), 5000, "no camera info. id is %zu", roi_i); continue; } - auto & roi_msgs = cached_roi_msgs_.at(roi_i); + auto & roi_msgs = det_2d.cached_det2d_msgs; // check if the roi is collected if (roi_msgs.size() == 0) continue; @@ -304,7 +325,7 @@ void FusionNode::subCallback( int64_t matched_stamp = -1; std::list outdate_stamps; for (const auto & [roi_stamp, value] : roi_msgs) { - int64_t new_stamp = timestamp_nsec + input_offset_ms_.at(roi_i) * static_cast(1e6); + int64_t new_stamp = timestamp_nsec + det_2d.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)) { @@ -328,7 +349,7 @@ void FusionNode::subCallback( fuseOnSingleImage(*input_msg, roi_i, *(roi_msgs[matched_stamp]), *output_msg); roi_msgs.erase(matched_stamp); - is_fused_.at(roi_i) = true; + det_2d.is_fused = true; // add timestamp interval for debug if (debug_publisher_) { @@ -337,7 +358,7 @@ void FusionNode::subCallback( "debug/roi" + std::to_string(roi_i) + "/timestamp_interval_ms", timestamp_interval_ms); debug_publisher_->publish( "debug/roi" + std::to_string(roi_i) + "/timestamp_interval_offset_ms", - timestamp_interval_ms - input_offset_ms_.at(roi_i)); + timestamp_interval_ms - det_2d.input_offset_ms); } } } @@ -345,7 +366,7 @@ void FusionNode::subCallback( cached_msg_.second = output_msg; // PROCESS: if all camera fused, postprocess and publish the main message - if (std::count(is_fused_.begin(), is_fused_.end(), true) == static_cast(rois_number_)) { + if (checkAllDet2dFused(det2d_list_)) { exportProcess(); } else { // PROCESS: if all of rois are not collected, publish the old Msg(if exists) and cache the @@ -363,22 +384,23 @@ void FusionNode::roiCallback( stop_watch_ptr_->toc("processing_time", true); + auto & det_2d = det2d_list_.at(roi_i); + int64_t timestamp_nsec = (*input_roi_msg).header.stamp.sec * static_cast(1e9) + (*input_roi_msg).header.stamp.nanosec; // if cached Msg exist, try to match if (cached_msg_.second != nullptr) { - int64_t new_stamp = cached_msg_.first + input_offset_ms_.at(roi_i) * static_cast(1e6); + int64_t new_stamp = cached_msg_.first + det_2d.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) && is_fused_.at(roi_i) == false) { + if (interval < match_threshold_ms_ * static_cast(1e6) && det_2d.is_fused == false) { // check camera info - if (camera_info_map_.find(roi_i) == camera_info_map_.end()) { + if (det_2d.camera_projector_ptr == nullptr) { RCLCPP_WARN_THROTTLE( this->get_logger(), *this->get_clock(), 5000, "no camera info. id is %zu", roi_i); - (cached_roi_msgs_.at(roi_i))[timestamp_nsec] = input_roi_msg; + (cached_det2d_msgs_.at(roi_i))[timestamp_nsec] = input_roi_msg; return; } @@ -387,7 +409,7 @@ void FusionNode::roiCallback( } // PROCESS: fuse the main message with the roi message fuseOnSingleImage(*(cached_msg_.second), roi_i, *input_roi_msg, *(cached_msg_.second)); - is_fused_.at(roi_i) = true; + det_2d.is_fused = true; if (debug_publisher_) { double timestamp_interval_ms = (timestamp_nsec - cached_msg_.first) / 1e6; @@ -395,11 +417,11 @@ void FusionNode::roiCallback( "debug/roi" + std::to_string(roi_i) + "/timestamp_interval_ms", timestamp_interval_ms); debug_publisher_->publish( "debug/roi" + std::to_string(roi_i) + "/timestamp_interval_offset_ms", - timestamp_interval_ms - input_offset_ms_.at(roi_i)); + timestamp_interval_ms - det_2d.input_offset_ms); } // PROCESS: if all camera fused, postprocess and publish the main message - if (std::count(is_fused_.begin(), is_fused_.end(), true) == static_cast(rois_number_)) { + if (checkAllDet2dFused(det2d_list_)) { std::lock_guard lock(mutex_cached_msgs_); exportProcess(); } @@ -408,7 +430,7 @@ void FusionNode::roiCallback( } } // store roi msg if not matched - (cached_roi_msgs_.at(roi_i))[timestamp_nsec] = input_roi_msg; + det_2d.cached_det2d_msgs[timestamp_nsec] = input_roi_msg; } template @@ -434,7 +456,9 @@ void FusionNode::timer_callback() } // reset flags whether the message is fused or not - std::fill(is_fused_.begin(), is_fused_.end(), false); + for (auto & det_2d : det2d_list_) { + det_2d.is_fused = false; + } cached_msg_.second = nullptr; mutex_cached_msgs_.unlock(); From 50e468276b93b56966e014ffe1361bd61b8a3c0d Mon Sep 17 00:00:00 2001 From: Taekjin LEE Date: Wed, 25 Dec 2024 18:43:55 +0900 Subject: [PATCH 06/33] Refactor fusion_node.cpp and fusion_node.hpp for improved code organization and readability Signed-off-by: Taekjin LEE --- .../fusion_node.hpp | 7 +--- .../src/fusion_node.cpp | 37 +++++++------------ .../src/pointpainting_fusion/node.cpp | 4 +- .../src/roi_cluster_fusion/node.cpp | 5 ++- .../src/roi_detected_object_fusion/node.cpp | 5 ++- .../src/roi_pointcloud_fusion/node.cpp | 2 +- .../segmentation_pointcloud_fusion/node.cpp | 5 ++- 7 files changed, 28 insertions(+), 37 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 ec94765b50ec7..9b3c46ddb3651 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 @@ -62,6 +62,7 @@ using autoware_perception_msgs::msg::ObjectClassification; template struct Det2dManager { + std::size_t id = 0; // camera projection std::unique_ptr camera_projector_ptr; bool project_to_unrectified_image = false; @@ -127,16 +128,12 @@ class FusionNode : public rclcpp::Node tf2_ros::Buffer tf_buffer_; tf2_ros::TransformListener tf_listener_; - std::vector point_project_to_unrectified_image_; - // camera_info std::vector> det2d_list_; - std::map camera_info_map_; std::vector::SharedPtr> camera_info_subs_; // camera projection - std::vector camera_projectors_; - std::vector approx_camera_projection_; + float approx_grid_cell_w_size_; float approx_grid_cell_h_size_; 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 da3ce9f8a2734..232d1c4eaa42f 100644 --- a/perception/autoware_image_projection_based_fusion/src/fusion_node.cpp +++ b/perception/autoware_image_projection_based_fusion/src/fusion_node.cpp @@ -127,16 +127,16 @@ FusionNode::FusionNode( det2d_list_.resize(rois_number_); // camera projection settings - camera_projectors_.resize(rois_number_); - point_project_to_unrectified_image_ = + 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."); } - approx_camera_projection_ = declare_parameter>("approximate_camera_projection"); - if (rois_number_ != approx_camera_projection_.size()) { - const std::size_t current_size = approx_camera_projection_.size(); + std::vector approx_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( this->get_logger(), "The number of elements in approximate_camera_projection should be the same as in " @@ -144,16 +144,17 @@ FusionNode::FusionNode( "It has %zu elements.", current_size); if (current_size < rois_number_) { - approx_camera_projection_.resize(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; + approx_camera_projection.at(i) = true; } } } 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); + point_project_to_unrectified_image.at(roi_i); + det2d_list_.at(roi_i).approximate_camera_projection = approx_camera_projection.at(roi_i); } approx_grid_cell_w_size_ = declare_parameter("approximation_grid_cell_width"); @@ -209,17 +210,6 @@ void FusionNode::cameraInfoCallback( { // create the CameraProjection when the camera info arrives for the first time // assuming the camera info does not change while the node is running - if ( - camera_info_map_.find(camera_id) == camera_info_map_.end() && - checkCameraInfo(*input_camera_info_msg)) { - camera_projectors_.at(camera_id) = CameraProjection( - *input_camera_info_msg, approx_grid_cell_w_size_, approx_grid_cell_h_size_, - point_project_to_unrectified_image_.at(camera_id), approx_camera_projection_.at(camera_id)); - camera_projectors_.at(camera_id).initialize(); - - camera_info_map_[camera_id] = *input_camera_info_msg; - } - auto & det_2d = det2d_list_.at(camera_id); if (!det_2d.camera_projector_ptr && checkCameraInfo(*input_camera_info_msg)) { det_2d.camera_projector_ptr = std::make_unique( @@ -307,8 +297,9 @@ void FusionNode::subCallback( int64_t timestamp_nsec = (*output_msg).header.stamp.sec * static_cast(1e9) + (*output_msg).header.stamp.nanosec; // for loop for each roi - for (std::size_t roi_i = 0; roi_i < rois_number_; ++roi_i) { - auto & det_2d = det2d_list_.at(roi_i); + for (auto & det_2d : det2d_list_) { + const auto roi_i = det_2d.id; + // check camera info if (det_2d.camera_projector_ptr == nullptr) { RCLCPP_WARN_THROTTLE( 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 25d9064ddbfc7..2759b0f23087d 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 @@ -335,13 +335,13 @@ dc | dc dc dc dc ||zc| p_y = point_camera.y(); p_z = point_camera.z(); - if (camera_projectors_[image_id].isOutsideHorizontalView(p_x, p_z)) { + if (det2d_list_.at(image_id).camera_projector_ptr->isOutsideHorizontalView(p_x, p_z)) { continue; } // project Eigen::Vector2d projected_point; - if (camera_projectors_[image_id].calcImageProjectedPoint( + if (det2d_list_.at(image_id).camera_projector_ptr->calcImageProjectedPoint( cv::Point3d(p_x, p_y, p_z), projected_point)) { // iterate 2d bbox for (const auto & feature_object : objects) { 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 32db5319bb487..1d9af40399673 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 @@ -100,7 +100,8 @@ void RoiClusterFusionNode::fuseOnSingleImage( std::unique_ptr st_ptr; if (time_keeper_) st_ptr = std::make_unique(__func__, *time_keeper_); - const sensor_msgs::msg::CameraInfo & camera_info = camera_projectors_[image_id].getCameraInfo(); + const sensor_msgs::msg::CameraInfo & camera_info = + det2d_list_.at(image_id).camera_projector_ptr->getCameraInfo(); // get transform from cluster frame id to camera optical frame id geometry_msgs::msg::TransformStamped transform_stamped; @@ -149,7 +150,7 @@ void RoiClusterFusionNode::fuseOnSingleImage( } Eigen::Vector2d projected_point; - if (camera_projectors_[image_id].calcImageProjectedPoint( + if (det2d_list_.at(image_id).camera_projector_ptr->calcImageProjectedPoint( cv::Point3d(*iter_x, *iter_y, *iter_z), projected_point)) { const int px = static_cast(projected_point.x()); const int py = static_cast(projected_point.y()); 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 7be18d59fdbf1..5619764e24c74 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 @@ -131,7 +131,8 @@ RoiDetectedObjectFusionNode::generateDetectedObjectRoIs( return object_roi_map; } const auto & passthrough_object_flags = passthrough_object_flags_map_.at(timestamp_nsec); - const sensor_msgs::msg::CameraInfo & camera_info = camera_projectors_[image_id].getCameraInfo(); + const sensor_msgs::msg::CameraInfo & camera_info = + det2d_list_.at(image_id).camera_projector_ptr->getCameraInfo(); const double image_width = static_cast(camera_info.width); const double image_height = static_cast(camera_info.height); @@ -162,7 +163,7 @@ RoiDetectedObjectFusionNode::generateDetectedObjectRoIs( } Eigen::Vector2d proj_point; - if (camera_projectors_[image_id].calcImageProjectedPoint( + if (det2d_list_.at(image_id).camera_projector_ptr->calcImageProjectedPoint( cv::Point3d(point.x(), point.y(), point.z()), proj_point)) { const double px = proj_point.x(); const double py = proj_point.y(); 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 998072d87774e..ba2d5ff2ba370 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 @@ -162,7 +162,7 @@ void RoiPointCloudFusionNode::fuseOnSingleImage( } Eigen::Vector2d projected_point; - if (camera_projectors_[image_id].calcImageProjectedPoint( + if (det2d_list_.at(image_id).camera_projector_ptr->calcImageProjectedPoint( cv::Point3d(transformed_x, transformed_y, transformed_z), projected_point)) { for (std::size_t i = 0; i < output_objs.size(); ++i) { auto & feature_obj = output_objs.at(i); 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 486ae291abc6a..0c6da43137781 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 @@ -100,7 +100,8 @@ void SegmentPointCloudFusionNode::fuseOnSingleImage( return; } - const sensor_msgs::msg::CameraInfo & camera_info = camera_projectors_[image_id].getCameraInfo(); + const sensor_msgs::msg::CameraInfo & camera_info = + det2d_list_.at(image_id).camera_projector_ptr->getCameraInfo(); std::vector mask_data(input_mask.data.begin(), input_mask.data.end()); cv::Mat mask = perception_utils::runLengthDecoder(mask_data, input_mask.height, input_mask.width); @@ -150,7 +151,7 @@ void SegmentPointCloudFusionNode::fuseOnSingleImage( } Eigen::Vector2d projected_point; - if (!camera_projectors_[image_id].calcImageProjectedPoint( + if (!det2d_list_.at(image_id).camera_projector_ptr->calcImageProjectedPoint( cv::Point3d(transformed_x, transformed_y, transformed_z), projected_point)) { continue; } From 17f9ef32dc9bd44f0953910abf7aec94106bdc47 Mon Sep 17 00:00:00 2001 From: Taekjin LEE Date: Wed, 25 Dec 2024 19:22:44 +0900 Subject: [PATCH 07/33] Refactor fusion_node.hpp and fusion_node.cpp for improved code organization and readability Signed-off-by: Taekjin LEE --- .../fusion_node.hpp | 13 ++-- .../src/fusion_node.cpp | 72 +++++++++---------- 2 files changed, 40 insertions(+), 45 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 9b3c46ddb3651..6a19d90d459cf 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 @@ -131,12 +131,11 @@ class FusionNode : public rclcpp::Node // camera_info std::vector> det2d_list_; - std::vector::SharedPtr> camera_info_subs_; // camera projection - float approx_grid_cell_w_size_; float approx_grid_cell_h_size_; + // timer rclcpp::TimerBase::SharedPtr timer_; double timeout_ms_{}; double match_threshold_ms_{}; @@ -144,15 +143,11 @@ class FusionNode : public rclcpp::Node /** \brief A vector of subscriber. */ typename rclcpp::Subscription::SharedPtr sub_; std::vector::SharedPtr> rois_subs_; - - // offsets between cameras and the lidars - std::vector input_offset_ms_; + std::vector::SharedPtr> camera_info_subs_; // cache for fusion - std::vector is_fused_; - std::pair - cached_msg_; // first element is the timestamp in nanoseconds, second element is the message - std::vector> cached_det2d_msgs_; + int64_t cached_det3d_msg_timestamp_; + typename TargetMsg3D::SharedPtr cached_det3d_msg_ptr_; std::mutex mutex_cached_msgs_; // output publisher 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 232d1c4eaa42f..dcdb0d3147977 100644 --- a/perception/autoware_image_projection_based_fusion/src/fusion_node.cpp +++ b/perception/autoware_image_projection_based_fusion/src/fusion_node.cpp @@ -83,12 +83,7 @@ FusionNode::FusionNode( "/sensing/camera/camera" + std::to_string(roi_i) + "/camera_info"); } - 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."); - } - - // sub camera info + // subscribe camera info camera_info_subs_.resize(rois_number_); for (std::size_t roi_i = 0; roi_i < rois_number_; ++roi_i) { std::function fnc = @@ -97,10 +92,8 @@ FusionNode::FusionNode( input_camera_info_topics.at(roi_i), rclcpp::QoS{1}.best_effort(), fnc); } - // sub rois + // subscribe rois rois_subs_.resize(rois_number_); - cached_det2d_msgs_.resize(rois_number_); - is_fused_.resize(rois_number_, false); 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); @@ -108,7 +101,7 @@ FusionNode::FusionNode( input_rois_topics.at(roi_i), rclcpp::QoS{1}.best_effort(), roi_callback); } - // subscribers + // subscribe 3d detection std::function sub_callback = std::bind(&FusionNode::subCallback, this, std::placeholders::_1); sub_ = @@ -123,8 +116,11 @@ FusionNode::FusionNode( timer_ = rclcpp::create_timer( this, get_clock(), period_ns, std::bind(&FusionNode::timer_callback, this)); - // camera manager initialization - det2d_list_.resize(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 = @@ -150,16 +146,19 @@ FusionNode::FusionNode( } } } + approx_grid_cell_w_size_ = declare_parameter("approximation_grid_cell_width"); + approx_grid_cell_h_size_ = declare_parameter("approximation_grid_cell_height"); + + // camera manager 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); + det2d_list_.at(roi_i).input_offset_ms = input_offset_ms.at(roi_i); } - approx_grid_cell_w_size_ = declare_parameter("approximation_grid_cell_width"); - approx_grid_cell_h_size_ = declare_parameter("approximation_grid_cell_height"); - // debugger if (declare_parameter("debug_mode", false)) { std::vector input_camera_topics; @@ -231,8 +230,8 @@ void FusionNode::exportProcess() { timer_->cancel(); - postprocess(*(cached_msg_.second)); - publish(*(cached_msg_.second)); + postprocess(*(cached_det3d_msg_ptr_)); + publish(*(cached_det3d_msg_ptr_)); // add processing time for debug if (debug_publisher_) { @@ -240,7 +239,7 @@ void FusionNode::exportProcess() const double pipeline_latency_ms = std::chrono::duration( std::chrono::nanoseconds( - (this->get_clock()->now() - cached_msg_.second->header.stamp).nanoseconds())) + (this->get_clock()->now() - cached_det3d_msg_ptr_->header.stamp).nanoseconds())) .count(); debug_publisher_->publish( "debug/cyclic_time_ms", cyclic_time_ms); @@ -256,19 +255,19 @@ void FusionNode::exportProcess() for (auto & det_2d : det2d_list_) { det_2d.is_fused = false; } - cached_msg_.second = nullptr; + cached_det3d_msg_ptr_ = nullptr; } template void FusionNode::subCallback( - const typename TargetMsg3D::ConstSharedPtr input_msg) + const typename TargetMsg3D::ConstSharedPtr det3d_msg) { std::unique_ptr st_ptr; if (time_keeper_) st_ptr = std::make_unique(__func__, *time_keeper_); std::lock_guard lock(mutex_cached_msgs_); - if (cached_msg_.second != nullptr) { + 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); @@ -289,7 +288,7 @@ void FusionNode::subCallback( stop_watch_ptr_->toc("processing_time", true); // PROCESS: preprocess the main message - typename TargetMsg3D::SharedPtr output_msg = std::make_shared(*input_msg); + typename TargetMsg3D::SharedPtr output_msg = std::make_shared(*det3d_msg); preprocess(*output_msg); // PROCESS: fuse the main message with the cached roi messages @@ -338,7 +337,7 @@ void FusionNode::subCallback( debugger_->clear(); } - fuseOnSingleImage(*input_msg, roi_i, *(roi_msgs[matched_stamp]), *output_msg); + fuseOnSingleImage(*det3d_msg, roi_i, *(roi_msgs[matched_stamp]), *output_msg); roi_msgs.erase(matched_stamp); det_2d.is_fused = true; @@ -353,8 +352,8 @@ void FusionNode::subCallback( } } } - cached_msg_.first = timestamp_nsec; - cached_msg_.second = output_msg; + cached_det3d_msg_timestamp_ = timestamp_nsec; + cached_det3d_msg_ptr_ = output_msg; // PROCESS: if all camera fused, postprocess and publish the main message if (checkAllDet2dFused(det2d_list_)) { @@ -368,7 +367,7 @@ void FusionNode::subCallback( template void FusionNode::roiCallback( - const typename Msg2D::ConstSharedPtr input_roi_msg, const std::size_t roi_i) + const typename Msg2D::ConstSharedPtr det2d_msg, const std::size_t roi_i) { std::unique_ptr st_ptr; if (time_keeper_) st_ptr = std::make_unique(__func__, *time_keeper_); @@ -377,12 +376,13 @@ void FusionNode::roiCallback( auto & det_2d = det2d_list_.at(roi_i); - int64_t timestamp_nsec = (*input_roi_msg).header.stamp.sec * static_cast(1e9) + - (*input_roi_msg).header.stamp.nanosec; + 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_msg_.second != nullptr) { - int64_t new_stamp = cached_msg_.first + det_2d.input_offset_ms * static_cast(1e6); + if (cached_det3d_msg_ptr_ != nullptr) { + int64_t new_stamp = + cached_det3d_msg_timestamp_ + det_2d.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 @@ -391,7 +391,7 @@ void FusionNode::roiCallback( if (det_2d.camera_projector_ptr == nullptr) { RCLCPP_WARN_THROTTLE( this->get_logger(), *this->get_clock(), 5000, "no camera info. id is %zu", roi_i); - (cached_det2d_msgs_.at(roi_i))[timestamp_nsec] = input_roi_msg; + det_2d.cached_det2d_msgs[timestamp_nsec] = det2d_msg; return; } @@ -399,11 +399,11 @@ void FusionNode::roiCallback( debugger_->clear(); } // PROCESS: fuse the main message with the roi message - fuseOnSingleImage(*(cached_msg_.second), roi_i, *input_roi_msg, *(cached_msg_.second)); + fuseOnSingleImage(*(cached_det3d_msg_ptr_), roi_i, *det2d_msg, *(cached_det3d_msg_ptr_)); det_2d.is_fused = true; if (debug_publisher_) { - double timestamp_interval_ms = (timestamp_nsec - cached_msg_.first) / 1e6; + double timestamp_interval_ms = (timestamp_nsec - cached_det3d_msg_timestamp_) / 1e6; debug_publisher_->publish( "debug/roi" + std::to_string(roi_i) + "/timestamp_interval_ms", timestamp_interval_ms); debug_publisher_->publish( @@ -421,7 +421,7 @@ void FusionNode::roiCallback( } } // store roi msg if not matched - det_2d.cached_det2d_msgs[timestamp_nsec] = input_roi_msg; + det_2d.cached_det2d_msgs[timestamp_nsec] = det2d_msg; } template @@ -441,7 +441,7 @@ void FusionNode::timer_callback() timer_->cancel(); if (mutex_cached_msgs_.try_lock()) { // PROCESS: if timeout, postprocess cached msg - if (cached_msg_.second != nullptr) { + if (cached_det3d_msg_ptr_ != nullptr) { stop_watch_ptr_->toc("processing_time", true); exportProcess(); } @@ -450,7 +450,7 @@ void FusionNode::timer_callback() for (auto & det_2d : det2d_list_) { det_2d.is_fused = false; } - cached_msg_.second = nullptr; + cached_det3d_msg_ptr_ = nullptr; mutex_cached_msgs_.unlock(); } else { From e1eb9a5d902e5ca08ec105775a6fbdf6054aef90 Mon Sep 17 00:00:00 2001 From: Taekjin LEE Date: Wed, 25 Dec 2024 19:45:26 +0900 Subject: [PATCH 08/33] feat: Refactor fusion_node.cpp for improved code organization and readability Signed-off-by: Taekjin LEE --- .../src/fusion_node.cpp | 48 +++++++++---------- 1 file changed, 24 insertions(+), 24 deletions(-) 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 dcdb0d3147977..ff2010b4336d5 100644 --- a/perception/autoware_image_projection_based_fusion/src/fusion_node.cpp +++ b/perception/autoware_image_projection_based_fusion/src/fusion_node.cpp @@ -209,12 +209,12 @@ void FusionNode::cameraInfoCallback( { // 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 & det_2d = det2d_list_.at(camera_id); - if (!det_2d.camera_projector_ptr && checkCameraInfo(*input_camera_info_msg)) { - det_2d.camera_projector_ptr = std::make_unique( + auto & det2d = det2d_list_.at(camera_id); + if (!det2d.camera_projector_ptr && checkCameraInfo(*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_, - det_2d.project_to_unrectified_image, det_2d.approximate_camera_projection); - det_2d.camera_projector_ptr->initialize(); + det2d.project_to_unrectified_image, det2d.approximate_camera_projection); + det2d.camera_projector_ptr->initialize(); } } @@ -252,8 +252,8 @@ void FusionNode::exportProcess() } // reset flags - for (auto & det_2d : det2d_list_) { - det_2d.is_fused = false; + for (auto & det2d : det2d_list_) { + det2d.is_fused = false; } cached_det3d_msg_ptr_ = nullptr; } @@ -296,16 +296,16 @@ void FusionNode::subCallback( int64_t timestamp_nsec = (*output_msg).header.stamp.sec * static_cast(1e9) + (*output_msg).header.stamp.nanosec; // for loop for each roi - for (auto & det_2d : det2d_list_) { - const auto roi_i = det_2d.id; + for (auto & det2d : det2d_list_) { + const auto roi_i = det2d.id; // check camera info - if (det_2d.camera_projector_ptr == nullptr) { + 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 & roi_msgs = det_2d.cached_det2d_msgs; + auto & roi_msgs = det2d.cached_det2d_msgs; // check if the roi is collected if (roi_msgs.size() == 0) continue; @@ -315,7 +315,7 @@ void FusionNode::subCallback( int64_t matched_stamp = -1; std::list outdate_stamps; for (const auto & [roi_stamp, value] : roi_msgs) { - int64_t new_stamp = timestamp_nsec + det_2d.input_offset_ms * static_cast(1e6); + 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)) { @@ -339,7 +339,7 @@ void FusionNode::subCallback( fuseOnSingleImage(*det3d_msg, roi_i, *(roi_msgs[matched_stamp]), *output_msg); roi_msgs.erase(matched_stamp); - det_2d.is_fused = true; + det2d.is_fused = true; // add timestamp interval for debug if (debug_publisher_) { @@ -348,7 +348,7 @@ void FusionNode::subCallback( "debug/roi" + std::to_string(roi_i) + "/timestamp_interval_ms", timestamp_interval_ms); debug_publisher_->publish( "debug/roi" + std::to_string(roi_i) + "/timestamp_interval_offset_ms", - timestamp_interval_ms - det_2d.input_offset_ms); + timestamp_interval_ms - det2d.input_offset_ms); } } } @@ -374,7 +374,7 @@ void FusionNode::roiCallback( stop_watch_ptr_->toc("processing_time", true); - auto & det_2d = det2d_list_.at(roi_i); + auto & det2d = det2d_list_.at(roi_i); int64_t timestamp_nsec = (*det2d_msg).header.stamp.sec * static_cast(1e9) + (*det2d_msg).header.stamp.nanosec; @@ -382,16 +382,16 @@ void FusionNode::roiCallback( // if cached Msg exist, try to match if (cached_det3d_msg_ptr_ != nullptr) { int64_t new_stamp = - cached_det3d_msg_timestamp_ + det_2d.input_offset_ms * static_cast(1e6); + 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) && det_2d.is_fused == false) { + if (interval < match_threshold_ms_ * static_cast(1e6) && det2d.is_fused == false) { // check camera info - if (det_2d.camera_projector_ptr == nullptr) { + if (det2d.camera_projector_ptr == nullptr) { RCLCPP_WARN_THROTTLE( this->get_logger(), *this->get_clock(), 5000, "no camera info. id is %zu", roi_i); - det_2d.cached_det2d_msgs[timestamp_nsec] = det2d_msg; + det2d.cached_det2d_msgs[timestamp_nsec] = det2d_msg; return; } @@ -400,7 +400,7 @@ void FusionNode::roiCallback( } // PROCESS: fuse the main message with the roi message fuseOnSingleImage(*(cached_det3d_msg_ptr_), roi_i, *det2d_msg, *(cached_det3d_msg_ptr_)); - det_2d.is_fused = true; + det2d.is_fused = true; if (debug_publisher_) { double timestamp_interval_ms = (timestamp_nsec - cached_det3d_msg_timestamp_) / 1e6; @@ -408,7 +408,7 @@ void FusionNode::roiCallback( "debug/roi" + std::to_string(roi_i) + "/timestamp_interval_ms", timestamp_interval_ms); debug_publisher_->publish( "debug/roi" + std::to_string(roi_i) + "/timestamp_interval_offset_ms", - timestamp_interval_ms - det_2d.input_offset_ms); + timestamp_interval_ms - det2d.input_offset_ms); } // PROCESS: if all camera fused, postprocess and publish the main message @@ -421,7 +421,7 @@ void FusionNode::roiCallback( } } // store roi msg if not matched - det_2d.cached_det2d_msgs[timestamp_nsec] = det2d_msg; + det2d.cached_det2d_msgs[timestamp_nsec] = det2d_msg; } template @@ -447,8 +447,8 @@ void FusionNode::timer_callback() } // reset flags whether the message is fused or not - for (auto & det_2d : det2d_list_) { - det_2d.is_fused = false; + for (auto & det2d : det2d_list_) { + det2d.is_fused = false; } cached_det3d_msg_ptr_ = nullptr; From bfade15ad9e94991ff8d0df274c850be075b2d2f Mon Sep 17 00:00:00 2001 From: Taekjin LEE Date: Wed, 25 Dec 2024 20:33:02 +0900 Subject: [PATCH 09/33] Refactor fusion_node.cpp for improved code organization and readability Signed-off-by: Taekjin LEE --- .../src/fusion_node.cpp | 6 +----- 1 file changed, 1 insertion(+), 5 deletions(-) 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 ff2010b4336d5..82dc31918aa11 100644 --- a/perception/autoware_image_projection_based_fusion/src/fusion_node.cpp +++ b/perception/autoware_image_projection_based_fusion/src/fusion_node.cpp @@ -266,12 +266,10 @@ void FusionNode::subCallback( if (time_keeper_) st_ptr = std::make_unique(__func__, *time_keeper_); std::lock_guard lock(mutex_cached_msgs_); - 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(); } @@ -372,13 +370,12 @@ void FusionNode::roiCallback( std::unique_ptr st_ptr; if (time_keeper_) st_ptr = std::make_unique(__func__, *time_keeper_); + std::lock_guard lock(mutex_cached_msgs_); 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 = @@ -413,7 +410,6 @@ void FusionNode::roiCallback( // PROCESS: if all camera fused, postprocess and publish the main message if (checkAllDet2dFused(det2d_list_)) { - std::lock_guard lock(mutex_cached_msgs_); exportProcess(); } processing_time_ms = processing_time_ms + stop_watch_ptr_->toc("processing_time", true); From 78e8a013a54400977f07805e0ef72e6f10fd6cb7 Mon Sep 17 00:00:00 2001 From: Taekjin LEE Date: Thu, 26 Dec 2024 12:00:30 +0900 Subject: [PATCH 10/33] feat: implement mutex per 2d detection process Signed-off-by: Taekjin LEE --- .../fusion_node.hpp | 3 ++- .../src/fusion_node.cpp | 21 ++++++++++++------- 2 files changed, 16 insertions(+), 8 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 6a19d90d459cf..a73ebd459139d 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 @@ -76,6 +76,7 @@ struct Det2dManager // cache std::map cached_det2d_msgs; + std::unique_ptr mtx_ptr; }; template @@ -148,7 +149,7 @@ class FusionNode : public rclcpp::Node // cache for fusion int64_t cached_det3d_msg_timestamp_; typename TargetMsg3D::SharedPtr cached_det3d_msg_ptr_; - std::mutex mutex_cached_msgs_; + std::mutex mutex_det3d_msg_; // output publisher typename rclcpp::Publisher::SharedPtr pub_ptr_; 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 82dc31918aa11..93e7e60e22d3f 100644 --- a/perception/autoware_image_projection_based_fusion/src/fusion_node.cpp +++ b/perception/autoware_image_projection_based_fusion/src/fusion_node.cpp @@ -152,6 +152,7 @@ FusionNode::FusionNode( // camera manager initialization det2d_list_.resize(rois_number_); for (std::size_t roi_i = 0; roi_i < rois_number_; ++roi_i) { + det2d_list_.at(roi_i).mtx_ptr = std::make_unique(); 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); @@ -265,11 +266,11 @@ void FusionNode::subCallback( std::unique_ptr st_ptr; if (time_keeper_) st_ptr = std::make_unique(__func__, *time_keeper_); - std::lock_guard lock(mutex_cached_msgs_); 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); + std::lock_guard lock(mutex_det3d_msg_); exportProcess(); } @@ -296,6 +297,7 @@ void FusionNode::subCallback( // for loop for each roi for (auto & det2d : det2d_list_) { const auto roi_i = det2d.id; + std::lock_guard lock(*(det2d.mtx_ptr)); // check camera info if (det2d.camera_projector_ptr == nullptr) { @@ -350,14 +352,16 @@ void FusionNode::subCallback( } } } + + // PROCESS: check if the fused message is ready to publish + std::lock_guard lock(mutex_det3d_msg_); cached_det3d_msg_timestamp_ = timestamp_nsec; cached_det3d_msg_ptr_ = output_msg; - - // PROCESS: if all camera fused, postprocess and publish the main message if (checkAllDet2dFused(det2d_list_)) { + // if all camera fused, postprocess and publish the main message exportProcess(); } else { - // PROCESS: if all of rois are not collected, publish the old Msg(if exists) and cache the + // 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); } @@ -370,14 +374,17 @@ void FusionNode::roiCallback( std::unique_ptr st_ptr; if (time_keeper_) st_ptr = std::make_unique(__func__, *time_keeper_); - std::lock_guard lock(mutex_cached_msgs_); stop_watch_ptr_->toc("processing_time", true); auto & det2d = det2d_list_.at(roi_i); + std::lock_guard lock(*(det2d.mtx_ptr)); + 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) { + std::lock_guard lock(mutex_det3d_msg_); + int64_t new_stamp = cached_det3d_msg_timestamp_ + det2d.input_offset_ms * static_cast(1e6); int64_t interval = abs(timestamp_nsec - new_stamp); @@ -435,7 +442,7 @@ void FusionNode::timer_callback() using std::chrono_literals::operator""ms; timer_->cancel(); - if (mutex_cached_msgs_.try_lock()) { + if (mutex_det3d_msg_.try_lock()) { // PROCESS: if timeout, postprocess cached msg if (cached_det3d_msg_ptr_ != nullptr) { stop_watch_ptr_->toc("processing_time", true); @@ -448,7 +455,7 @@ void FusionNode::timer_callback() } cached_det3d_msg_ptr_ = nullptr; - mutex_cached_msgs_.unlock(); + mutex_det3d_msg_.unlock(); } else { // TIMING: retry the process after 10ms try { From a23469288c490cef3c891a816d9e41b57faa5408 Mon Sep 17 00:00:00 2001 From: Taekjin LEE Date: Thu, 26 Dec 2024 13:30:33 +0900 Subject: [PATCH 11/33] Refactor fusion_node.hpp and fusion_node.cpp for improved code organization and readability Signed-off-by: Taekjin LEE --- .../fusion_node.hpp | 42 ++++++++++++------- .../src/fusion_node.cpp | 26 ++++++------ 2 files changed, 40 insertions(+), 28 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 a73ebd459139d..04e3066f812c0 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 @@ -62,34 +62,21 @@ using autoware_perception_msgs::msg::ObjectClassification; template struct Det2dManager { + // 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; std::unique_ptr mtx_ptr; }; -template -bool checkAllDet2dFused(const std::vector> & det2d_list) -{ - for (const auto & det2d : det2d_list) { - if (!det2d.is_fused) { - return false; - } - } - return true; -} - template class FusionNode : public rclcpp::Node { @@ -116,6 +103,30 @@ class FusionNode : public rclcpp::Node void setPeriod(const int64_t new_period); void exportProcess(); + // 2d detection management methods + bool checkAllDet2dFused() + { + std::lock_guard lock(mutex_det2d_flags_); + for (const auto & det2d : det2d_list_) { + if (!det2d.is_fused) { + return false; + } + } + return true; + } + void setDet2dFused(Det2dManager & det2d) + { + std::lock_guard lock(mutex_det2d_flags_); + det2d.is_fused = true; + } + void clearAllDet2dFlags() + { + std::lock_guard lock(mutex_det2d_flags_); + for (auto & det2d : det2d_list_) { + det2d.is_fused = false; + } + } + // Custom process methods virtual void preprocess(TargetMsg3D & output_msg); virtual void fuseOnSingleImage( @@ -129,8 +140,9 @@ class FusionNode : public rclcpp::Node tf2_ros::Buffer tf_buffer_; tf2_ros::TransformListener tf_listener_; - // camera_info + // 2d detection management std::vector> det2d_list_; + std::mutex mutex_det2d_flags_; // camera projection float approx_grid_cell_w_size_; 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 93e7e60e22d3f..620fe1a48a873 100644 --- a/perception/autoware_image_projection_based_fusion/src/fusion_node.cpp +++ b/perception/autoware_image_projection_based_fusion/src/fusion_node.cpp @@ -251,11 +251,6 @@ void FusionNode::exportProcess() "debug/pipeline_latency_ms", pipeline_latency_ms); processing_time_ms = 0; } - - // reset flags - for (auto & det2d : det2d_list_) { - det2d.is_fused = false; - } cached_det3d_msg_ptr_ = nullptr; } @@ -272,6 +267,9 @@ void FusionNode::subCallback( stop_watch_ptr_->toc("processing_time", true); std::lock_guard lock(mutex_det3d_msg_); exportProcess(); + + // reset flags + clearAllDet2dFlags(); } // TIMING: reset timer to the timeout time @@ -339,7 +337,7 @@ void FusionNode::subCallback( fuseOnSingleImage(*det3d_msg, roi_i, *(roi_msgs[matched_stamp]), *output_msg); roi_msgs.erase(matched_stamp); - det2d.is_fused = true; + setDet2dFused(det2d); // add timestamp interval for debug if (debug_publisher_) { @@ -357,9 +355,12 @@ void FusionNode::subCallback( std::lock_guard lock(mutex_det3d_msg_); cached_det3d_msg_timestamp_ = timestamp_nsec; cached_det3d_msg_ptr_ = output_msg; - if (checkAllDet2dFused(det2d_list_)) { + if (checkAllDet2dFused()) { // if all camera fused, postprocess and publish the main message exportProcess(); + + // reset flags + clearAllDet2dFlags(); } else { // if all of rois are not collected, publish the old Msg(if exists) and cache the // current Msg @@ -404,7 +405,7 @@ void FusionNode::roiCallback( } // PROCESS: fuse the main message with the roi message fuseOnSingleImage(*(cached_det3d_msg_ptr_), roi_i, *det2d_msg, *(cached_det3d_msg_ptr_)); - det2d.is_fused = true; + setDet2dFused(det2d); if (debug_publisher_) { double timestamp_interval_ms = (timestamp_nsec - cached_det3d_msg_timestamp_) / 1e6; @@ -416,8 +417,10 @@ void FusionNode::roiCallback( } // PROCESS: if all camera fused, postprocess and publish the main message - if (checkAllDet2dFused(det2d_list_)) { + if (checkAllDet2dFused()) { exportProcess(); + // reset flags + clearAllDet2dFlags(); } processing_time_ms = processing_time_ms + stop_watch_ptr_->toc("processing_time", true); return; @@ -450,10 +453,7 @@ void FusionNode::timer_callback() } // reset flags whether the message is fused or not - for (auto & det2d : det2d_list_) { - det2d.is_fused = false; - } - cached_det3d_msg_ptr_ = nullptr; + clearAllDet2dFlags(); mutex_det3d_msg_.unlock(); } else { From fb1373f8b9903851e5554e10e0dec6b3188ef645 Mon Sep 17 00:00:00 2001 From: Taekjin LEE Date: Thu, 26 Dec 2024 14:09:27 +0900 Subject: [PATCH 12/33] Refactor fusion_node.hpp and fusion_node.cpp for improved code organization and readability Signed-off-by: Taekjin LEE --- .../fusion_node.hpp | 2 +- .../pointpainting_fusion/node.hpp | 3 +- .../roi_cluster_fusion/node.hpp | 3 +- .../roi_detected_object_fusion/node.hpp | 6 ++- .../roi_pointcloud_fusion/node.hpp | 3 +- .../segmentation_pointcloud_fusion/node.hpp | 4 +- .../src/fusion_node.cpp | 14 +++--- .../src/pointpainting_fusion/node.cpp | 45 +++++++++---------- .../src/roi_cluster_fusion/node.cpp | 10 ++--- .../src/roi_detected_object_fusion/node.cpp | 13 +++--- .../src/roi_pointcloud_fusion/node.cpp | 7 +-- .../segmentation_pointcloud_fusion/node.cpp | 7 ++- 12 files changed, 60 insertions(+), 57 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 04e3066f812c0..99fed8d20cf1a 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 @@ -130,7 +130,7 @@ class FusionNode : public rclcpp::Node // Custom process methods virtual void preprocess(TargetMsg3D & output_msg); virtual void fuseOnSingleImage( - const TargetMsg3D & input_msg, const std::size_t image_id, const Msg2D & input_roi_msg, + const TargetMsg3D & input_msg, const Det2dManager & det2d, const Msg2D & input_roi_msg, TargetMsg3D & output_msg) = 0; virtual void postprocess(TargetMsg3D & output_msg); virtual void publish(const TargetMsg3D & output_msg); 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 cd6cd87976522..b4973922a093e 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 @@ -51,7 +51,8 @@ class PointPaintingFusionNode void preprocess(sensor_msgs::msg::PointCloud2 & pointcloud_msg) override; void fuseOnSingleImage( - const sensor_msgs::msg::PointCloud2 & input_pointcloud_msg, const std::size_t image_id, + const sensor_msgs::msg::PointCloud2 & input_pointcloud_msg, + const Det2dManager & det2d, const DetectedObjectsWithFeature & input_roi_msg, sensor_msgs::msg::PointCloud2 & 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 903b153af0681..81a63b6f16cd6 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 @@ -36,7 +36,8 @@ class RoiClusterFusionNode void postprocess(DetectedObjectsWithFeature & output_cluster_msg) override; void fuseOnSingleImage( - const DetectedObjectsWithFeature & input_cluster_msg, const std::size_t image_id, + const DetectedObjectsWithFeature & input_cluster_msg, + const Det2dManager & det2d, const DetectedObjectsWithFeature & input_roi_msg, DetectedObjectsWithFeature & 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 43ec134168ef9..9ab11bd423f9c 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 @@ -41,11 +41,13 @@ class RoiDetectedObjectFusionNode void preprocess(DetectedObjects & output_msg) override; void fuseOnSingleImage( - const DetectedObjects & input_object_msg, const std::size_t image_id, + const DetectedObjects & input_object_msg, + const Det2dManager & det2d, const DetectedObjectsWithFeature & input_roi_msg, DetectedObjects & output_object_msg) override; std::map generateDetectedObjectRoIs( - const DetectedObjects & input_object_msg, const std::size_t & image_id, + const DetectedObjects & input_object_msg, + const Det2dManager & det2d, const Eigen::Affine3d & object2camera_affine); void fuseObjectsOnImage( 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 2f2c8222e196f..e2487558f1e33 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 @@ -48,7 +48,8 @@ class RoiPointCloudFusionNode void postprocess(sensor_msgs::msg::PointCloud2 & pointcloud_msg) override; void fuseOnSingleImage( - const PointCloud2 & input_pointcloud_msg, const std::size_t image_id, + const PointCloud2 & input_pointcloud_msg, + const Det2dManager & det2d, const DetectedObjectsWithFeature & input_roi_msg, PointCloud2 & output_pointcloud_msg) override; bool out_of_scope(const DetectedObjectWithFeature & obj) 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 7454cb7bcd173..47e15e0f8ccd7 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 @@ -60,8 +60,8 @@ class SegmentPointCloudFusionNode : public FusionNode & det2d, + const Image & input_mask, PointCloud2 & output_pointcloud_msg) override; bool out_of_scope(const PointCloud2 & filtered_cloud) override; inline void copyPointCloud( 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 620fe1a48a873..4caedb1362fdb 100644 --- a/perception/autoware_image_projection_based_fusion/src/fusion_node.cpp +++ b/perception/autoware_image_projection_based_fusion/src/fusion_node.cpp @@ -303,16 +303,16 @@ void FusionNode::subCallback( this->get_logger(), *this->get_clock(), 5000, "no camera info. id is %zu", roi_i); continue; } - auto & roi_msgs = det2d.cached_det2d_msgs; + auto & det2d_msgs = det2d.cached_det2d_msgs; // check if the roi is collected - if (roi_msgs.size() == 0) continue; + 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] : roi_msgs) { + 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); @@ -326,7 +326,7 @@ void FusionNode::subCallback( } } for (auto stamp : outdate_stamps) { - roi_msgs.erase(stamp); + det2d_msgs.erase(stamp); } // PROCESS: if matched, fuse the main message with the roi message @@ -335,8 +335,8 @@ void FusionNode::subCallback( debugger_->clear(); } - fuseOnSingleImage(*det3d_msg, roi_i, *(roi_msgs[matched_stamp]), *output_msg); - roi_msgs.erase(matched_stamp); + fuseOnSingleImage(*det3d_msg, det2d, *(det2d_msgs[matched_stamp]), *output_msg); + det2d_msgs.erase(matched_stamp); setDet2dFused(det2d); // add timestamp interval for debug @@ -404,7 +404,7 @@ void FusionNode::roiCallback( debugger_->clear(); } // PROCESS: fuse the main message with the roi message - fuseOnSingleImage(*(cached_det3d_msg_ptr_), roi_i, *det2d_msg, *(cached_det3d_msg_ptr_)); + fuseOnSingleImage(*(cached_det3d_msg_ptr_), det2d, *det2d_msg, *(cached_det3d_msg_ptr_)); setDet2dFused(det2d); if (debug_publisher_) { 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 2759b0f23087d..a67d469a0aa96 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 @@ -253,7 +253,8 @@ void PointPaintingFusionNode::preprocess(sensor_msgs::msg::PointCloud2 & painted void PointPaintingFusionNode::fuseOnSingleImage( __attribute__((unused)) const sensor_msgs::msg::PointCloud2 & input_pointcloud_msg, - const std::size_t image_id, const DetectedObjectsWithFeature & input_roi_msg, + const Det2dManager & det2d, + const DetectedObjectsWithFeature & input_roi_msg, sensor_msgs::msg::PointCloud2 & painted_pointcloud_msg) { if (painted_pointcloud_msg.data.empty() || painted_pointcloud_msg.fields.empty()) { @@ -335,28 +336,26 @@ dc | dc dc dc dc ||zc| p_y = point_camera.y(); p_z = point_camera.z(); - if (det2d_list_.at(image_id).camera_projector_ptr->isOutsideHorizontalView(p_x, p_z)) { - continue; - } + if (det2d.camera_projector_ptr->isOutsideHorizontalView(p_x, p_z)) { + continue; + } - // project - Eigen::Vector2d projected_point; - if (det2d_list_.at(image_id).camera_projector_ptr->calcImageProjectedPoint( - cv::Point3d(p_x, p_y, p_z), projected_point)) { - // iterate 2d bbox - for (const auto & feature_object : objects) { - sensor_msgs::msg::RegionOfInterest roi = feature_object.feature.roi; - // paint current point if it is inside bbox - int label2d = feature_object.object.classification.front().label; - if ( - !isUnknown(label2d) && - isInsideBbox(projected_point.x(), projected_point.y(), roi, 1.0)) { - // cppcheck-suppress invalidPointerCast - auto p_class = reinterpret_cast(&output[stride + class_offset]); - for (const auto & cls : isClassTable_) { - // add up the class values if the point belongs to multiple classes - *p_class = cls.second(label2d) ? (class_index_[cls.first] + *p_class) : *p_class; - } + // project + Eigen::Vector2d projected_point; + if (det2d.camera_projector_ptr->calcImageProjectedPoint( + cv::Point3d(p_x, p_y, p_z), projected_point)) { + // iterate 2d bbox + for (const auto & feature_object : objects) { + sensor_msgs::msg::RegionOfInterest roi = feature_object.feature.roi; + // paint current point if it is inside bbox + int label2d = feature_object.object.classification.front().label; + if ( + !isUnknown(label2d) && isInsideBbox(projected_point.x(), projected_point.y(), roi, 1.0)) { + // cppcheck-suppress invalidPointerCast + auto p_class = reinterpret_cast(&output[stride + class_offset]); + for (const auto & cls : isClassTable_) { + // add up the class values if the point belongs to multiple classes + *p_class = cls.second(label2d) ? (class_index_[cls.first] + *p_class) : *p_class; } } if (debugger_) { @@ -381,7 +380,7 @@ dc | dc dc dc dc ||zc| debugger_->image_rois_ = debug_image_rois; debugger_->obstacle_points_ = debug_image_points; - debugger_->publishImage(image_id, input_roi_msg.header.stamp); + debugger_->publishImage(det2d.id, input_roi_msg.header.stamp); } } 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 1d9af40399673..756db7fd806db 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 @@ -94,14 +94,14 @@ void RoiClusterFusionNode::postprocess(DetectedObjectsWithFeature & output_clust } void RoiClusterFusionNode::fuseOnSingleImage( - const DetectedObjectsWithFeature & input_cluster_msg, const std::size_t image_id, + const DetectedObjectsWithFeature & input_cluster_msg, + const Det2dManager & det2d, const DetectedObjectsWithFeature & input_roi_msg, DetectedObjectsWithFeature & output_cluster_msg) { std::unique_ptr st_ptr; if (time_keeper_) st_ptr = std::make_unique(__func__, *time_keeper_); - const sensor_msgs::msg::CameraInfo & camera_info = - det2d_list_.at(image_id).camera_projector_ptr->getCameraInfo(); + const sensor_msgs::msg::CameraInfo & camera_info = det2d.camera_projector_ptr->getCameraInfo(); // get transform from cluster frame id to camera optical frame id geometry_msgs::msg::TransformStamped transform_stamped; @@ -150,7 +150,7 @@ void RoiClusterFusionNode::fuseOnSingleImage( } Eigen::Vector2d projected_point; - if (det2d_list_.at(image_id).camera_projector_ptr->calcImageProjectedPoint( + if (det2d.camera_projector_ptr->calcImageProjectedPoint( cv::Point3d(*iter_x, *iter_y, *iter_z), projected_point)) { const int px = static_cast(projected_point.x()); const int py = static_cast(projected_point.y()); @@ -232,7 +232,7 @@ void RoiClusterFusionNode::fuseOnSingleImage( // note: debug objects are safely cleared in fusion_node.cpp if (debugger_) { - debugger_->publishImage(image_id, input_roi_msg.header.stamp); + debugger_->publishImage(det2d.id, input_roi_msg.header.stamp); } } 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 5619764e24c74..9be337b9e5028 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 @@ -82,7 +82,7 @@ void RoiDetectedObjectFusionNode::preprocess(DetectedObjects & output_msg) } void RoiDetectedObjectFusionNode::fuseOnSingleImage( - const DetectedObjects & input_object_msg, const std::size_t image_id, + const DetectedObjects & input_object_msg, const Det2dManager & det2d, const DetectedObjectsWithFeature & input_roi_msg, DetectedObjects & output_object_msg __attribute__((unused))) { @@ -101,7 +101,7 @@ void RoiDetectedObjectFusionNode::fuseOnSingleImage( } const auto object_roi_map = - generateDetectedObjectRoIs(input_object_msg, image_id, object2camera_affine); + generateDetectedObjectRoIs(input_object_msg, det2d, object2camera_affine); fuseObjectsOnImage(input_object_msg, input_roi_msg.feature_objects, object_roi_map); if (debugger_) { @@ -109,13 +109,13 @@ void RoiDetectedObjectFusionNode::fuseOnSingleImage( for (std::size_t roi_i = 0; roi_i < input_roi_msg.feature_objects.size(); ++roi_i) { debugger_->image_rois_.push_back(input_roi_msg.feature_objects.at(roi_i).feature.roi); } - debugger_->publishImage(image_id, input_roi_msg.header.stamp); + debugger_->publishImage(det2d.id, input_roi_msg.header.stamp); } } std::map RoiDetectedObjectFusionNode::generateDetectedObjectRoIs( - const DetectedObjects & input_object_msg, const std::size_t & image_id, + const DetectedObjects & input_object_msg, const Det2dManager & det2d, const Eigen::Affine3d & object2camera_affine) { std::unique_ptr st_ptr; @@ -131,8 +131,7 @@ RoiDetectedObjectFusionNode::generateDetectedObjectRoIs( return object_roi_map; } const auto & passthrough_object_flags = passthrough_object_flags_map_.at(timestamp_nsec); - const sensor_msgs::msg::CameraInfo & camera_info = - det2d_list_.at(image_id).camera_projector_ptr->getCameraInfo(); + const sensor_msgs::msg::CameraInfo & camera_info = det2d.camera_projector_ptr->getCameraInfo(); const double image_width = static_cast(camera_info.width); const double image_height = static_cast(camera_info.height); @@ -163,7 +162,7 @@ RoiDetectedObjectFusionNode::generateDetectedObjectRoIs( } Eigen::Vector2d proj_point; - if (det2d_list_.at(image_id).camera_projector_ptr->calcImageProjectedPoint( + if (det2d.camera_projector_ptr->calcImageProjectedPoint( cv::Point3d(point.x(), point.y(), point.z()), proj_point)) { const double px = proj_point.x(); const double py = proj_point.y(); 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 ba2d5ff2ba370..17013bc5f456b 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 @@ -86,7 +86,8 @@ void RoiPointCloudFusionNode::postprocess( } } void RoiPointCloudFusionNode::fuseOnSingleImage( - const sensor_msgs::msg::PointCloud2 & input_pointcloud_msg, const std::size_t image_id, + const sensor_msgs::msg::PointCloud2 & input_pointcloud_msg, + const Det2dManager & det2d, const DetectedObjectsWithFeature & input_roi_msg, __attribute__((unused)) sensor_msgs::msg::PointCloud2 & output_pointcloud_msg) { @@ -162,7 +163,7 @@ void RoiPointCloudFusionNode::fuseOnSingleImage( } Eigen::Vector2d projected_point; - if (det2d_list_.at(image_id).camera_projector_ptr->calcImageProjectedPoint( + if (det2d.camera_projector_ptr->calcImageProjectedPoint( cv::Point3d(transformed_x, transformed_y, transformed_z), projected_point)) { for (std::size_t i = 0; i < output_objs.size(); ++i) { auto & feature_obj = output_objs.at(i); @@ -202,7 +203,7 @@ void RoiPointCloudFusionNode::fuseOnSingleImage( if (debugger_) { debugger_->image_rois_ = debug_image_rois; debugger_->obstacle_points_ = debug_image_points; - debugger_->publishImage(image_id, input_roi_msg.header.stamp); + debugger_->publishImage(det2d.id, input_roi_msg.header.stamp); } } 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 0c6da43137781..d68d302167624 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 @@ -87,7 +87,7 @@ void SegmentPointCloudFusionNode::postprocess(PointCloud2 & pointcloud_msg) } void SegmentPointCloudFusionNode::fuseOnSingleImage( - const PointCloud2 & input_pointcloud_msg, const std::size_t image_id, + const PointCloud2 & input_pointcloud_msg, const Det2dManager & det2d, [[maybe_unused]] const Image & input_mask, __attribute__((unused)) PointCloud2 & output_cloud) { std::unique_ptr st_ptr; @@ -100,8 +100,7 @@ void SegmentPointCloudFusionNode::fuseOnSingleImage( return; } - const sensor_msgs::msg::CameraInfo & camera_info = - det2d_list_.at(image_id).camera_projector_ptr->getCameraInfo(); + const sensor_msgs::msg::CameraInfo & camera_info = det2d.camera_projector_ptr->getCameraInfo(); std::vector mask_data(input_mask.data.begin(), input_mask.data.end()); cv::Mat mask = perception_utils::runLengthDecoder(mask_data, input_mask.height, input_mask.width); @@ -151,7 +150,7 @@ void SegmentPointCloudFusionNode::fuseOnSingleImage( } Eigen::Vector2d projected_point; - if (!det2d_list_.at(image_id).camera_projector_ptr->calcImageProjectedPoint( + if (!det2d.camera_projector_ptr->calcImageProjectedPoint( cv::Point3d(transformed_x, transformed_y, transformed_z), projected_point)) { continue; } From b855809bc345d90253372232b27a5b417d4c1273 Mon Sep 17 00:00:00 2001 From: Taekjin LEE Date: Thu, 26 Dec 2024 14:24:15 +0900 Subject: [PATCH 13/33] revise template, inputs first and output at the last Signed-off-by: Taekjin LEE --- .../fusion_node.hpp | 22 +++---- .../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 +- .../src/fusion_node.cpp | 63 +++++++++---------- .../src/pointpainting_fusion/node.cpp | 2 +- .../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 +- 12 files changed, 51 insertions(+), 54 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 99fed8d20cf1a..8f43d0a1a376e 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 @@ -77,7 +77,7 @@ struct Det2dManager std::unique_ptr mtx_ptr; }; -template +template class FusionNode : public rclcpp::Node { public: @@ -95,7 +95,7 @@ class FusionNode : public rclcpp::Node const sensor_msgs::msg::CameraInfo::ConstSharedPtr input_camera_info_msg, const std::size_t camera_id); // callback for main subscription - void subCallback(const typename TargetMsg3D::ConstSharedPtr input_msg); + void subCallback(const typename Msg3D::ConstSharedPtr input_msg); // callback for roi subscription void roiCallback(const typename Msg2D::ConstSharedPtr input_roi_msg, const std::size_t roi_i); // callback for timer @@ -128,12 +128,12 @@ class FusionNode : public rclcpp::Node } // Custom process methods - virtual void preprocess(TargetMsg3D & output_msg); + virtual void preprocess(Msg3D & output_msg); virtual void fuseOnSingleImage( - const TargetMsg3D & input_msg, const Det2dManager & det2d, const Msg2D & input_roi_msg, - TargetMsg3D & output_msg) = 0; - virtual void postprocess(TargetMsg3D & output_msg); - virtual void publish(const TargetMsg3D & output_msg); + const Msg3D & input_msg, const Det2dManager & det2d, const Msg2D & input_roi_msg, + Msg3D & output_msg) = 0; + virtual void postprocess(Msg3D & output_msg); + virtual void publish(const Msg3D & output_msg); // Members std::size_t rois_number_{1}; @@ -154,21 +154,21 @@ class FusionNode : public rclcpp::Node double match_threshold_ms_{}; /** \brief A vector of subscriber. */ - typename rclcpp::Subscription::SharedPtr sub_; + typename rclcpp::Subscription::SharedPtr sub_; std::vector::SharedPtr> rois_subs_; std::vector::SharedPtr> camera_info_subs_; // cache for fusion int64_t cached_det3d_msg_timestamp_; - typename TargetMsg3D::SharedPtr cached_det3d_msg_ptr_; + typename Msg3D::SharedPtr cached_det3d_msg_ptr_; std::mutex mutex_det3d_msg_; // output publisher - typename rclcpp::Publisher::SharedPtr pub_ptr_; + typename rclcpp::Publisher::SharedPtr pub_ptr_; // debugger std::shared_ptr debugger_; - virtual bool out_of_scope(const ObjType & obj) = 0; + virtual bool out_of_scope(const ExportObj & obj) = 0; float filter_scope_min_x_; float filter_scope_max_x_; float filter_scope_min_y_; 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 b4973922a093e..aff28f825ed59 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 @@ -42,7 +42,7 @@ inline bool isInsideBbox( } class PointPaintingFusionNode -: public FusionNode +: public FusionNode { public: explicit PointPaintingFusionNode(const rclcpp::NodeOptions & options); 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 81a63b6f16cd6..52f1e5e283288 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 @@ -26,7 +26,7 @@ const std::map IOU_MODE_MAP{{"iou", 0}, {"iou_x", 1}, {"io class RoiClusterFusionNode : public FusionNode< - DetectedObjectsWithFeature, DetectedObjectWithFeature, DetectedObjectsWithFeature> + DetectedObjectsWithFeature, DetectedObjectsWithFeature, DetectedObjectWithFeature> { public: explicit RoiClusterFusionNode(const rclcpp::NodeOptions & options); 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 9ab11bd423f9c..fff0d5107ed39 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 @@ -32,7 +32,7 @@ namespace autoware::image_projection_based_fusion using sensor_msgs::msg::RegionOfInterest; class RoiDetectedObjectFusionNode -: public FusionNode +: public FusionNode { public: explicit RoiDetectedObjectFusionNode(const rclcpp::NodeOptions & options); 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 e2487558f1e33..70801c5cd3c14 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 @@ -26,7 +26,7 @@ namespace autoware::image_projection_based_fusion { class RoiPointCloudFusionNode -: public FusionNode +: public FusionNode { private: int min_cluster_size_{1}; 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 47e15e0f8ccd7..814dababe9ae7 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 @@ -34,7 +34,7 @@ namespace autoware::image_projection_based_fusion { -class SegmentPointCloudFusionNode : public FusionNode +class SegmentPointCloudFusionNode : public FusionNode { private: rclcpp::Publisher::SharedPtr pub_pointcloud_ptr_; 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 4caedb1362fdb..3c660edb6157a 100644 --- a/perception/autoware_image_projection_based_fusion/src/fusion_node.cpp +++ b/perception/autoware_image_projection_based_fusion/src/fusion_node.cpp @@ -46,8 +46,8 @@ namespace autoware::image_projection_based_fusion { using autoware::universe_utils::ScopedTimeTrack; -template -FusionNode::FusionNode( +template +FusionNode::FusionNode( const std::string & node_name, const rclcpp::NodeOptions & options) : Node(node_name, options), tf_buffer_(this->get_clock()), tf_listener_(tf_buffer_) { @@ -102,13 +102,12 @@ FusionNode::FusionNode( } // subscribe 3d detection - std::function sub_callback = + std::function sub_callback = std::bind(&FusionNode::subCallback, this, std::placeholders::_1); - sub_ = - this->create_subscription("input", rclcpp::QoS(1).best_effort(), sub_callback); + sub_ = this->create_subscription("input", rclcpp::QoS(1).best_effort(), sub_callback); // publisher - pub_ptr_ = this->create_publisher("output", rclcpp::QoS{1}); + pub_ptr_ = this->create_publisher("output", rclcpp::QoS{1}); // Set timer const auto period_ns = std::chrono::duration_cast( @@ -203,8 +202,8 @@ FusionNode::FusionNode( filter_scope_max_z_ = declare_parameter("filter_scope_max_z"); } -template -void FusionNode::cameraInfoCallback( +template +void FusionNode::cameraInfoCallback( const sensor_msgs::msg::CameraInfo::ConstSharedPtr input_camera_info_msg, const std::size_t camera_id) { @@ -219,15 +218,14 @@ void FusionNode::cameraInfoCallback( } } -template -void FusionNode::preprocess(TargetMsg3D & ouput_msg - __attribute__((unused))) +template +void FusionNode::preprocess(Msg3D & ouput_msg __attribute__((unused))) { // do nothing by default } -template -void FusionNode::exportProcess() +template +void FusionNode::exportProcess() { timer_->cancel(); @@ -254,9 +252,9 @@ void FusionNode::exportProcess() cached_det3d_msg_ptr_ = nullptr; } -template -void FusionNode::subCallback( - const typename TargetMsg3D::ConstSharedPtr det3d_msg) +template +void FusionNode::subCallback( + const typename Msg3D::ConstSharedPtr det3d_msg) { std::unique_ptr st_ptr; if (time_keeper_) st_ptr = std::make_unique(__func__, *time_keeper_); @@ -285,7 +283,7 @@ void FusionNode::subCallback( stop_watch_ptr_->toc("processing_time", true); // PROCESS: preprocess the main message - typename TargetMsg3D::SharedPtr output_msg = std::make_shared(*det3d_msg); + typename Msg3D::SharedPtr output_msg = std::make_shared(*det3d_msg); preprocess(*output_msg); // PROCESS: fuse the main message with the cached roi messages @@ -368,8 +366,8 @@ void FusionNode::subCallback( } } -template -void FusionNode::roiCallback( +template +void FusionNode::roiCallback( const typename Msg2D::ConstSharedPtr det2d_msg, const std::size_t roi_i) { std::unique_ptr st_ptr; @@ -430,15 +428,14 @@ void FusionNode::roiCallback( det2d.cached_det2d_msgs[timestamp_nsec] = det2d_msg; } -template -void FusionNode::postprocess(TargetMsg3D & output_msg - __attribute__((unused))) +template +void FusionNode::postprocess(Msg3D & output_msg __attribute__((unused))) { // do nothing by default } -template -void FusionNode::timer_callback() +template +void FusionNode::timer_callback() { std::unique_ptr st_ptr; if (time_keeper_) st_ptr = std::make_unique(__func__, *time_keeper_); @@ -468,8 +465,8 @@ void FusionNode::timer_callback() } } -template -void FusionNode::setPeriod(const int64_t new_period) +template +void FusionNode::setPeriod(const int64_t new_period) { if (!timer_) { return; @@ -485,8 +482,8 @@ void FusionNode::setPeriod(const int64_t new_period) } } -template -void FusionNode::publish(const TargetMsg3D & output_msg) +template +void FusionNode::publish(const Msg3D & output_msg) { if (pub_ptr_->get_subscription_count() < 1) { return; @@ -495,10 +492,10 @@ void FusionNode::publish(const TargetMsg3D & output_msg } // explicit instantiation for the supported types -template class FusionNode; +template class FusionNode; template class FusionNode< - DetectedObjectsWithFeature, DetectedObjectWithFeature, DetectedObjectsWithFeature>; -template class FusionNode; -template class FusionNode; -template class FusionNode; + DetectedObjectsWithFeature, DetectedObjectsWithFeature, DetectedObjectWithFeature>; +template class FusionNode; +template class FusionNode; +template class FusionNode; } // namespace autoware::image_projection_based_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 a67d469a0aa96..6d60cc3637a37 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 @@ -91,7 +91,7 @@ inline bool isUnknown(int label2d) } PointPaintingFusionNode::PointPaintingFusionNode(const rclcpp::NodeOptions & options) -: FusionNode( +: FusionNode( "pointpainting_fusion", options) { omp_num_threads_ = this->declare_parameter("omp_params.num_threads"); 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 756db7fd806db..1279bfb2cdc1b 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 @@ -41,7 +41,7 @@ namespace autoware::image_projection_based_fusion using autoware::universe_utils::ScopedTimeTrack; RoiClusterFusionNode::RoiClusterFusionNode(const rclcpp::NodeOptions & options) -: FusionNode( +: FusionNode( "roi_cluster_fusion", options) { trust_object_iou_mode_ = declare_parameter("trust_object_iou_mode"); 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 9be337b9e5028..9eac48c13a290 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 @@ -31,7 +31,7 @@ namespace autoware::image_projection_based_fusion using autoware::universe_utils::ScopedTimeTrack; RoiDetectedObjectFusionNode::RoiDetectedObjectFusionNode(const rclcpp::NodeOptions & options) -: FusionNode( +: FusionNode( "roi_detected_object_fusion", options) { fusion_params_.passthrough_lower_bound_probability_thresholds = 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 17013bc5f456b..83fa7476c7a82 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 @@ -37,7 +37,7 @@ namespace autoware::image_projection_based_fusion using autoware::universe_utils::ScopedTimeTrack; RoiPointCloudFusionNode::RoiPointCloudFusionNode(const rclcpp::NodeOptions & options) -: FusionNode( +: FusionNode( "roi_pointcloud_fusion", options) { fuse_unknown_only_ = declare_parameter("fuse_unknown_only"); 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 d68d302167624..8c1fa2144e292 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 @@ -36,7 +36,7 @@ namespace autoware::image_projection_based_fusion using autoware::universe_utils::ScopedTimeTrack; SegmentPointCloudFusionNode::SegmentPointCloudFusionNode(const rclcpp::NodeOptions & options) -: FusionNode("segmentation_pointcloud_fusion", options) +: FusionNode("segmentation_pointcloud_fusion", options) { filter_distance_threshold_ = declare_parameter("filter_distance_threshold"); for (auto & item : filter_semantic_label_target_list_) { From 063e794349454b6e9514581e55e10797aaf3aa6d Mon Sep 17 00:00:00 2001 From: Taekjin LEE Date: Thu, 26 Dec 2024 15:15:30 +0900 Subject: [PATCH 14/33] explicit in and out types 1 Signed-off-by: Taekjin LEE --- .../fusion_node.hpp | 4 +++- .../pointpainting_fusion/node.hpp | 5 ++--- .../roi_cluster_fusion/node.hpp | 19 ++++++---------- .../roi_detected_object_fusion/node.hpp | 11 ++++------ .../roi_pointcloud_fusion/node.hpp | 14 +++++------- .../src/fusion_node.cpp | 22 ++++++++++++++----- .../src/pointpainting_fusion/node.cpp | 14 +++++------- .../src/roi_cluster_fusion/node.cpp | 14 +++++------- .../src/roi_detected_object_fusion/node.cpp | 10 ++++----- .../src/roi_pointcloud_fusion/node.cpp | 11 ++++------ 10 files changed, 58 insertions(+), 66 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 8f43d0a1a376e..0e57c991c536e 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 @@ -53,7 +53,9 @@ using autoware_perception_msgs::msg::DetectedObjects; using sensor_msgs::msg::CameraInfo; using sensor_msgs::msg::Image; using sensor_msgs::msg::PointCloud2; -using tier4_perception_msgs::msg::DetectedObjectsWithFeature; +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; 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 aff28f825ed59..6a8a162676d26 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 @@ -42,7 +42,7 @@ inline bool isInsideBbox( } class PointPaintingFusionNode -: public FusionNode +: public FusionNode { public: explicit PointPaintingFusionNode(const rclcpp::NodeOptions & options); @@ -52,8 +52,7 @@ class PointPaintingFusionNode void fuseOnSingleImage( const sensor_msgs::msg::PointCloud2 & input_pointcloud_msg, - const Det2dManager & det2d, - const DetectedObjectsWithFeature & input_roi_msg, + const Det2dManager & det2d, const RoiMsgType & input_roi_msg, sensor_msgs::msg::PointCloud2 & painted_pointcloud_msg) override; void postprocess(sensor_msgs::msg::PointCloud2 & 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 52f1e5e283288..baae10c1afce8 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 @@ -24,22 +24,18 @@ namespace autoware::image_projection_based_fusion { const std::map IOU_MODE_MAP{{"iou", 0}, {"iou_x", 1}, {"iou_y", 2}}; -class RoiClusterFusionNode -: public FusionNode< - DetectedObjectsWithFeature, DetectedObjectsWithFeature, DetectedObjectWithFeature> +class RoiClusterFusionNode : public FusionNode { public: explicit RoiClusterFusionNode(const rclcpp::NodeOptions & options); protected: - void preprocess(DetectedObjectsWithFeature & output_cluster_msg) override; - void postprocess(DetectedObjectsWithFeature & output_cluster_msg) override; + void preprocess(ClusterMsgType & output_cluster_msg) override; + void postprocess(ClusterMsgType & output_cluster_msg) override; void fuseOnSingleImage( - const DetectedObjectsWithFeature & input_cluster_msg, - const Det2dManager & det2d, - const DetectedObjectsWithFeature & input_roi_msg, - DetectedObjectsWithFeature & output_cluster_msg) override; + const ClusterMsgType & input_cluster_msg, const Det2dManager & det2d, + const RoiMsgType & input_roi_msg, ClusterMsgType & output_cluster_msg) override; std::string trust_object_iou_mode_{"iou"}; bool use_cluster_semantic_type_{false}; @@ -54,12 +50,11 @@ class RoiClusterFusionNode double trust_object_distance_; std::string non_trust_object_iou_mode_{"iou_x"}; - bool is_far_enough(const DetectedObjectWithFeature & obj, const double distance_threshold); - bool out_of_scope(const DetectedObjectWithFeature & obj) override; + bool is_far_enough(const ClusterObjType & obj, const double distance_threshold); + bool out_of_scope(const ClusterObjType & obj) override; double cal_iou_by_mode( const sensor_msgs::msg::RegionOfInterest & roi_1, const sensor_msgs::msg::RegionOfInterest & roi_2, const std::string iou_mode); - // bool CheckUnknown(const DetectedObjectsWithFeature & obj); }; } // namespace autoware::image_projection_based_fusion 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 fff0d5107ed39..c811308cd51c8 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 @@ -31,8 +31,7 @@ namespace autoware::image_projection_based_fusion using sensor_msgs::msg::RegionOfInterest; -class RoiDetectedObjectFusionNode -: public FusionNode +class RoiDetectedObjectFusionNode : public FusionNode { public: explicit RoiDetectedObjectFusionNode(const rclcpp::NodeOptions & options); @@ -41,13 +40,11 @@ class RoiDetectedObjectFusionNode void preprocess(DetectedObjects & output_msg) override; void fuseOnSingleImage( - const DetectedObjects & input_object_msg, - const Det2dManager & det2d, - const DetectedObjectsWithFeature & input_roi_msg, DetectedObjects & output_object_msg) override; + const DetectedObjects & input_object_msg, const Det2dManager & det2d, + const RoiMsgType & input_roi_msg, DetectedObjects & output_object_msg) override; std::map generateDetectedObjectRoIs( - const DetectedObjects & input_object_msg, - const Det2dManager & det2d, + const DetectedObjects & input_object_msg, const Det2dManager & det2d, const Eigen::Affine3d & object2camera_affine); void fuseObjectsOnImage( 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 70801c5cd3c14..a650a67dd6b1d 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 @@ -25,8 +25,7 @@ namespace autoware::image_projection_based_fusion { -class RoiPointCloudFusionNode -: public FusionNode +class RoiPointCloudFusionNode : public FusionNode { private: int min_cluster_size_{1}; @@ -34,8 +33,8 @@ class RoiPointCloudFusionNode bool fuse_unknown_only_{true}; double cluster_2d_tolerance_; - rclcpp::Publisher::SharedPtr pub_objects_ptr_; - std::vector output_fused_objects_; + rclcpp::Publisher::SharedPtr pub_objects_ptr_; + std::vector output_fused_objects_; rclcpp::Publisher::SharedPtr cluster_debug_pub_; /* data */ @@ -48,10 +47,9 @@ class RoiPointCloudFusionNode void postprocess(sensor_msgs::msg::PointCloud2 & pointcloud_msg) override; void fuseOnSingleImage( - const PointCloud2 & input_pointcloud_msg, - const Det2dManager & det2d, - const DetectedObjectsWithFeature & input_roi_msg, PointCloud2 & output_pointcloud_msg) override; - bool out_of_scope(const DetectedObjectWithFeature & obj) override; + const PointCloud2 & input_pointcloud_msg, const Det2dManager & det2d, + const RoiMsgType & input_roi_msg, PointCloud2 & output_pointcloud_msg) override; + bool out_of_scope(const ClusterObjType & obj) override; }; } // 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 3c660edb6157a..32590b30251ab 100644 --- a/perception/autoware_image_projection_based_fusion/src/fusion_node.cpp +++ b/perception/autoware_image_projection_based_fusion/src/fusion_node.cpp @@ -491,11 +491,21 @@ void FusionNode::publish(const Msg3D & output_msg) pub_ptr_->publish(output_msg); } -// explicit instantiation for the supported types -template class FusionNode; -template class FusionNode< - DetectedObjectsWithFeature, DetectedObjectsWithFeature, DetectedObjectWithFeature>; -template class FusionNode; -template class FusionNode; +// Explicit instantiation for the supported types + +// pointpainting fusion +template class FusionNode; + +// roi cluster fusion +template class FusionNode; + +// roi detected-object fusion +template class FusionNode; + +// roi pointcloud fusion +template class FusionNode; + +// segment pointcloud fusion template class FusionNode; + } // namespace autoware::image_projection_based_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 6d60cc3637a37..607ee010c19df 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 @@ -91,8 +91,7 @@ inline bool isUnknown(int label2d) } PointPaintingFusionNode::PointPaintingFusionNode(const rclcpp::NodeOptions & options) -: FusionNode( - "pointpainting_fusion", options) +: FusionNode("pointpainting_fusion", options) { omp_num_threads_ = this->declare_parameter("omp_params.num_threads"); const float score_threshold = @@ -195,7 +194,7 @@ PointPaintingFusionNode::PointPaintingFusionNode(const rclcpp::NodeOptions & opt } } -void PointPaintingFusionNode::preprocess(sensor_msgs::msg::PointCloud2 & painted_pointcloud_msg) +void PointPaintingFusionNode::preprocess(PointCloud2 & painted_pointcloud_msg) { std::unique_ptr st_ptr; if (time_keeper_) st_ptr = std::make_unique(__func__, *time_keeper_); @@ -252,10 +251,9 @@ void PointPaintingFusionNode::preprocess(sensor_msgs::msg::PointCloud2 & painted } void PointPaintingFusionNode::fuseOnSingleImage( - __attribute__((unused)) const sensor_msgs::msg::PointCloud2 & input_pointcloud_msg, - const Det2dManager & det2d, - const DetectedObjectsWithFeature & input_roi_msg, - sensor_msgs::msg::PointCloud2 & painted_pointcloud_msg) + __attribute__((unused)) const PointCloud2 & input_pointcloud_msg, + const Det2dManager & det2d, const RoiMsgType & input_roi_msg, + PointCloud2 & painted_pointcloud_msg) { if (painted_pointcloud_msg.data.empty() || painted_pointcloud_msg.fields.empty()) { RCLCPP_WARN_STREAM_THROTTLE( @@ -384,7 +382,7 @@ dc | dc dc dc dc ||zc| } } -void PointPaintingFusionNode::postprocess(sensor_msgs::msg::PointCloud2 & painted_pointcloud_msg) +void PointPaintingFusionNode::postprocess(PointCloud2 & painted_pointcloud_msg) { std::unique_ptr st_ptr; if (time_keeper_) st_ptr = std::make_unique(__func__, *time_keeper_); 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 1279bfb2cdc1b..321254a9bd673 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 @@ -41,8 +41,7 @@ namespace autoware::image_projection_based_fusion using autoware::universe_utils::ScopedTimeTrack; RoiClusterFusionNode::RoiClusterFusionNode(const rclcpp::NodeOptions & options) -: FusionNode( - "roi_cluster_fusion", options) +: FusionNode("roi_cluster_fusion", options) { trust_object_iou_mode_ = declare_parameter("trust_object_iou_mode"); non_trust_object_iou_mode_ = declare_parameter("non_trust_object_iou_mode"); @@ -56,7 +55,7 @@ RoiClusterFusionNode::RoiClusterFusionNode(const rclcpp::NodeOptions & options) trust_object_distance_ = declare_parameter("trust_object_distance"); } -void RoiClusterFusionNode::preprocess(DetectedObjectsWithFeature & output_cluster_msg) +void RoiClusterFusionNode::preprocess(ClusterMsgType & output_cluster_msg) { std::unique_ptr st_ptr; if (time_keeper_) st_ptr = std::make_unique(__func__, *time_keeper_); @@ -71,7 +70,7 @@ void RoiClusterFusionNode::preprocess(DetectedObjectsWithFeature & output_cluste } } -void RoiClusterFusionNode::postprocess(DetectedObjectsWithFeature & output_cluster_msg) +void RoiClusterFusionNode::postprocess(ClusterMsgType & output_cluster_msg) { std::unique_ptr st_ptr; if (time_keeper_) st_ptr = std::make_unique(__func__, *time_keeper_); @@ -79,7 +78,7 @@ void RoiClusterFusionNode::postprocess(DetectedObjectsWithFeature & output_clust if (!remove_unknown_) { return; } - DetectedObjectsWithFeature known_objects; + ClusterMsgType known_objects; known_objects.feature_objects.reserve(output_cluster_msg.feature_objects.size()); for (auto & feature_object : output_cluster_msg.feature_objects) { bool is_roi_label_known = feature_object.object.classification.front().label != @@ -94,9 +93,8 @@ void RoiClusterFusionNode::postprocess(DetectedObjectsWithFeature & output_clust } void RoiClusterFusionNode::fuseOnSingleImage( - const DetectedObjectsWithFeature & input_cluster_msg, - const Det2dManager & det2d, - const DetectedObjectsWithFeature & input_roi_msg, DetectedObjectsWithFeature & output_cluster_msg) + const ClusterMsgType & input_cluster_msg, const Det2dManager & det2d, + const RoiMsgType & input_roi_msg, ClusterMsgType & output_cluster_msg) { std::unique_ptr st_ptr; if (time_keeper_) st_ptr = std::make_unique(__func__, *time_keeper_); 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 9eac48c13a290..59f74ad5e2fdc 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 @@ -31,8 +31,7 @@ namespace autoware::image_projection_based_fusion using autoware::universe_utils::ScopedTimeTrack; RoiDetectedObjectFusionNode::RoiDetectedObjectFusionNode(const rclcpp::NodeOptions & options) -: FusionNode( - "roi_detected_object_fusion", options) +: FusionNode("roi_detected_object_fusion", options) { fusion_params_.passthrough_lower_bound_probability_thresholds = declare_parameter>("passthrough_lower_bound_probability_thresholds"); @@ -82,9 +81,8 @@ void RoiDetectedObjectFusionNode::preprocess(DetectedObjects & output_msg) } void RoiDetectedObjectFusionNode::fuseOnSingleImage( - const DetectedObjects & input_object_msg, const Det2dManager & det2d, - const DetectedObjectsWithFeature & input_roi_msg, - DetectedObjects & output_object_msg __attribute__((unused))) + const DetectedObjects & input_object_msg, const Det2dManager & det2d, + const RoiMsgType & input_roi_msg, DetectedObjects & output_object_msg __attribute__((unused))) { std::unique_ptr st_ptr; if (time_keeper_) st_ptr = std::make_unique(__func__, *time_keeper_); @@ -115,7 +113,7 @@ void RoiDetectedObjectFusionNode::fuseOnSingleImage( std::map RoiDetectedObjectFusionNode::generateDetectedObjectRoIs( - const DetectedObjects & input_object_msg, const Det2dManager & det2d, + const DetectedObjects & input_object_msg, const Det2dManager & det2d, const Eigen::Affine3d & object2camera_affine) { std::unique_ptr st_ptr; 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 83fa7476c7a82..fba4bb961bf71 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 @@ -37,15 +37,13 @@ namespace autoware::image_projection_based_fusion using autoware::universe_utils::ScopedTimeTrack; RoiPointCloudFusionNode::RoiPointCloudFusionNode(const rclcpp::NodeOptions & options) -: FusionNode( - "roi_pointcloud_fusion", options) +: FusionNode("roi_pointcloud_fusion", options) { fuse_unknown_only_ = declare_parameter("fuse_unknown_only"); min_cluster_size_ = declare_parameter("min_cluster_size"); max_cluster_size_ = declare_parameter("max_cluster_size"); cluster_2d_tolerance_ = declare_parameter("cluster_2d_tolerance"); - pub_objects_ptr_ = - this->create_publisher("output_clusters", rclcpp::QoS{1}); + pub_objects_ptr_ = this->create_publisher("output_clusters", rclcpp::QoS{1}); cluster_debug_pub_ = this->create_publisher("debug/clusters", 1); } @@ -70,7 +68,7 @@ void RoiPointCloudFusionNode::postprocess( return; } - DetectedObjectsWithFeature output_msg; + ClusterMsgType output_msg; output_msg.header = pointcloud_msg.header; output_msg.feature_objects = output_fused_objects_; @@ -87,8 +85,7 @@ void RoiPointCloudFusionNode::postprocess( } void RoiPointCloudFusionNode::fuseOnSingleImage( const sensor_msgs::msg::PointCloud2 & input_pointcloud_msg, - const Det2dManager & det2d, - const DetectedObjectsWithFeature & input_roi_msg, + const Det2dManager & det2d, const RoiMsgType & input_roi_msg, __attribute__((unused)) sensor_msgs::msg::PointCloud2 & output_pointcloud_msg) { std::unique_ptr st_ptr; From e16e688d2a033c228e2a3c46983a120ee6affa3f Mon Sep 17 00:00:00 2001 From: Taekjin LEE Date: Thu, 26 Dec 2024 15:26:23 +0900 Subject: [PATCH 15/33] clarify pointcloud message type Signed-off-by: Taekjin LEE --- .../fusion_node.hpp | 2 +- .../pointpainting_fusion/node.hpp | 12 ++++------ .../roi_pointcloud_fusion/node.hpp | 12 +++++----- .../segmentation_pointcloud_fusion/node.hpp | 18 +++++++------- .../utils/utils.hpp | 12 +++++----- .../src/fusion_node.cpp | 6 ++--- .../src/pointpainting_fusion/node.cpp | 10 ++++---- .../src/roi_pointcloud_fusion/node.cpp | 24 +++++++++---------- .../segmentation_pointcloud_fusion/node.cpp | 18 +++++++------- .../src/utils/utils.cpp | 12 +++++----- 10 files changed, 63 insertions(+), 63 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 0e57c991c536e..13a21e9ed5377 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 @@ -52,7 +52,7 @@ using autoware_perception_msgs::msg::DetectedObject; using autoware_perception_msgs::msg::DetectedObjects; using sensor_msgs::msg::CameraInfo; using sensor_msgs::msg::Image; -using sensor_msgs::msg::PointCloud2; +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; 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 6a8a162676d26..812d9d1698ba2 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 @@ -41,21 +41,19 @@ inline bool isInsideBbox( proj_y >= roi.y_offset * zc && proj_y <= (roi.y_offset + roi.height) * zc; } -class PointPaintingFusionNode -: public FusionNode +class PointPaintingFusionNode : public FusionNode { public: explicit PointPaintingFusionNode(const rclcpp::NodeOptions & options); protected: - void preprocess(sensor_msgs::msg::PointCloud2 & pointcloud_msg) override; + void preprocess(PointCloudMsgType & pointcloud_msg) override; void fuseOnSingleImage( - const sensor_msgs::msg::PointCloud2 & input_pointcloud_msg, - const Det2dManager & det2d, const RoiMsgType & input_roi_msg, - sensor_msgs::msg::PointCloud2 & painted_pointcloud_msg) override; + const PointCloudMsgType & input_pointcloud_msg, const Det2dManager & det2d, + const RoiMsgType & input_roi_msg, PointCloudMsgType & painted_pointcloud_msg) override; - void postprocess(sensor_msgs::msg::PointCloud2 & painted_pointcloud_msg) override; + void postprocess(PointCloudMsgType & painted_pointcloud_msg) override; rclcpp::Publisher::SharedPtr obj_pub_ptr_; 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 a650a67dd6b1d..1cb0bc94e998f 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 @@ -25,7 +25,7 @@ namespace autoware::image_projection_based_fusion { -class RoiPointCloudFusionNode : public FusionNode +class RoiPointCloudFusionNode : public FusionNode { private: int min_cluster_size_{1}; @@ -35,20 +35,20 @@ class RoiPointCloudFusionNode : public FusionNode::SharedPtr pub_objects_ptr_; std::vector output_fused_objects_; - rclcpp::Publisher::SharedPtr cluster_debug_pub_; + rclcpp::Publisher::SharedPtr cluster_debug_pub_; /* data */ public: explicit RoiPointCloudFusionNode(const rclcpp::NodeOptions & options); protected: - void preprocess(sensor_msgs::msg::PointCloud2 & pointcloud_msg) override; + void preprocess(PointCloudMsgType & pointcloud_msg) override; - void postprocess(sensor_msgs::msg::PointCloud2 & pointcloud_msg) override; + void postprocess(PointCloudMsgType & pointcloud_msg) override; void fuseOnSingleImage( - const PointCloud2 & input_pointcloud_msg, const Det2dManager & det2d, - const RoiMsgType & input_roi_msg, PointCloud2 & output_pointcloud_msg) override; + const PointCloudMsgType & input_pointcloud_msg, const Det2dManager & det2d, + const RoiMsgType & input_roi_msg, PointCloudMsgType & output_pointcloud_msg) override; bool out_of_scope(const ClusterObjType & obj) 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 814dababe9ae7..eeb489dbb04ed 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 @@ -34,10 +34,10 @@ namespace autoware::image_projection_based_fusion { -class SegmentPointCloudFusionNode : public FusionNode +class SegmentPointCloudFusionNode : public FusionNode { private: - rclcpp::Publisher::SharedPtr pub_pointcloud_ptr_; + rclcpp::Publisher::SharedPtr pub_pointcloud_ptr_; std::vector filter_semantic_label_target_; float filter_distance_threshold_; // declare list of semantic label target, depend on trained data of yolox segmentation model @@ -55,18 +55,18 @@ class SegmentPointCloudFusionNode : public FusionNode & det2d, - const Image & input_mask, PointCloud2 & output_pointcloud_msg) override; + const PointCloudMsgType & input_pointcloud_msg, const Det2dManager & det2d, + const Image & input_mask, PointCloudMsgType & output_pointcloud_msg) override; - bool out_of_scope(const PointCloud2 & filtered_cloud) override; + bool out_of_scope(const PointCloudMsgType & filtered_cloud) override; inline void copyPointCloud( - const PointCloud2 & input, const int point_step, const size_t global_offset, - PointCloud2 & output, size_t & output_pointcloud_size) + const PointCloudMsgType & input, const int point_step, const size_t global_offset, + PointCloudMsgType & output, size_t & output_pointcloud_size) { std::memcpy(&output.data[output_pointcloud_size], &input.data[global_offset], point_step); output_pointcloud_size += point_step; 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 12577081713be..044a71ab57533 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 @@ -56,7 +56,7 @@ namespace autoware::image_projection_based_fusion { using PointCloud = pcl::PointCloud; -using PointCloud2 = sensor_msgs::msg::PointCloud2; + struct PointData { float distance; @@ -72,17 +72,17 @@ std::optional getTransformStamped( Eigen::Affine3d transformToEigen(const geometry_msgs::msg::Transform & t); void closest_cluster( - const PointCloud2 & cluster, const double cluster_2d_tolerance, const int min_cluster_size, - const pcl::PointXYZ & center, PointCloud2 & out_cluster); + const PointCloudMsgType & cluster, const double cluster_2d_tolerance, const int min_cluster_size, + const pcl::PointXYZ & center, PointCloudMsgType & out_cluster); void updateOutputFusedObjects( - std::vector & output_objs, std::vector & clusters, - const std::vector & clusters_data_size, const PointCloud2 & in_cloud, + std::vector & output_objs, std::vector & clusters, + const std::vector & clusters_data_size, const PointCloudMsgType & in_cloud, const std_msgs::msg::Header & in_roi_header, const tf2_ros::Buffer & tf_buffer, const int min_cluster_size, const int max_cluster_size, const float cluster_2d_tolerance, std::vector & output_fused_objects); -geometry_msgs::msg::Point getCentroid(const sensor_msgs::msg::PointCloud2 & pointcloud); +geometry_msgs::msg::Point getCentroid(const PointCloudMsgType & pointcloud); } // 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 32590b30251ab..da30af888a639 100644 --- a/perception/autoware_image_projection_based_fusion/src/fusion_node.cpp +++ b/perception/autoware_image_projection_based_fusion/src/fusion_node.cpp @@ -494,7 +494,7 @@ void FusionNode::publish(const Msg3D & output_msg) // Explicit instantiation for the supported types // pointpainting fusion -template class FusionNode; +template class FusionNode; // roi cluster fusion template class FusionNode; @@ -503,9 +503,9 @@ template class FusionNode; template class FusionNode; // roi pointcloud fusion -template class FusionNode; +template class FusionNode; // segment pointcloud fusion -template class FusionNode; +template class FusionNode; } // namespace autoware::image_projection_based_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 607ee010c19df..328a60456b762 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 @@ -91,7 +91,7 @@ inline bool isUnknown(int label2d) } PointPaintingFusionNode::PointPaintingFusionNode(const rclcpp::NodeOptions & options) -: FusionNode("pointpainting_fusion", options) +: FusionNode("pointpainting_fusion", options) { omp_num_threads_ = this->declare_parameter("omp_params.num_threads"); const float score_threshold = @@ -194,7 +194,7 @@ PointPaintingFusionNode::PointPaintingFusionNode(const rclcpp::NodeOptions & opt } } -void PointPaintingFusionNode::preprocess(PointCloud2 & painted_pointcloud_msg) +void PointPaintingFusionNode::preprocess(PointCloudMsgType & painted_pointcloud_msg) { std::unique_ptr st_ptr; if (time_keeper_) st_ptr = std::make_unique(__func__, *time_keeper_); @@ -251,9 +251,9 @@ void PointPaintingFusionNode::preprocess(PointCloud2 & painted_pointcloud_msg) } void PointPaintingFusionNode::fuseOnSingleImage( - __attribute__((unused)) const PointCloud2 & input_pointcloud_msg, + __attribute__((unused)) const PointCloudMsgType & input_pointcloud_msg, const Det2dManager & det2d, const RoiMsgType & input_roi_msg, - PointCloud2 & painted_pointcloud_msg) + PointCloudMsgType & painted_pointcloud_msg) { if (painted_pointcloud_msg.data.empty() || painted_pointcloud_msg.fields.empty()) { RCLCPP_WARN_STREAM_THROTTLE( @@ -382,7 +382,7 @@ dc | dc dc dc dc ||zc| } } -void PointPaintingFusionNode::postprocess(PointCloud2 & painted_pointcloud_msg) +void PointPaintingFusionNode::postprocess(PointCloudMsgType & painted_pointcloud_msg) { std::unique_ptr st_ptr; if (time_keeper_) st_ptr = std::make_unique(__func__, *time_keeper_); 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 fba4bb961bf71..18a16bbb31747 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 @@ -37,18 +37,18 @@ namespace autoware::image_projection_based_fusion using autoware::universe_utils::ScopedTimeTrack; RoiPointCloudFusionNode::RoiPointCloudFusionNode(const rclcpp::NodeOptions & options) -: FusionNode("roi_pointcloud_fusion", options) +: FusionNode( + "roi_pointcloud_fusion", options) { fuse_unknown_only_ = declare_parameter("fuse_unknown_only"); min_cluster_size_ = declare_parameter("min_cluster_size"); max_cluster_size_ = declare_parameter("max_cluster_size"); cluster_2d_tolerance_ = declare_parameter("cluster_2d_tolerance"); pub_objects_ptr_ = this->create_publisher("output_clusters", rclcpp::QoS{1}); - cluster_debug_pub_ = this->create_publisher("debug/clusters", 1); + cluster_debug_pub_ = this->create_publisher("debug/clusters", 1); } -void RoiPointCloudFusionNode::preprocess( - __attribute__((unused)) sensor_msgs::msg::PointCloud2 & pointcloud_msg) +void RoiPointCloudFusionNode::preprocess(__attribute__((unused)) PointCloudMsgType & pointcloud_msg) { std::unique_ptr st_ptr; if (time_keeper_) st_ptr = std::make_unique(__func__, *time_keeper_); @@ -56,8 +56,8 @@ void RoiPointCloudFusionNode::preprocess( return; } -void RoiPointCloudFusionNode::postprocess( - __attribute__((unused)) sensor_msgs::msg::PointCloud2 & pointcloud_msg) +void RoiPointCloudFusionNode::postprocess(__attribute__((unused)) + PointCloudMsgType & pointcloud_msg) { std::unique_ptr st_ptr; if (time_keeper_) st_ptr = std::make_unique(__func__, *time_keeper_); @@ -78,15 +78,15 @@ void RoiPointCloudFusionNode::postprocess( output_fused_objects_.clear(); // publish debug cluster if (cluster_debug_pub_->get_subscription_count() > 0) { - sensor_msgs::msg::PointCloud2 debug_cluster_msg; + PointCloudMsgType debug_cluster_msg; autoware::euclidean_cluster::convertObjectMsg2SensorMsg(output_msg, debug_cluster_msg); cluster_debug_pub_->publish(debug_cluster_msg); } } void RoiPointCloudFusionNode::fuseOnSingleImage( - const sensor_msgs::msg::PointCloud2 & input_pointcloud_msg, - const Det2dManager & det2d, const RoiMsgType & input_roi_msg, - __attribute__((unused)) sensor_msgs::msg::PointCloud2 & output_pointcloud_msg) + const PointCloudMsgType & input_pointcloud_msg, const Det2dManager & det2d, + const RoiMsgType & input_roi_msg, + __attribute__((unused)) PointCloudMsgType & output_pointcloud_msg) { std::unique_ptr st_ptr; if (time_keeper_) st_ptr = std::make_unique(__func__, *time_keeper_); @@ -135,10 +135,10 @@ void RoiPointCloudFusionNode::fuseOnSingleImage( const int z_offset = input_pointcloud_msg.fields[pcl::getFieldIndex(input_pointcloud_msg, "z")].offset; - sensor_msgs::msg::PointCloud2 transformed_cloud; + PointCloudMsgType transformed_cloud; tf2::doTransform(input_pointcloud_msg, transformed_cloud, transform_stamped); - std::vector clusters; + std::vector clusters; std::vector clusters_data_size; clusters.resize(output_objs.size()); for (auto & cluster : clusters) { 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 8c1fa2144e292..1e19f57dd6c04 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 @@ -36,7 +36,7 @@ namespace autoware::image_projection_based_fusion using autoware::universe_utils::ScopedTimeTrack; SegmentPointCloudFusionNode::SegmentPointCloudFusionNode(const rclcpp::NodeOptions & options) -: FusionNode("segmentation_pointcloud_fusion", options) +: FusionNode("segmentation_pointcloud_fusion", options) { filter_distance_threshold_ = declare_parameter("filter_distance_threshold"); for (auto & item : filter_semantic_label_target_list_) { @@ -50,7 +50,8 @@ SegmentPointCloudFusionNode::SegmentPointCloudFusionNode(const rclcpp::NodeOptio pub_debug_mask_ptr_ = image_transport::create_publisher(this, "~/debug/mask"); } -void SegmentPointCloudFusionNode::preprocess(__attribute__((unused)) PointCloud2 & pointcloud_msg) +void SegmentPointCloudFusionNode::preprocess(__attribute__((unused)) + PointCloudMsgType & pointcloud_msg) { std::unique_ptr st_ptr; if (time_keeper_) st_ptr = std::make_unique(__func__, *time_keeper_); @@ -58,12 +59,12 @@ void SegmentPointCloudFusionNode::preprocess(__attribute__((unused)) PointCloud2 return; } -void SegmentPointCloudFusionNode::postprocess(PointCloud2 & pointcloud_msg) +void SegmentPointCloudFusionNode::postprocess(PointCloudMsgType & pointcloud_msg) { std::unique_ptr st_ptr; if (time_keeper_) st_ptr = std::make_unique(__func__, *time_keeper_); - auto original_cloud = std::make_shared(pointcloud_msg); + auto original_cloud = std::make_shared(pointcloud_msg); int point_step = original_cloud->point_step; size_t output_pointcloud_size = 0; @@ -87,8 +88,9 @@ void SegmentPointCloudFusionNode::postprocess(PointCloud2 & pointcloud_msg) } void SegmentPointCloudFusionNode::fuseOnSingleImage( - const PointCloud2 & input_pointcloud_msg, const Det2dManager & det2d, - [[maybe_unused]] const Image & input_mask, __attribute__((unused)) PointCloud2 & output_cloud) + const PointCloudMsgType & input_pointcloud_msg, const Det2dManager & det2d, + [[maybe_unused]] const Image & input_mask, + __attribute__((unused)) PointCloudMsgType & output_cloud) { std::unique_ptr st_ptr; if (time_keeper_) st_ptr = std::make_unique(__func__, *time_keeper_); @@ -128,7 +130,7 @@ void SegmentPointCloudFusionNode::fuseOnSingleImage( transform_stamped = transform_stamped_optional.value(); } - PointCloud2 transformed_cloud; + PointCloudMsgType transformed_cloud; tf2::doTransform(input_pointcloud_msg, transformed_cloud, transform_stamped); int point_step = input_pointcloud_msg.point_step; @@ -169,7 +171,7 @@ void SegmentPointCloudFusionNode::fuseOnSingleImage( } bool SegmentPointCloudFusionNode::out_of_scope(__attribute__((unused)) - const PointCloud2 & filtered_cloud) + const PointCloudMsgType & filtered_cloud) { return false; } 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 4456d25b18167..81424d4c23a34 100644 --- a/perception/autoware_image_projection_based_fusion/src/utils/utils.cpp +++ b/perception/autoware_image_projection_based_fusion/src/utils/utils.cpp @@ -67,8 +67,8 @@ Eigen::Affine3d transformToEigen(const geometry_msgs::msg::Transform & t) } void closest_cluster( - const PointCloud2 & cluster, const double cluster_2d_tolerance, const int min_cluster_size, - const pcl::PointXYZ & center, PointCloud2 & out_cluster) + const PointCloudMsgType & cluster, const double cluster_2d_tolerance, const int min_cluster_size, + const pcl::PointXYZ & center, PointCloudMsgType & out_cluster) { // sort point by distance to camera origin @@ -123,8 +123,8 @@ void closest_cluster( } void updateOutputFusedObjects( - std::vector & output_objs, std::vector & clusters, - const std::vector & clusters_data_size, const PointCloud2 & in_cloud, + std::vector & output_objs, std::vector & clusters, + const std::vector & clusters_data_size, const PointCloudMsgType & in_cloud, const std_msgs::msg::Header & in_roi_header, const tf2_ros::Buffer & tf_buffer, const int min_cluster_size, const int max_cluster_size, const float cluster_2d_tolerance, std::vector & output_fused_objects) @@ -162,7 +162,7 @@ void updateOutputFusedObjects( // TODO(badai-nguyen): change to interface to select refine criteria like closest, largest // to output refine cluster and centroid - sensor_msgs::msg::PointCloud2 refine_cluster; + PointCloudMsgType refine_cluster; closest_cluster( cluster, cluster_2d_tolerance, min_cluster_size, camera_orig_point_frame, refine_cluster); if ( @@ -184,7 +184,7 @@ void updateOutputFusedObjects( } } -geometry_msgs::msg::Point getCentroid(const sensor_msgs::msg::PointCloud2 & pointcloud) +geometry_msgs::msg::Point getCentroid(const PointCloudMsgType & pointcloud) { geometry_msgs::msg::Point centroid; centroid.x = 0.0f; From a5da70e4bbb00343f0c1a0a807c8d5ab50a85936 Mon Sep 17 00:00:00 2001 From: Taekjin LEE Date: Thu, 26 Dec 2024 16:33:09 +0900 Subject: [PATCH 16/33] Refactor fusion_node.hpp and fusion_node.cpp for improved code organization and readability Signed-off-by: Taekjin LEE --- .../image_projection_based_fusion/fusion_node.hpp | 1 - .../pointpainting_fusion/node.hpp | 2 -- .../roi_cluster_fusion/node.hpp | 4 ++-- .../roi_detected_object_fusion/node.hpp | 2 +- .../roi_pointcloud_fusion/node.hpp | 3 +-- .../segmentation_pointcloud_fusion/node.hpp | 1 - .../src/fusion_node.cpp | 4 ++-- .../src/pointpainting_fusion/node.cpp | 4 ---- .../src/roi_cluster_fusion/node.cpp | 3 +-- .../src/roi_pointcloud_fusion/node.cpp | 8 +------- .../src/segmentation_pointcloud_fusion/node.cpp | 5 ----- 11 files changed, 8 insertions(+), 29 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 13a21e9ed5377..d0441f4609148 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 @@ -170,7 +170,6 @@ class FusionNode : public rclcpp::Node // debugger std::shared_ptr debugger_; - virtual bool out_of_scope(const ExportObj & obj) = 0; float filter_scope_min_x_; float filter_scope_max_x_; float filter_scope_min_y_; 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 812d9d1698ba2..7ec0d48fa9a98 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 @@ -70,8 +70,6 @@ class PointPaintingFusionNode : public FusionNode detector_ptr_{nullptr}; - - bool out_of_scope(const DetectedObjects & obj) override; }; } // namespace autoware::image_projection_based_fusion #endif // AUTOWARE__IMAGE_PROJECTION_BASED_FUSION__POINTPAINTING_FUSION__NODE_HPP_ 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 baae10c1afce8..7df8045cbb8ab 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 @@ -24,7 +24,7 @@ namespace autoware::image_projection_based_fusion { const std::map IOU_MODE_MAP{{"iou", 0}, {"iou_x", 1}, {"iou_y", 2}}; -class RoiClusterFusionNode : public FusionNode +class RoiClusterFusionNode : public FusionNode { public: explicit RoiClusterFusionNode(const rclcpp::NodeOptions & options); @@ -51,7 +51,7 @@ class RoiClusterFusionNode : public FusionNode +class RoiPointCloudFusionNode : public FusionNode { private: int min_cluster_size_{1}; @@ -49,7 +49,6 @@ class RoiPointCloudFusionNode : public FusionNode & det2d, const RoiMsgType & input_roi_msg, PointCloudMsgType & output_pointcloud_msg) override; - bool out_of_scope(const ClusterObjType & obj) override; }; } // namespace autoware::image_projection_based_fusion 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 eeb489dbb04ed..dc136b74eedb7 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 @@ -63,7 +63,6 @@ class SegmentPointCloudFusionNode : public FusionNode & det2d, const Image & input_mask, PointCloudMsgType & output_pointcloud_msg) override; - bool out_of_scope(const PointCloudMsgType & filtered_cloud) override; inline void copyPointCloud( const PointCloudMsgType & input, const int point_step, const size_t global_offset, PointCloudMsgType & output, size_t & output_pointcloud_size) 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 da30af888a639..d2892475a9605 100644 --- a/perception/autoware_image_projection_based_fusion/src/fusion_node.cpp +++ b/perception/autoware_image_projection_based_fusion/src/fusion_node.cpp @@ -497,13 +497,13 @@ void FusionNode::publish(const Msg3D & output_msg) template class FusionNode; // roi cluster fusion -template class FusionNode; +template class FusionNode; // roi detected-object fusion template class FusionNode; // roi pointcloud fusion -template class FusionNode; +template class FusionNode; // segment pointcloud fusion template class FusionNode; 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 328a60456b762..0077198a9bad0 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 @@ -424,10 +424,6 @@ void PointPaintingFusionNode::postprocess(PointCloudMsgType & painted_pointcloud } } -bool PointPaintingFusionNode::out_of_scope(__attribute__((unused)) const DetectedObjects & obj) -{ - return false; -} } // namespace autoware::image_projection_based_fusion #include 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 321254a9bd673..010d993745432 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 @@ -41,7 +41,7 @@ namespace autoware::image_projection_based_fusion using autoware::universe_utils::ScopedTimeTrack; RoiClusterFusionNode::RoiClusterFusionNode(const rclcpp::NodeOptions & options) -: FusionNode("roi_cluster_fusion", options) +: FusionNode("roi_cluster_fusion", options) { trust_object_iou_mode_ = declare_parameter("trust_object_iou_mode"); non_trust_object_iou_mode_ = declare_parameter("non_trust_object_iou_mode"); @@ -167,7 +167,6 @@ void RoiClusterFusionNode::fuseOnSingleImage( } sensor_msgs::msg::RegionOfInterest roi; - // roi.do_rectify = m_camera_info_.at(id).do_rectify; roi.x_offset = min_x; roi.y_offset = min_y; roi.width = max_x - min_x; 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 18a16bbb31747..31e1acb0d531c 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 @@ -37,8 +37,7 @@ namespace autoware::image_projection_based_fusion using autoware::universe_utils::ScopedTimeTrack; RoiPointCloudFusionNode::RoiPointCloudFusionNode(const rclcpp::NodeOptions & options) -: FusionNode( - "roi_pointcloud_fusion", options) +: FusionNode("roi_pointcloud_fusion", options) { fuse_unknown_only_ = declare_parameter("fuse_unknown_only"); min_cluster_size_ = declare_parameter("min_cluster_size"); @@ -204,11 +203,6 @@ void RoiPointCloudFusionNode::fuseOnSingleImage( } } -bool RoiPointCloudFusionNode::out_of_scope(__attribute__((unused)) - const DetectedObjectWithFeature & obj) -{ - return false; -} } // namespace autoware::image_projection_based_fusion #include 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 1e19f57dd6c04..0f4bc1874712a 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 @@ -170,11 +170,6 @@ void SegmentPointCloudFusionNode::fuseOnSingleImage( } } -bool SegmentPointCloudFusionNode::out_of_scope(__attribute__((unused)) - const PointCloudMsgType & filtered_cloud) -{ - return false; -} } // namespace autoware::image_projection_based_fusion #include From 84a0f0bc46f76e05e82556a394e8c2e2635b3ae5 Mon Sep 17 00:00:00 2001 From: Taekjin LEE Date: Thu, 26 Dec 2024 17:12:49 +0900 Subject: [PATCH 17/33] Refactor fusion_node.hpp and fusion_node.cpp for improved code organization and readability Signed-off-by: Taekjin LEE --- .../fusion_node.hpp | 2 +- .../pointpainting_fusion/node.hpp | 3 + .../roi_cluster_fusion/node.hpp | 3 + .../roi_detected_object_fusion/node.hpp | 4 +- .../roi_pointcloud_fusion/node.hpp | 4 +- .../segmentation_pointcloud_fusion/node.hpp | 4 ++ .../src/fusion_node.cpp | 13 ++--- .../src/pointpainting_fusion/node.cpp | 15 ++++- .../src/roi_cluster_fusion/node.cpp | 55 +++++++++++-------- .../src/roi_detected_object_fusion/node.cpp | 5 +- .../src/roi_pointcloud_fusion/node.cpp | 20 ++++--- .../segmentation_pointcloud_fusion/node.cpp | 11 ++++ 12 files changed, 94 insertions(+), 45 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 d0441f4609148..bab0e49f80d47 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 @@ -166,7 +166,7 @@ class FusionNode : public rclcpp::Node std::mutex mutex_det3d_msg_; // output publisher - typename rclcpp::Publisher::SharedPtr pub_ptr_; + // typename rclcpp::Publisher::SharedPtr pub_ptr_; // debugger std::shared_ptr debugger_; 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 7ec0d48fa9a98..6b0d46fae3238 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 @@ -55,7 +55,10 @@ class PointPaintingFusionNode : public FusionNode::SharedPtr obj_pub_ptr_; + rclcpp::Publisher::SharedPtr pub_ptr_; int omp_num_threads_{1}; float score_threshold_{0.0}; 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 7df8045cbb8ab..25e9f495ce0d4 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,11 +32,14 @@ class RoiClusterFusionNode : public FusionNode & det2d, const RoiMsgType & input_roi_msg, ClusterMsgType & output_cluster_msg) override; + rclcpp::Publisher::SharedPtr pub_ptr_; + std::string trust_object_iou_mode_{"iou"}; bool use_cluster_semantic_type_{false}; bool only_allow_inside_cluster_{false}; 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 8ffc435964ed3..c036638628c7b 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 @@ -31,7 +31,7 @@ namespace autoware::image_projection_based_fusion using sensor_msgs::msg::RegionOfInterest; -class RoiDetectedObjectFusionNode : public FusionNode +class RoiDetectedObjectFusionNode : public FusionNode { public: explicit RoiDetectedObjectFusionNode(const rclcpp::NodeOptions & options); @@ -56,6 +56,8 @@ class RoiDetectedObjectFusionNode : public FusionNode::SharedPtr pub_ptr_; + private: struct { 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 345fa6772ef4b..25fea584e8634 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 @@ -42,13 +42,15 @@ class RoiPointCloudFusionNode : public FusionNode::SharedPtr pub_ptr_; void postprocess(PointCloudMsgType & pointcloud_msg) override; void fuseOnSingleImage( const PointCloudMsgType & input_pointcloud_msg, const Det2dManager & det2d, const RoiMsgType & input_roi_msg, PointCloudMsgType & output_pointcloud_msg) override; + + void publish(const PointCloudMsgType & output_msg) override; }; } // namespace autoware::image_projection_based_fusion 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 dc136b74eedb7..fb4aa89fd5fd5 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 @@ -70,6 +70,10 @@ class SegmentPointCloudFusionNode : public FusionNode::SharedPtr pub_ptr_; }; } // 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 d2892475a9605..cdac56d4e9b0c 100644 --- a/perception/autoware_image_projection_based_fusion/src/fusion_node.cpp +++ b/perception/autoware_image_projection_based_fusion/src/fusion_node.cpp @@ -106,8 +106,8 @@ FusionNode::FusionNode( std::bind(&FusionNode::subCallback, this, std::placeholders::_1); sub_ = this->create_subscription("input", rclcpp::QoS(1).best_effort(), sub_callback); - // publisher - pub_ptr_ = this->create_publisher("output", rclcpp::QoS{1}); + // // publisher + // pub_ptr_ = this->create_publisher("output", rclcpp::QoS{1}); // Set timer const auto period_ns = std::chrono::duration_cast( @@ -483,12 +483,9 @@ void FusionNode::setPeriod(const int64_t new_period) } template -void FusionNode::publish(const Msg3D & output_msg) +void FusionNode::publish(const Msg3D & output_msg __attribute__((unused))) { - if (pub_ptr_->get_subscription_count() < 1) { - return; - } - pub_ptr_->publish(output_msg); + // do nothing by default } // Explicit instantiation for the supported types @@ -500,7 +497,7 @@ template class FusionNode; template class FusionNode; // roi detected-object fusion -template class FusionNode; +template class FusionNode; // roi pointcloud fusion template class FusionNode; 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 0077198a9bad0..be4ed863b1de9 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 @@ -155,11 +155,16 @@ PointPaintingFusionNode::PointPaintingFusionNode(const rclcpp::NodeOptions & opt const auto min_area_matrix = this->declare_parameter>("min_area_matrix"); const auto max_area_matrix = this->declare_parameter>("max_area_matrix"); + // subscriber std::function sub_callback = std::bind(&PointPaintingFusionNode::subCallback, this, std::placeholders::_1); sub_ = this->create_subscription( "~/input/pointcloud", rclcpp::SensorDataQoS().keep_last(3), sub_callback); + // publisher + pub_ptr_ = this->create_publisher("output", rclcpp::QoS{1}); + obj_pub_ptr_ = this->create_publisher("~/output/objects", rclcpp::QoS{1}); + detection_class_remapper_.setParameters( allow_remapping_by_area_matrix, min_area_matrix, max_area_matrix); @@ -186,8 +191,6 @@ PointPaintingFusionNode::PointPaintingFusionNode(const rclcpp::NodeOptions & opt detector_ptr_ = std::make_unique( encoder_param, head_param, densification_param, config); - obj_pub_ptr_ = this->create_publisher("~/output/objects", rclcpp::QoS{1}); - if (this->declare_parameter("build_only", false)) { RCLCPP_INFO(this->get_logger(), "TensorRT engine is built and shutdown node."); rclcpp::shutdown(); @@ -424,6 +427,14 @@ void PointPaintingFusionNode::postprocess(PointCloudMsgType & painted_pointcloud } } +void PointPaintingFusionNode::publish(const PointCloudMsgType & painted_pointcloud_msg) +{ + if (pub_ptr_->get_subscription_count() < 1) { + return; + } + pub_ptr_->publish(painted_pointcloud_msg); +} + } // namespace autoware::image_projection_based_fusion #include 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 010d993745432..9e4b475da295c 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 @@ -53,6 +53,9 @@ RoiClusterFusionNode::RoiClusterFusionNode(const rclcpp::NodeOptions & options) remove_unknown_ = declare_parameter("remove_unknown"); fusion_distance_ = declare_parameter("fusion_distance"); trust_object_distance_ = declare_parameter("trust_object_distance"); + + // publisher + pub_ptr_ = this->create_publisher("output", rclcpp::QoS{1}); } void RoiClusterFusionNode::preprocess(ClusterMsgType & output_cluster_msg) @@ -70,28 +73,6 @@ void RoiClusterFusionNode::preprocess(ClusterMsgType & output_cluster_msg) } } -void RoiClusterFusionNode::postprocess(ClusterMsgType & output_cluster_msg) -{ - std::unique_ptr st_ptr; - if (time_keeper_) st_ptr = std::make_unique(__func__, *time_keeper_); - - if (!remove_unknown_) { - return; - } - ClusterMsgType known_objects; - known_objects.feature_objects.reserve(output_cluster_msg.feature_objects.size()); - for (auto & feature_object : output_cluster_msg.feature_objects) { - bool is_roi_label_known = feature_object.object.classification.front().label != - autoware_perception_msgs::msg::ObjectClassification::UNKNOWN; - if ( - is_roi_label_known || - feature_object.object.existence_probability >= min_roi_existence_prob_) { - known_objects.feature_objects.push_back(feature_object); - } - } - output_cluster_msg.feature_objects = known_objects.feature_objects; -} - void RoiClusterFusionNode::fuseOnSingleImage( const ClusterMsgType & input_cluster_msg, const Det2dManager & det2d, const RoiMsgType & input_roi_msg, ClusterMsgType & output_cluster_msg) @@ -289,6 +270,36 @@ double RoiClusterFusionNode::cal_iou_by_mode( } } +void RoiClusterFusionNode::postprocess(ClusterMsgType & output_cluster_msg) +{ + std::unique_ptr st_ptr; + if (time_keeper_) st_ptr = std::make_unique(__func__, *time_keeper_); + + if (!remove_unknown_) { + return; + } + ClusterMsgType known_objects; + known_objects.feature_objects.reserve(output_cluster_msg.feature_objects.size()); + for (auto & feature_object : output_cluster_msg.feature_objects) { + bool is_roi_label_known = feature_object.object.classification.front().label != + autoware_perception_msgs::msg::ObjectClassification::UNKNOWN; + if ( + is_roi_label_known || + feature_object.object.existence_probability >= min_roi_existence_prob_) { + known_objects.feature_objects.push_back(feature_object); + } + } + output_cluster_msg.feature_objects = known_objects.feature_objects; +} + +void RoiClusterFusionNode::publish(const ClusterMsgType & output_msg) +{ + if (pub_ptr_->get_subscription_count() < 1) { + return; + } + pub_ptr_->publish(output_msg); +} + } // namespace autoware::image_projection_based_fusion #include 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 59f74ad5e2fdc..8e6f16e8d69f8 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 @@ -31,7 +31,7 @@ namespace autoware::image_projection_based_fusion using autoware::universe_utils::ScopedTimeTrack; RoiDetectedObjectFusionNode::RoiDetectedObjectFusionNode(const rclcpp::NodeOptions & options) -: FusionNode("roi_detected_object_fusion", options) +: FusionNode("roi_detected_object_fusion", options) { fusion_params_.passthrough_lower_bound_probability_thresholds = declare_parameter>("passthrough_lower_bound_probability_thresholds"); @@ -47,6 +47,9 @@ RoiDetectedObjectFusionNode::RoiDetectedObjectFusionNode(const rclcpp::NodeOptio can_assign_vector.data(), label_num, label_num); fusion_params_.can_assign_matrix = can_assign_matrix_tmp.transpose(); } + + // publisher + pub_ptr_ = this->create_publisher("output", rclcpp::QoS{1}); } void RoiDetectedObjectFusionNode::preprocess(DetectedObjects & output_msg) 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 31e1acb0d531c..3cf5295858555 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 @@ -45,18 +45,12 @@ RoiPointCloudFusionNode::RoiPointCloudFusionNode(const rclcpp::NodeOptions & opt cluster_2d_tolerance_ = declare_parameter("cluster_2d_tolerance"); pub_objects_ptr_ = this->create_publisher("output_clusters", rclcpp::QoS{1}); cluster_debug_pub_ = this->create_publisher("debug/clusters", 1); -} - -void RoiPointCloudFusionNode::preprocess(__attribute__((unused)) PointCloudMsgType & pointcloud_msg) -{ - std::unique_ptr st_ptr; - if (time_keeper_) st_ptr = std::make_unique(__func__, *time_keeper_); - return; + // publisher + pub_ptr_ = this->create_publisher("output", rclcpp::QoS{1}); } -void RoiPointCloudFusionNode::postprocess(__attribute__((unused)) - PointCloudMsgType & pointcloud_msg) +void RoiPointCloudFusionNode::postprocess(PointCloudMsgType & pointcloud_msg) { std::unique_ptr st_ptr; if (time_keeper_) st_ptr = std::make_unique(__func__, *time_keeper_); @@ -203,6 +197,14 @@ void RoiPointCloudFusionNode::fuseOnSingleImage( } } +void RoiPointCloudFusionNode::publish(const PointCloudMsgType & output_msg) +{ + if (pub_ptr_->get_subscription_count() < 1) { + return; + } + pub_ptr_->publish(output_msg); +} + } // namespace autoware::image_projection_based_fusion #include 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 0f4bc1874712a..767e66640abbd 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 @@ -48,6 +48,9 @@ SegmentPointCloudFusionNode::SegmentPointCloudFusionNode(const rclcpp::NodeOptio } is_publish_debug_mask_ = declare_parameter("is_publish_debug_mask"); pub_debug_mask_ptr_ = image_transport::create_publisher(this, "~/debug/mask"); + + // publisher + pub_ptr_ = this->create_publisher("output", rclcpp::QoS{1}); } void SegmentPointCloudFusionNode::preprocess(__attribute__((unused)) @@ -170,6 +173,14 @@ void SegmentPointCloudFusionNode::fuseOnSingleImage( } } +void SegmentPointCloudFusionNode::publish(const PointCloudMsgType & output_msg) +{ + if (pub_ptr_->get_subscription_count() < 1) { + return; + } + pub_ptr_->publish(output_msg); +} + } // namespace autoware::image_projection_based_fusion #include From 3a9e9810b66d7e7a2203f581f38d0941b03264a9 Mon Sep 17 00:00:00 2001 From: Taekjin LEE Date: Thu, 26 Dec 2024 17:33:07 +0900 Subject: [PATCH 18/33] Refactor fusion_node.hpp and fusion_node.cpp for improved code organization and readability Signed-off-by: Taekjin LEE --- .../roi_cluster_fusion/node.hpp | 1 + .../roi_pointcloud_fusion/node.hpp | 21 ++++++------ .../segmentation_pointcloud_fusion/node.hpp | 32 ++++++++++--------- .../src/pointpainting_fusion/node.cpp | 4 --- .../src/roi_pointcloud_fusion/node.cpp | 4 +-- 5 files changed, 30 insertions(+), 32 deletions(-) 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 25e9f495ce0d4..7a13a5960d7c3 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 @@ -40,6 +40,7 @@ class RoiClusterFusionNode : public FusionNode::SharedPtr pub_ptr_; +private: std::string trust_object_iou_mode_{"iou"}; bool use_cluster_semantic_type_{false}; bool only_allow_inside_cluster_{false}; 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 25fea584e8634..4b6d06bc39a63 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 @@ -27,22 +27,13 @@ namespace autoware::image_projection_based_fusion { class RoiPointCloudFusionNode : public FusionNode { -private: - int min_cluster_size_{1}; - int max_cluster_size_{20}; - bool fuse_unknown_only_{true}; - double cluster_2d_tolerance_; - - rclcpp::Publisher::SharedPtr pub_objects_ptr_; - std::vector output_fused_objects_; - rclcpp::Publisher::SharedPtr cluster_debug_pub_; - - /* data */ public: explicit RoiPointCloudFusionNode(const rclcpp::NodeOptions & options); protected: + rclcpp::Publisher::SharedPtr pub_objects_ptr_; rclcpp::Publisher::SharedPtr pub_ptr_; + rclcpp::Publisher::SharedPtr cluster_debug_pub_; void postprocess(PointCloudMsgType & pointcloud_msg) override; @@ -51,6 +42,14 @@ class RoiPointCloudFusionNode : public FusionNode output_fused_objects_; }; } // namespace autoware::image_projection_based_fusion 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 fb4aa89fd5fd5..ce81455100d56 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 @@ -36,21 +36,6 @@ namespace autoware::image_projection_based_fusion { class SegmentPointCloudFusionNode : public FusionNode { -private: - rclcpp::Publisher::SharedPtr pub_pointcloud_ptr_; - std::vector filter_semantic_label_target_; - float filter_distance_threshold_; - // declare list of semantic label target, depend on trained data of yolox segmentation model - std::vector> filter_semantic_label_target_list_ = { - {"UNKNOWN", false}, {"BUILDING", false}, {"WALL", false}, {"OBSTACLE", false}, - {"TRAFFIC_LIGHT", false}, {"TRAFFIC_SIGN", false}, {"PERSON", false}, {"VEHICLE", false}, - {"BIKE", false}, {"ROAD", false}, {"SIDEWALK", false}, {"ROAD_PAINT", false}, - {"CURBSTONE", false}, {"CROSSWALK", false}, {"VEGETATION", false}, {"SKY", false}}; - - image_transport::Publisher pub_debug_mask_ptr_; - bool is_publish_debug_mask_; - std::unordered_set filter_global_offset_set_; - public: explicit SegmentPointCloudFusionNode(const rclcpp::NodeOptions & options); @@ -73,7 +58,24 @@ class SegmentPointCloudFusionNode : public FusionNode::SharedPtr pub_ptr_; + + // debug + image_transport::Publisher pub_debug_mask_ptr_; + +private: + std::vector filter_semantic_label_target_; + float filter_distance_threshold_; + // declare list of semantic label target, depend on trained data of yolox segmentation model + std::vector> filter_semantic_label_target_list_ = { + {"UNKNOWN", false}, {"BUILDING", false}, {"WALL", false}, {"OBSTACLE", false}, + {"TRAFFIC_LIGHT", false}, {"TRAFFIC_SIGN", false}, {"PERSON", false}, {"VEHICLE", false}, + {"BIKE", false}, {"ROAD", false}, {"SIDEWALK", false}, {"ROAD_PAINT", false}, + {"CURBSTONE", false}, {"CROSSWALK", false}, {"VEGETATION", false}, {"SKY", false}}; + + bool is_publish_debug_mask_; + std::unordered_set filter_global_offset_set_; }; } // namespace autoware::image_projection_based_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 be4ed863b1de9..a849987aa4e8f 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 @@ -288,10 +288,6 @@ void PointPaintingFusionNode::fuseOnSingleImage( lidar2cam_affine = _transformToEigen(transform_stamped_optional.value().transform); } - // transform - // sensor_msgs::msg::PointCloud2 transformed_pointcloud; - // tf2::doTransform(painted_pointcloud_msg, transformed_pointcloud, transform_stamped); - const auto x_offset = painted_pointcloud_msg.fields .at(static_cast(autoware::point_types::PointXYZIRCIndex::X)) .offset; 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 3cf5295858555..ac0acd60aee50 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 @@ -43,11 +43,11 @@ RoiPointCloudFusionNode::RoiPointCloudFusionNode(const rclcpp::NodeOptions & opt min_cluster_size_ = declare_parameter("min_cluster_size"); max_cluster_size_ = declare_parameter("max_cluster_size"); cluster_2d_tolerance_ = declare_parameter("cluster_2d_tolerance"); - pub_objects_ptr_ = this->create_publisher("output_clusters", rclcpp::QoS{1}); - cluster_debug_pub_ = this->create_publisher("debug/clusters", 1); // publisher pub_ptr_ = this->create_publisher("output", rclcpp::QoS{1}); + pub_objects_ptr_ = this->create_publisher("output_clusters", rclcpp::QoS{1}); + cluster_debug_pub_ = this->create_publisher("debug/clusters", 1); } void RoiPointCloudFusionNode::postprocess(PointCloudMsgType & pointcloud_msg) From 2d3370138e2ae7dccadc1ed7db144a511f68a6c9 Mon Sep 17 00:00:00 2001 From: Taekjin LEE Date: Thu, 26 Dec 2024 17:47:27 +0900 Subject: [PATCH 19/33] Refactor publisher types in fusion_node.hpp and node.hpp Signed-off-by: Taekjin LEE --- .../image_projection_based_fusion/fusion_node.hpp | 2 +- .../pointpainting_fusion/node.hpp | 3 +-- .../roi_cluster_fusion/node.hpp | 2 -- .../roi_detected_object_fusion/node.hpp | 2 -- .../roi_pointcloud_fusion/node.hpp | 3 +-- .../segmentation_pointcloud_fusion/node.hpp | 3 --- .../src/pointpainting_fusion/node.cpp | 12 ++++++------ .../src/roi_pointcloud_fusion/node.cpp | 14 +++++++------- 8 files changed, 16 insertions(+), 25 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 bab0e49f80d47..9696b39801a3b 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 @@ -166,7 +166,7 @@ class FusionNode : public rclcpp::Node std::mutex mutex_det3d_msg_; // output publisher - // typename rclcpp::Publisher::SharedPtr pub_ptr_; + typename rclcpp::Publisher::SharedPtr pub_ptr_; // debugger std::shared_ptr debugger_; 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 6b0d46fae3238..24b3db8d01f9f 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 @@ -57,8 +57,7 @@ class PointPaintingFusionNode : public FusionNode::SharedPtr obj_pub_ptr_; - rclcpp::Publisher::SharedPtr pub_ptr_; + rclcpp::Publisher::SharedPtr point_pub_ptr_; int omp_num_threads_{1}; float score_threshold_{0.0}; 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 7a13a5960d7c3..8b73efcb111f5 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 @@ -38,8 +38,6 @@ class RoiClusterFusionNode : public FusionNode & det2d, const RoiMsgType & input_roi_msg, ClusterMsgType & output_cluster_msg) override; - rclcpp::Publisher::SharedPtr pub_ptr_; - private: std::string trust_object_iou_mode_{"iou"}; bool use_cluster_semantic_type_{false}; 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 c036638628c7b..c01683cf2b58d 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 @@ -56,8 +56,6 @@ class RoiDetectedObjectFusionNode : public FusionNode::SharedPtr pub_ptr_; - private: struct { 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 4b6d06bc39a63..f456ea1594972 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 @@ -31,8 +31,7 @@ class RoiPointCloudFusionNode : public FusionNode::SharedPtr pub_objects_ptr_; - rclcpp::Publisher::SharedPtr pub_ptr_; + rclcpp::Publisher::SharedPtr point_pub_ptr_; rclcpp::Publisher::SharedPtr cluster_debug_pub_; void postprocess(PointCloudMsgType & 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 ce81455100d56..9d78bc77ca69f 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 @@ -58,9 +58,6 @@ class SegmentPointCloudFusionNode : public FusionNode::SharedPtr pub_ptr_; - // debug image_transport::Publisher pub_debug_mask_ptr_; 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 a849987aa4e8f..54355a0f99c11 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 @@ -162,8 +162,8 @@ PointPaintingFusionNode::PointPaintingFusionNode(const rclcpp::NodeOptions & opt "~/input/pointcloud", rclcpp::SensorDataQoS().keep_last(3), sub_callback); // publisher - pub_ptr_ = this->create_publisher("output", rclcpp::QoS{1}); - obj_pub_ptr_ = this->create_publisher("~/output/objects", rclcpp::QoS{1}); + point_pub_ptr_ = this->create_publisher("output", rclcpp::QoS{1}); + pub_ptr_ = this->create_publisher("~/output/objects", rclcpp::QoS{1}); detection_class_remapper_.setParameters( allow_remapping_by_area_matrix, min_area_matrix, max_area_matrix); @@ -387,7 +387,7 @@ void PointPaintingFusionNode::postprocess(PointCloudMsgType & painted_pointcloud if (time_keeper_) st_ptr = std::make_unique(__func__, *time_keeper_); const auto objects_sub_count = - obj_pub_ptr_->get_subscription_count() + obj_pub_ptr_->get_intra_process_subscription_count(); + pub_ptr_->get_subscription_count() + pub_ptr_->get_intra_process_subscription_count(); if (objects_sub_count < 1) { return; } @@ -419,16 +419,16 @@ void PointPaintingFusionNode::postprocess(PointCloudMsgType & painted_pointcloud detection_class_remapper_.mapClasses(output_msg); if (objects_sub_count > 0) { - obj_pub_ptr_->publish(output_msg); + pub_ptr_->publish(output_msg); } } void PointPaintingFusionNode::publish(const PointCloudMsgType & painted_pointcloud_msg) { - if (pub_ptr_->get_subscription_count() < 1) { + if (point_pub_ptr_->get_subscription_count() < 1) { return; } - pub_ptr_->publish(painted_pointcloud_msg); + point_pub_ptr_->publish(painted_pointcloud_msg); } } // namespace autoware::image_projection_based_fusion 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 ac0acd60aee50..916bf010ff929 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 @@ -45,8 +45,8 @@ RoiPointCloudFusionNode::RoiPointCloudFusionNode(const rclcpp::NodeOptions & opt cluster_2d_tolerance_ = declare_parameter("cluster_2d_tolerance"); // publisher - pub_ptr_ = this->create_publisher("output", rclcpp::QoS{1}); - pub_objects_ptr_ = this->create_publisher("output_clusters", rclcpp::QoS{1}); + point_pub_ptr_ = this->create_publisher("output", rclcpp::QoS{1}); + pub_ptr_ = this->create_publisher("output_clusters", rclcpp::QoS{1}); cluster_debug_pub_ = this->create_publisher("debug/clusters", 1); } @@ -55,8 +55,8 @@ void RoiPointCloudFusionNode::postprocess(PointCloudMsgType & pointcloud_msg) std::unique_ptr st_ptr; if (time_keeper_) st_ptr = std::make_unique(__func__, *time_keeper_); - const auto objects_sub_count = pub_objects_ptr_->get_subscription_count() + - pub_objects_ptr_->get_intra_process_subscription_count(); + const auto objects_sub_count = + pub_ptr_->get_subscription_count() + pub_ptr_->get_intra_process_subscription_count(); if (objects_sub_count < 1) { return; } @@ -66,7 +66,7 @@ void RoiPointCloudFusionNode::postprocess(PointCloudMsgType & pointcloud_msg) output_msg.feature_objects = output_fused_objects_; if (objects_sub_count > 0) { - pub_objects_ptr_->publish(output_msg); + pub_ptr_->publish(output_msg); } output_fused_objects_.clear(); // publish debug cluster @@ -199,10 +199,10 @@ void RoiPointCloudFusionNode::fuseOnSingleImage( void RoiPointCloudFusionNode::publish(const PointCloudMsgType & output_msg) { - if (pub_ptr_->get_subscription_count() < 1) { + if (point_pub_ptr_->get_subscription_count() < 1) { return; } - pub_ptr_->publish(output_msg); + point_pub_ptr_->publish(output_msg); } } // namespace autoware::image_projection_based_fusion From 3dbe260734d5629bda13d05fc35eb1e519c59585 Mon Sep 17 00:00:00 2001 From: Taekjin LEE Date: Thu, 26 Dec 2024 18:19:49 +0900 Subject: [PATCH 20/33] fix: resolve cppcheck issue shadowVariable Signed-off-by: Taekjin LEE --- .../image_projection_based_fusion/fusion_node.hpp | 6 +++--- .../src/fusion_node.cpp | 10 +++++----- 2 files changed, 8 insertions(+), 8 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 9696b39801a3b..69367434c6d73 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 @@ -108,7 +108,7 @@ class FusionNode : public rclcpp::Node // 2d detection management methods bool checkAllDet2dFused() { - std::lock_guard lock(mutex_det2d_flags_); + std::lock_guard lock_det2d_flags(mutex_det2d_flags_); for (const auto & det2d : det2d_list_) { if (!det2d.is_fused) { return false; @@ -118,12 +118,12 @@ class FusionNode : public rclcpp::Node } void setDet2dFused(Det2dManager & det2d) { - std::lock_guard lock(mutex_det2d_flags_); + std::lock_guard lock_det2d_flags(mutex_det2d_flags_); det2d.is_fused = true; } void clearAllDet2dFlags() { - std::lock_guard lock(mutex_det2d_flags_); + std::lock_guard lock_det2d_flags(mutex_det2d_flags_); for (auto & det2d : det2d_list_) { det2d.is_fused = false; } 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 cdac56d4e9b0c..83f2226be0e5a 100644 --- a/perception/autoware_image_projection_based_fusion/src/fusion_node.cpp +++ b/perception/autoware_image_projection_based_fusion/src/fusion_node.cpp @@ -263,7 +263,7 @@ void FusionNode::subCallback( // 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); - std::lock_guard lock(mutex_det3d_msg_); + std::lock_guard lock_det3d(mutex_det3d_msg_); exportProcess(); // reset flags @@ -293,7 +293,7 @@ void FusionNode::subCallback( // for loop for each roi for (auto & det2d : det2d_list_) { const auto roi_i = det2d.id; - std::lock_guard lock(*(det2d.mtx_ptr)); + std::lock_guard lock_det2d(*(det2d.mtx_ptr)); // check camera info if (det2d.camera_projector_ptr == nullptr) { @@ -350,7 +350,7 @@ void FusionNode::subCallback( } // PROCESS: check if the fused message is ready to publish - std::lock_guard lock(mutex_det3d_msg_); + std::lock_guard lock_det3d(mutex_det3d_msg_); cached_det3d_msg_timestamp_ = timestamp_nsec; cached_det3d_msg_ptr_ = output_msg; if (checkAllDet2dFused()) { @@ -376,13 +376,13 @@ void FusionNode::roiCallback( stop_watch_ptr_->toc("processing_time", true); auto & det2d = det2d_list_.at(roi_i); - std::lock_guard lock(*(det2d.mtx_ptr)); + std::lock_guard lock_det2d(*(det2d.mtx_ptr)); 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) { - std::lock_guard lock(mutex_det3d_msg_); + std::lock_guard lock_det3d(mutex_det3d_msg_); int64_t new_stamp = cached_det3d_msg_timestamp_ + det2d.input_offset_ms * static_cast(1e6); From f278873a80641b57a9f5c8761902514d728a0314 Mon Sep 17 00:00:00 2001 From: Taekjin LEE Date: Thu, 26 Dec 2024 18:41:32 +0900 Subject: [PATCH 21/33] Refactor fusion_node.hpp and fusion_node.cpp for improved code organization and readability Signed-off-by: Taekjin LEE --- .../fusion_node.hpp | 12 +++++++----- .../src/fusion_node.cpp | 18 ++++++++---------- 2 files changed, 15 insertions(+), 15 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 69367434c6d73..d7b3e539a390e 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 @@ -165,11 +165,7 @@ class FusionNode : public rclcpp::Node typename Msg3D::SharedPtr cached_det3d_msg_ptr_; std::mutex mutex_det3d_msg_; - // output publisher - typename rclcpp::Publisher::SharedPtr pub_ptr_; - - // debugger - std::shared_ptr debugger_; + // parameters for out_of_scope filter float filter_scope_min_x_; float filter_scope_max_x_; float filter_scope_min_y_; @@ -177,6 +173,12 @@ class FusionNode : public rclcpp::Node float filter_scope_min_z_; float filter_scope_max_z_; + // output publisher + typename rclcpp::Publisher::SharedPtr pub_ptr_; + + // debugger + std::shared_ptr debugger_; + /** \brief processing time publisher. **/ std::unique_ptr> stop_watch_ptr_; std::unique_ptr debug_publisher_; 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 83f2226be0e5a..2f321f29971a6 100644 --- a/perception/autoware_image_projection_based_fusion/src/fusion_node.cpp +++ b/perception/autoware_image_projection_based_fusion/src/fusion_node.cpp @@ -106,9 +106,6 @@ FusionNode::FusionNode( std::bind(&FusionNode::subCallback, this, std::placeholders::_1); sub_ = this->create_subscription("input", rclcpp::QoS(1).best_effort(), sub_callback); - // // publisher - // pub_ptr_ = this->create_publisher("output", rclcpp::QoS{1}); - // Set timer const auto period_ns = std::chrono::duration_cast( std::chrono::duration(timeout_ms_)); @@ -159,6 +156,14 @@ FusionNode::FusionNode( det2d_list_.at(roi_i).input_offset_ms = input_offset_ms.at(roi_i); } + // parameters for out_of_scope filter + filter_scope_min_x_ = declare_parameter("filter_scope_min_x"); + filter_scope_max_x_ = declare_parameter("filter_scope_max_x"); + filter_scope_min_y_ = declare_parameter("filter_scope_min_y"); + filter_scope_max_y_ = declare_parameter("filter_scope_max_y"); + filter_scope_min_z_ = declare_parameter("filter_scope_min_z"); + filter_scope_max_z_ = declare_parameter("filter_scope_max_z"); + // debugger if (declare_parameter("debug_mode", false)) { std::vector input_camera_topics; @@ -193,13 +198,6 @@ FusionNode::FusionNode( stop_watch_ptr_->tic("cyclic_time"); stop_watch_ptr_->tic("processing_time"); } - - filter_scope_min_x_ = declare_parameter("filter_scope_min_x"); - filter_scope_max_x_ = declare_parameter("filter_scope_max_x"); - filter_scope_min_y_ = declare_parameter("filter_scope_min_y"); - filter_scope_max_y_ = declare_parameter("filter_scope_max_y"); - filter_scope_min_z_ = declare_parameter("filter_scope_min_z"); - filter_scope_max_z_ = declare_parameter("filter_scope_max_z"); } template From ff83f0ba1faabdece883b429eff87a55e6462048 Mon Sep 17 00:00:00 2001 From: Taekjin LEE Date: Fri, 27 Dec 2024 17:26:30 +0900 Subject: [PATCH 22/33] chore: rename Det2dManager to Det2dStatus Signed-off-by: Taekjin LEE --- .../image_projection_based_fusion/fusion_node.hpp | 8 ++++---- .../pointpainting_fusion/node.hpp | 2 +- .../roi_cluster_fusion/node.hpp | 2 +- .../roi_detected_object_fusion/node.hpp | 4 ++-- .../roi_pointcloud_fusion/node.hpp | 2 +- .../segmentation_pointcloud_fusion/node.hpp | 2 +- .../src/fusion_node.cpp | 2 +- .../src/pointpainting_fusion/node.cpp | 2 +- .../src/roi_cluster_fusion/node.cpp | 2 +- .../src/roi_detected_object_fusion/node.cpp | 4 ++-- .../src/roi_pointcloud_fusion/node.cpp | 2 +- .../src/segmentation_pointcloud_fusion/node.cpp | 2 +- 12 files changed, 17 insertions(+), 17 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 d7b3e539a390e..e205984e146d1 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 @@ -62,7 +62,7 @@ using autoware::image_projection_based_fusion::CameraProjection; using autoware_perception_msgs::msg::ObjectClassification; template -struct Det2dManager +struct Det2dStatus { // camera index std::size_t id = 0; @@ -116,7 +116,7 @@ class FusionNode : public rclcpp::Node } return true; } - void setDet2dFused(Det2dManager & det2d) + void setDet2dFused(Det2dStatus & det2d) { std::lock_guard lock_det2d_flags(mutex_det2d_flags_); det2d.is_fused = true; @@ -132,7 +132,7 @@ class FusionNode : public rclcpp::Node // Custom process methods virtual void preprocess(Msg3D & output_msg); virtual void fuseOnSingleImage( - const Msg3D & input_msg, const Det2dManager & det2d, const Msg2D & input_roi_msg, + const Msg3D & input_msg, const Det2dStatus & det2d, const Msg2D & input_roi_msg, Msg3D & output_msg) = 0; virtual void postprocess(Msg3D & output_msg); virtual void publish(const Msg3D & output_msg); @@ -143,7 +143,7 @@ class FusionNode : public rclcpp::Node tf2_ros::TransformListener tf_listener_; // 2d detection management - std::vector> det2d_list_; + std::vector> det2d_list_; std::mutex mutex_det2d_flags_; // camera projection 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 24b3db8d01f9f..e8ee09cbf3334 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 PointCloudMsgType & input_pointcloud_msg, const Det2dStatus & det2d, const RoiMsgType & input_roi_msg, PointCloudMsgType & painted_pointcloud_msg) override; void postprocess(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 8b73efcb111f5..aee9a1ec9654e 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 @@ -35,7 +35,7 @@ class RoiClusterFusionNode : public FusionNode & det2d, + const ClusterMsgType & input_cluster_msg, const Det2dStatus & det2d, const RoiMsgType & input_roi_msg, ClusterMsgType & output_cluster_msg) override; private: 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 c01683cf2b58d..7064760a9ac07 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 @@ -40,11 +40,11 @@ class RoiDetectedObjectFusionNode : public FusionNode & det2d, + const DetectedObjects & input_object_msg, const Det2dStatus & det2d, const RoiMsgType & input_roi_msg, DetectedObjects & output_object_msg) override; std::map generateDetectedObjectRoIs( - const DetectedObjects & input_object_msg, const Det2dManager & det2d, + const DetectedObjects & input_object_msg, const Det2dStatus & det2d, const Eigen::Affine3d & object2camera_affine); void fuseObjectsOnImage( 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 f456ea1594972..25209bb725f3a 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 @@ -37,7 +37,7 @@ class RoiPointCloudFusionNode : public FusionNode & det2d, + const PointCloudMsgType & input_pointcloud_msg, const Det2dStatus & det2d, const RoiMsgType & input_roi_msg, PointCloudMsgType & output_pointcloud_msg) override; void publish(const PointCloudMsgType & output_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 9d78bc77ca69f..b0503c8308146 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 @@ -45,7 +45,7 @@ class SegmentPointCloudFusionNode : public FusionNode & det2d, + const PointCloudMsgType & input_pointcloud_msg, const Det2dStatus & det2d, const Image & input_mask, PointCloudMsgType & output_pointcloud_msg) override; inline void copyPointCloud( 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 2f321f29971a6..296d72e79adb7 100644 --- a/perception/autoware_image_projection_based_fusion/src/fusion_node.cpp +++ b/perception/autoware_image_projection_based_fusion/src/fusion_node.cpp @@ -145,7 +145,7 @@ FusionNode::FusionNode( approx_grid_cell_w_size_ = declare_parameter("approximation_grid_cell_width"); approx_grid_cell_h_size_ = declare_parameter("approximation_grid_cell_height"); - // camera manager initialization + // 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).mtx_ptr = std::make_unique(); 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 54355a0f99c11..3bbec603095c7 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 @@ -255,7 +255,7 @@ void PointPaintingFusionNode::preprocess(PointCloudMsgType & painted_pointcloud_ void PointPaintingFusionNode::fuseOnSingleImage( __attribute__((unused)) const PointCloudMsgType & input_pointcloud_msg, - const Det2dManager & det2d, const RoiMsgType & input_roi_msg, + const Det2dStatus & det2d, const RoiMsgType & input_roi_msg, PointCloudMsgType & painted_pointcloud_msg) { if (painted_pointcloud_msg.data.empty() || painted_pointcloud_msg.fields.empty()) { 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 9e4b475da295c..a3bc830dfa402 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 @@ -74,7 +74,7 @@ void RoiClusterFusionNode::preprocess(ClusterMsgType & output_cluster_msg) } void RoiClusterFusionNode::fuseOnSingleImage( - const ClusterMsgType & input_cluster_msg, const Det2dManager & det2d, + const ClusterMsgType & input_cluster_msg, const Det2dStatus & det2d, const RoiMsgType & input_roi_msg, ClusterMsgType & output_cluster_msg) { std::unique_ptr st_ptr; 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 8e6f16e8d69f8..9a58b56117bb8 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 @@ -84,7 +84,7 @@ void RoiDetectedObjectFusionNode::preprocess(DetectedObjects & output_msg) } void RoiDetectedObjectFusionNode::fuseOnSingleImage( - const DetectedObjects & input_object_msg, const Det2dManager & det2d, + const DetectedObjects & input_object_msg, const Det2dStatus & det2d, const RoiMsgType & input_roi_msg, DetectedObjects & output_object_msg __attribute__((unused))) { std::unique_ptr st_ptr; @@ -116,7 +116,7 @@ void RoiDetectedObjectFusionNode::fuseOnSingleImage( std::map RoiDetectedObjectFusionNode::generateDetectedObjectRoIs( - const DetectedObjects & input_object_msg, const Det2dManager & det2d, + const DetectedObjects & input_object_msg, const Det2dStatus & det2d, const Eigen::Affine3d & object2camera_affine) { std::unique_ptr st_ptr; 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 916bf010ff929..b5aabbdb83ddd 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 @@ -77,7 +77,7 @@ void RoiPointCloudFusionNode::postprocess(PointCloudMsgType & pointcloud_msg) } } void RoiPointCloudFusionNode::fuseOnSingleImage( - const PointCloudMsgType & input_pointcloud_msg, const Det2dManager & det2d, + 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 767e66640abbd..490b712daf186 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 @@ -91,7 +91,7 @@ void SegmentPointCloudFusionNode::postprocess(PointCloudMsgType & pointcloud_msg } void SegmentPointCloudFusionNode::fuseOnSingleImage( - const PointCloudMsgType & input_pointcloud_msg, const Det2dManager & det2d, + const PointCloudMsgType & input_pointcloud_msg, const Det2dStatus & det2d, [[maybe_unused]] const Image & input_mask, __attribute__((unused)) PointCloudMsgType & output_cloud) { From ba66d2e355230e6b1db9073b17d82bbc4cc80ca4 Mon Sep 17 00:00:00 2001 From: Taekjin LEE Date: Fri, 27 Dec 2024 17:35:33 +0900 Subject: [PATCH 23/33] revert mutex related changes Signed-off-by: Taekjin LEE --- .../fusion_node.hpp | 17 +--------- .../src/fusion_node.cpp | 33 ++++++++++--------- 2 files changed, 19 insertions(+), 31 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 e205984e146d1..c0a373d4ca239 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 @@ -76,7 +76,6 @@ struct Det2dStatus double input_offset_ms = 0.0; // cache std::map cached_det2d_msgs; - std::unique_ptr mtx_ptr; }; template @@ -108,7 +107,6 @@ class FusionNode : public rclcpp::Node // 2d detection management methods bool checkAllDet2dFused() { - std::lock_guard lock_det2d_flags(mutex_det2d_flags_); for (const auto & det2d : det2d_list_) { if (!det2d.is_fused) { return false; @@ -116,18 +114,6 @@ class FusionNode : public rclcpp::Node } return true; } - void setDet2dFused(Det2dStatus & det2d) - { - std::lock_guard lock_det2d_flags(mutex_det2d_flags_); - det2d.is_fused = true; - } - void clearAllDet2dFlags() - { - std::lock_guard lock_det2d_flags(mutex_det2d_flags_); - for (auto & det2d : det2d_list_) { - det2d.is_fused = false; - } - } // Custom process methods virtual void preprocess(Msg3D & output_msg); @@ -144,7 +130,6 @@ class FusionNode : public rclcpp::Node // 2d detection management std::vector> det2d_list_; - std::mutex mutex_det2d_flags_; // camera projection float approx_grid_cell_w_size_; @@ -163,7 +148,7 @@ class FusionNode : public rclcpp::Node // cache for fusion int64_t cached_det3d_msg_timestamp_; typename Msg3D::SharedPtr cached_det3d_msg_ptr_; - std::mutex mutex_det3d_msg_; + std::mutex mutex_cached_msgs_; // parameters for out_of_scope filter float filter_scope_min_x_; 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 296d72e79adb7..bbe80389321e0 100644 --- a/perception/autoware_image_projection_based_fusion/src/fusion_node.cpp +++ b/perception/autoware_image_projection_based_fusion/src/fusion_node.cpp @@ -148,7 +148,6 @@ FusionNode::FusionNode( // 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).mtx_ptr = std::make_unique(); 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); @@ -261,13 +260,16 @@ void FusionNode::subCallback( // 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); - std::lock_guard lock_det3d(mutex_det3d_msg_); exportProcess(); // reset flags - clearAllDet2dFlags(); + for (auto & det2d : det2d_list_) { + det2d.is_fused = false; + } } + std::lock_guard lock(mutex_cached_msgs_); + // TIMING: reset timer to the timeout time auto period = std::chrono::duration_cast( std::chrono::duration(timeout_ms_)); @@ -291,7 +293,6 @@ void FusionNode::subCallback( // for loop for each roi for (auto & det2d : det2d_list_) { const auto roi_i = det2d.id; - std::lock_guard lock_det2d(*(det2d.mtx_ptr)); // check camera info if (det2d.camera_projector_ptr == nullptr) { @@ -333,7 +334,7 @@ void FusionNode::subCallback( fuseOnSingleImage(*det3d_msg, det2d, *(det2d_msgs[matched_stamp]), *output_msg); det2d_msgs.erase(matched_stamp); - setDet2dFused(det2d); + det2d.is_fused = true; // add timestamp interval for debug if (debug_publisher_) { @@ -348,7 +349,6 @@ void FusionNode::subCallback( } // PROCESS: check if the fused message is ready to publish - std::lock_guard lock_det3d(mutex_det3d_msg_); cached_det3d_msg_timestamp_ = timestamp_nsec; cached_det3d_msg_ptr_ = output_msg; if (checkAllDet2dFused()) { @@ -356,7 +356,9 @@ void FusionNode::subCallback( exportProcess(); // reset flags - clearAllDet2dFlags(); + 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 @@ -374,14 +376,11 @@ void FusionNode::roiCallback( stop_watch_ptr_->toc("processing_time", true); auto & det2d = det2d_list_.at(roi_i); - std::lock_guard lock_det2d(*(det2d.mtx_ptr)); 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) { - std::lock_guard lock_det3d(mutex_det3d_msg_); - int64_t new_stamp = cached_det3d_msg_timestamp_ + det2d.input_offset_ms * static_cast(1e6); int64_t interval = abs(timestamp_nsec - new_stamp); @@ -401,7 +400,7 @@ void FusionNode::roiCallback( } // PROCESS: fuse the main message with the roi message fuseOnSingleImage(*(cached_det3d_msg_ptr_), det2d, *det2d_msg, *(cached_det3d_msg_ptr_)); - setDet2dFused(det2d); + det2d.is_fused = true; if (debug_publisher_) { double timestamp_interval_ms = (timestamp_nsec - cached_det3d_msg_timestamp_) / 1e6; @@ -416,7 +415,9 @@ void FusionNode::roiCallback( if (checkAllDet2dFused()) { exportProcess(); // reset flags - clearAllDet2dFlags(); + for (auto & det2d : det2d_list_) { + det2d.is_fused = false; + } } processing_time_ms = processing_time_ms + stop_watch_ptr_->toc("processing_time", true); return; @@ -440,7 +441,7 @@ void FusionNode::timer_callback() using std::chrono_literals::operator""ms; timer_->cancel(); - if (mutex_det3d_msg_.try_lock()) { + if (mutex_cached_msgs_.try_lock()) { // PROCESS: if timeout, postprocess cached msg if (cached_det3d_msg_ptr_ != nullptr) { stop_watch_ptr_->toc("processing_time", true); @@ -448,9 +449,11 @@ void FusionNode::timer_callback() } // reset flags whether the message is fused or not - clearAllDet2dFlags(); + for (auto & det2d : det2d_list_) { + det2d.is_fused = false; + } - mutex_det3d_msg_.unlock(); + mutex_cached_msgs_.unlock(); } else { // TIMING: retry the process after 10ms try { From 12d699726937faf958ac59231fe1976087746b4a Mon Sep 17 00:00:00 2001 From: Taekjin LEE Date: Fri, 27 Dec 2024 17:54:07 +0900 Subject: [PATCH 24/33] refactor: review member and method's access Signed-off-by: Taekjin LEE --- .../fusion_node.hpp | 46 ++++++++++--------- 1 file changed, 25 insertions(+), 21 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 c0a373d4ca239..f9b5521c9024c 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 @@ -90,15 +90,12 @@ class FusionNode : public rclcpp::Node explicit FusionNode( const std::string & node_name, const rclcpp::NodeOptions & options, int queue_size); -protected: +private: // Common process methods void cameraInfoCallback( const sensor_msgs::msg::CameraInfo::ConstSharedPtr input_camera_info_msg, const std::size_t camera_id); - // callback for main subscription - void subCallback(const typename Msg3D::ConstSharedPtr input_msg); - // callback for roi subscription - void roiCallback(const typename Msg2D::ConstSharedPtr input_roi_msg, const std::size_t roi_i); + // callback for timer void timer_callback(); void setPeriod(const int64_t new_period); @@ -115,6 +112,29 @@ class FusionNode : public rclcpp::Node return true; } + // camera projection + float approx_grid_cell_w_size_; + float approx_grid_cell_h_size_; + + // timer + rclcpp::TimerBase::SharedPtr timer_; + double timeout_ms_{}; + double match_threshold_ms_{}; + + 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::mutex mutex_cached_msgs_; + +protected: + // callback for main subscription + void subCallback(const typename Msg3D::ConstSharedPtr input_msg); + // callback for roi subscription + void roiCallback(const typename Msg2D::ConstSharedPtr input_roi_msg, const std::size_t roi_i); + // Custom process methods virtual void preprocess(Msg3D & output_msg); virtual void fuseOnSingleImage( @@ -131,24 +151,8 @@ class FusionNode : public rclcpp::Node // 2d detection management std::vector> det2d_list_; - // camera projection - float approx_grid_cell_w_size_; - float approx_grid_cell_h_size_; - - // timer - rclcpp::TimerBase::SharedPtr timer_; - double timeout_ms_{}; - double match_threshold_ms_{}; - /** \brief A vector of subscriber. */ typename rclcpp::Subscription::SharedPtr sub_; - 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::mutex mutex_cached_msgs_; // parameters for out_of_scope filter float filter_scope_min_x_; From 1bac72ab4343265a4f1584d986d2cf3f9bb448c3 Mon Sep 17 00:00:00 2001 From: Taekjin LEE Date: Fri, 27 Dec 2024 18:09:38 +0900 Subject: [PATCH 25/33] fix: resolve shadowVariable of 'det2d' Signed-off-by: Taekjin LEE --- .../src/fusion_node.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) 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 bbe80389321e0..dd3be41b1b4f6 100644 --- a/perception/autoware_image_projection_based_fusion/src/fusion_node.cpp +++ b/perception/autoware_image_projection_based_fusion/src/fusion_node.cpp @@ -415,8 +415,8 @@ void FusionNode::roiCallback( if (checkAllDet2dFused()) { exportProcess(); // reset flags - for (auto & det2d : det2d_list_) { - det2d.is_fused = false; + for (auto & status : det2d_list_) { + status.is_fused = false; } } processing_time_ms = processing_time_ms + stop_watch_ptr_->toc("processing_time", true); From ca1a474d36539113399f40c44e8e64e14a2536c3 Mon Sep 17 00:00:00 2001 From: Taekjin LEE Date: Tue, 7 Jan 2025 10:55:54 +0900 Subject: [PATCH 26/33] fix missing line Signed-off-by: Taekjin LEE --- .../src/pointpainting_fusion/node.cpp | 76 ++++++++++--------- 1 file changed, 39 insertions(+), 37 deletions(-) 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 3bbec603095c7..f531c55cbbf83 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 @@ -333,51 +333,53 @@ dc | dc dc dc dc ||zc| p_y = point_camera.y(); p_z = point_camera.z(); - if (det2d.camera_projector_ptr->isOutsideHorizontalView(p_x, p_z)) { - continue; - } + if (det2d.camera_projector_ptr->isOutsideHorizontalView(p_x, p_z)) { + continue; + } - // project - Eigen::Vector2d projected_point; - if (det2d.camera_projector_ptr->calcImageProjectedPoint( - cv::Point3d(p_x, p_y, p_z), projected_point)) { - // iterate 2d bbox - for (const auto & feature_object : objects) { - sensor_msgs::msg::RegionOfInterest roi = feature_object.feature.roi; - // paint current point if it is inside bbox - int label2d = feature_object.object.classification.front().label; - if ( - !isUnknown(label2d) && isInsideBbox(projected_point.x(), projected_point.y(), roi, 1.0)) { - // cppcheck-suppress invalidPointerCast - auto p_class = reinterpret_cast(&output[stride + class_offset]); - for (const auto & cls : isClassTable_) { - // add up the class values if the point belongs to multiple classes - *p_class = cls.second(label2d) ? (class_index_[cls.first] + *p_class) : *p_class; + // project + Eigen::Vector2d projected_point; + if (det2d.camera_projector_ptr->calcImageProjectedPoint( + cv::Point3d(p_x, p_y, p_z), projected_point)) { + // iterate 2d bbox + for (const auto & feature_object : objects) { + sensor_msgs::msg::RegionOfInterest roi = feature_object.feature.roi; + // paint current point if it is inside bbox + int label2d = feature_object.object.classification.front().label; + if ( + !isUnknown(label2d) && + isInsideBbox(projected_point.x(), projected_point.y(), roi, 1.0)) { + // cppcheck-suppress invalidPointerCast + auto p_class = reinterpret_cast(&output[stride + class_offset]); + for (const auto & cls : isClassTable_) { + // add up the class values if the point belongs to multiple classes + *p_class = cls.second(label2d) ? (class_index_[cls.first] + *p_class) : *p_class; + } + } + if (debugger_) { + int thread_id = omp_get_thread_num(); + local_vectors[thread_id].push_back(projected_point); } - } - if (debugger_) { - int thread_id = omp_get_thread_num(); - local_vectors[thread_id].push_back(projected_point); } } } - } - if (debugger_) { - std::unique_ptr inner_st_ptr; - if (time_keeper_) - inner_st_ptr = std::make_unique("publish debug message", *time_keeper_); + if (debugger_) { + std::unique_ptr inner_st_ptr; + if (time_keeper_) + inner_st_ptr = std::make_unique("publish debug message", *time_keeper_); - for (const auto & feature_object : input_roi_msg.feature_objects) { - debug_image_rois.push_back(feature_object.feature.roi); - } + for (const auto & feature_object : input_roi_msg.feature_objects) { + debug_image_rois.push_back(feature_object.feature.roi); + } - for (const auto & local_vec : local_vectors) { - debug_image_points.insert(debug_image_points.end(), local_vec.begin(), local_vec.end()); - } + for (const auto & local_vec : local_vectors) { + debug_image_points.insert(debug_image_points.end(), local_vec.begin(), local_vec.end()); + } - debugger_->image_rois_ = debug_image_rois; - debugger_->obstacle_points_ = debug_image_points; - debugger_->publishImage(det2d.id, input_roi_msg.header.stamp); + debugger_->image_rois_ = debug_image_rois; + debugger_->obstacle_points_ = debug_image_points; + debugger_->publishImage(det2d.id, input_roi_msg.header.stamp); + } } } From 3b5f47b3a2da8688bc7e28be8f545e45f9ed9c95 Mon Sep 17 00:00:00 2001 From: Taekjin LEE Date: Tue, 7 Jan 2025 16:04:08 +0900 Subject: [PATCH 27/33] refactor message postprocess and publish methods Signed-off-by: Taekjin LEE --- .../fusion_node.hpp | 4 +- .../pointpainting_fusion/node.hpp | 5 +- .../roi_cluster_fusion/node.hpp | 6 +- .../roi_detected_object_fusion/node.hpp | 2 + .../roi_pointcloud_fusion/node.hpp | 6 +- .../segmentation_pointcloud_fusion/node.hpp | 5 +- .../src/fusion_node.cpp | 22 ++++--- .../src/pointpainting_fusion/node.cpp | 20 ++++--- .../src/roi_cluster_fusion/node.cpp | 28 ++++----- .../src/roi_detected_object_fusion/node.cpp | 50 +++++++++------- .../src/roi_pointcloud_fusion/node.cpp | 57 +++++++++---------- .../segmentation_pointcloud_fusion/node.cpp | 55 +++++++++--------- 12 files changed, 140 insertions(+), 120 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 f9b5521c9024c..30fd5e4197135 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 @@ -140,8 +140,8 @@ class FusionNode : public rclcpp::Node virtual void fuseOnSingleImage( const Msg3D & input_msg, const Det2dStatus & det2d, const Msg2D & input_roi_msg, Msg3D & output_msg) = 0; - virtual void postprocess(Msg3D & output_msg); - virtual void publish(const Msg3D & output_msg); + virtual void postprocess(const Msg3D & processing_msg, ExportObj & output_msg); + virtual void publish(const ExportObj & output_msg); // Members std::size_t rois_number_{1}; 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 e8ee09cbf3334..097cd454b99bf 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 @@ -53,9 +53,10 @@ class PointPaintingFusionNode : public FusionNode & det2d, const RoiMsgType & input_roi_msg, PointCloudMsgType & painted_pointcloud_msg) override; - void postprocess(PointCloudMsgType & painted_pointcloud_msg) override; + void postprocess( + const PointCloudMsgType & painted_pointcloud_msg, DetectedObjects & output_msg) override; - void publish(const PointCloudMsgType & output_msg) override; + void publish(const DetectedObjects & output_msg) override; rclcpp::Publisher::SharedPtr point_pub_ptr_; 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 aee9a1ec9654e..f1084013a8ed3 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 @@ -31,13 +31,15 @@ class RoiClusterFusionNode : public FusionNode & det2d, const RoiMsgType & input_roi_msg, ClusterMsgType & output_cluster_msg) override; + void postprocess(const ClusterMsgType & output_cluster_msg, ClusterMsgType & output_msg) override; + + void publish(const ClusterMsgType & output_msg) override; + private: std::string trust_object_iou_mode_{"iou"}; bool use_cluster_semantic_type_{false}; 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 7064760a9ac07..8d1fcddf1447d 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 @@ -52,6 +52,8 @@ class RoiDetectedObjectFusionNode : public FusionNode & image_rois, const std::map & object_roi_map); + void postprocess(const DetectedObjects & processing_msg, DetectedObjects & output_msg) override; + void publish(const DetectedObjects & output_msg) override; bool out_of_scope(const DetectedObject & obj); 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 25209bb725f3a..298cca900ddf8 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,13 +34,13 @@ class RoiPointCloudFusionNode : public FusionNode::SharedPtr point_pub_ptr_; rclcpp::Publisher::SharedPtr cluster_debug_pub_; - void postprocess(PointCloudMsgType & pointcloud_msg) override; - void fuseOnSingleImage( const PointCloudMsgType & input_pointcloud_msg, const Det2dStatus & det2d, const RoiMsgType & input_roi_msg, PointCloudMsgType & output_pointcloud_msg) override; - void publish(const PointCloudMsgType & output_msg) override; + void postprocess(const PointCloudMsgType & pointcloud_msg, ClusterMsgType & output_msg) override; + + void publish(const ClusterMsgType & output_msg) override; private: int min_cluster_size_{1}; 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 b0503c8308146..aac6c385adbd9 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,8 +42,6 @@ class SegmentPointCloudFusionNode : public FusionNode & det2d, const Image & input_mask, PointCloudMsgType & output_pointcloud_msg) override; @@ -56,6 +54,9 @@ class SegmentPointCloudFusionNode : public FusionNode::exportProcess() { timer_->cancel(); - postprocess(*(cached_det3d_msg_ptr_)); - publish(*(cached_det3d_msg_ptr_)); + ExportObj output_msg; + postprocess(*(cached_det3d_msg_ptr_), output_msg); + publish(output_msg); // add processing time for debug if (debug_publisher_) { @@ -427,12 +428,6 @@ void FusionNode::roiCallback( det2d.cached_det2d_msgs[timestamp_nsec] = det2d_msg; } -template -void FusionNode::postprocess(Msg3D & output_msg __attribute__((unused))) -{ - // do nothing by default -} - template void FusionNode::timer_callback() { @@ -484,7 +479,16 @@ void FusionNode::setPeriod(const int64_t new_period) } template -void FusionNode::publish(const Msg3D & output_msg __attribute__((unused))) +void FusionNode::postprocess( + const Msg3D & processing_msg __attribute__((unused)), + ExportObj & output_msg __attribute__((unused))) +{ + // do nothing by default +} + +template +void FusionNode::publish(const ExportObj & output_msg + __attribute__((unused))) { // do nothing by default } 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 f531c55cbbf83..a02d39ddac82a 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 @@ -383,11 +383,15 @@ dc | dc dc dc dc ||zc| } } -void PointPaintingFusionNode::postprocess(PointCloudMsgType & painted_pointcloud_msg) +void PointPaintingFusionNode::postprocess( + const PointCloudMsgType & painted_pointcloud_msg, DetectedObjects & output_msg) { std::unique_ptr st_ptr; if (time_keeper_) st_ptr = std::make_unique(__func__, *time_keeper_); + output_msg.header = painted_pointcloud_msg.header; + output_msg.objects.clear(); + const auto objects_sub_count = pub_ptr_->get_subscription_count() + pub_ptr_->get_intra_process_subscription_count(); if (objects_sub_count < 1) { @@ -414,23 +418,23 @@ void PointPaintingFusionNode::postprocess(PointCloudMsgType & painted_pointcloud raw_objects.emplace_back(obj); } - autoware_perception_msgs::msg::DetectedObjects output_msg; - output_msg.header = painted_pointcloud_msg.header; + // prepare output message output_msg.objects = iou_bev_nms_.apply(raw_objects); detection_class_remapper_.mapClasses(output_msg); - if (objects_sub_count > 0) { - pub_ptr_->publish(output_msg); + // publish debug message: painted pointcloud + if (point_pub_ptr_->get_subscription_count() < 1) { + point_pub_ptr_->publish(painted_pointcloud_msg); } } -void PointPaintingFusionNode::publish(const PointCloudMsgType & painted_pointcloud_msg) +void PointPaintingFusionNode::publish(const DetectedObjects & output_msg) { - if (point_pub_ptr_->get_subscription_count() < 1) { + if (pub_ptr_->get_subscription_count() < 1) { return; } - point_pub_ptr_->publish(painted_pointcloud_msg); + pub_ptr_->publish(output_msg); } } // namespace autoware::image_projection_based_fusion 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 a3bc830dfa402..078a08adf4c4f 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 @@ -270,26 +270,26 @@ double RoiClusterFusionNode::cal_iou_by_mode( } } -void RoiClusterFusionNode::postprocess(ClusterMsgType & output_cluster_msg) +void RoiClusterFusionNode::postprocess( + const ClusterMsgType & processing_msg, ClusterMsgType & output_msg) { std::unique_ptr st_ptr; if (time_keeper_) st_ptr = std::make_unique(__func__, *time_keeper_); - if (!remove_unknown_) { - return; - } - ClusterMsgType known_objects; - known_objects.feature_objects.reserve(output_cluster_msg.feature_objects.size()); - for (auto & feature_object : output_cluster_msg.feature_objects) { - bool is_roi_label_known = feature_object.object.classification.front().label != - autoware_perception_msgs::msg::ObjectClassification::UNKNOWN; - if ( - is_roi_label_known || - feature_object.object.existence_probability >= min_roi_existence_prob_) { - known_objects.feature_objects.push_back(feature_object); + output_msg = processing_msg; + + if (remove_unknown_) { + // filter by object classification and existence probability + output_msg.feature_objects.clear(); + for (const auto & feature_object : processing_msg.feature_objects) { + if ( + feature_object.object.classification.front().label != + autoware_perception_msgs::msg::ObjectClassification::UNKNOWN || + feature_object.object.existence_probability >= min_roi_existence_prob_) { + output_msg.feature_objects.push_back(feature_object); + } } } - output_cluster_msg.feature_objects = known_objects.feature_objects; } void RoiClusterFusionNode::publish(const ClusterMsgType & output_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 9a58b56117bb8..5bbe3d158dabd 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 @@ -290,14 +290,15 @@ bool RoiDetectedObjectFusionNode::out_of_scope(const DetectedObject & obj) return is_out; } -void RoiDetectedObjectFusionNode::publish(const DetectedObjects & output_msg) +void RoiDetectedObjectFusionNode::postprocess( + const DetectedObjects & processing_msg, DetectedObjects & output_msg) { - if (pub_ptr_->get_subscription_count() < 1) { - return; - } + output_msg.header = processing_msg.header; + output_msg.objects.clear(); - int64_t timestamp_nsec = - output_msg.header.stamp.sec * static_cast(1e9) + output_msg.header.stamp.nanosec; + // filter out ignored objects + int64_t timestamp_nsec = processing_msg.header.stamp.sec * static_cast(1e9) + + processing_msg.header.stamp.nanosec; if ( passthrough_object_flags_map_.size() == 0 || fused_object_flags_map_.size() == 0 || ignored_object_flags_map_.size() == 0) { @@ -309,38 +310,47 @@ void RoiDetectedObjectFusionNode::publish(const DetectedObjects & output_msg) ignored_object_flags_map_.count(timestamp_nsec) == 0) { return; } + auto & passthrough_object_flags = passthrough_object_flags_map_.at(timestamp_nsec); auto & fused_object_flags = fused_object_flags_map_.at(timestamp_nsec); - auto & ignored_object_flags = ignored_object_flags_map_.at(timestamp_nsec); - - DetectedObjects output_objects_msg, debug_fused_objects_msg, debug_ignored_objects_msg; - output_objects_msg.header = output_msg.header; - debug_fused_objects_msg.header = output_msg.header; - debug_ignored_objects_msg.header = output_msg.header; - for (std::size_t obj_i = 0; obj_i < output_msg.objects.size(); ++obj_i) { - const auto & obj = output_msg.objects.at(obj_i); - if (passthrough_object_flags.at(obj_i)) { - output_objects_msg.objects.emplace_back(obj); + for (std::size_t obj_i = 0; obj_i < processing_msg.objects.size(); ++obj_i) { + const auto & obj = processing_msg.objects.at(obj_i); + if (passthrough_object_flags.at(obj_i) || fused_object_flags.at(obj_i)) { + output_msg.objects.emplace_back(obj); } + } + + // debug messages + auto & ignored_object_flags = ignored_object_flags_map_.at(timestamp_nsec); + DetectedObjects debug_fused_objects_msg, debug_ignored_objects_msg; + debug_fused_objects_msg.header = processing_msg.header; + debug_ignored_objects_msg.header = processing_msg.header; + for (std::size_t obj_i = 0; obj_i < processing_msg.objects.size(); ++obj_i) { + const auto & obj = processing_msg.objects.at(obj_i); if (fused_object_flags.at(obj_i)) { - output_objects_msg.objects.emplace_back(obj); debug_fused_objects_msg.objects.emplace_back(obj); } if (ignored_object_flags.at(obj_i)) { debug_ignored_objects_msg.objects.emplace_back(obj); } } - - pub_ptr_->publish(output_objects_msg); - debug_publisher_->publish("debug/fused_objects", debug_fused_objects_msg); debug_publisher_->publish("debug/ignored_objects", debug_ignored_objects_msg); + // clear flags passthrough_object_flags_map_.erase(timestamp_nsec); fused_object_flags_map_.erase(timestamp_nsec); ignored_object_flags_map_.erase(timestamp_nsec); } +void RoiDetectedObjectFusionNode::publish(const DetectedObjects & output_msg) +{ + if (pub_ptr_->get_subscription_count() < 1) { + return; + } + pub_ptr_->publish(output_msg); +} + } // namespace autoware::image_projection_based_fusion #include 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 b5aabbdb83ddd..b1eef00053946 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 @@ -50,32 +50,6 @@ RoiPointCloudFusionNode::RoiPointCloudFusionNode(const rclcpp::NodeOptions & opt cluster_debug_pub_ = this->create_publisher("debug/clusters", 1); } -void RoiPointCloudFusionNode::postprocess(PointCloudMsgType & pointcloud_msg) -{ - std::unique_ptr st_ptr; - if (time_keeper_) st_ptr = std::make_unique(__func__, *time_keeper_); - - const auto objects_sub_count = - pub_ptr_->get_subscription_count() + pub_ptr_->get_intra_process_subscription_count(); - if (objects_sub_count < 1) { - return; - } - - ClusterMsgType output_msg; - output_msg.header = pointcloud_msg.header; - output_msg.feature_objects = output_fused_objects_; - - if (objects_sub_count > 0) { - pub_ptr_->publish(output_msg); - } - output_fused_objects_.clear(); - // publish debug cluster - if (cluster_debug_pub_->get_subscription_count() > 0) { - PointCloudMsgType debug_cluster_msg; - autoware::euclidean_cluster::convertObjectMsg2SensorMsg(output_msg, debug_cluster_msg); - cluster_debug_pub_->publish(debug_cluster_msg); - } -} void RoiPointCloudFusionNode::fuseOnSingleImage( const PointCloudMsgType & input_pointcloud_msg, const Det2dStatus & det2d, const RoiMsgType & input_roi_msg, @@ -197,14 +171,37 @@ void RoiPointCloudFusionNode::fuseOnSingleImage( } } -void RoiPointCloudFusionNode::publish(const PointCloudMsgType & output_msg) +void RoiPointCloudFusionNode::postprocess( + const PointCloudMsgType & pointcloud_msg, ClusterMsgType & output_msg) { - if (point_pub_ptr_->get_subscription_count() < 1) { - return; + std::unique_ptr st_ptr; + if (time_keeper_) st_ptr = std::make_unique(__func__, *time_keeper_); + + output_msg.header = pointcloud_msg.header; + output_msg.feature_objects = output_fused_objects_; + + output_fused_objects_.clear(); + + // publish debug cluster + if (cluster_debug_pub_->get_subscription_count() > 0) { + PointCloudMsgType debug_cluster_msg; + autoware::euclidean_cluster::convertObjectMsg2SensorMsg(output_msg, debug_cluster_msg); + cluster_debug_pub_->publish(debug_cluster_msg); + } + if (point_pub_ptr_->get_subscription_count() > 0) { + point_pub_ptr_->publish(pointcloud_msg); } - point_pub_ptr_->publish(output_msg); } +void RoiPointCloudFusionNode::publish(const ClusterMsgType & output_msg) +{ + const auto objects_sub_count = + pub_ptr_->get_subscription_count() + pub_ptr_->get_intra_process_subscription_count(); + if (objects_sub_count < 1) { + return; + } + pub_ptr_->publish(output_msg); +} } // namespace autoware::image_projection_based_fusion #include 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 490b712daf186..acac51dded077 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,34 +62,6 @@ void SegmentPointCloudFusionNode::preprocess(__attribute__((unused)) return; } -void SegmentPointCloudFusionNode::postprocess(PointCloudMsgType & pointcloud_msg) -{ - std::unique_ptr st_ptr; - if (time_keeper_) st_ptr = std::make_unique(__func__, *time_keeper_); - - auto original_cloud = std::make_shared(pointcloud_msg); - - int point_step = original_cloud->point_step; - size_t output_pointcloud_size = 0; - pointcloud_msg.data.clear(); - pointcloud_msg.data.resize(original_cloud->data.size()); - - for (size_t global_offset = 0; global_offset < original_cloud->data.size(); - global_offset += point_step) { - if (filter_global_offset_set_.find(global_offset) == filter_global_offset_set_.end()) { - copyPointCloud( - *original_cloud, point_step, global_offset, pointcloud_msg, output_pointcloud_size); - } - } - - pointcloud_msg.data.resize(output_pointcloud_size); - pointcloud_msg.row_step = output_pointcloud_size / pointcloud_msg.height; - pointcloud_msg.width = output_pointcloud_size / pointcloud_msg.point_step / pointcloud_msg.height; - - filter_global_offset_set_.clear(); - return; -} - void SegmentPointCloudFusionNode::fuseOnSingleImage( const PointCloudMsgType & input_pointcloud_msg, const Det2dStatus & det2d, [[maybe_unused]] const Image & input_mask, @@ -173,6 +145,33 @@ void SegmentPointCloudFusionNode::fuseOnSingleImage( } } +void SegmentPointCloudFusionNode::postprocess( + const PointCloudMsgType & pointcloud_msg, PointCloudMsgType & output_msg) +{ + std::unique_ptr st_ptr; + if (time_keeper_) st_ptr = std::make_unique(__func__, *time_keeper_); + + output_msg.header = pointcloud_msg.header; + output_msg.data.clear(); + output_msg.data.resize(pointcloud_msg.data.size()); + const int point_step = pointcloud_msg.point_step; + + size_t output_pointcloud_size = 0; + for (size_t global_offset = 0; global_offset < pointcloud_msg.data.size(); + global_offset += point_step) { + if (filter_global_offset_set_.find(global_offset) == filter_global_offset_set_.end()) { + copyPointCloud(pointcloud_msg, point_step, global_offset, output_msg, output_pointcloud_size); + } + } + + output_msg.data.resize(output_pointcloud_size); + output_msg.row_step = output_pointcloud_size / output_msg.height; + output_msg.width = output_pointcloud_size / output_msg.point_step / output_msg.height; + + filter_global_offset_set_.clear(); + return; +} + void SegmentPointCloudFusionNode::publish(const PointCloudMsgType & output_msg) { if (pub_ptr_->get_subscription_count() < 1) { From 56f304112cffadc022c869784a85d2aef485492f Mon Sep 17 00:00:00 2001 From: Taekjin LEE Date: Tue, 7 Jan 2025 16:13:40 +0900 Subject: [PATCH 28/33] publish the main message is common Signed-off-by: Taekjin LEE --- .../pointpainting_fusion/node.hpp | 2 -- .../roi_cluster_fusion/node.hpp | 2 -- .../roi_detected_object_fusion/node.hpp | 2 -- .../segmentation_pointcloud_fusion/node.hpp | 2 -- .../src/fusion_node.cpp | 8 +++++--- .../src/pointpainting_fusion/node.cpp | 8 -------- .../src/roi_cluster_fusion/node.cpp | 8 -------- .../src/roi_detected_object_fusion/node.cpp | 8 -------- .../src/segmentation_pointcloud_fusion/node.cpp | 8 -------- 9 files changed, 5 insertions(+), 43 deletions(-) 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 097cd454b99bf..aedc32e353fea 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 @@ -56,8 +56,6 @@ class PointPaintingFusionNode : public FusionNode::SharedPtr point_pub_ptr_; int omp_num_threads_{1}; 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 f1084013a8ed3..2516fc2c2a6a0 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 @@ -38,8 +38,6 @@ class RoiClusterFusionNode : public FusionNode::postprocess( } template -void FusionNode::publish(const ExportObj & output_msg - __attribute__((unused))) +void FusionNode::publish(const ExportObj & output_msg) { - // do nothing by default + if (pub_ptr_->get_subscription_count() < 1) { + return; + } + pub_ptr_->publish(output_msg); } // Explicit instantiation for the supported types 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 a02d39ddac82a..cd213b78ed9a0 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 @@ -429,14 +429,6 @@ void PointPaintingFusionNode::postprocess( } } -void PointPaintingFusionNode::publish(const DetectedObjects & output_msg) -{ - if (pub_ptr_->get_subscription_count() < 1) { - return; - } - pub_ptr_->publish(output_msg); -} - } // namespace autoware::image_projection_based_fusion #include 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 078a08adf4c4f..0a9d9e3391882 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 @@ -292,14 +292,6 @@ void RoiClusterFusionNode::postprocess( } } -void RoiClusterFusionNode::publish(const ClusterMsgType & output_msg) -{ - if (pub_ptr_->get_subscription_count() < 1) { - return; - } - pub_ptr_->publish(output_msg); -} - } // namespace autoware::image_projection_based_fusion #include 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 5bbe3d158dabd..37769ad73e8c7 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 @@ -343,14 +343,6 @@ void RoiDetectedObjectFusionNode::postprocess( ignored_object_flags_map_.erase(timestamp_nsec); } -void RoiDetectedObjectFusionNode::publish(const DetectedObjects & output_msg) -{ - if (pub_ptr_->get_subscription_count() < 1) { - return; - } - pub_ptr_->publish(output_msg); -} - } // namespace autoware::image_projection_based_fusion #include 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 acac51dded077..7d63cac7273c6 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 @@ -172,14 +172,6 @@ void SegmentPointCloudFusionNode::postprocess( return; } -void SegmentPointCloudFusionNode::publish(const PointCloudMsgType & output_msg) -{ - if (pub_ptr_->get_subscription_count() < 1) { - return; - } - pub_ptr_->publish(output_msg); -} - } // namespace autoware::image_projection_based_fusion #include From f34b017efd429f81036e1578454a03bc71d5bb4b Mon Sep 17 00:00:00 2001 From: Taekjin LEE Date: Tue, 7 Jan 2025 16:17:28 +0900 Subject: [PATCH 29/33] fix: replace pointcloud message type by the typename Signed-off-by: Taekjin LEE --- .../src/pointpainting_fusion/node.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) 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 cd213b78ed9a0..ac9fbb518d130 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 @@ -156,9 +156,9 @@ PointPaintingFusionNode::PointPaintingFusionNode(const rclcpp::NodeOptions & opt const auto max_area_matrix = this->declare_parameter>("max_area_matrix"); // subscriber - std::function sub_callback = + std::function sub_callback = std::bind(&PointPaintingFusionNode::subCallback, this, std::placeholders::_1); - sub_ = this->create_subscription( + sub_ = this->create_subscription( "~/input/pointcloud", rclcpp::SensorDataQoS().keep_last(3), sub_callback); // publisher From 647f6d68d9c0ae8504106ff607957cdbb079fb5f Mon Sep 17 00:00:00 2001 From: Taekjin LEE Date: Tue, 7 Jan 2025 16:31:51 +0900 Subject: [PATCH 30/33] review member access Signed-off-by: Taekjin LEE --- .../pointpainting_fusion/node.hpp | 2 +- .../image_projection_based_fusion/roi_cluster_fusion/node.hpp | 3 +-- .../roi_detected_object_fusion/node.hpp | 3 +-- .../roi_pointcloud_fusion/node.hpp | 3 +-- .../segmentation_pointcloud_fusion/node.hpp | 3 +-- 5 files changed, 5 insertions(+), 9 deletions(-) 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 aedc32e353fea..e4cd2f1562967 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 @@ -46,7 +46,7 @@ class PointPaintingFusionNode : public FusionNode passthrough_lower_bound_probability_thresholds{}; 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 298cca900ddf8..77a1745faa7e5 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 @@ -30,7 +30,7 @@ class RoiPointCloudFusionNode : public FusionNode::SharedPtr point_pub_ptr_; rclcpp::Publisher::SharedPtr cluster_debug_pub_; @@ -42,7 +42,6 @@ class RoiPointCloudFusionNode : public FusionNode filter_semantic_label_target_; float filter_distance_threshold_; // declare list of semantic label target, depend on trained data of yolox segmentation model From e95a54b1afa3b6e8260fe225941ca8faba35c93e Mon Sep 17 00:00:00 2001 From: Taekjin LEE Date: Tue, 7 Jan 2025 17:59:47 +0900 Subject: [PATCH 31/33] Refactor fusion_node.hpp and fusion_node.cpp for improved code organization and readability Signed-off-by: Taekjin LEE --- .../fusion_node.hpp | 3 +- .../src/fusion_node.cpp | 123 ++++++++++-------- 2 files changed, 69 insertions(+), 57 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 30fd5e4197135..b8881bc6abff7 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 @@ -130,6 +130,8 @@ class FusionNode : public rclcpp::Node std::mutex mutex_cached_msgs_; protected: + void setDet2DStatus(std::size_t rois_number); + // callback for main subscription void subCallback(const typename Msg3D::ConstSharedPtr input_msg); // callback for roi subscription @@ -144,7 +146,6 @@ class FusionNode : public rclcpp::Node virtual void publish(const ExportObj & output_msg); // Members - std::size_t rois_number_{1}; tf2_ros::Buffer tf_buffer_; tf2_ros::TransformListener tf_listener_; 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 f0c10c194a04c..310c68f12db88 100644 --- a/perception/autoware_image_projection_based_fusion/src/fusion_node.cpp +++ b/perception/autoware_image_projection_based_fusion/src/fusion_node.cpp @@ -52,16 +52,16 @@ FusionNode::FusionNode( : Node(node_name, options), tf_buffer_(this->get_clock()), tf_listener_(tf_buffer_) { // set rois_number - rois_number_ = static_cast(declare_parameter("rois_number")); - if (rois_number_ < 1) { - RCLCPP_WARN( - this->get_logger(), "minimum rois_number is 1. current rois_number is %zu", rois_number_); - rois_number_ = 1; + const std::size_t 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); } - 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 @@ -70,10 +70,11 @@ 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_); - for (std::size_t roi_i = 0; roi_i < rois_number_; ++roi_i) { + 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) { 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)); @@ -84,8 +85,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::cameraInfoCallback, this, std::placeholders::_1, roi_i); camera_info_subs_.at(roi_i) = this->create_subscription( @@ -93,8 +94,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::roiCallback, this, std::placeholders::_1, roi_i); rois_subs_.at(roi_i) = this->create_subscription( @@ -112,49 +113,13 @@ FusionNode::FusionNode( timer_ = rclcpp::create_timer( this, get_clock(), period_ns, std::bind(&FusionNode::timer_callback, this)); - // 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."); - } + // initialization on each 2d detections + setDet2DStatus(rois_number); - // 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()) { - 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()) { - const std::size_t current_size = approx_camera_projection.size(); - RCLCPP_WARN( - this->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++) { - approx_camera_projection.at(i) = true; - } - } - } + // parameters for approximation grid approx_grid_cell_w_size_ = declare_parameter("approximation_grid_cell_width"); approx_grid_cell_h_size_ = declare_parameter("approximation_grid_cell_height"); - // 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); - det2d_list_.at(roi_i).input_offset_ms = input_offset_ms.at(roi_i); - } - // parameters for out_of_scope filter filter_scope_min_x_ = declare_parameter("filter_scope_min_x"); filter_scope_max_x_ = declare_parameter("filter_scope_max_x"); @@ -166,8 +131,8 @@ FusionNode::FusionNode( // debugger if (declare_parameter("debug_mode", false)) { 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"); @@ -175,7 +140,7 @@ FusionNode::FusionNode( std::size_t 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); } // time keeper @@ -199,6 +164,52 @@ FusionNode::FusionNode( } } +template +void FusionNode::setDet2DStatus(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()) { + 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()) { + 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++) { + 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_.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); + det2d_list_.at(roi_i).input_offset_ms = input_offset_ms.at(roi_i); + } +} + template void FusionNode::cameraInfoCallback( const sensor_msgs::msg::CameraInfo::ConstSharedPtr input_camera_info_msg, From cc51e6b4cb462cb5b689bef6724b35bf56ec7034 Mon Sep 17 00:00:00 2001 From: Taekjin LEE Date: Tue, 7 Jan 2025 18:54:17 +0900 Subject: [PATCH 32/33] refactor: fix condition for publishing painted pointcloud message Signed-off-by: Taekjin LEE --- .../src/pointpainting_fusion/node.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) 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 ac9fbb518d130..aa276d16e3a54 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 @@ -424,7 +424,7 @@ void PointPaintingFusionNode::postprocess( detection_class_remapper_.mapClasses(output_msg); // publish debug message: painted pointcloud - if (point_pub_ptr_->get_subscription_count() < 1) { + if (point_pub_ptr_->get_subscription_count() > 0) { point_pub_ptr_->publish(painted_pointcloud_msg); } } From dd60d8e5b26ecb5897ead8518ec1ae2acfbea7d2 Mon Sep 17 00:00:00 2001 From: Taekjin LEE Date: Wed, 8 Jan 2025 15:59:24 +0900 Subject: [PATCH 33/33] fix: remove unused variable Signed-off-by: Taekjin LEE --- .../image_projection_based_fusion/pointpainting_fusion/node.hpp | 1 - 1 file changed, 1 deletion(-) 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 e4cd2f1562967..d67d98be61834 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 @@ -59,7 +59,6 @@ class PointPaintingFusionNode : public FusionNode::SharedPtr point_pub_ptr_; int omp_num_threads_{1}; - float score_threshold_{0.0}; std::vector class_names_; std::map class_index_; std::map> isClassTable_;