clarify pointcloud message type

Signed-off-by: Taekjin LEE <taekjin.lee@tier4.jp>

Author: technolojin

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;

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<sensor_msgs::msg::PointCloud2, RoiMsgType, DetectedObjects>
+class PointPaintingFusionNode : public FusionNode<PointCloudMsgType, RoiMsgType, DetectedObjects>
 {
 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<RoiMsgType> & det2d, const RoiMsgType & input_roi_msg,
-    sensor_msgs::msg::PointCloud2 & painted_pointcloud_msg) override;
+    const PointCloudMsgType & input_pointcloud_msg, const Det2dManager<RoiMsgType> & 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<DetectedObjects>::SharedPtr obj_pub_ptr_;
 

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<PointCloud2, RoiMsgType, ClusterObjType>
+class RoiPointCloudFusionNode : public FusionNode<PointCloudMsgType, RoiMsgType, ClusterObjType>
 {
 private:
   int min_cluster_size_{1};
@@ -35,20 +35,20 @@ class RoiPointCloudFusionNode : public FusionNode<PointCloud2, RoiMsgType, Clust
 
   rclcpp::Publisher<ClusterMsgType>::SharedPtr pub_objects_ptr_;
   std::vector<ClusterObjType> output_fused_objects_;
-  rclcpp::Publisher<sensor_msgs::msg::PointCloud2>::SharedPtr cluster_debug_pub_;
+  rclcpp::Publisher<PointCloudMsgType>::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<RoiMsgType> & det2d,
-    const RoiMsgType & input_roi_msg, PointCloud2 & output_pointcloud_msg) override;
+    const PointCloudMsgType & input_pointcloud_msg, const Det2dManager<RoiMsgType> & det2d,
+    const RoiMsgType & input_roi_msg, PointCloudMsgType & output_pointcloud_msg) override;
   bool out_of_scope(const ClusterObjType & obj) override;
 };
 

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<PointCloud2, Image, PointCloud2>
+class SegmentPointCloudFusionNode : public FusionNode<PointCloudMsgType, Image, PointCloudMsgType>
 {
 private:
-  rclcpp::Publisher<PointCloud2>::SharedPtr pub_pointcloud_ptr_;
+  rclcpp::Publisher<PointCloudMsgType>::SharedPtr pub_pointcloud_ptr_;
   std::vector<bool> 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<PointCloud2, Image, PointC
   explicit SegmentPointCloudFusionNode(const rclcpp::NodeOptions & options);
 
 protected:
-  void preprocess(PointCloud2 & pointcloud_msg) override;
+  void preprocess(PointCloudMsgType & pointcloud_msg) override;
 
-  void postprocess(PointCloud2 & pointcloud_msg) override;
+  void postprocess(PointCloudMsgType & pointcloud_msg) override;
 
   void fuseOnSingleImage(
-    const PointCloud2 & input_pointcloud_msg, const Det2dManager<Image> & det2d,
-    const Image & input_mask, PointCloud2 & output_pointcloud_msg) override;
+    const PointCloudMsgType & input_pointcloud_msg, const Det2dManager<Image> & 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;

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<pcl::PointXYZ>;
-using PointCloud2 = sensor_msgs::msg::PointCloud2;
+
 struct PointData
 {
   float distance;
@@ -72,17 +72,17 @@ std::optional<geometry_msgs::msg::TransformStamped> 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<DetectedObjectWithFeature> & output_objs, std::vector<PointCloud2> & clusters,
-  const std::vector<size_t> & clusters_data_size, const PointCloud2 & in_cloud,
+  std::vector<DetectedObjectWithFeature> & output_objs, std::vector<PointCloudMsgType> & clusters,
+  const std::vector<size_t> & 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<DetectedObjectWithFeature> & 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
 

perception/autoware_image_projection_based_fusion/src/fusion_node.cpp

@@ -494,7 +494,7 @@ void FusionNode<Msg3D, Msg2D, ExportObj>::publish(const Msg3D & output_msg)
 // Explicit instantiation for the supported types
 
 // pointpainting fusion
-template class FusionNode<PointCloud2, RoiMsgType, DetectedObjects>;
+template class FusionNode<PointCloudMsgType, RoiMsgType, DetectedObjects>;
 
 // roi cluster fusion
 template class FusionNode<ClusterMsgType, RoiMsgType, ClusterObjType>;
@@ -503,9 +503,9 @@ template class FusionNode<ClusterMsgType, RoiMsgType, ClusterObjType>;
 template class FusionNode<DetectedObjects, RoiMsgType, DetectedObject>;
 
 // roi pointcloud fusion
-template class FusionNode<PointCloud2, RoiMsgType, ClusterObjType>;
+template class FusionNode<PointCloudMsgType, RoiMsgType, ClusterObjType>;
 
 // segment pointcloud fusion
-template class FusionNode<PointCloud2, Image, PointCloud2>;
+template class FusionNode<PointCloudMsgType, Image, PointCloudMsgType>;
 
 }  // namespace autoware::image_projection_based_fusion

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<PointCloud2, RoiMsgType, DetectedObjects>("pointpainting_fusion", options)
+: FusionNode<PointCloudMsgType, RoiMsgType, DetectedObjects>("pointpainting_fusion", options)
 {
   omp_num_threads_ = this->declare_parameter<int>("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<ScopedTimeTrack> st_ptr;
   if (time_keeper_) st_ptr = std::make_unique<ScopedTimeTrack>(__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<RoiMsgType> & 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(
@@ -378,7 +378,7 @@ dc   | dc dc dc  dc ||zc|
   }
 }
 
-void PointPaintingFusionNode::postprocess(PointCloud2 & painted_pointcloud_msg)
+void PointPaintingFusionNode::postprocess(PointCloudMsgType & painted_pointcloud_msg)
 {
   std::unique_ptr<ScopedTimeTrack> st_ptr;
   if (time_keeper_) st_ptr = std::make_unique<ScopedTimeTrack>(__func__, *time_keeper_);

perception/autoware_image_projection_based_fusion/src/roi_pointcloud_fusion/node.cpp

@@ -37,27 +37,27 @@ namespace autoware::image_projection_based_fusion
 using autoware::universe_utils::ScopedTimeTrack;
 
 RoiPointCloudFusionNode::RoiPointCloudFusionNode(const rclcpp::NodeOptions & options)
-: FusionNode<PointCloud2, RoiMsgType, DetectedObjectWithFeature>("roi_pointcloud_fusion", options)
+: FusionNode<PointCloudMsgType, RoiMsgType, DetectedObjectWithFeature>(
+    "roi_pointcloud_fusion", options)
 {
   fuse_unknown_only_ = declare_parameter<bool>("fuse_unknown_only");
   min_cluster_size_ = declare_parameter<int>("min_cluster_size");
   max_cluster_size_ = declare_parameter<int>("max_cluster_size");
   cluster_2d_tolerance_ = declare_parameter<double>("cluster_2d_tolerance");
   pub_objects_ptr_ = this->create_publisher<ClusterMsgType>("output_clusters", rclcpp::QoS{1});
-  cluster_debug_pub_ = this->create_publisher<sensor_msgs::msg::PointCloud2>("debug/clusters", 1);
+  cluster_debug_pub_ = this->create_publisher<PointCloudMsgType>("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<ScopedTimeTrack> st_ptr;
   if (time_keeper_) st_ptr = std::make_unique<ScopedTimeTrack>(__func__, *time_keeper_);
 
   return;
 }
 
-void RoiPointCloudFusionNode::postprocess(
-  __attribute__((unused)) sensor_msgs::msg::PointCloud2 & pointcloud_msg)
+void RoiPointCloudFusionNode::postprocess(__attribute__((unused))
+                                          PointCloudMsgType & pointcloud_msg)
 {
   std::unique_ptr<ScopedTimeTrack> st_ptr;
   if (time_keeper_) st_ptr = std::make_unique<ScopedTimeTrack>(__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<RoiMsgType> & det2d, const RoiMsgType & input_roi_msg,
-  __attribute__((unused)) sensor_msgs::msg::PointCloud2 & output_pointcloud_msg)
+  const PointCloudMsgType & input_pointcloud_msg, const Det2dManager<RoiMsgType> & det2d,
+  const RoiMsgType & input_roi_msg,
+  __attribute__((unused)) PointCloudMsgType & output_pointcloud_msg)
 {
   std::unique_ptr<ScopedTimeTrack> st_ptr;
   if (time_keeper_) st_ptr = std::make_unique<ScopedTimeTrack>(__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<sensor_msgs::msg::PointCloud2> clusters;
+  std::vector<PointCloudMsgType> clusters;
   std::vector<size_t> clusters_data_size;
   clusters.resize(output_objs.size());
   for (auto & cluster : clusters) {

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<PointCloud2, Image, PointCloud2>("segmentation_pointcloud_fusion", options)
+: FusionNode<PointCloudMsgType, Image, PointCloudMsgType>("segmentation_pointcloud_fusion", options)
 {
   filter_distance_threshold_ = declare_parameter<float>("filter_distance_threshold");
   for (auto & item : filter_semantic_label_target_list_) {
@@ -50,20 +50,21 @@ 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<ScopedTimeTrack> st_ptr;
   if (time_keeper_) st_ptr = std::make_unique<ScopedTimeTrack>(__func__, *time_keeper_);
 
   return;
 }
 
-void SegmentPointCloudFusionNode::postprocess(PointCloud2 & pointcloud_msg)
+void SegmentPointCloudFusionNode::postprocess(PointCloudMsgType & pointcloud_msg)
 {
   std::unique_ptr<ScopedTimeTrack> st_ptr;
   if (time_keeper_) st_ptr = std::make_unique<ScopedTimeTrack>(__func__, *time_keeper_);
 
-  auto original_cloud = std::make_shared<PointCloud2>(pointcloud_msg);
+  auto original_cloud = std::make_shared<PointCloudMsgType>(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<Image> & det2d,
-  [[maybe_unused]] const Image & input_mask, __attribute__((unused)) PointCloud2 & output_cloud)
+  const PointCloudMsgType & input_pointcloud_msg, const Det2dManager<Image> & det2d,
+  [[maybe_unused]] const Image & input_mask,
+  __attribute__((unused)) PointCloudMsgType & output_cloud)
 {
   std::unique_ptr<ScopedTimeTrack> st_ptr;
   if (time_keeper_) st_ptr = std::make_unique<ScopedTimeTrack>(__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;
 }

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<DetectedObjectWithFeature> & output_objs, std::vector<PointCloud2> & clusters,
-  const std::vector<size_t> & clusters_data_size, const PointCloud2 & in_cloud,
+  std::vector<DetectedObjectWithFeature> & output_objs, std::vector<PointCloudMsgType> & clusters,
+  const std::vector<size_t> & 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<DetectedObjectWithFeature> & 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;