/camera/depth/points_proc -> robot_sensor_msgs::PointCloud2 -> source pc_marking
/camera/depth/image_raw + /camera/depth/camera_info -> robot_sensor_msgs::DepthCameraData::ConstPtr -> source pc_clearing

/camera/depth/data không cần là ROS topic thật trong gói test này; nó là tên source logic để match với config robot_costmap_2d, giống contract trong sensor_converter.cpp.

Topic phụ được node subscribe để log kiểm tra dữ liệu:

/camera_right/depth/camera_info
/odom
/tf
/tf_static

Build

cd /home/duongtd/T800_ws
catkin_make --pkg depth_local_costmap_noetic_test
source devel/setup.bash

Chạy

roslaunch depth_local_costmap_noetic_test depth_local_costmap_test.launch

Launch mặc định chạy headless để số CPU phản ánh pipeline, không tính RViz. Muốn mở GUI:

roslaunch depth_local_costmap_noetic_test depth_local_costmap_test.launch rviz:=true

Nếu robot dùng base_footprint:

roslaunch depth_local_costmap_noetic_test depth_local_costmap_test.launch base_frame:=base_footprint

Nếu hệ thống chỉ có /odom nhưng chưa publish TF odom -> base_link, bật bridge tạm:

roslaunch depth_local_costmap_noetic_test depth_local_costmap_test.launch publish_odom_tf:=true

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.05 m:

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:

roslaunch depth_local_costmap_noetic_test depth_local_costmap_test.launch enable_depth_cloud_voxel_filter:=false

Kiểm tra runtime

rostopic echo -n1 /camera/depth/points_proc/header
rostopic echo -n1 /camera/depth/image_raw/header
rostopic echo -n1 /camera/depth/camera_info/header
rostopic echo -n1 /local_costmap/costmap/info
rostopic echo -n1 /local_costmap/costmap/footprint/header
rosrun tf tf_echo odom base_link
rostopic list | grep local_costmap

Frame của /camera/depth/points_proc và /camera/depth/image_raw phải transform được về odom; nếu không, obstacle/voxel layer sẽ drop dữ liệu.

Log runtime cần thấy:

Fed depth cloud to robot_costmap_2d
Fed DepthCameraData to robot_costmap_2d
Published local costmap for RViz

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:

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:

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/costmap_common_params.yaml
config/costmap_local_params_plugins_no_virtual_walls.yaml
config/costmap_local_params.yaml

Khi test trên NUC, export:

export PNKX_NAV_CORE_CONFIG_DIR=/home/robotics/DuongTD/dev_ws/src/test/depth_local_costmap_noetic_test/config

robot_cpp::NodeHandle sẽ auto-load 3 file trên. Hiện robot_costmap_2d vẫn còn một số loader cũ dùng getSourceFile() và tự nối thêm /config, nên package cũng giữ các file shim tương thích trong:

/home/robotics/DuongTD/dev_ws/src/test/depth_local_costmap_noetic_test/config/config/*.yaml

Với robot_costmap_2d chưa patch resolver plugin path, library_path trong config được đặt thẳng tới NUC:

library_path: /home/robotics/DuongTD/dev_ws/devel/lib/libplugins.so