fix(occupancy_grid_map_outlier_filter): add intensity field (#6797)

* fix(occupancy_grid_map_outlier_filter): add intensity field

Signed-off-by: badai-nguyen <dai.nguyen@tier4.jp>

* fix: add intensity

Signed-off-by: badai-nguyen <dai.nguyen@tier4.jp>

* fix: using empty padding field

Signed-off-by: badai-nguyen <dai.nguyen@tier4.jp>

* refactor

Signed-off-by: badai-nguyen <dai.nguyen@tier4.jp>

* Revert "refactor"

This reverts commit d16b3e0.

* refactor

Signed-off-by: badai-nguyen <dai.nguyen@tier4.jp>

* fix: radius filter

Signed-off-by: badai-nguyen <dai.nguyen@tier4.jp>

* chore: typo

Signed-off-by: badai-nguyen <dai.nguyen@tier4.jp>

* fix: typo

Signed-off-by: badai-nguyen <dai.nguyen@tier4.jp>

---------

Signed-off-by: badai-nguyen <dai.nguyen@tier4.jp>

Author: badai-nguyen

perception/occupancy_grid_map_outlier_filter/include/occupancy_grid_map_outlier_filter/occupancy_grid_map_outlier_filter_nodelet.hpp

@@ -46,18 +46,16 @@ using geometry_msgs::msg::Pose;
 using nav_msgs::msg::OccupancyGrid;
 using sensor_msgs::msg::PointCloud2;
 using std_msgs::msg::Header;
-using PclPointCloud = pcl::PointCloud<pcl::PointXYZ>;
 
 class RadiusSearch2dFilter
 {
 public:
   explicit RadiusSearch2dFilter(rclcpp::Node & node);
   void filter(
-    const PclPointCloud & input, const Pose & pose, PclPointCloud & output,
-    PclPointCloud & outlier);
+    const PointCloud2 & input, const Pose & pose, PointCloud2 & output, PointCloud2 & outlier);
   void filter(
-    const PclPointCloud & high_conf_input, const PclPointCloud & low_conf_input, const Pose & pose,
-    PclPointCloud & output, PclPointCloud & outlier);
+    const PointCloud2 & high_conf_input, const PointCloud2 & low_conf_input, const Pose & pose,
+    PointCloud2 & output, PointCloud2 & outlier);
 
 private:
   float search_radius_;
@@ -79,22 +77,25 @@ class OccupancyGridMapOutlierFilterComponent : public rclcpp::Node
     const PointCloud2::ConstSharedPtr & input_pointcloud);
   void filterByOccupancyGridMap(
     const OccupancyGrid & occupancy_grid_map, const PointCloud2 & pointcloud,
-    PclPointCloud & high_confidence, PclPointCloud & low_confidence, PclPointCloud & out_ogm);
+    PointCloud2 & high_confidence, PointCloud2 & low_confidence, PointCloud2 & out_ogm);
   void splitPointCloudFrontBack(
     const PointCloud2::ConstSharedPtr & input_pc, PointCloud2 & front_pc, PointCloud2 & behind_pc);
+  void initializerPointCloud2(const PointCloud2 & input, PointCloud2 & output);
+  void finalizePointCloud2(const PointCloud2 & input, PointCloud2 & output);
+  void concatPointCloud2(PointCloud2 & output, const PointCloud2 & input);
 
 private:
   class Debugger
   {
   public:
     explicit Debugger(OccupancyGridMapOutlierFilterComponent & node);
-    void publishOutlier(const PclPointCloud & input, const Header & header);
-    void publishHighConfidence(const PclPointCloud & input, const Header & header);
-    void publishLowConfidence(const PclPointCloud & input, const Header & header);
+    void publishOutlier(const PointCloud2 & input, const Header & header);
+    void publishHighConfidence(const PointCloud2 & input, const Header & header);
+    void publishLowConfidence(const PointCloud2 & input, const Header & header);
 
   private:
     void transformToBaseLink(
-      const PclPointCloud & input, const Header & header, PointCloud2 & output);
+      const PointCloud2 & input, const Header & header, PointCloud2 & output);
     rclcpp::Publisher<PointCloud2>::SharedPtr outlier_pointcloud_pub_;
     rclcpp::Publisher<PointCloud2>::SharedPtr low_confidence_pointcloud_pub_;
     rclcpp::Publisher<PointCloud2>::SharedPtr high_confidence_pointcloud_pub_;