add pcl

CMakeLists.txt

@@ -9,6 +9,8 @@ find_package(catkin REQUIRED COMPONENTS
   data_convert
   geometry_msgs
   nav_msgs
+  pcl_conversions
+  pcl_ros
   robot_depth_image_proc
   robot_costmap_2d
   robot_cpp
@@ -20,6 +22,11 @@ find_package(catkin REQUIRED COMPONENTS
   tf2_ros
 )
 
+find_package(PCL REQUIRED COMPONENTS
+  common
+  filters
+)
+
 find_library(TF3_LIBRARY
   NAMES tf3
   PATHS /usr/lib /usr/local/lib /usr/lib/x86_64-linux-gnu
@@ -38,11 +45,13 @@ add_executable(depth_local_costmap_noetic_test_node
 target_include_directories(depth_local_costmap_noetic_test_node
   PRIVATE
     ${catkin_INCLUDE_DIRS}
+    ${PCL_INCLUDE_DIRS}
 )
 
 target_link_libraries(depth_local_costmap_noetic_test_node
   PRIVATE
     ${catkin_LIBRARIES}
+    ${PCL_LIBRARIES}
     ${TF3_LIBRARY}
 )
 

README.md

@@ -55,6 +55,20 @@ roslaunch depth_local_costmap_noetic_test depth_local_costmap_test.launch publis
 
 Không bật `publish_odom_tf` nếu đã có node khác publish TF này.
 
+Point cloud marking `/camera/depth/points_proc` được downsample bằng PCL
+`VoxelGrid` trước khi feed vào `robot_costmap_2d`, giống hướng trong
+`amr_control/src/sensor_converter.cpp`. Mặc định leaf size là `0.10 m`:
+
+```bash
+roslaunch depth_local_costmap_noetic_test depth_local_costmap_test.launch depth_cloud_voxel_leaf_size:=0.10
+```
+
+Nếu muốn so sánh full cloud:
+
+```bash
+roslaunch depth_local_costmap_noetic_test depth_local_costmap_test.launch enable_depth_cloud_voxel_filter:=false
+```
+
 ## Kiểm tra runtime
 
 ```bash

launch/depth_local_costmap_test.launch

@@ -2,6 +2,8 @@
   <arg name="base_frame" default="base_link"/>
   <arg name="global_frame" default="odom"/>
   <arg name="depth_cloud_topic" default="/camera/depth/points_proc"/>
+  <arg name="enable_depth_cloud_voxel_filter" default="true"/>
+  <arg name="depth_cloud_voxel_leaf_size" default="0.10"/>
   <arg name="depth_image_topic" default="/camera/depth/image_raw"/>
   <arg name="camera_info_topic" default="/camera/depth/camera_info"/>
   <arg name="depth_camera_data_topic" default="/camera/depth/data"/>
@@ -20,6 +22,8 @@
     <param name="publish_rate" value="$(arg publish_rate)"/>
     <param name="odom_topic" value="$(arg odom_topic)"/>
     <param name="depth_cloud_topic" value="$(arg depth_cloud_topic)"/>
+    <param name="enable_depth_cloud_voxel_filter" value="$(arg enable_depth_cloud_voxel_filter)"/>
+    <param name="depth_cloud_voxel_leaf_size" value="$(arg depth_cloud_voxel_leaf_size)"/>
     <param name="depth_image_topic" value="$(arg depth_image_topic)"/>
     <param name="camera_info_topic" value="$(arg camera_info_topic)"/>
     <param name="depth_camera_data_topic" value="$(arg depth_camera_data_topic)"/>

package.xml

@@ -12,6 +12,8 @@
   <depend>data_convert</depend>
   <depend>geometry_msgs</depend>
   <depend>nav_msgs</depend>
+  <depend>pcl_conversions</depend>
+  <depend>pcl_ros</depend>
   <depend>robot_depth_image_proc</depend>
   <depend>robot_costmap_2d</depend>
   <depend>robot_cpp</depend>

src/depth_local_costmap_noetic_test_node.cpp

@@ -26,6 +26,11 @@
 #include <tf2_ros/transform_listener.h>
 #include <tf3/buffer_core.h>
 
+#include <data_convert/pcl_convert.h>
+#include <pcl/filters/voxel_grid.h>
+#include <pcl/point_cloud.h>
+#include <pcl/point_types.h>
+
 #include <atomic>
 #include <cmath>
 #include <exception>
@@ -265,6 +270,10 @@ public:
     : local_costmap_(local_costmap)
   {
     private_nh.param("depth_cloud_topic", depth_cloud_topic_, std::string("/camera/depth/points_proc"));
+    private_nh.param("enable_depth_cloud_voxel_filter", enable_voxel_filter_, true);
+    private_nh.param("depth_cloud_voxel_leaf_size", voxel_leaf_size_, 0.10);
+    voxel_leaf_size_ = std::max(0.0, voxel_leaf_size_);
+
     depth_cloud_sub_ =
         private_nh.subscribe(depth_cloud_topic_, 1, &DepthCloudFeeder::depthCloudCallback, this);
   }
@@ -276,16 +285,48 @@ private:
       return;
 
     const robot_sensor_msgs::PointCloud2 robot_cloud = toRobotPointCloud2(*msg);
-    feedVoxelLayers(local_costmap_, robot_cloud, depth_cloud_topic_);
+    const robot_sensor_msgs::PointCloud2 marking_cloud = filterMarkingCloud(robot_cloud);
+    feedVoxelLayers(local_costmap_, marking_cloud, depth_cloud_topic_);
 
-    ROS_INFO_THROTTLE(5.0, "Fed depth cloud to robot_costmap_2d: %ux%u frame=%s",
+    ROS_INFO_THROTTLE(5.0,
+                      "Fed depth cloud to robot_costmap_2d: in=%ux%u out=%ux%u leaf=%.3f frame=%s",
                       msg->width,
                       msg->height,
+                      marking_cloud.width,
+                      marking_cloud.height,
+                      enable_voxel_filter_ ? voxel_leaf_size_ : 0.0,
                       msg->header.frame_id.c_str());
   }
 
+  robot_sensor_msgs::PointCloud2 filterMarkingCloud(const robot_sensor_msgs::PointCloud2& cloud) const
+  {
+    if (!enable_voxel_filter_ || voxel_leaf_size_ <= 0.0 || cloud.width == 0 || cloud.height == 0)
+      return cloud;
+
+    pcl::PointCloud<pcl::PointXYZ> pcl_cloud;
+    pcl::fromROBOTMsg(cloud, pcl_cloud);
+    if (pcl_cloud.empty())
+      return cloud;
+
+    pcl::VoxelGrid<pcl::PointXYZ> voxel;
+    voxel.setInputCloud(pcl_cloud.makeShared());
+    voxel.setLeafSize(static_cast<float>(voxel_leaf_size_),
+                      static_cast<float>(voxel_leaf_size_),
+                      static_cast<float>(voxel_leaf_size_));
+
+    pcl::PointCloud<pcl::PointXYZ> filtered;
+    voxel.filter(filtered);
+
+    robot_sensor_msgs::PointCloud2 output;
+    pcl::toROBOTMsg(filtered, output);
+    output.header = cloud.header;
+    return output;
+  }
+
   robot_costmap_2d::Costmap2DROBOT& local_costmap_;
   std::string depth_cloud_topic_;
+  bool enable_voxel_filter_{true};
+  double voxel_leaf_size_{0.10};
   ros::Subscriber depth_cloud_sub_;
 };