otimal

README.md

@@ -35,10 +35,11 @@ source devel/setup.bash
 roslaunch depth_local_costmap_noetic_test depth_local_costmap_test.launch
 ```
 
-Launch mặc định mở RViz với config sẵn. Nếu chỉ muốn chạy node không mở GUI:
+Launch mặc định chạy headless để số CPU phản ánh pipeline, không tính RViz.
+Muốn mở GUI:
 
 ```bash
-roslaunch depth_local_costmap_noetic_test depth_local_costmap_test.launch rviz:=false
+roslaunch depth_local_costmap_noetic_test depth_local_costmap_test.launch rviz:=true
 ```
 
 Nếu robot dùng `base_footprint`:
@@ -57,10 +58,10 @@ 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`:
+`amr_control/src/sensor_converter.cpp`. Mặc định leaf size là `0.05 m`:
 
 ```bash
-roslaunch depth_local_costmap_noetic_test depth_local_costmap_test.launch depth_cloud_voxel_leaf_size:=0.10
+roslaunch depth_local_costmap_noetic_test depth_local_costmap_test.launch depth_cloud_voxel_leaf_size:=0.05
 ```
 
 Nếu muốn so sánh full cloud:
@@ -94,6 +95,68 @@ Các topic RViz giống hướng `amr_control::AmrPublisher`:
 - `/local_costmap/costmap` kiểu `nav_msgs/OccupancyGrid`
 - `/local_costmap/costmap/footprint` kiểu `geometry_msgs/PolygonStamped`
 
+## Cấu hình CPU/10 Hz mặc định
+
+Các mặc định dùng cho một Intel D435i:
+
+- `robot_depth_image_proc`: chỉ một camera, xử lý tối đa `15 Hz`, RViz tắt.
+- marking cloud: `filter_decimation=4`, sau đó PCL `VoxelGrid=0.05 m`.
+- clearing depth: xử lý tối đa `15 Hz`.
+- local costmap: update `10 Hz`, publish OccupancyGrid `5 Hz`.
+- frustum clearing: `pixel_step=2`, `column_cover_distance=0.2 m`.
+
+Chạy hai pipeline headless:
+
+```bash
+roslaunch robot_depth_image_proc depth_image_proc_gazebo.launch \
+  multi_cam:=false processing_rate:=15.0 rviz:=false
+
+roslaunch depth_local_costmap_noetic_test depth_local_costmap_test.launch \
+  local_costmap_update_frequency:=10.0 \
+  depth_clearing_processing_rate:=15.0 \
+  depth_cloud_voxel_leaf_size:=0.05 \
+  frustum_clearing_pixel_step:=2 \
+  column_cover_distance:=0.2 \
+  rviz:=false
+```
+
+`processing_rate<=0` hoặc `depth_clearing_processing_rate<=0` sẽ tắt rate
+limit để A/B test. Camera vẫn có thể publish depth raw ở 30 Hz; chỉ các hot
+path copy/filter/project/clearing được giới hạn.
+
+Mỗi 5 giây, log `depth performance` và `Depth clearing performance` báo
+input Hz, processed Hz, số frame bỏ qua, thời gian trung bình và max. Log
+`Costmap performance` có `avg/p95/p99_cycle_ms`.
+
+Đo trên NUC trong ít nhất 60 giây sau warm-up:
+
+```bash
+rostopic hz /camera/depth/image_raw
+rostopic hz /camera/depth/points_proc
+rostopic hz /local_costmap/costmap
+pidstat -p "$(pgrep -d, -f 'depth_image_proc_node|depth_local_costmap_noetic_test_node')" 1
+```
+
+Mục tiêu:
+
+- `/camera/depth/points_proc`: `14-16 Hz`.
+- costmap update thực tế: ít nhất `9.5 Hz`, tính từ `cycles` trong log
+  `Costmap performance` chia cho cửa sổ 5 giây.
+- `/local_costmap/costmap`: khoảng `5 Hz` vì OccupancyGrid được publish riêng.
+- `p95 < 80 ms`, `p99 < 100 ms`.
+- chạy headless: từng node không quá khoảng `30% CPU`, tổng không quá khoảng
+  `60%` theo thang `htop` mà `100%` là một logical CPU.
+
+`column_cover_distance=0.2 m` là giá trị clearing mạnh đã giải quyết ghost
+trong bài test hiện tại. Nó không phải giá trị suy ra an toàn từ vertical FOV;
+phải test lại với vật cản thật cao đến `1.0 m` trước khi đưa lên robot chạy.
+Nếu cần baseline hình học bảo thủ, dùng `column_cover_distance:=-1.0`.
+
+Chỉ chuyển sang pha chia sẻ ảnh depth đã edge-filter giữa hai node nếu
+`depth_image_proc_node` vẫn vượt mục tiêu CPU. Chỉ thay PCL `VoxelGrid` bằng
+downsampler trực tiếp `PointCloud2` nếu profiler xác nhận PCL còn là hotspot;
+hai thay đổi đó cần benchmark NUC sau bước hiện tại để giữ nguyên semantics.
+
 Config local chính của package này dùng cùng kiểu tách file với
 `pnkx_nav_core/config`:
 

