Skip to content

Commit 14ee08b

Browse files
technolojintechnolojin
authoredJan 8, 2025
refactor(autoware_image_projection_based_fusion): organize 2d-detection related members (#9789)
* chore: input_camera_topics_ is only for debug Signed-off-by: Taekjin LEE <taekjin.lee@tier4.jp> * feat: fuse main message with cached roi messages in fusion_node.cpp Signed-off-by: Taekjin LEE <taekjin.lee@tier4.jp> * chore: add comments on each process step, organize methods Signed-off-by: Taekjin LEE <taekjin.lee@tier4.jp> * 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 <taekjin.lee@tier4.jp> * 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 <taekjin.lee@tier4.jp> * Refactor fusion_node.cpp and fusion_node.hpp for improved code organization and readability Signed-off-by: Taekjin LEE <taekjin.lee@tier4.jp> * Refactor fusion_node.hpp and fusion_node.cpp for improved code organization and readability Signed-off-by: Taekjin LEE <taekjin.lee@tier4.jp> * feat: Refactor fusion_node.cpp for improved code organization and readability Signed-off-by: Taekjin LEE <taekjin.lee@tier4.jp> * Refactor fusion_node.cpp for improved code organization and readability Signed-off-by: Taekjin LEE <taekjin.lee@tier4.jp> * feat: implement mutex per 2d detection process Signed-off-by: Taekjin LEE <taekjin.lee@tier4.jp> * Refactor fusion_node.hpp and fusion_node.cpp for improved code organization and readability Signed-off-by: Taekjin LEE <taekjin.lee@tier4.jp> * Refactor fusion_node.hpp and fusion_node.cpp for improved code organization and readability Signed-off-by: Taekjin LEE <taekjin.lee@tier4.jp> * revise template, inputs first and output at the last Signed-off-by: Taekjin LEE <taekjin.lee@tier4.jp> * explicit in and out types 1 Signed-off-by: Taekjin LEE <taekjin.lee@tier4.jp> * clarify pointcloud message type Signed-off-by: Taekjin LEE <taekjin.lee@tier4.jp> * Refactor fusion_node.hpp and fusion_node.cpp for improved code organization and readability Signed-off-by: Taekjin LEE <taekjin.lee@tier4.jp> * Refactor fusion_node.hpp and fusion_node.cpp for improved code organization and readability Signed-off-by: Taekjin LEE <taekjin.lee@tier4.jp> * Refactor fusion_node.hpp and fusion_node.cpp for improved code organization and readability Signed-off-by: Taekjin LEE <taekjin.lee@tier4.jp> * Refactor publisher types in fusion_node.hpp and node.hpp Signed-off-by: Taekjin LEE <taekjin.lee@tier4.jp> * fix: resolve cppcheck issue shadowVariable Signed-off-by: Taekjin LEE <taekjin.lee@tier4.jp> * Refactor fusion_node.hpp and fusion_node.cpp for improved code organization and readability Signed-off-by: Taekjin LEE <taekjin.lee@tier4.jp> * chore: rename Det2dManager to Det2dStatus Signed-off-by: Taekjin LEE <taekjin.lee@tier4.jp> * revert mutex related changes Signed-off-by: Taekjin LEE <taekjin.lee@tier4.jp> * refactor: review member and method's access Signed-off-by: Taekjin LEE <taekjin.lee@tier4.jp> * fix: resolve shadowVariable of 'det2d' Signed-off-by: Taekjin LEE <taekjin.lee@tier4.jp> * fix missing line Signed-off-by: Taekjin LEE <taekjin.lee@tier4.jp> * refactor message postprocess and publish methods Signed-off-by: Taekjin LEE <taekjin.lee@tier4.jp> * publish the main message is common Signed-off-by: Taekjin LEE <taekjin.lee@tier4.jp> * fix: replace pointcloud message type by the typename Signed-off-by: Taekjin LEE <taekjin.lee@tier4.jp> * review member access Signed-off-by: Taekjin LEE <taekjin.lee@tier4.jp> * Refactor fusion_node.hpp and fusion_node.cpp for improved code organization and readability Signed-off-by: Taekjin LEE <taekjin.lee@tier4.jp> * refactor: fix condition for publishing painted pointcloud message Signed-off-by: Taekjin LEE <taekjin.lee@tier4.jp> * fix: remove unused variable Signed-off-by: Taekjin LEE <taekjin.lee@tier4.jp> --------- Signed-off-by: Taekjin LEE <taekjin.lee@tier4.jp>
1 parent 9ce874c commit 14ee08b

File tree

14 files changed

+603
-579
lines changed

14 files changed

+603
-579
lines changed
 

‎perception/autoware_image_projection_based_fusion/include/autoware/image_projection_based_fusion/fusion_node.hpp

+73-51
Original file line numberDiff line numberDiff line change
@@ -52,14 +52,33 @@ using autoware_perception_msgs::msg::DetectedObject;
5252
using autoware_perception_msgs::msg::DetectedObjects;
5353
using sensor_msgs::msg::CameraInfo;
5454
using sensor_msgs::msg::Image;
55-
using sensor_msgs::msg::PointCloud2;
56-
using tier4_perception_msgs::msg::DetectedObjectsWithFeature;
55+
using PointCloudMsgType = sensor_msgs::msg::PointCloud2;
56+
using RoiMsgType = tier4_perception_msgs::msg::DetectedObjectsWithFeature;
57+
using ClusterMsgType = tier4_perception_msgs::msg::DetectedObjectsWithFeature;
58+
using ClusterObjType = tier4_perception_msgs::msg::DetectedObjectWithFeature;
5759
using tier4_perception_msgs::msg::DetectedObjectWithFeature;
5860
using PointCloud = pcl::PointCloud<pcl::PointXYZ>;
5961
using autoware::image_projection_based_fusion::CameraProjection;
6062
using autoware_perception_msgs::msg::ObjectClassification;
6163

62-
template <class TargetMsg3D, class ObjType, class Msg2D>
64+
template <class Msg2D>
65+
struct Det2dStatus
66+
{
67+
// camera index
68+
std::size_t id = 0;
69+
// camera projection
70+
std::unique_ptr<CameraProjection> camera_projector_ptr;
71+
bool project_to_unrectified_image = false;
72+
bool approximate_camera_projection = false;
73+
// process flags
74+
bool is_fused = false;
75+
// timing
76+
double input_offset_ms = 0.0;
77+
// cache
78+
std::map<int64_t, typename Msg2D::ConstSharedPtr> cached_det2d_msgs;
79+
};
80+
81+
template <class Msg3D, class Msg2D, class ExportObj>
6382
class FusionNode : public rclcpp::Node
6483
{
6584
public:
@@ -71,82 +90,85 @@ class FusionNode : public rclcpp::Node
7190
explicit FusionNode(
7291
const std::string & node_name, const rclcpp::NodeOptions & options, int queue_size);
7392

74-
protected:
93+
private:
94+
// Common process methods
7595
void cameraInfoCallback(
7696
const sensor_msgs::msg::CameraInfo::ConstSharedPtr input_camera_info_msg,
7797
const std::size_t camera_id);
7898

79-
virtual void preprocess(TargetMsg3D & output_msg);
80-
81-
// callback for Msg subscription
82-
virtual void subCallback(const typename TargetMsg3D::ConstSharedPtr input_msg);
83-
84-
// callback for roi subscription
85-
86-
virtual void roiCallback(
87-
const typename Msg2D::ConstSharedPtr input_roi_msg, const std::size_t roi_i);
88-
89-
virtual void fuseOnSingleImage(
90-
const TargetMsg3D & input_msg, const std::size_t image_id, const Msg2D & input_roi_msg,
91-
TargetMsg3D & output_msg) = 0;
92-
93-
// set args if you need
94-
virtual void postprocess(TargetMsg3D & output_msg);
95-
96-
virtual void publish(const TargetMsg3D & output_msg);
97-
99+
// callback for timer
98100
void timer_callback();
99101
void setPeriod(const int64_t new_period);
102+
void exportProcess();
103+
104+
// 2d detection management methods
105+
bool checkAllDet2dFused()
106+
{
107+
for (const auto & det2d : det2d_list_) {
108+
if (!det2d.is_fused) {
109+
return false;
110+
}
111+
}
112+
return true;
113+
}
100114

101-
std::size_t rois_number_{1};
102-
tf2_ros::Buffer tf_buffer_;
103-
tf2_ros::TransformListener tf_listener_;
104-
105-
std::vector<bool> point_project_to_unrectified_image_;
106-
107-
// camera_info
108-
std::map<std::size_t, sensor_msgs::msg::CameraInfo> camera_info_map_;
109-
std::vector<rclcpp::Subscription<sensor_msgs::msg::CameraInfo>::SharedPtr> camera_info_subs_;
110115
// camera projection
111-
std::vector<CameraProjection> camera_projectors_;
112-
std::vector<bool> approx_camera_projection_;
113116
float approx_grid_cell_w_size_;
114117
float approx_grid_cell_h_size_;
115118

119+
// timer
116120
rclcpp::TimerBase::SharedPtr timer_;
117121
double timeout_ms_{};
118122
double match_threshold_ms_{};
119-
std::vector<std::string> input_rois_topics_;
120-
std::vector<std::string> input_camera_info_topics_;
121-
std::vector<std::string> input_camera_topics_;
122123

123-
/** \brief A vector of subscriber. */
124-
typename rclcpp::Subscription<TargetMsg3D>::SharedPtr sub_;
125124
std::vector<typename rclcpp::Subscription<Msg2D>::SharedPtr> rois_subs_;
126-
127-
// offsets between cameras and the lidars
128-
std::vector<double> input_offset_ms_;
125+
std::vector<rclcpp::Subscription<sensor_msgs::msg::CameraInfo>::SharedPtr> camera_info_subs_;
129126

130127
// cache for fusion
131-
std::vector<bool> is_fused_;
132-
std::pair<int64_t, typename TargetMsg3D::SharedPtr>
133-
cached_msg_; // first element is the timestamp in nanoseconds, second element is the message
134-
std::vector<std::map<int64_t, typename Msg2D::ConstSharedPtr>> cached_roi_msgs_;
128+
int64_t cached_det3d_msg_timestamp_;
129+
typename Msg3D::SharedPtr cached_det3d_msg_ptr_;
135130
std::mutex mutex_cached_msgs_;
136131

137-
// output publisher
138-
typename rclcpp::Publisher<TargetMsg3D>::SharedPtr pub_ptr_;
132+
protected:
133+
void setDet2DStatus(std::size_t rois_number);
139134

140-
// debugger
141-
std::shared_ptr<Debugger> debugger_;
142-
virtual bool out_of_scope(const ObjType & obj) = 0;
135+
// callback for main subscription
136+
void subCallback(const typename Msg3D::ConstSharedPtr input_msg);
137+
// callback for roi subscription
138+
void roiCallback(const typename Msg2D::ConstSharedPtr input_roi_msg, const std::size_t roi_i);
139+
140+
// Custom process methods
141+
virtual void preprocess(Msg3D & output_msg);
142+
virtual void fuseOnSingleImage(
143+
const Msg3D & input_msg, const Det2dStatus<Msg2D> & det2d, const Msg2D & input_roi_msg,
144+
Msg3D & output_msg) = 0;
145+
virtual void postprocess(const Msg3D & processing_msg, ExportObj & output_msg);
146+
virtual void publish(const ExportObj & output_msg);
147+
148+
// Members
149+
tf2_ros::Buffer tf_buffer_;
150+
tf2_ros::TransformListener tf_listener_;
151+
152+
// 2d detection management
153+
std::vector<Det2dStatus<Msg2D>> det2d_list_;
154+
155+
/** \brief A vector of subscriber. */
156+
typename rclcpp::Subscription<Msg3D>::SharedPtr sub_;
157+
158+
// parameters for out_of_scope filter
143159
float filter_scope_min_x_;
144160
float filter_scope_max_x_;
145161
float filter_scope_min_y_;
146162
float filter_scope_max_y_;
147163
float filter_scope_min_z_;
148164
float filter_scope_max_z_;
149165

166+
// output publisher
167+
typename rclcpp::Publisher<ExportObj>::SharedPtr pub_ptr_;
168+
169+
// debugger
170+
std::shared_ptr<Debugger> debugger_;
171+
150172
/** \brief processing time publisher. **/
151173
std::unique_ptr<autoware::universe_utils::StopWatch<std::chrono::milliseconds>> stop_watch_ptr_;
152174
std::unique_ptr<autoware::universe_utils::DebugPublisher> debug_publisher_;

‎perception/autoware_image_projection_based_fusion/include/autoware/image_projection_based_fusion/pointpainting_fusion/node.hpp

+8-12
Original file line numberDiff line numberDiff line change
@@ -42,27 +42,25 @@ inline bool isInsideBbox(
4242
proj_y >= roi.y_offset * zc && proj_y <= (roi.y_offset + roi.height) * zc;
4343
}
4444

45-
class PointPaintingFusionNode
46-
: public FusionNode<sensor_msgs::msg::PointCloud2, DetectedObjects, DetectedObjectsWithFeature>
45+
class PointPaintingFusionNode : public FusionNode<PointCloudMsgType, RoiMsgType, DetectedObjects>
4746
{
4847
public:
4948
explicit PointPaintingFusionNode(const rclcpp::NodeOptions & options);
5049

51-
protected:
52-
void preprocess(sensor_msgs::msg::PointCloud2 & pointcloud_msg) override;
50+
private:
51+
void preprocess(PointCloudMsgType & pointcloud_msg) override;
5352

5453
void fuseOnSingleImage(
55-
const sensor_msgs::msg::PointCloud2 & input_pointcloud_msg, const std::size_t image_id,
56-
const DetectedObjectsWithFeature & input_roi_msg,
57-
sensor_msgs::msg::PointCloud2 & painted_pointcloud_msg) override;
54+
const PointCloudMsgType & input_pointcloud_msg, const Det2dStatus<RoiMsgType> & det2d,
55+
const RoiMsgType & input_roi_msg, PointCloudMsgType & painted_pointcloud_msg) override;
5856

59-
void postprocess(sensor_msgs::msg::PointCloud2 & painted_pointcloud_msg) override;
57+
void postprocess(
58+
const PointCloudMsgType & painted_pointcloud_msg, DetectedObjects & output_msg) override;
6059

61-
rclcpp::Publisher<DetectedObjects>::SharedPtr obj_pub_ptr_;
60+
rclcpp::Publisher<PointCloudMsgType>::SharedPtr point_pub_ptr_;
6261
std::unique_ptr<autoware::universe_utils::DiagnosticsInterface> diagnostics_interface_ptr_;
6362

6463
int omp_num_threads_{1};
65-
float score_threshold_{0.0};
6664
std::vector<std::string> class_names_;
6765
std::map<std::string, float> class_index_;
6866
std::map<std::string, std::function<bool(int)>> isClassTable_;
@@ -74,8 +72,6 @@ class PointPaintingFusionNode
7472
autoware::lidar_centerpoint::DetectionClassRemapper detection_class_remapper_;
7573

7674
std::unique_ptr<image_projection_based_fusion::PointPaintingTRT> detector_ptr_{nullptr};
77-
78-
bool out_of_scope(const DetectedObjects & obj) override;
7975
};
8076
} // namespace autoware::image_projection_based_fusion
8177
#endif // AUTOWARE__IMAGE_PROJECTION_BASED_FUSION__POINTPAINTING_FUSION__NODE_HPP_

