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:

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:

updateBounds() để từng layer mở rộng vùng cần cập nhật.
Reset vùng tương ứng trên master costmap.
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:

Giá trị fallback trong code khi một key không tồn tại trong YAML.
Các file YAML tên cố định nằm dưới thư mục config/.
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:

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:

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:

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ở:

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:

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:

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

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

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

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ỉ:

(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:

Đo khoảng cách từ tâm robot_base_frame tới mép trước, sau, trái, phải.
Khai báo các đỉnh theo thứ tự quanh polygon, không tự cắt nhau.
Kiểm tra pose quay tại chỗ gần tường và góc kệ.
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ỉ:

[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.

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.
Chốt footprint: đo robot và tải hàng thật, kiểm tra quay tại chỗ.
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.
Chỉ bật marking: xác nhận vật cản xuất hiện đúng vị trí, đúng height và range.
Bật clearing: laser/point cloud dùng raytrace; depth camera dùng source DepthCameraData với frustum clearing.
Chỉnh voxel: đặt dải z, sau đó tăng mark_threshold nếu nhiễu đơn điểm.
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.
Đ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ữ:

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:

cd /home/duongtd/T800_ws
catkin_make --pkg robot_costmap_2d

Các executable test được tạo khi BUILD_COSTMAP_TESTS=ON:

./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

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