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 9a89973240d94..8dfba16ac8d05 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) {