config/config/costmap_common_params.yaml

@@ -30,9 +30,15 @@ obstacles:
     marking: true
     inf_is_valid: false
     frustum_clearing_enabled: true
-    frustum_clearing_pixel_step: 4
+    frustum_clearing_pixel_step: 2
     frustum_min_range: 0.20
     frustum_max_range: 3.5
+    frustum_skip_distance: 0.05
+    frustum_column_clearing: true
+    column_clear_min_height: 0.10
+    column_clear_max_height: -1.0
+    column_skip_distance: 0.02
+    column_cover_distance: 0.2
     observation_persistence: 0.0
     expected_update_rate: 0.5
     obstacle_range: 2.5

config/config/costmap_local_params.yaml

@@ -4,8 +4,8 @@ local_costmap:
   library_path: /home/robotics/DuongTD/dev_ws/devel/lib/libplugins.so
   global_frame: odom
   robot_base_frame: base_link
-  update_frequency: 6.0
-  publish_frequency: 6.0
+  update_frequency: 10.0
+  publish_frequency: 5.0
   rolling_window: true
   track_unknown_space: false
   raytrace_range: 3.0

config/config/costmap_params.yaml

@@ -25,7 +25,7 @@ robot_costmap_2d:
   transform_tolerance: 1.0
   performance_metrics_enabled: true
   performance_metrics_period: 5.0
-  update_frequency: 6.0
+  update_frequency: 10.0
   width: 8.0
   height: 8.0
   resolution: 0.05

config/config/obstacle_layer_params.yaml

@@ -30,7 +30,7 @@ obstacle_layer:
     marking: true
     inf_is_valid: false
     frustum_clearing_enabled: true
-    frustum_clearing_pixel_step: 4
+    frustum_clearing_pixel_step: 2
     frustum_min_range: 0.20
     frustum_max_range: 3.5
     # 3D clearing rays stop this far [m] before the measured surface.
@@ -50,7 +50,7 @@ obstacle_layer:
     column_skip_distance: 0.02
     # Full columns only cleared beyond this distance [m] (vertical FOV must
     # cover the whole band). Negative: auto from camera intrinsics + mount.
-    column_cover_distance: -1.0
+    column_cover_distance: 0.2
     observation_persistence: 0.0
     expected_update_rate: 0.5
     obstacle_range: 2.5

config/costmap_common_params.yaml

@@ -30,7 +30,7 @@ obstacles:
     marking: true
     inf_is_valid: false
     frustum_clearing_enabled: true
-    frustum_clearing_pixel_step: 4
+    frustum_clearing_pixel_step: 2
     frustum_min_range: 0.20
     frustum_max_range: 3.5
     frustum_skip_distance: 0.05
@@ -38,7 +38,7 @@ obstacles:
     column_clear_min_height: 0.10
     column_clear_max_height: -1.0
     column_skip_distance: 0.02
-    column_cover_distance: -1.0
+    column_cover_distance: 0.2
     observation_persistence: 0.0
     expected_update_rate: 0.5
     obstacle_range: 2.5

config/costmap_local_params.yaml

@@ -4,8 +4,8 @@ local_costmap:
   library_path: /home/robotics/DuongTD/dev_ws/devel/lib/libplugins.so
   global_frame: odom
   robot_base_frame: base_link
-  update_frequency: 6.0
-  publish_frequency: 6.0
+  update_frequency: 10.0
+  publish_frequency: 5.0
   rolling_window: true
   track_unknown_space: false
   raytrace_range: 3.0

launch/depth_local_costmap_test.launch

@@ -3,7 +3,7 @@
   <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_cloud_voxel_leaf_size" default="0.05"/>
   <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"/>
