Skip to content

Commit ad5c361

Browse files
committed
Refactor fusion_node.hpp and fusion_node.cpp for improved code organization and readability
Signed-off-by: Taekjin LEE <taekjin.lee@tier4.jp>
1 parent c56479d commit ad5c361

File tree

5 files changed

+30
-32
lines changed
  • perception/autoware_image_projection_based_fusion

5 files changed

+30
-32
lines changed

Diff for: perception/autoware_image_projection_based_fusion/include/autoware/image_projection_based_fusion/roi_cluster_fusion/node.hpp

+1
Original file line numberDiff line numberDiff line change
@@ -40,6 +40,7 @@ class RoiClusterFusionNode : public FusionNode<ClusterMsgType, RoiMsgType, Clust
4040

4141
rclcpp::Publisher<ClusterMsgType>::SharedPtr pub_ptr_;
4242

43+
private:
4344
std::string trust_object_iou_mode_{"iou"};
4445
bool use_cluster_semantic_type_{false};
4546
bool only_allow_inside_cluster_{false};

Diff for: perception/autoware_image_projection_based_fusion/include/autoware/image_projection_based_fusion/roi_pointcloud_fusion/node.hpp

+10-11
Original file line numberDiff line numberDiff line change
@@ -27,22 +27,13 @@ namespace autoware::image_projection_based_fusion
2727
{
2828
class RoiPointCloudFusionNode : public FusionNode<PointCloudMsgType, RoiMsgType, ClusterMsgType>
2929
{
30-
private:
31-
int min_cluster_size_{1};
32-
int max_cluster_size_{20};
33-
bool fuse_unknown_only_{true};
34-
double cluster_2d_tolerance_;
35-
36-
rclcpp::Publisher<ClusterMsgType>::SharedPtr pub_objects_ptr_;
37-
std::vector<ClusterObjType> output_fused_objects_;
38-
rclcpp::Publisher<PointCloudMsgType>::SharedPtr cluster_debug_pub_;
39-
40-
/* data */
4130
public:
4231
explicit RoiPointCloudFusionNode(const rclcpp::NodeOptions & options);
4332

4433
protected:
34+
rclcpp::Publisher<ClusterMsgType>::SharedPtr pub_objects_ptr_;
4535
rclcpp::Publisher<PointCloudMsgType>::SharedPtr pub_ptr_;
36+
rclcpp::Publisher<PointCloudMsgType>::SharedPtr cluster_debug_pub_;
4637

4738
void postprocess(PointCloudMsgType & pointcloud_msg) override;
4839

@@ -51,6 +42,14 @@ class RoiPointCloudFusionNode : public FusionNode<PointCloudMsgType, RoiMsgType,
5142
const RoiMsgType & input_roi_msg, PointCloudMsgType & output_pointcloud_msg) override;
5243

5344
void publish(const PointCloudMsgType & output_msg) override;
45+
46+
private:
47+
int min_cluster_size_{1};
48+
int max_cluster_size_{20};
49+
bool fuse_unknown_only_{true};
50+
double cluster_2d_tolerance_;
51+
52+
std::vector<ClusterObjType> output_fused_objects_;
5453
};
5554

5655
} // namespace autoware::image_projection_based_fusion

Diff for: perception/autoware_image_projection_based_fusion/include/autoware/image_projection_based_fusion/segmentation_pointcloud_fusion/node.hpp

