# 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/` của package này. Launch file set:

```xml
<env name="PNKX_NAV_CORE_CONFIG_DIR" value="$(find depth_local_costmap_noetic_test)"/>
```