@@ -11,8 +11,12 @@
   <arg name="odom_topic" default="/odom"/>
   <arg name="publish_odom_tf" default="false"/>
   <arg name="publish_rate" default="5.0"/>
-  <arg name="rviz" default="true"/>
-  <arg name="frustum_clearing_pixel_step" default="4"/>
+  <arg name="local_costmap_update_frequency" default="10.0"/>
+  <arg name="depth_clearing_processing_rate" default="15.0"/>
+  <arg name="feeder_performance_metrics_enabled" default="true"/>
+  <arg name="feeder_performance_metrics_period" default="5.0"/>
+  <arg name="rviz" default="false"/>
+  <arg name="frustum_clearing_pixel_step" default="2"/>
   <!-- Camera-only ghost-obstacle handling: LETHAL cells not re-observed for
        this many seconds are freed (covers cells outside the current FOV that
        frustum clearing can never reach). 0 disables decay. -->
@@ -32,7 +36,7 @@
        (tighter than frustum_skip_distance so ghosts hugging static obstacles
        still get cleared). -->
   <arg name="column_skip_distance" default="0.02"/>
-  <arg name="column_cover_distance" default="-1.0"/>
+  <arg name="column_cover_distance" default="0.2"/>
 
   <node pkg="depth_local_costmap_noetic_test"
         type="depth_local_costmap_noetic_test_node"
@@ -41,6 +45,9 @@
     <param name="base_frame" value="$(arg base_frame)"/>
     <param name="publish_odom_tf" value="$(arg publish_odom_tf)"/>
     <param name="publish_rate" value="$(arg publish_rate)"/>
+    <param name="depth_clearing_processing_rate" value="$(arg depth_clearing_processing_rate)"/>
+    <param name="feeder_performance_metrics_enabled" value="$(arg feeder_performance_metrics_enabled)"/>
+    <param name="feeder_performance_metrics_period" value="$(arg feeder_performance_metrics_period)"/>
     <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)"/>
@@ -52,6 +59,7 @@
 
     <param name="local_costmap/robot_base_frame" value="$(arg base_frame)"/>
     <param name="local_costmap/global_frame" value="$(arg global_frame)"/>
+    <param name="local_costmap/update_frequency" value="$(arg local_costmap_update_frequency)"/>
     <param name="local_costmap/obstacles/observation_sources" value="pc_marking pc_clearing"/>
 
     <param name="local_costmap/obstacles/pc_marking/topic" value="$(arg depth_cloud_topic)"/>

src/depth_local_costmap_noetic_test_node.cpp

@@ -9,6 +9,7 @@
 #include <nav_msgs/Odometry.h>
 #include <nav_msgs/OccupancyGrid.h>
 #include <robot_depth_image_proc/depth_frame_filter.h>
+#include <robot_depth_image_proc/processing_rate_limiter.h>
 #include <robot_depth_image_proc/ros_message_conversions.h>
 #include <robot/robot.h>
 #include <robot_geometry_msgs/PoseStamped.h>
@@ -32,7 +33,10 @@
 #include <pcl/point_cloud.h>
 #include <pcl/point_types.h>
 
+#include <algorithm>
 #include <atomic>
+#include <chrono>
+#include <cstdint>
 #include <cmath>
 #include <exception>
 #include <memory>
@@ -272,7 +276,7 @@ public:
   {
     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);
+    private_nh.param("depth_cloud_voxel_leaf_size", voxel_leaf_size_, 0.05);
     voxel_leaf_size_ = std::max(0.0, voxel_leaf_size_);
 
     depth_cloud_sub_ =
@@ -327,7 +331,7 @@ private:
   robot_costmap_2d::Costmap2DROBOT& local_costmap_;
   std::string depth_cloud_topic_;
   bool enable_voxel_filter_{true};
-  double voxel_leaf_size_{0.10};
+  double voxel_leaf_size_{0.05};
   ros::Subscriber depth_cloud_sub_;
 };
 
@@ -345,6 +349,19 @@ public:
                      depth_camera_data_topic_,
                      std::string("/camera/depth/data"));
     private_nh.param("enable_depth_clearing_filter", enable_clearing_filter_, enable_clearing_filter_);
