# 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
```

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
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`

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
```