‎perception/autoware_image_projection_based_fusion/include/autoware/image_projection_based_fusion/roi_cluster_fusion/node.hpp

+9-12
Original file line numberDiff line numberDiff line change
@@ -24,21 +24,19 @@ namespace autoware::image_projection_based_fusion
2424
{
2525
const std::map<std::string, uint8_t> IOU_MODE_MAP{{"iou", 0}, {"iou_x", 1}, {"iou_y", 2}};
2626

27-
class RoiClusterFusionNode
28-
: public FusionNode<
29-
DetectedObjectsWithFeature, DetectedObjectWithFeature, DetectedObjectsWithFeature>
27+
class RoiClusterFusionNode : public FusionNode<ClusterMsgType, RoiMsgType, ClusterMsgType>
3028
{
3129
public:
3230
explicit RoiClusterFusionNode(const rclcpp::NodeOptions & options);
3331

34-
protected:
35-
void preprocess(DetectedObjectsWithFeature & output_cluster_msg) override;
36-
void postprocess(DetectedObjectsWithFeature & output_cluster_msg) override;
32+
private:
33+
void preprocess(ClusterMsgType & output_cluster_msg) override;
3734

3835
void fuseOnSingleImage(
39-
const DetectedObjectsWithFeature & input_cluster_msg, const std::size_t image_id,
40-
const DetectedObjectsWithFeature & input_roi_msg,
41-
DetectedObjectsWithFeature & output_cluster_msg) override;
36+
const ClusterMsgType & input_cluster_msg, const Det2dStatus<RoiMsgType> & det2d,
37+
const RoiMsgType & input_roi_msg, ClusterMsgType & output_cluster_msg) override;
38+
39+
void postprocess(const ClusterMsgType & output_cluster_msg, ClusterMsgType & output_msg) override;
4240

4341
std::string trust_object_iou_mode_{"iou"};
4442
bool use_cluster_semantic_type_{false};
@@ -53,12 +51,11 @@ class RoiClusterFusionNode
5351
double trust_object_distance_;
5452
std::string non_trust_object_iou_mode_{"iou_x"};
5553

56-
bool is_far_enough(const DetectedObjectWithFeature & obj, const double distance_threshold);
57-
bool out_of_scope(const DetectedObjectWithFeature & obj) override;
54+
bool is_far_enough(const ClusterObjType & obj, const double distance_threshold);
55+
bool out_of_scope(const ClusterObjType & obj);
5856
double cal_iou_by_mode(
5957
const sensor_msgs::msg::RegionOfInterest & roi_1,
6058
const sensor_msgs::msg::RegionOfInterest & roi_2, const std::string iou_mode);
61-
// bool CheckUnknown(const DetectedObjectsWithFeature & obj);
6259
};
6360