+    private_nh.param(
+        "depth_clearing_processing_rate", processing_rate_hz_, processing_rate_hz_);
+    private_nh.param(
+        "feeder_performance_metrics_enabled",
+        performance_metrics_enabled_,
+        performance_metrics_enabled_);
+    private_nh.param(
+        "feeder_performance_metrics_period",
+        performance_metrics_period_,
+        performance_metrics_period_);
+    performance_metrics_period_ = std::max(1.0, performance_metrics_period_);
+    processing_rate_limiter_.setRate(processing_rate_hz_);
+    metrics_window_start_ = ros::WallTime::now();
 
     camera_info_sub_ =
         private_nh.subscribe(camera_info_topic_, 1, &DepthCameraDataFeeder::cameraInfoCallback, this);
@@ -443,6 +460,15 @@ private:
       return;
     }
 
+    ++received_frames_;
+    if (!processing_rate_limiter_.shouldProcess())
+    {
+      ++skipped_frames_;
+      reportPerformanceIfDue();
+      return;
+    }
+
+    const auto processing_start = std::chrono::steady_clock::now();
     robot_sensor_msgs::DepthCameraData::Ptr depth_camera_data(new robot_sensor_msgs::DepthCameraData());
     depth_camera_data->depth = depth_image_proc::toRobotImage(*msg);
 
@@ -464,6 +490,7 @@ private:
 
     robot_sensor_msgs::DepthCameraData::ConstPtr const_depth_camera_data = depth_camera_data;
     feedVoxelLayers(local_costmap_, const_depth_camera_data, depth_camera_data_topic_);
+    recordProcessedFrame(processing_start);
 
     ROS_INFO_THROTTLE(5.0, "Fed DepthCameraData to robot_costmap_2d: %ux%u encoding=%s frame=%s topic=%s",
                       msg->width,
@@ -471,6 +498,48 @@ private:
                       msg->encoding.c_str(),
                       depth_camera_data->header.frame_id.c_str(),
                       depth_camera_data_topic_.c_str());
+    reportPerformanceIfDue();
+  }
+
+  void recordProcessedFrame(const std::chrono::steady_clock::time_point processing_start)
+  {
+    const double elapsed_ms =
+        std::chrono::duration<double, std::milli>(
+            std::chrono::steady_clock::now() - processing_start).count();
+    ++processed_frames_;
+    processing_time_ms_ += elapsed_ms;
+    max_processing_time_ms_ = std::max(max_processing_time_ms_, elapsed_ms);
+  }
+
+  void reportPerformanceIfDue()
+  {
+    if (!performance_metrics_enabled_)
+      return;
+
+    const ros::WallTime now = ros::WallTime::now();
+    const double window_seconds = (now - metrics_window_start_).toSec();
+    if (window_seconds < performance_metrics_period_)
+      return;
+
+    const double input_hz = static_cast<double>(received_frames_) / window_seconds;
+    const double processed_hz = static_cast<double>(processed_frames_) / window_seconds;
+    const double average_ms =
+        processed_frames_ > 0 ? processing_time_ms_ / static_cast<double>(processed_frames_) : 0.0;
+    ROS_INFO(
+        "Depth clearing performance: input=%.1f Hz processed=%.1f Hz "
+        "skipped=%llu avg=%.2f ms max=%.2f ms",
+        input_hz,
+        processed_hz,
+        static_cast<unsigned long long>(skipped_frames_),
+        average_ms,
+        max_processing_time_ms_);
+
+    metrics_window_start_ = now;
+    received_frames_ = 0;
+    processed_frames_ = 0;
+    skipped_frames_ = 0;
+    processing_time_ms_ = 0.0;
+    max_processing_time_ms_ = 0.0;
   }
 
   robot_costmap_2d::Costmap2DROBOT& local_costmap_;
@@ -478,6 +547,16 @@ private:
   std::string camera_info_topic_;
   std::string depth_camera_data_topic_;
   bool enable_clearing_filter_{true};
+  double processing_rate_hz_{15.0};
+  depth_image_proc::ProcessingRateLimiter processing_rate_limiter_;
+  bool performance_metrics_enabled_{true};
+  double performance_metrics_period_{5.0};
+  ros::WallTime metrics_window_start_;
+  std::uint64_t received_frames_{0};
+  std::uint64_t processed_frames_{0};
+  std::uint64_t skipped_frames_{0};
+  double processing_time_ms_{0.0};
+  double max_processing_time_ms_{0.0};
   depth_image_proc::DepthFilterConfig clearing_filter_config_;
   depth_image_proc::DepthFrameFilter clearing_frame_filter_;
   robot_sensor_msgs::CameraInfo camera_info_;