186 lines
6.8 KiB
Markdown
186 lines
6.8 KiB
Markdown
# depth_local_costmap_noetic_test
|
|
|
|
Package ROS Noetic C++ tối thiểu để chạy **local costmap only** bằng stack T800:
|
|
|
|
- `/home/duongtd/T800_ws/src/AMR_T800/pnkx_nav_core/src/Libraries/costmap_2d`
|
|
- package catkin: `robot_costmap_2d`
|
|
|
|
Node test nhận dữ liệu ROS Noetic và convert sang message `robot_*` trước khi feed trực tiếp vào `VoxelLayer` bằng `layer->dataCallBack(...)`.
|
|
|
|
Luồng dữ liệu chính:
|
|
|
|
- `/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
|
|
|
|
```bash
|
|
cd /home/duongtd/T800_ws
|
|
catkin_make --pkg depth_local_costmap_noetic_test
|
|
source devel/setup.bash
|
|
```
|
|
|
|
## Chạy
|
|
|
|
```bash
|
|
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:
|
|
|
|
```bash
|
|
roslaunch depth_local_costmap_noetic_test depth_local_costmap_test.launch rviz:=true
|
|
```
|
|
|
|
Nếu robot dùng `base_footprint`:
|
|
|
|
```bash
|
|
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:
|
|
|
|
```bash
|
|
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`:
|
|
|
|
```bash
|
|
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:
|
|
|
|
```bash
|
|
roslaunch depth_local_costmap_noetic_test depth_local_costmap_test.launch enable_depth_cloud_voxel_filter:=false
|
|
```
|
|
|
|
## Kiểm tra runtime
|
|
|
|
```bash
|
|
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:
|
|
|
|
```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/costmap_common_params.yaml`
|
|
- `config/costmap_local_params_plugins_no_virtual_walls.yaml`
|
|
- `config/costmap_local_params.yaml`
|
|
|
|
Khi test trên NUC, export:
|
|
|
|
```bash
|
|
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:
|
|
|
|
```text
|
|
/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:
|
|
|
|
```yaml
|
|
library_path: /home/robotics/DuongTD/dev_ws/devel/lib/libplugins.so
|
|
```
|