diff --git a/README.md b/README.md
new file mode 100644
index 0000000..233e8aa
--- /dev/null
+++ b/README.md
@@ -0,0 +1,636 @@
+# 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
+```