6461
} // namespace autoware::image_projection_based_fusion

‎perception/autoware_image_projection_based_fusion/include/autoware/image_projection_based_fusion/roi_detected_object_fusion/node.hpp

+7-9
Original file line numberDiff line numberDiff line change
@@ -31,33 +31,31 @@ namespace autoware::image_projection_based_fusion
3131

3232
using sensor_msgs::msg::RegionOfInterest;
3333

34-
class RoiDetectedObjectFusionNode
35-
: public FusionNode<DetectedObjects, DetectedObject, DetectedObjectsWithFeature>
34+
class RoiDetectedObjectFusionNode : public FusionNode<DetectedObjects, RoiMsgType, DetectedObjects>
3635
{
3736
public:
3837
explicit RoiDetectedObjectFusionNode(const rclcpp::NodeOptions & options);
3938

40-
protected:
39+
private:
4140
void preprocess(DetectedObjects & output_msg) override;
4241

4342
void fuseOnSingleImage(
44-
const DetectedObjects & input_object_msg, const std::size_t image_id,
45-
const DetectedObjectsWithFeature & input_roi_msg, DetectedObjects & output_object_msg) override;
43+
const DetectedObjects & input_object_msg, const Det2dStatus<RoiMsgType> & det2d,
44+
const RoiMsgType & input_roi_msg, DetectedObjects & output_object_msg) override;
4645

