106 lines
3.7 KiB
Markdown
106 lines
3.7 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 mở RViz với config sẵn. Nếu chỉ muốn chạy node không mở GUI:
|
|
|
|
```bash
|
|
roslaunch depth_local_costmap_noetic_test depth_local_costmap_test.launch rviz:=false
|
|
```
|
|
|
|
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.
|
|
|
|
## 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`
|
|
|
|
Config local cho `robot_costmap_2d` nằm trong thư mục `config/config/` của package này khi export:
|
|
|
|
```bash
|
|
export PNKX_NAV_CORE_CONFIG_DIR=/home/robotics/DuongTD/dev_ws/src/test/depth_local_costmap_noetic_test/config
|
|
```
|
|
|
|
Lý do là `robot_costmap_2d::getSourceFile()` tự nối thêm `/config`, nên export tới package `config/` sẽ làm runtime đọc:
|
|
|
|
```text
|
|
/home/robotics/DuongTD/dev_ws/src/test/depth_local_costmap_noetic_test/config/config/*.yaml
|
|
```
|
|
|
|
File test đang bám theo:
|
|
|
|
- `costmap_common_params.yaml`: source `pc_marking` và `pc_clearing`
|
|
- `costmap_local_params_plugins_no_virtual_walls.yaml`: plugin `obstacles` kiểu `VoxelLayer`, `inflation` kiểu `InflationLayer`
|
|
- `costmap_local_params.yaml`: `global_frame=odom`, `rolling_window=true`, `width/height=8.0`, `resolution=0.05`
|
|
|
|
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
|
|
```
|