637 lines
27 KiB
Markdown
637 lines
27 KiB
Markdown
# robot_costmap_2d
|
|
|
|
`robot_costmap_2d` là thư viện costmap dạng nhiều lớp của T800. Package duy trì
|
|
lưới chi phí 2D dùng cho global/local planner, nhận bản đồ tĩnh và dữ liệu cảm
|
|
biến, xóa vùng trống, đánh dấu vật cản, sau đó tạo vùng chi phí an toàn quanh
|
|
vật cản.
|
|
|
|
Package hỗ trợ C++17, catkin và standalone CMake. Các plugin được nạp bằng
|
|
`boost::dll` từ thư viện `libplugins`.
|
|
|
|
## 1. Luồng dữ liệu và kiến trúc
|
|
|
|
Luồng cập nhật chính:
|
|
|
|
```text
|
|
OccupancyGrid -------------------------> StaticLayer ---------+
|
|
LaserScan / PointCloud / PointCloud2 --> ObstacleLayer -------+--> master costmap
|
|
PointCloud2 + DepthCameraData ---------> VoxelLayer ----------+
|
|
master costmap ------------------------> InflationLayer -------+
|
|
```
|
|
|
|
Mỗi chu kỳ, `LayeredCostmap` gọi lần lượt:
|
|
|
|
1. `updateBounds()` để từng layer mở rộng vùng cần cập nhật.
|
|
2. Reset vùng tương ứng trên master costmap.
|
|
3. `updateCosts()` theo đúng thứ tự trong danh sách `plugins`.
|
|
|
|
Vì vậy thứ tự plugin có ảnh hưởng trực tiếp tới kết quả. Trong cấu hình chạy
|
|
thực tế, nên đặt layer bản đồ trước, layer vật cản sau và `InflationLayer` cuối
|
|
cùng để cả vật cản tĩnh lẫn vật cản động đều được inflation.
|
|
|
|
Các plugin được build trong package:
|
|
|
|
| Plugin | Vai trò |
|
|
| --- | --- |
|
|
| `StaticLayer` | Đưa `OccupancyGrid` tĩnh vào costmap. |
|
|
| `ObstacleLayer` | Marking/clearing 2D từ `LaserScan`, `PointCloud`, `PointCloud2`. |
|
|
| `VoxelLayer` | Lưu vật cản theo voxel 3D, chiếu kết quả xuống costmap 2D và hỗ trợ clearing theo frustum depth camera. |
|
|
| `InflationLayer` | Tạo vùng chi phí giảm dần quanh ô vật cản. |
|
|
| `CriticalLayer` | Gộp vùng critical do hệ thống T800 cung cấp. |
|
|
| `DirectionalLayer` | Gộp thông tin vùng có hướng di chuyển. |
|
|
| `PreferredLayer` | Gộp vùng ưu tiên. |
|
|
| `UnPreferredLayer` | Gộp vùng không ưu tiên. |
|
|
|
|
## 2. Cách package nạp cấu hình
|
|
|
|
Tham số có ba tầng ưu tiên, tầng sau ghi đè tầng trước:
|
|
|
|
1. Giá trị fallback trong code khi một key không tồn tại trong YAML.
|
|
2. Các file YAML tên cố định nằm dưới thư mục `config/`.
|
|
3. Tham số trong `robot::NodeHandle`, thường được launch nạp cho
|
|
`global_costmap` hoặc `local_costmap`.
|
|
|
|
Biến môi trường `PNKX_NAV_CORE_CONFIG_DIR` phải trỏ tới thư mục cha có thư mục
|
|
con `config/`. Hàm nạp sẽ tìm đệ quy các file sau:
|
|
|
|
- `costmap_params.yaml`
|
|
- `static_layer_params.yaml`
|
|
- `obstacle_layer_params.yaml`
|
|
- `voxel_layer_params.yaml`
|
|
- `inflation_layer_params.yaml`
|
|
|
|
Ví dụ dùng các file mặc định nằm ngay trong package:
|
|
|
|
```bash
|
|
export PNKX_NAV_CORE_CONFIG_DIR=/home/duongtd/T800_ws/src/AMR_T800/pnkx_nav_core/src/Libraries/costmap_2d
|
|
```
|
|
|
|
Ở hệ thống chạy thật, cấu hình launch-facing nằm trong
|
|
`Controllers/Packages/amr_startup/config/`; launch nạp
|
|
`costmap_common_params.yaml` riêng vào namespace `global_costmap` và
|
|
`local_costmap`, sau đó nạp file global/local tương ứng.
|
|
|
|
> Không đặt nhiều file trùng tên trong các nhánh con của cùng một thư mục
|
|
> `config/`. Hàm tìm kiếm dừng ở file đầu tiên tìm thấy, nên nguồn cấu hình sẽ
|
|
> khó xác định.
|
|
|
|
## 3. Cấu hình mặc định của package
|
|
|
|
Các file trong `config/` là fallback và cũng được test của package sử dụng.
|
|
Chúng mô tả giá trị mặc định, không phải cấu hình hoàn chỉnh để chạy robot.
|
|
|
|
### 3.1. Costmap chính
|
|
|
|
File `config/costmap_params.yaml`:
|
|
|
|
```yaml
|
|
robot_costmap_2d:
|
|
global_frame: map
|
|
robot_base_frame: base_link
|
|
rolling_window: false
|
|
track_unknown_space: false
|
|
|
|
plugins:
|
|
- name: static_layer
|
|
type: StaticLayer
|
|
- name: inflation_layer
|
|
type: InflationLayer
|
|
- name: obstacle_layer
|
|
type: ObstacleLayer
|
|
- name: voxel_layer
|
|
type: VoxelLayer
|
|
|
|
library_path: ./libplugins.so
|
|
|
|
footprint:
|
|
- [0.3, 0.3]
|
|
- [0.3, -0.3]
|
|
- [-0.3, -0.3]
|
|
- [-0.3, 0.3]
|
|
|
|
transform_tolerance: 0.0
|
|
performance_metrics_enabled: false
|
|
performance_metrics_period: 5.0
|
|
update_frequency: 1.0
|
|
width: 0.0
|
|
height: 0.0
|
|
resolution: 0.0
|
|
origin_x: 0.0
|
|
origin_y: 0.0
|
|
footprint_padding: 0.0
|
|
robot_radius: 0.0
|
|
```
|
|
|
|
Các giá trị `width`, `height` và `resolution` bằng `0.0` chỉ là placeholder.
|
|
Khi không có `StaticLayer` resize costmap từ bản đồ, bắt buộc ghi đè cả ba giá
|
|
trị bằng số dương trước khi chạy.
|
|
|
|
Danh sách plugin fallback ở trên phản ánh file hiện tại. Cấu hình deployment
|
|
nên khai báo lại plugin và đặt `InflationLayer` cuối danh sách.
|
|
|
|
### 3.2. Static layer
|
|
|
|
File `config/static_layer_params.yaml`:
|
|
|
|
```yaml
|
|
static_layer:
|
|
enabled: true
|
|
map_topic: map
|
|
first_map_only: false
|
|
subscribe_to_updates: false
|
|
track_unknown_space: true
|
|
use_maximum: false
|
|
lethal_cost_threshold: 100
|
|
unknown_cost_value: -1
|
|
trinary_costmap: true
|
|
base_frame_id: map
|
|
```
|
|
|
|
### 3.3. Obstacle layer
|
|
|
|
File `config/obstacle_layer_params.yaml` hiện chứa các giá trị cơ sở:
|
|
|
|
```yaml
|
|
obstacle_layer:
|
|
track_unknown_space: true
|
|
transform_tolerance: 0.2
|
|
topic: map
|
|
sensor_frame: laser_frame
|
|
observation_persistence: 0.0
|
|
expected_update_rate: 0.0
|
|
data_type: PointCloud
|
|
min_obstacle_height: 0.0
|
|
max_obstacle_height: 2.0
|
|
inf_is_valid: false
|
|
clearing: false
|
|
marking: true
|
|
obstacle_range: 2.5
|
|
raytrace_range: 3.0
|
|
footprint_clearing_enabled: true
|
|
combination_method: 1
|
|
```
|
|
|
|
`ObstacleLayer` chỉ tạo buffer khi có `observation_sources`. Các tham số
|
|
`topic`, `data_type`, `marking`, `clearing`, range và height phải được đặt dưới
|
|
từng source. Do file fallback trên chưa khai báo `observation_sources`, nó không
|
|
tự đăng ký nguồn cảm biến nào.
|
|
|
|
### 3.4. Voxel layer
|
|
|
|
File `config/voxel_layer_params.yaml`:
|
|
|
|
```yaml
|
|
voxel_layer:
|
|
enabled: true
|
|
footprint_clearing_enabled: true
|
|
max_obstacle_height: 3.0
|
|
origin_z: 0.0
|
|
z_resolution: 0.2
|
|
z_voxels: 16
|
|
unknown_threshold: 15.0
|
|
mark_threshold: 0
|
|
combination_method: 1
|
|
frustum_clearing_enabled: true
|
|
frustum_clearing_pixel_step: 8
|
|
frustum_min_range: 0.20
|
|
frustum_max_range: 3.0
|
|
frustum_depth_camera_topic: /camera/depth/data
|
|
```
|
|
|
|
Trong implementation hiện tại, các tham số `frustum_*` được đọc theo từng
|
|
observation source bởi `ObstacleLayer`, là lớp cha của `VoxelLayer`. Vì vậy,
|
|
đừng chỉ chỉnh các key `frustum_*` trong `voxel_layer_params.yaml`; hãy đặt
|
|
chúng dưới source depth camera trong `costmap_common_params.yaml`.
|
|
|
|
Topic dùng để dispatch dữ liệu depth là `topic` của source. Key
|
|
`frustum_depth_camera_topic` vẫn có trong cấu hình fallback nhưng không thay thế
|
|
cho `pc_clearing.topic` trong contract hiện tại.
|
|
|
|
### 3.5. Inflation layer
|
|
|
|
File `config/inflation_layer_params.yaml`:
|
|
|
|
```yaml
|
|
inflation_layer:
|
|
enabled: true
|
|
inflate_unknown: false
|
|
cost_scaling_factor: 15.0
|
|
inflation_radius: 0.55
|
|
```
|
|
|
|
## 4. Cấu hình tham khảo cho T800
|
|
|
|
Ví dụ sau dùng laser để marking/clearing 2D, point cloud đã xử lý để marking
|
|
vật cản 3D và message gộp `DepthCameraData` để clearing theo frustum.
|
|
|
|
### 4.1. Tham số dùng chung cho global và local costmap
|
|
|
|
```yaml
|
|
robot_base_frame: base_link
|
|
transform_tolerance: 1.0
|
|
footprint_padding: 0.0
|
|
|
|
# Polygon phải đo theo robot thật, đơn vị mét, trong robot_base_frame.
|
|
footprint:
|
|
- [0.583, -0.48]
|
|
- [0.583, 0.48]
|
|
- [-0.583, 0.48]
|
|
- [-0.583, -0.48]
|
|
|
|
obstacles:
|
|
observation_sources: b_scan pc_marking pc_clearing
|
|
|
|
b_scan:
|
|
topic: /b_scan
|
|
data_type: LaserScan
|
|
sensor_frame: ""
|
|
marking: true
|
|
clearing: true
|
|
inf_is_valid: true
|
|
observation_persistence: 0.0
|
|
expected_update_rate: 0.0
|
|
obstacle_range: 2.5
|
|
raytrace_range: 3.0
|
|
min_obstacle_height: 0.0
|
|
max_obstacle_height: 0.25
|
|
frustum_clearing_enabled: false
|
|
|
|
# Giữ PointCloud2 cho marking và persistence nếu cần.
|
|
pc_marking:
|
|
topic: /camera/depth/points_proc
|
|
data_type: PointCloud2
|
|
sensor_frame: ""
|
|
marking: true
|
|
clearing: false
|
|
inf_is_valid: false
|
|
observation_persistence: 0.0
|
|
expected_update_rate: 0.5
|
|
obstacle_range: 2.5
|
|
raytrace_range: 3.0
|
|
min_obstacle_height: 0.10
|
|
max_obstacle_height: 1.00
|
|
frustum_clearing_enabled: false
|
|
|
|
# Clearing dùng raw depth + CameraInfo trong cùng một message.
|
|
pc_clearing:
|
|
topic: /camera/depth/data
|
|
data_type: DepthCameraData
|
|
sensor_frame: ""
|
|
marking: false
|
|
clearing: false
|
|
observation_persistence: 0.0
|
|
expected_update_rate: 0.5
|
|
frustum_clearing_enabled: true
|
|
frustum_clearing_pixel_step: 8
|
|
frustum_min_range: 0.20
|
|
frustum_max_range: 3.5
|
|
```
|
|
|
|
Với `DepthCameraData`, cờ `frustum_clearing_enabled: true` chọn buffer depth
|
|
riêng. Clearing được thực hiện trực tiếp theo tia camera trong `VoxelLayer`, vì
|
|
vậy không cần đặt `clearing: true` cho source này.
|
|
|
|
Message `DepthCameraData` phải thỏa các điều kiện:
|
|
|
|
- Depth encoding là `16UC1`, `mono16` hoặc `32FC1`.
|
|
- `width`, `height`, `step` và kích thước `data` hợp lệ.
|
|
- `CameraInfo.K[0]` (`fx`) và `K[4]` (`fy`) lớn hơn `0`.
|
|
- Kích thước depth và camera info khớp nhau nếu camera info khai báo kích thước.
|
|
- Frame của depth và camera info không mâu thuẫn.
|
|
- Có TF từ optical frame của camera tới `global_frame` costmap.
|
|
|
|
### 4.2. Global costmap
|
|
|
|
```yaml
|
|
global_costmap:
|
|
library_path: libplugins
|
|
global_frame: map
|
|
robot_base_frame: base_link
|
|
update_frequency: 1.0
|
|
rolling_window: false
|
|
track_unknown_space: true
|
|
resolution: 0.05
|
|
|
|
plugins:
|
|
- {name: navigation_map, type: StaticLayer}
|
|
- {name: obstacles, type: VoxelLayer}
|
|
- {name: inflation, type: InflationLayer}
|
|
|
|
navigation_map:
|
|
enabled: true
|
|
map_topic: /map
|
|
track_unknown_space: true
|
|
trinary_costmap: true
|
|
lethal_cost_threshold: 100
|
|
|
|
obstacles:
|
|
enabled: true
|
|
footprint_clearing_enabled: true
|
|
origin_z: 0.0
|
|
z_resolution: 0.2
|
|
z_voxels: 16
|
|
unknown_threshold: 15
|
|
mark_threshold: 0
|
|
combination_method: 1
|
|
|
|
inflation:
|
|
enabled: true
|
|
inflate_unknown: false
|
|
inflation_radius: 0.60
|
|
cost_scaling_factor: 10.0
|
|
```
|
|
|
|
Khi dùng static map, kích thước, resolution và origin có thể được lấy từ
|
|
`OccupancyGrid`. Nếu tắt static map, phải khai báo `width`, `height`,
|
|
`resolution`, `origin_x` và `origin_y` hợp lệ.
|
|
|
|
### 4.3. Local costmap
|
|
|
|
```yaml
|
|
local_costmap:
|
|
library_path: libplugins
|
|
global_frame: odom
|
|
robot_base_frame: base_link
|
|
update_frequency: 6.0
|
|
rolling_window: true
|
|
track_unknown_space: false
|
|
width: 8.0
|
|
height: 8.0
|
|
resolution: 0.05
|
|
origin_x: 0.0
|
|
origin_y: 0.0
|
|
|
|
plugins:
|
|
- {name: obstacles, type: VoxelLayer}
|
|
- {name: inflation, type: InflationLayer}
|
|
|
|
obstacles:
|
|
enabled: true
|
|
footprint_clearing_enabled: true
|
|
origin_z: 0.0
|
|
z_resolution: 0.15
|
|
z_voxels: 8
|
|
unknown_threshold: 7
|
|
mark_threshold: 0
|
|
combination_method: 1
|
|
|
|
inflation:
|
|
enabled: true
|
|
inflate_unknown: false
|
|
inflation_radius: 0.55
|
|
cost_scaling_factor: 10.0
|
|
```
|
|
|
|
Với rolling window, code tự cập nhật origin theo pose robot; `origin_x` và
|
|
`origin_y` ban đầu không phải tâm cửa sổ cố định quanh robot.
|
|
|
|
## 5. Ý nghĩa và cách chỉnh từng nhóm tham số
|
|
|
|
### 5.1. Hình học và kích thước costmap
|
|
|
|
| Tham số | Đơn vị | Mặc định package | Ý nghĩa và cách chỉnh |
|
|
| --- | ---: | ---: | --- |
|
|
| `global_frame` | frame | `map` | Global costmap thường dùng `map`; local costmap thường dùng `odom`. |
|
|
| `robot_base_frame` | frame | `base_link` | Phải có TF ổn định từ frame này tới `global_frame`. |
|
|
| `rolling_window` | bool | `false` | Bật cho local costmap để cửa sổ đi theo robot. |
|
|
| `track_unknown_space` | bool | `false` | Bật khi planner cần phân biệt vùng chưa biết với vùng trống. |
|
|
| `width`, `height` | m | `0.0` | Kích thước cửa sổ. Tăng để nhìn xa hơn nhưng tăng CPU/RAM theo diện tích. |
|
|
| `resolution` | m/cell | `0.0` | Giảm để chi tiết hơn nhưng số cell tăng theo nghịch đảo bình phương. Giá trị chạy T800 thường là `0.05`. |
|
|
| `origin_x`, `origin_y` | m | `0.0` | Góc dưới trái của costmap không rolling. Với static map thường lấy từ map. |
|
|
| `update_frequency` | Hz | `1.0` | Tốc độ tính costmap. Local cần nhanh hơn global nhưng không nên cao hơn khả năng cấp dữ liệu/CPU. |
|
|
| `transform_tolerance` | s | `0.0` | Dung sai TF. Chỉ tăng vừa đủ cho jitter; không dùng để che lỗi timestamp hoặc TF bị mất. |
|
|
| `footprint_padding` | m | `0.0` | Biên an toàn cộng đều quanh footprint. |
|
|
| `robot_radius` | m | `0.0` | Dùng cho robot tròn. Với T800 dạng chữ nhật nên khai báo polygon `footprint`. |
|
|
|
|
Số cell 2D xấp xỉ:
|
|
|
|
```text
|
|
(width / resolution) * (height / resolution)
|
|
```
|
|
|
|
Ví dụ cửa sổ `8 m x 8 m`, resolution `0.05 m` có `160 x 160 = 25,600`
|
|
cell. Nếu dùng `16` lớp voxel thì phần voxel có khoảng `409,600` ô.
|
|
|
|
### 5.2. Footprint
|
|
|
|
`footprint` là polygon theo mét trong `robot_base_frame`. Đây là tham số an toàn,
|
|
phải đo theo kích thước ngoài cùng thực tế của robot và tải hàng, không đo theo
|
|
khung chassis bên trong.
|
|
|
|
Quy trình chỉnh:
|
|
|
|
1. Đo khoảng cách từ tâm `robot_base_frame` tới mép trước, sau, trái, phải.
|
|
2. Khai báo các đỉnh theo thứ tự quanh polygon, không tự cắt nhau.
|
|
3. Kiểm tra pose quay tại chỗ gần tường và góc kệ.
|
|
4. Chỉ dùng `footprint_padding` cho sai số nhỏ; không dùng padding để bù một
|
|
footprint sai lớn.
|
|
|
|
### 5.3. StaticLayer
|
|
|
|
| Tham số | Mặc định | Ý nghĩa và cách chỉnh |
|
|
| --- | ---: | --- |
|
|
| `enabled` | `true` | Bật/tắt layer. |
|
|
| `map_topic` | `map` | Topic `OccupancyGrid`. |
|
|
| `first_map_only` | `false` | `true` nếu map không thay đổi và muốn bỏ các map gửi lại. |
|
|
| `subscribe_to_updates` | `false` | Bật nếu map server gửi `OccupancyGridUpdate`. |
|
|
| `track_unknown_space` | `true` | Giữ ô `unknown_cost_value` là `NO_INFORMATION`; tắt để coi unknown là free. |
|
|
| `use_maximum` | `false` | `false`: overwrite master; `true`: lấy max để không làm mất cost đã có. |
|
|
| `lethal_cost_threshold` | `100` | Occupancy value từ ngưỡng này trở lên được coi là lethal; code clamp trong `[0, 100]`. |
|
|
| `unknown_cost_value` | `-1` | Giá trị unknown trong map đầu vào. |
|
|
| `trinary_costmap` | `true` | Chỉ phân loại free/lethal/unknown; tắt để scale dải occupancy thành cost. |
|
|
| `base_frame_id` | `map` | Frame dùng bởi layer map trong implementation T800. |
|
|
|
|
### 5.4. Observation source của ObstacleLayer/VoxelLayer
|
|
|
|
`observation_sources` là chuỗi các tên source cách nhau bằng khoảng trắng, ví
|
|
dụ `b_scan pc_marking pc_clearing`. Mỗi tên phải có một map tham số cùng tên.
|
|
|
|
| Tham số | Mặc định code | Ý nghĩa và cách chỉnh |
|
|
| --- | ---: | --- |
|
|
| `topic` | `map` | Topic input. Đây cũng là key dispatch callback, phải khớp tuyệt đối. |
|
|
| `data_type` | `PointCloud` | Một trong `LaserScan`, `PointCloud`, `PointCloud2`, `DepthCameraData`. |
|
|
| `sensor_frame` | rỗng | Để rỗng để dùng `header.frame_id`; chỉ đặt khi cần ép origin của sensor. |
|
|
| `marking` | `true` | Đánh dấu điểm quan sát thành vật cản. |
|
|
| `clearing` | `false` | Raytrace PointCloud/LaserScan để xóa vùng trống. Không điều khiển depth-frustum clearing. |
|
|
| `inf_is_valid` | `false` | Với `LaserScan`, coi `+Inf` là tia không gặp vật cản để clearing. Không áp dụng cho point cloud. |
|
|
| `observation_persistence` | `0.0 s` | `0`: chỉ giữ mẫu mới nhất; tăng khi sensor thưa nhưng có thể tạo ghost obstacle. |
|
|
| `expected_update_rate` | `0.0 s` | Khoảng thời gian cập nhật mong đợi; `0`: không kiểm tra stale. Trong code đây là duration, không phải Hz. |
|
|
| `min_obstacle_height` | `0.0 m` | Bỏ điểm thấp hơn ngưỡng, hữu ích để lọc sàn. |
|
|
| `max_obstacle_height` | `2.0 m` | Bỏ điểm cao hơn ngưỡng. Phải phù hợp chiều cao robot/kệ và dải z của voxel. |
|
|
| `obstacle_range` | `2.5 m` | Khoảng cách tối đa dùng để marking. |
|
|
| `raytrace_range` | `3.0 m` | Khoảng cách tối đa dùng để clearing. Thường đặt lớn hơn `obstacle_range`. |
|
|
|
|
Lưu ý `expected_update_rate` được truyền vào `robot::Duration`. Ví dụ `0.5`
|
|
nghĩa là kỳ vọng có dữ liệu ít nhất mỗi `0.5 s`, tương đương tối thiểu `2 Hz`.
|
|
|
|
### 5.5. VoxelLayer
|
|
|
|
| Tham số | Mặc định YAML | Ý nghĩa và cách chỉnh |
|
|
| --- | ---: | --- |
|
|
| `enabled` | `true` | Bật/tắt layer. |
|
|
| `origin_z` | `0.0 m` | Đáy của voxel grid trong hệ tọa độ costmap. |
|
|
| `z_resolution` | `0.2 m` | Chiều cao mỗi voxel. Giảm để phân giải z tốt hơn nhưng dễ nhiễu và tốn xử lý hơn. |
|
|
| `z_voxels` | `16` | Số lớp z; implementation dùng tối đa 16 bit cho mỗi cột, nên giữ trong `1..16`. |
|
|
| `max_obstacle_height` | `3.0 m` | Trần điểm hợp lệ của layer. |
|
|
| `unknown_threshold` | `15` | Số voxel unknown cần để cột 2D còn unknown; cần chỉnh cùng `z_voxels`. |
|
|
| `mark_threshold` | `0` | Số voxel marked cần để cột 2D thành vật cản. Tăng nếu một điểm nhiễu đơn lẻ thường tạo vật cản giả. |
|
|
| `combination_method` | `1` | `0`: overwrite master, `1`: lấy maximum. Thường dùng `1` để không xóa cost layer trước. |
|
|
| `footprint_clearing_enabled` | `true` | Xóa vật cản nằm trong footprint hiện tại của robot. |
|
|
|
|
Dải z của voxel xấp xỉ:
|
|
|
|
```text
|
|
[origin_z, origin_z + z_resolution * z_voxels)
|
|
```
|
|
|
|
Dải này phải bao phủ vùng `min_obstacle_height..max_obstacle_height` mà robot
|
|
cần quan sát. Không tăng `max_obstacle_height` vượt khỏi voxel grid mà không
|
|
đồng thời kiểm tra `origin_z`, `z_resolution` và `z_voxels`.
|
|
|
|
### 5.6. Depth frustum clearing
|
|
|
|
| Tham số | Mặc định code | Ý nghĩa và cách chỉnh |
|
|
| --- | ---: | --- |
|
|
| `frustum_clearing_enabled` | `false` | Chọn đường clearing trực tiếp từ `DepthCameraData`. |
|
|
| `frustum_clearing_pixel_step` | `8 px` | Lấy một tia mỗi N pixel theo cả hai chiều. Tăng để giảm CPU, giảm để clear dày hơn. Code clamp tối thiểu là `1`. |
|
|
| `frustum_min_range` | `0.20 m` | Không clear vùng quá gần camera, nơi depth thường không đáng tin. |
|
|
| `frustum_max_range` | `3.0 m` | Chiều dài ray tối đa khi pixel không có depth hợp lệ hoặc depth ở xa. |
|
|
|
|
Mỗi pixel được sample tạo một tia từ camera. Với depth hợp lệ, ray dừng trước
|
|
điểm đo khoảng `2 * resolution` để không xóa chính vật cản. Với pixel không hợp
|
|
lệ, ray có thể clear tới `frustum_max_range`; vì vậy không đặt range vượt vùng
|
|
camera thực sự đáng tin.
|
|
|
|
Gợi ý tuning:
|
|
|
|
- Bắt đầu với `pixel_step: 8`.
|
|
- Nếu còn các dải ghost obstacle mỏng giữa các tia, thử `6`, rồi `4`.
|
|
- Nếu CPU cao, thử `10`, `12` hoặc giảm `frustum_max_range`.
|
|
- `frustum_min_range` nên lớn hơn hoặc bằng khoảng mù gần của camera.
|
|
- `frustum_max_range` nên nhỉnh hơn `pc_marking.obstacle_range`, nhưng không
|
|
vượt quá range depth ổn định trong môi trường thực tế.
|
|
|
|
### 5.7. InflationLayer
|
|
|
|
| Tham số | Mặc định | Ý nghĩa và cách chỉnh |
|
|
| --- | ---: | --- |
|
|
| `enabled` | `true` | Bật/tắt inflation. |
|
|
| `inflation_radius` | `0.55 m` | Bán kính tối đa có cost quanh vật cản. Tăng để robot tránh xa hơn. |
|
|
| `cost_scaling_factor` | `15.0` | Hệ số suy giảm mũ. **Tăng** giá trị làm cost giảm nhanh hơn và vùng cost mạnh hẹp hơn; **giảm** giá trị làm robot giữ khoảng cách mềm xa hơn. |
|
|
| `inflate_unknown` | `false` | Có inflation vùng unknown hay không. Bật có thể làm planner thận trọng hơn nhưng dễ chặn đường trong map chưa hoàn chỉnh. |
|
|
|
|
`inflation_radius` phải được chọn sau khi footprint đúng. Bán kính này nên lớn
|
|
hơn inscribed radius cộng biên an toàn mong muốn; tăng radius không thể sửa một
|
|
footprint sai.
|
|
|
|
### 5.8. Performance metrics
|
|
|
|
| Tham số | Mặc định | Ý nghĩa |
|
|
| --- | ---: | --- |
|
|
| `performance_metrics_enabled` | `false` | Log thời gian `updateBounds` và `updateCosts` theo từng layer. |
|
|
| `performance_metrics_period` | `5.0 s` | Chu kỳ tổng hợp và in metrics. |
|
|
|
|
Bật metrics trong lúc tuning CPU, sau đó có thể tắt để giảm log runtime.
|
|
|
|
## 6. Quy trình tuning khuyến nghị
|
|
|
|
Chỉ thay một nhóm tham số mỗi lần và lưu lại bag/log trước khi chỉnh.
|
|
|
|
1. **Kiểm tra TF và timestamp**: phải có transform liên tục từ từng sensor frame
|
|
tới `map`/`odom`. Không tuning costmap khi TF còn lỗi.
|
|
2. **Chốt footprint**: đo robot và tải hàng thật, kiểm tra quay tại chỗ.
|
|
3. **Chọn resolution và kích thước cửa sổ**: bắt đầu `0.05 m`; local thường
|
|
`6..10 m` tùy vận tốc và khoảng phanh.
|
|
4. **Chỉ bật marking**: xác nhận vật cản xuất hiện đúng vị trí, đúng height và
|
|
range.
|
|
5. **Bật clearing**: laser/point cloud dùng raytrace; depth camera dùng source
|
|
`DepthCameraData` với frustum clearing.
|
|
6. **Chỉnh voxel**: đặt dải z, sau đó tăng `mark_threshold` nếu nhiễu đơn điểm.
|
|
7. **Chỉnh inflation**: chỉnh `inflation_radius` trước, sau đó mới chỉnh
|
|
`cost_scaling_factor` theo khoảng cách đường đi mong muốn.
|
|
8. **Đo tải CPU**: bật performance metrics; chỉ tăng frequency hoặc giảm
|
|
resolution khi chu kỳ cập nhật vẫn hoàn thành ổn định.
|
|
|
|
Các ràng buộc nên giữ:
|
|
|
|
```text
|
|
resolution > 0
|
|
width > 0 và height > 0 nếu không lấy size từ static map
|
|
raytrace_range >= obstacle_range
|
|
frustum_max_range >= frustum_min_range >= 0
|
|
1 <= z_voxels <= 16
|
|
origin_z + z_resolution * z_voxels đủ bao phủ dải vật cản cần quan sát
|
|
InflationLayer nằm sau các layer tạo vật cản
|
|
```
|
|
|
|
## 7. Tuning theo triệu chứng
|
|
|
|
| Triệu chứng | Kiểm tra trước | Hướng chỉnh |
|
|
| --- | --- | --- |
|
|
| Vật cản đã đi nhưng vẫn còn trên costmap | TF, topic clearing, dữ liệu có còn cập nhật | Bật đúng `clearing`; với depth dùng `DepthCameraData` + `frustum_clearing_enabled`; giảm `observation_persistence`; giảm `pixel_step` nếu còn khe giữa tia. |
|
|
| Vật cản thật không được đánh dấu | Topic/type/frame, range và height | Kiểm tra `marking`, `min/max_obstacle_height`, `obstacle_range`; với voxel thử `mark_threshold: 0` trước. |
|
|
| Vật cản chớp tắt | Tần số sensor, packet drop, TF | Đặt `expected_update_rate` đúng chu kỳ; tăng nhẹ `observation_persistence` nhưng phải kiểm tra ghost obstacle. |
|
|
| Robot đi quá sát vật cản | Footprint trước, inflation sau | Tăng `inflation_radius` hoặc giảm `cost_scaling_factor`. |
|
|
| Robot tránh quá xa/không tìm được đường | Footprint, unknown space, inflation | Giảm `inflation_radius` hoặc tăng `cost_scaling_factor`; kiểm tra `inflate_unknown`. |
|
|
| Costmap local trễ hoặc CPU cao | Metrics theo layer | Tăng `resolution`, giảm `width/height`, giảm `update_frequency`, tăng depth `pixel_step`, giảm range hoặc giảm mật độ `/camera/depth/points_proc`. |
|
|
| Costmap báo stale/not current | Sensor thực tế có đúng chu kỳ không | Tăng giá trị `expected_update_rate` theo đơn vị giây hoặc đặt `0` để tắt kiểm tra trong lúc chẩn đoán. |
|
|
| Clearing xóa xuyên vật cản depth | Depth invalid, range quá lớn, TF camera | Giảm `frustum_max_range`, tăng `frustum_min_range`, kiểm tra encoding/calibration và đảm bảo point-cloud marking hoạt động. |
|
|
| Sensor origin nằm ngoài voxel map | `global_frame`, rolling window, TF z | Sửa TF/origin, tăng cửa sổ phù hợp; không chỉ tăng tolerance. |
|
|
| Vật cản sàn/nhiễu thấp xuất hiện | Height filter và calibration | Tăng `min_obstacle_height` từng bước nhỏ; không tăng quá đáy vật cản robot cần tránh. |
|
|
|
|
## 8. Lưu ý riêng cho depth camera T800
|
|
|
|
- Marking và clearing có contract khác nhau: `/camera/depth/points_proc`
|
|
(`PointCloud2`) dùng cho marking/persistence; `/camera/depth/data`
|
|
(`DepthCameraData`) dùng cho frustum clearing.
|
|
- Không cấu hình cùng một full point cloud để vừa marking vừa clearing nếu mục
|
|
tiêu là giảm tải. Đường frustum dùng raw depth semantics và sampling theo
|
|
pixel, tránh xử lý toàn bộ cloud thêm lần nữa.
|
|
- Mỗi camera nên có cặp source riêng, ví dụ `pc0_marking pc0_clearing
|
|
pc1_marking pc1_clearing`, với topic, frame, range và pixel step riêng.
|
|
- `observation_persistence: 0.0` giữ mẫu mới nhất. Chỉ tăng khi đã đo được tần
|
|
suất sensor và hiểu rõ thời gian ghost obstacle chấp nhận được.
|
|
- Khi mất marking, kiểm tra lần lượt output sau bước chuyển depth thành
|
|
`/camera/depth/points_proc`, TF sang costmap frame và buffer của
|
|
`ObstacleLayer` trước khi chỉnh `mark_threshold`.
|
|
|
|
## 9. Build và kiểm tra
|
|
|
|
Build package trong workspace:
|
|
|
|
```bash
|
|
cd /home/duongtd/T800_ws
|
|
catkin_make --pkg robot_costmap_2d
|
|
```
|
|
|
|
Các executable test được tạo khi `BUILD_COSTMAP_TESTS=ON`:
|
|
|
|
```bash
|
|
./devel/lib/robot_costmap_2d/test_array_parser
|
|
./devel/lib/robot_costmap_2d/test_costmap
|
|
./devel/lib/robot_costmap_2d/test_plugin
|
|
```
|
|
|
|
Kiểm tra tối thiểu trước khi chạy robot:
|
|
|
|
- YAML parse được và đúng namespace global/local.
|
|
- `library_path` tìm thấy `libplugins`.
|
|
- Plugin được tạo đúng tên và đúng thứ tự.
|
|
- TF giữa sensor, `robot_base_frame` và `global_frame` sẵn sàng.
|
|
- Sensor topic, `data_type` và `header.frame_id` khớp cấu hình.
|
|
- Costmap update ổn định, không stale và không vượt ngân sách chu kỳ.
|
|
- Footprint và inflation đã được kiểm tra ở tốc độ thấp trước.
|
|
|
|
## 10. Cấu trúc package
|
|
|
|
```text
|
|
costmap_2d/
|
|
├── config/ # Fallback YAML của package
|
|
├── include/robot_costmap_2d/ # Public headers
|
|
├── plugins/ # Layer implementations
|
|
├── src/ # Costmap core và observation buffer
|
|
├── test/ # Unit/integration tests
|
|
├── CMakeLists.txt
|
|
└── package.xml
|
|
```
|