+17-15
Original file line numberDiff line numberDiff line change
@@ -36,21 +36,6 @@ namespace autoware::image_projection_based_fusion
3636
{
3737
class SegmentPointCloudFusionNode : public FusionNode<PointCloudMsgType, Image, PointCloudMsgType>
3838
{
39-
private:
40-
rclcpp::Publisher<PointCloudMsgType>::SharedPtr pub_pointcloud_ptr_;
41-
std::vector<bool> filter_semantic_label_target_;
42-
float filter_distance_threshold_;
43-
// declare list of semantic label target, depend on trained data of yolox segmentation model
44-
std::vector<std::pair<std::string, bool>> filter_semantic_label_target_list_ = {
45-
{"UNKNOWN", false}, {"BUILDING", false}, {"WALL", false}, {"OBSTACLE", false},
46-
{"TRAFFIC_LIGHT", false}, {"TRAFFIC_SIGN", false}, {"PERSON", false}, {"VEHICLE", false},
47-
{"BIKE", false}, {"ROAD", false}, {"SIDEWALK", false}, {"ROAD_PAINT", false},
48-
{"CURBSTONE", false}, {"CROSSWALK", false}, {"VEGETATION", false}, {"SKY", false}};
49-
50-
image_transport::Publisher pub_debug_mask_ptr_;
51-
bool is_publish_debug_mask_;
52-
std::unordered_set<size_t> filter_global_offset_set_;
53-
5439
public:
5540
explicit SegmentPointCloudFusionNode(const rclcpp::NodeOptions & options);
5641

@@ -73,7 +58,24 @@ class SegmentPointCloudFusionNode : public FusionNode<PointCloudMsgType, Image,
7358

7459
void publish(const PointCloudMsgType & output_msg) override;
7560

61+
// publisher
7662
rclcpp::Publisher<PointCloudMsgType>::SharedPtr pub_ptr_;
63+
64+
// debug
65+
image_transport::Publisher pub_debug_mask_ptr_;
66+
67+
private:
68+
std::vector<bool> filter_semantic_label_target_;
69+
float filter_distance_threshold_;
70+
// declare list of semantic label target, depend on trained data of yolox segmentation model
71+
std::vector<std::pair<std::string, bool>> filter_semantic_label_target_list_ = {
72+
{"UNKNOWN", false}, {"BUILDING", false}, {"WALL", false}, {"OBSTACLE", false},
73+
{"TRAFFIC_LIGHT", false}, {"TRAFFIC_SIGN", false}, {"PERSON", false}, {"VEHICLE", false},
74+
{"BIKE", false}, {"ROAD", false}, {"SIDEWALK", false}, {"ROAD_PAINT", false},
75+
{"CURBSTONE", false}, {"CROSSWALK", false}, {"VEGETATION", false}, {"SKY", false}};
76+
77+
bool is_publish_debug_mask_;
78+
std::unordered_set<size_t> filter_global_offset_set_;
7779
};
7880

7981
} // namespace autoware::image_projection_based_fusion

Diff for: perception/autoware_image_projection_based_fusion/src/pointpainting_fusion/node.cpp

-4
Original file line numberDiff line numberDiff line change
@@ -288,10 +288,6 @@ void PointPaintingFusionNode::fuseOnSingleImage(
288288
lidar2cam_affine = _transformToEigen(transform_stamped_optional.value().transform);
289289
}
290290

291-
// transform
292-
// sensor_msgs::msg::PointCloud2 transformed_pointcloud;
293-
// tf2::doTransform(painted_pointcloud_msg, transformed_pointcloud, transform_stamped);
294-
295291
const auto x_offset = painted_pointcloud_msg.fields
296292
.at(static_cast<size_t>(autoware::point_types::PointXYZIRCIndex::X))
297293
.offset;

Diff for: perception/autoware_image_projection_based_fusion/src/roi_pointcloud_fusion/node.cpp

+2-2
Original file line numberDiff line numberDiff line change
@@ -43,11 +43,11 @@ RoiPointCloudFusionNode::RoiPointCloudFusionNode(const rclcpp::NodeOptions & opt
4343
min_cluster_size_ = declare_parameter<int>("min_cluster_size");
4444
max_cluster_size_ = declare_parameter<int>("max_cluster_size");
4545
cluster_2d_tolerance_ = declare_parameter<double>("cluster_2d_tolerance");
46-
pub_objects_ptr_ = this->create_publisher<ClusterMsgType>("output_clusters", rclcpp::QoS{1});
47-
cluster_debug_pub_ = this->create_publisher<PointCloudMsgType>("debug/clusters", 1);
4846

4947
// publisher
5048
pub_ptr_ = this->create_publisher<PointCloudMsgType>("output", rclcpp::QoS{1});
49+
pub_objects_ptr_ = this->create_publisher<ClusterMsgType>("output_clusters", rclcpp::QoS{1});
50+
cluster_debug_pub_ = this->create_publisher<PointCloudMsgType>("debug/clusters", 1);
5151
}
5252

5353
void RoiPointCloudFusionNode::postprocess(PointCloudMsgType & pointcloud_msg)

0 commit comments

Comments
 (0)