fix(occupancy_grid_map_outlier_filter): add intensity field (autoware…

…foundation#6797) * fix(occupancy_grid_map_outlier_filter): add intensity field Signed-off-by: badai-nguyen <[email protected]> * fix: add intensity Signed-off-by: badai-nguyen <[email protected]> * fix: using empty padding field Signed-off-by: badai-nguyen <[email protected]> * refactor Signed-off-by: badai-nguyen <[email protected]> * Revert "refactor" This reverts commit d16b3e0. * refactor Signed-off-by: badai-nguyen <[email protected]> * fix: radius filter Signed-off-by: badai-nguyen <[email protected]> * chore: typo Signed-off-by: badai-nguyen <[email protected]> * fix: typo Signed-off-by: badai-nguyen <[email protected]> --------- Signed-off-by: badai-nguyen <[email protected]>
tier4 · May 9, 2024 · 308b11c · 308b11c
1 parent 0c5abdd
commit 308b11c
Show file tree

Hide file tree

Showing 2 changed files with 175 additions and 95 deletions.
diff --git a/...r/include/occupancy_grid_map_outlier_filter/occupancy_grid_map_outlier_filter_nodelet.hpp b/...r/include/occupancy_grid_map_outlier_filter/occupancy_grid_map_outlier_filter_nodelet.hpp
@@ -45,18 +45,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_;
@@ -78,22 +76,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_;