4746
std::map<std::size_t, DetectedObjectWithFeature> generateDetectedObjectRoIs(
48-
const DetectedObjects & input_object_msg, const std::size_t & image_id,
47+
const DetectedObjects & input_object_msg, const Det2dStatus<RoiMsgType> & det2d,
4948
const Eigen::Affine3d & object2camera_affine);
5049

5150
void fuseObjectsOnImage(
5251
const DetectedObjects & input_object_msg,
5352
const std::vector<DetectedObjectWithFeature> & image_rois,
5453
const std::map<std::size_t, DetectedObjectWithFeature> & object_roi_map);
5554

56-
void publish(const DetectedObjects & output_msg) override;
55+
void postprocess(const DetectedObjects & processing_msg, DetectedObjects & output_msg) override;
5756

58-
bool out_of_scope(const DetectedObject & obj) override;
57+
bool out_of_scope(const DetectedObject & obj);
5958

60-
private:
6159
struct
6260
{
6361
std::vector<double> passthrough_lower_bound_probability_thresholds{};

‎perception/autoware_image_projection_based_fusion/include/autoware/image_projection_based_fusion/roi_pointcloud_fusion/node.hpp

+16-19
Original file line numberDiff line numberDiff line change
@@ -25,32 +25,29 @@
2525

2626
namespace autoware::image_projection_based_fusion
2727
{
28-
class RoiPointCloudFusionNode
29-
: public FusionNode<PointCloud2, DetectedObjectWithFeature, DetectedObjectsWithFeature>
28+
class RoiPointCloudFusionNode : public FusionNode<PointCloudMsgType, RoiMsgType, ClusterMsgType>
3029
{
30+
public:
31+
explicit RoiPointCloudFusionNode(const rclcpp::NodeOptions & options);
32+
3133
private:
32-
int min_cluster_size_{1};
33-
int max_cluster_size_{20};
34-
bool fuse_unknown_only_{true};
35-
double cluster_2d_tolerance_;
34+
rclcpp::Publisher<PointCloudMsgType>::SharedPtr point_pub_ptr_;
35+
rclcpp::Publisher<PointCloudMsgType>::SharedPtr cluster_debug_pub_;
3636

37-
rclcpp::Publisher<DetectedObjectsWithFeature>::SharedPtr pub_objects_ptr_;
38-
std::vector<DetectedObjectWithFeature> output_fused_objects_;
39-
rclcpp::Publisher<sensor_msgs::msg::PointCloud2>::SharedPtr cluster_debug_pub_;
37+
void fuseOnSingleImage(
38+
const PointCloudMsgType & input_pointcloud_msg, const Det2dStatus<RoiMsgType> & det2d,
39+
const RoiMsgType & input_roi_msg, PointCloudMsgType & output_pointcloud_msg) override;
4040

41-
/* data */
42-
public:
43-
explicit RoiPointCloudFusionNode(const rclcpp::NodeOptions & options);
41+
void postprocess(const PointCloudMsgType & pointcloud_msg, ClusterMsgType & output_msg) override;
4442

45-
protected:
46-
void preprocess(sensor_msgs::msg::PointCloud2 & pointcloud_msg) override;
43+
void publish(const ClusterMsgType & output_msg) override;
4744

48-
void postprocess(sensor_msgs::msg::PointCloud2 & pointcloud_msg) override;
45+
int min_cluster_size_{1};
46+
int max_cluster_size_{20};
47+
bool fuse_unknown_only_{true};
48+
double cluster_2d_tolerance_;
4949

50-
void fuseOnSingleImage(
51-
const PointCloud2 & input_pointcloud_msg, const std::size_t image_id,
52-
const DetectedObjectsWithFeature & input_roi_msg, PointCloud2 & output_pointcloud_msg) override;
53-
bool out_of_scope(const DetectedObjectWithFeature & obj) override;
50+
std::vector<ClusterObjType> output_fused_objects_;
5451
};
5552

5653
} // namespace autoware::image_projection_based_fusion

0 commit comments

Comments
 (0)
Failed to load comments.