From e079772c59c0f60ac8569695c4f9ca1f242fdc5d Mon Sep 17 00:00:00 2001 From: Taekjin LEE Date: Thu, 26 Dec 2024 17:47:27 +0900 Subject: [PATCH] Refactor publisher types in fusion_node.hpp and node.hpp --- .../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 4884eacdba704..9a89973240d94 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); @@ -383,7 +383,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; } @@ -415,16 +415,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