optimal voxel layer

add file readme
2026-07-23 11:39:03 +07:00 · 2026-07-23 11:18:04 +07:00 · 2026-07-14 11:37:35 +07:00 · 2026-07-14 11:08:23 +07:00 · 2026-07-14 09:42:35 +07:00 · 2026-07-10 11:27:20 +07:00
26 changed files with 2323 additions and 340 deletions
--- a/CMakeLists.txt
+++ b/CMakeLists.txt
@@ -49,7 +49,7 @@ if (NOT BUILDING_WITH_CATKIN)
    robot_cpp
    robot_time
  )
-
+  find_library(TF3_LIBRARY NAMES tf3 PATHS /usr/lib /usr/local/lib /usr/lib/x86_64-linux-gnu)
 else()

 # ========================================================
@@ -64,7 +64,6 @@ else()
    robot_laser_geometry
    robot_visualization_msgs
    robot_voxel_grid
-    tf3
    robot_tf3_geometry_msgs
    robot_tf3_sensor_msgs
    data_convert
@@ -73,10 +72,12 @@ else()
    robot_time
  )

+  find_library(TF3_LIBRARY NAMES tf3)
+  
  catkin_package(
    INCLUDE_DIRS include
    LIBRARIES robot_costmap_2d plugins
-    CATKIN_DEPENDS robot_std_msgs robot_sensor_msgs geometry_msgs robot_nav_msgs robot_map_msgs robot_laser_geometry robot_visualization_msgs robot_voxel_grid tf3 robot_tf3_geometry_msgs robot_tf3_sensor_msgs data_convert robot_xmlrpcpp robot_cpp robot_time
+    CATKIN_DEPENDS robot_std_msgs robot_sensor_msgs geometry_msgs robot_nav_msgs robot_map_msgs robot_laser_geometry robot_visualization_msgs robot_voxel_grid robot_tf3_geometry_msgs robot_tf3_sensor_msgs data_convert robot_xmlrpcpp robot_cpp robot_time
    DEPENDS  PCL Boost 
  )

@@ -87,6 +88,7 @@ else()
    ${Boost_INCLUDE_DIRS}
    ${GTEST_INCLUDE_DIRS}
    ${PCL_INCLUDE_DIRS}
+    ${TF3_INCLUDE_DIR}
  )
  link_directories(${PCL_LIBRARY_DIRS})
 endif()
@@ -122,6 +124,7 @@ if(BUILDING_WITH_CATKIN)
      ${EIGEN3_INCLUDE_DIRS}
      ${Boost_INCLUDE_DIRS}
      ${PCL_INCLUDE_DIRS}
+      ${TF3_INCLUDE_DIR}
  )

  target_link_libraries(robot_costmap_2d
@@ -130,6 +133,7 @@ if(BUILDING_WITH_CATKIN)
    PRIVATE yaml-cpp
    PRIVATE dl
    PRIVATE ${PCL_LIBRARIES}
+    PRIVATE ${TF3_LIBRARY}
  )

 else()
@@ -141,6 +145,7 @@ else()
      ${EIGEN3_INCLUDE_DIRS}
      ${Boost_INCLUDE_DIRS}
      ${PCL_INCLUDE_DIRS}
+      ${TF3_INCLUDE_DIR}
  )

  target_link_libraries(robot_costmap_2d
@@ -151,6 +156,7 @@ else()
      yaml-cpp
      dl
      ${PCL_LIBRARIES}
+      ${TF3_LIBRARY}
  )

  set_target_properties(robot_costmap_2d PROPERTIES
@@ -189,6 +195,7 @@ if(BUILDING_WITH_CATKIN)
    PRIVATE ${catkin_LIBRARIES}
    PRIVATE Boost::boost Boost::system Boost::thread Boost::filesystem
    PRIVATE yaml-cpp
+    PRIVATE ${TF3_LIBRARY}
  )

 else()
@@ -205,6 +212,7 @@ else()
    PRIVATE yaml-cpp
    PRIVATE robot_time
    PRIVATE robot_cpp
+    PRIVATE ${TF3_LIBRARY}
  )

  set_target_properties(plugins PROPERTIES
@@ -273,23 +281,30 @@ endif()
 option(BUILD_COSTMAP_TESTS "Build robot_costmap_2d test executables" ON)

 if(BUILD_COSTMAP_TESTS)
+  find_package(GTest REQUIRED)
+  find_package(Boost REQUIRED COMPONENTS system thread filesystem)
+  find_library(TF3_LIBRARY NAMES tf3 PATHS /usr/lib /usr/local/lib /usr/lib/x86_64-linux-gnu)
+
  if(EXISTS ${CMAKE_CURRENT_SOURCE_DIR}/test/array_parser_test.cpp)
    add_executable(test_array_parser test/array_parser_test.cpp)
-    target_link_libraries(test_array_parser PRIVATE
+    target_link_libraries(test_array_parser PRIVATE 
      robot_costmap_2d
      GTest::GTest
      GTest::Main
-      Threads::Threads
+      Boost::system Boost::thread
+      ${TF3_LIBRARY}
    )
  endif()

  if(EXISTS ${CMAKE_CURRENT_SOURCE_DIR}/test/coordinates_test.cpp)
    add_executable(test_costmap test/coordinates_test.cpp)
    target_link_libraries(test_costmap PRIVATE
+      plugins
      robot_costmap_2d
      GTest::GTest
      GTest::Main
-      Threads::Threads
+      Boost::system Boost::thread
+      ${TF3_LIBRARY}
    )
  endif()

@@ -300,11 +315,11 @@ if(BUILD_COSTMAP_TESTS)
      Boost::boost Boost::filesystem Boost::system
      yaml-cpp
      dl
-      Threads::Threads
-      tf3
+      Boost::system Boost::thread
      robot_time
      GTest::GTest
      GTest::Main
+      ${TF3_LIBRARY}
    )
  endif()
 endif()
--- a/README.md
+++ 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
+```
--- a/config/costmap_params.yaml
+++ b/config/costmap_params.yaml
@@ -25,6 +25,8 @@ robot_costmap_2d:
    - [-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
@@ -33,4 +35,4 @@ robot_costmap_2d:
  origin_y: 0.0

  footprint_padding: 0.0
-  robot_radius: 0.0
+  robot_radius: 0.0
--- a/config/voxel_layer_params.yaml
+++ b/config/voxel_layer_params.yaml
@@ -8,4 +8,8 @@ voxel_layer:
  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
--- a/include/robot_costmap_2d/costmap_2d.h
+++ b/include/robot_costmap_2d/costmap_2d.h
@@ -425,6 +425,7 @@ protected:
  double origin_y_;
  unsigned char* costmap_;
  unsigned char default_value_;
+  std::vector<unsigned char> rolling_window_scratch_;

  class MarkCell
  {
--- a/include/robot_costmap_2d/costmap_2d_robot.h
+++ b/include/robot_costmap_2d/costmap_2d_robot.h
@@ -47,14 +47,13 @@
 #include <robot_geometry_msgs/PoseStamped.h>

 #include <tf3/LinearMath/Transform.h>
-#include <robot/rate.h>
+#include <robot/robot.h>
 #include <data_convert/data_convert.h>
 #include <robot_tf3_geometry_msgs/tf3_geometry_msgs.h>

 #include <robot_xmlrpcpp/XmlRpcValue.h>

-#include <robot/node_handle.h>
-#include <robot/plugin_loader_helper.h>
+

 class RobotSuperValue : public robot_xmlrpcpp::XmlRpcValue
 {
@@ -199,13 +198,19 @@ public:
    return padded_footprint_;
  }

+  inline const robot_geometry_msgs::PolygonStamped& getRobotFootprintPolygonStamped() const noexcept
+  {
+    return footprint_;
+  }
+
  /** @brief Return the current unpadded footprint of the robot as a vector of points.
   *
   * This is the raw version of the footprint without padding.
   *
   * The footprint initially comes from the rosparam "footprint" but
   * can be overwritten by dynamic reconfigure or by messages received
-   * on the "footprint" topic. */
+   * on the "footprint" topic. 
+   */
  inline const std::vector<robot_geometry_msgs::Point>& getUnpaddedRobotFootprint() const noexcept
  {
    return unpadded_footprint_;
@@ -250,6 +255,7 @@ protected:
  double transform_tolerance_;  ///< timeout before transform errors

 private:
+  void copyParentParameters(const std::string& costmap_name, const std::string& plugin_name, const std::string& plugin_type, robot::NodeHandle& nh);
  /** @brief Set the footprint from the new_config object.
   *
   * If the values of footprint and robot_radius are the same in
@@ -270,10 +276,11 @@ private:

  std::vector<robot_geometry_msgs::Point> unpadded_footprint_;
  std::vector<robot_geometry_msgs::Point> padded_footprint_;
+  robot_geometry_msgs::PolygonStamped footprint_;
  float footprint_padding_;

 private:
-  void getParams(const std::string& config_file_name, robot::NodeHandle& nh);
+  void getParams(const std::string& config_file_name,const std::string& name, robot::NodeHandle& nh);
 };
 // class Costmap2DROBOT
 }  // namespace robot_costmap_2d
--- a/include/robot_costmap_2d/footprint.h
+++ b/include/robot_costmap_2d/footprint.h
@@ -43,7 +43,7 @@
 #include <robot_geometry_msgs/Point.h>
 #include <robot_geometry_msgs/Point32.h>

-#include <robot/node_handle.h>
+#include <robot/robot.h>
 #include <robot_xmlrpcpp/XmlRpcValue.h> 

 namespace robot_costmap_2d
--- a/include/robot_costmap_2d/inflation_layer.h
+++ b/include/robot_costmap_2d/inflation_layer.h
@@ -42,6 +42,9 @@
 #include <robot_costmap_2d/layered_costmap.h>
 #include <boost/thread.hpp>

+#include <cstdint>
+#include <vector>
+
 namespace robot_costmap_2d
 {
 /**
@@ -77,8 +80,7 @@ public:
  virtual ~InflationLayer()
  {
    deleteKernels();
-    if (seen_)
-        delete[] seen_;
+    delete inflation_access_;
  }

  virtual void onInitialize();
@@ -96,8 +98,9 @@ public:
  /** @brief  Given a distance, compute a cost.
   * @param  distance The distance from an obstacle in cells
   * @return A cost value for the distance */
-  virtual inline unsigned char computeCost(double distance) const
+  virtual unsigned char computeCost(double distance) const override
  {
+    // robot::log_warning("InflationLayer::computeCost() is deprecated. Please use costLookup() instead.");
    unsigned char cost = 0;
    if (distance == 0)
      cost = LETHAL_OBSTACLE;
@@ -183,10 +186,13 @@ private:

  unsigned int cell_inflation_radius_;
  unsigned int cached_cell_inflation_radius_;
-  std::map<double, std::vector<CellData> > inflation_cells_;
+  std::vector<std::vector<CellData>> inflation_cells_;
+  std::vector<double> distance_levels_;
+  std::vector<unsigned int> distance_bin_lookup_;
+  unsigned int distance_lookup_size_ = 0;

-  bool* seen_;
-  int seen_size_;
+  std::vector<std::uint32_t> seen_;
+  std::uint32_t seen_generation_ = 0;

  unsigned char** cached_costs_;
  double** cached_distances_;
--- a/include/robot_costmap_2d/layer.h
+++ b/include/robot_costmap_2d/layer.h
@@ -42,7 +42,7 @@
 #include <robot_costmap_2d/utils.h>
 #include <string>
 #include <tf3/buffer_core.h>
-#include <robot/node_handle.h>
+#include <robot/robot.h>
 namespace robot_costmap_2d
 {

@@ -85,6 +85,8 @@ public:
   */
  virtual void updateCosts(Costmap2D& master_grid, int min_i, int min_j, int max_i, int max_j) {}

+  virtual unsigned char computeCost(double distance) const { throw std::runtime_error("Function computeCost is not Support."); };
+
  /** @brief Stop publishers. */
  virtual void deactivate() {}

--- a/include/robot_costmap_2d/layered_costmap.h
+++ b/include/robot_costmap_2d/layered_costmap.h
@@ -43,6 +43,8 @@
 #include <robot_costmap_2d/costmap_2d.h>
 #include <vector>
 #include <string>
+#include <chrono>
+#include <cstdint>

 namespace robot_costmap_2d
 {
@@ -71,6 +73,8 @@ public:
   */
  void updateMap(double robot_x, double robot_y, double robot_yaw);

+  void setPerformanceMetrics(bool enabled, double reporting_period_seconds);
+
  inline const std::string& getGlobalFrameID() const noexcept
  {
    return global_frame_;
@@ -155,6 +159,17 @@ public:
  double getInscribedRadius() { return inscribed_radius_; }

 private:
+  struct LayerPerformance
+  {
+    std::uint64_t bounds_nanoseconds = 0;
+    std::uint64_t costs_nanoseconds = 0;
+    std::uint64_t bounds_calls = 0;
+    std::uint64_t costs_calls = 0;
+  };
+
+  void resetPerformanceMetrics();
+  void maybeReportPerformance();
+
  Costmap2D costmap_;
  std::string global_frame_;

@@ -170,6 +185,15 @@ private:
  bool size_locked_;
  double circumscribed_radius_, inscribed_radius_;
  std::vector<robot_geometry_msgs::Point> footprint_;
+
+  bool performance_metrics_enabled_ = false;
+  double performance_metrics_period_seconds_ = 5.0;
+  std::chrono::steady_clock::time_point performance_window_start_;
+  std::uint64_t performance_cycle_nanoseconds_ = 0;
+  std::uint64_t performance_reset_nanoseconds_ = 0;
+  std::uint64_t performance_cycles_ = 0;
+  std::vector<std::uint64_t> performance_cycle_samples_;
+  std::vector<LayerPerformance> layer_performance_;
 };

 }  // namespace robot_costmap_2d
--- a/include/robot_costmap_2d/observation.h
+++ b/include/robot_costmap_2d/observation.h
@@ -34,10 +34,140 @@

 #include <robot_geometry_msgs/Point.h>
 #include <robot_sensor_msgs/PointCloud2.h>
+#include <robot_sensor_msgs/DepthCameraData.h>
+#include <boost/make_shared.hpp>
+#include <boost/shared_ptr.hpp>
+#include <utility>

 namespace robot_costmap_2d
 {

+/**
+ * @brief A depth frame and its per-source frustum-clearing configuration.
+ *
+ * The message is shared so returning buffered observations does not copy the
+ * full depth image on every costmap update.
+ */
+class DepthCameraObservation
+{
+public:
+  DepthCameraObservation()
+    : data_handle_(),
+      data_(nullptr),
+      topic_(),
+      pixel_step_(0),
+      min_range_(0.0),
+      max_range_(0.0)
+  {
+  }
+
+  DepthCameraObservation(
+      const robot_sensor_msgs::DepthCameraData& data,
+      std::string topic,
+      const robot::Time& received_time,
+      unsigned int pixel_step,
+      double min_range,
+      double max_range)
+    : data_handle_(boost::make_shared<robot_sensor_msgs::DepthCameraData>(data)),
+      data_(data_handle_.get()),
+      topic_(std::move(topic)),
+      received_time_(received_time),
+      pixel_step_(pixel_step),
+      min_range_(min_range),
+      max_range_(max_range)
+  {
+  }
+
+  DepthCameraObservation(
+      robot_sensor_msgs::DepthCameraData::ConstPtr data,
+      std::string topic,
+      const robot::Time& received_time,
+      unsigned int pixel_step,
+      double min_range,
+      double max_range)
+    : data_handle_(std::move(data)),
+      data_(data_handle_.get()),
+      topic_(std::move(topic)),
+      received_time_(received_time),
+      pixel_step_(pixel_step),
+      min_range_(min_range),
+      max_range_(max_range)
+  {
+  }
+
+  DepthCameraObservation(const DepthCameraObservation& other)
+    : data_handle_(other.data_handle_),
+      data_(data_handle_.get()),
+      topic_(other.topic_),
+      received_time_(other.received_time_),
+      pixel_step_(other.pixel_step_),
+      min_range_(other.min_range_),
+      max_range_(other.max_range_)
+  {
+  }
+
+  DepthCameraObservation(DepthCameraObservation&& other) noexcept
+    : data_handle_(std::move(other.data_handle_)),
+      data_(data_handle_.get()),
+      topic_(std::move(other.topic_)),
+      received_time_(other.received_time_),
+      pixel_step_(other.pixel_step_),
+      min_range_(other.min_range_),
+      max_range_(other.max_range_)
+  {
+    other.data_ = nullptr;
+    other.pixel_step_ = 0;
+    other.min_range_ = 0.0;
+    other.max_range_ = 0.0;
+  }
+
+  DepthCameraObservation& operator=(const DepthCameraObservation& other)
+  {
+    if (this == &other)
+      return *this;
+
+    data_handle_ = other.data_handle_;
+    data_ = data_handle_.get();
+    topic_ = other.topic_;
+    received_time_ = other.received_time_;
+    pixel_step_ = other.pixel_step_;
+    min_range_ = other.min_range_;
+    max_range_ = other.max_range_;
+    return *this;
+  }
+
+  DepthCameraObservation& operator=(DepthCameraObservation&& other) noexcept
+  {
+    if (this == &other)
+      return *this;
+
+    data_handle_ = std::move(other.data_handle_);
+    data_ = data_handle_.get();
+    topic_ = std::move(other.topic_);
+    received_time_ = other.received_time_;
+    pixel_step_ = other.pixel_step_;
+    min_range_ = other.min_range_;
+    max_range_ = other.max_range_;
+
+    other.data_ = nullptr;
+    other.pixel_step_ = 0;
+    other.min_range_ = 0.0;
+    other.max_range_ = 0.0;
+
+    return *this;
+  }
+
+  ~DepthCameraObservation() = default;
+
+  robot_sensor_msgs::DepthCameraData::ConstPtr data_handle_;
+  const robot_sensor_msgs::DepthCameraData* data_;
+  std::string topic_;
+  robot::Time received_time_;
+  unsigned int pixel_step_;
+  double min_range_;
+  double max_range_;
+};
+
 /**
 * @brief Stores an observation in terms of a point cloud and the origin of the source
 * @note Tried to make members and constructor arguments const but the compiler would not accept the default
@@ -50,14 +180,12 @@ public:
   * @brief  Creates an empty observation
   */
  Observation() :
-    cloud_(new robot_sensor_msgs::PointCloud2()), obstacle_range_(0.0), raytrace_range_(0.0)
+    cloud_handle_(boost::make_shared<robot_sensor_msgs::PointCloud2>()),
+    cloud_(cloud_handle_.get()), obstacle_range_(0.0), raytrace_range_(0.0)
  {
  }

-  virtual ~Observation()
-  {
-    delete cloud_;
-  }
+  virtual ~Observation() = default;

  /**
   * @brief  Creates an observation from an origin point and a point cloud
@@ -68,7 +196,17 @@ public:
   */
  Observation(robot_geometry_msgs::Point& origin, const robot_sensor_msgs::PointCloud2 &cloud,
              double obstacle_range, double raytrace_range) :
-      origin_(origin), cloud_(new robot_sensor_msgs::PointCloud2(cloud)),
+      origin_(origin), cloud_handle_(boost::make_shared<robot_sensor_msgs::PointCloud2>(cloud)),
+      cloud_(cloud_handle_.get()),
+      obstacle_range_(obstacle_range), raytrace_range_(raytrace_range)
+  {
+  }
+
+  Observation(robot_geometry_msgs::Point origin,
+              boost::shared_ptr<robot_sensor_msgs::PointCloud2> cloud,
+              double obstacle_range, double raytrace_range) :
+      origin_(std::move(origin)), cloud_handle_(std::move(cloud)),
+      cloud_(cloud_handle_.get()),
      obstacle_range_(obstacle_range), raytrace_range_(raytrace_range)
  {
  }
@@ -78,22 +216,59 @@ public:
   * @param obs The observation to copy
   */
  Observation(const Observation& obs) :
-      origin_(obs.origin_), cloud_(new robot_sensor_msgs::PointCloud2(*(obs.cloud_))),
+      origin_(obs.origin_), cloud_handle_(obs.cloud_handle_), cloud_(cloud_handle_.get()),
      obstacle_range_(obs.obstacle_range_), raytrace_range_(obs.raytrace_range_)
  {
  }

+  Observation(Observation&& obs) noexcept :
+      origin_(std::move(obs.origin_)), cloud_handle_(std::move(obs.cloud_handle_)),
+      cloud_(cloud_handle_.get()), obstacle_range_(obs.obstacle_range_),
+      raytrace_range_(obs.raytrace_range_)
+  {
+    obs.cloud_ = nullptr;
+  }
+
+  Observation& operator=(const Observation& obs)
+  {
+    if (this == &obs)
+      return *this;
+
+    origin_ = obs.origin_;
+    cloud_handle_ = obs.cloud_handle_;
+    cloud_ = cloud_handle_.get();
+    obstacle_range_ = obs.obstacle_range_;
+    raytrace_range_ = obs.raytrace_range_;
+    return *this;
+  }
+
+  Observation& operator=(Observation&& obs) noexcept
+  {
+    if (this == &obs)
+      return *this;
+
+    origin_ = std::move(obs.origin_);
+    cloud_handle_ = std::move(obs.cloud_handle_);
+    cloud_ = cloud_handle_.get();
+    obstacle_range_ = obs.obstacle_range_;
+    raytrace_range_ = obs.raytrace_range_;
+    obs.cloud_ = nullptr;
+    return *this;
+  }
+
  /**
   * @brief  Creates an observation from a point cloud
   * @param cloud The point cloud of the observation
   * @param obstacle_range The range out to which an observation should be able to insert obstacles
   */
  Observation(const robot_sensor_msgs::PointCloud2 &cloud, double obstacle_range) :
-      cloud_(new robot_sensor_msgs::PointCloud2(cloud)), obstacle_range_(obstacle_range), raytrace_range_(0.0)
+      cloud_handle_(boost::make_shared<robot_sensor_msgs::PointCloud2>(cloud)),
+      cloud_(cloud_handle_.get()), obstacle_range_(obstacle_range), raytrace_range_(0.0)
  {
  }

  robot_geometry_msgs::Point origin_;
+  boost::shared_ptr<robot_sensor_msgs::PointCloud2> cloud_handle_;
  robot_sensor_msgs::PointCloud2* cloud_;
  double obstacle_range_, raytrace_range_;
 };
--- a/include/robot_costmap_2d/observation_buffer.h
+++ b/include/robot_costmap_2d/observation_buffer.h
@@ -40,10 +40,9 @@
 #include <vector>
 #include <list>
 #include <string>
-#include <robot/time.h>
+#include <robot/robot.h>
 #include <robot_costmap_2d/observation.h>
 #include <tf3/buffer_core.h>
-
 #include <robot_sensor_msgs/PointCloud2.h>

 // Thread support
@@ -76,6 +75,13 @@ public:
                    double min_obstacle_height, double max_obstacle_height, double obstacle_range,
                    double raytrace_range, tf3::BufferCore& tf3_buffer, std::string global_frame,
                    std::string sensor_frame, double tf_tolerance);
+  
+  ObservationBuffer(std::string topic_name, double observation_keep_time, double expected_update_rate,
+                    double min_obstacle_height, double max_obstacle_height, double obstacle_range,
+                    double raytrace_range, unsigned int frustum_pixel_step, double frustum_min_range,
+                    double frustum_max_range, tf3::BufferCore& tf3_buffer, std::string global_frame,
+                    std::string sensor_frame, double tf_tolerance);
+

  /**
   * @brief  Destructor... cleans up
@@ -98,12 +104,24 @@ public:
   */
  void bufferCloud(const robot_sensor_msgs::PointCloud2& cloud);

+  /**
+   * @brief Store the newest depth frame without converting it to PointCloud2.
+   */
+  void bufferDepthCamera(const robot_sensor_msgs::DepthCameraData& depth);
+
+  void bufferDepthCamera(robot_sensor_msgs::DepthCameraData::ConstPtr depth);
+
  /**
   * @brief  Pushes copies of all current observations onto the end of the vector passed in
   * @param  observations The vector to be filled
   */
  void getObservations(std::vector<Observation>& observations);

+  /**
+   * @brief Append the current depth observation, if it has not expired.
+   */
+  void getDepthObservations(std::vector<DepthCameraObservation>& observations);
+
  /**
   * @brief  Check if the observation buffer is being update at its expected rate
   * @return True if it is being updated at the expected rate, false otherwise
@@ -137,6 +155,8 @@ private:
   */
  void purgeStaleObservations();

+  void purgeStaleDepthObservations();
+
  tf3::BufferCore& tf3_buffer_;
  const robot::Duration observation_keep_time_;
  const robot::Duration expected_update_rate_;
@@ -144,11 +164,16 @@ private:
  std::string global_frame_;
  std::string sensor_frame_;
  std::list<Observation> observation_list_;
+  std::list<DepthCameraObservation> depth_observation_list_;
+  // DepthCameraObservation depth_observation_;
  std::string topic_name_;
  double min_obstacle_height_, max_obstacle_height_;
  boost::recursive_mutex lock_;  ///< @brief A lock for accessing data in callbacks safely
  double obstacle_range_, raytrace_range_;
  double tf_tolerance_;
+  unsigned int frustum_pixel_step_;
+  double frustum_min_range_;
+  double frustum_max_range_;
 };
 }  // namespace robot_costmap_2d
 #endif  // ROBOT_COSTMAP_2D_OBSERVATION_BUFFER_H_
--- a/include/robot_costmap_2d/obstacle_layer.h
+++ b/include/robot_costmap_2d/obstacle_layer.h
@@ -38,6 +38,7 @@
 #ifndef ROBOT_COSTMAP_2D_OBSTACLE_LAYER_H_
 #define ROBOT_COSTMAP_2D_OBSTACLE_LAYER_H_

+#include <robot/robot.h>
 #include <robot_costmap_2d/costmap_layer.h>
 #include <robot_costmap_2d/layered_costmap.h>
 #include <robot_costmap_2d/observation_buffer.h>
@@ -45,13 +46,16 @@

 #include <robot_nav_msgs/OccupancyGrid.h>

+#include <mutex>
+
+#include <robot_sensor_msgs/DepthCameraData.h>
 #include <robot_sensor_msgs/LaserScan.h>
 #include <robot_laser_geometry/laser_geometry.hpp>
 #include <robot_sensor_msgs/PointCloud.h>
 #include <robot_sensor_msgs/PointCloud2.h>
 #include <robot_sensor_msgs/point_cloud_conversion.h>

-#include <robot/console.h>
+

 namespace robot_costmap_2d
 {
@@ -127,6 +131,12 @@ protected:
  void pointCloud2Callback(const robot_sensor_msgs::PointCloud2& message,
                           const boost::shared_ptr<robot_costmap_2d::ObservationBuffer>& buffer);

+  /**
+   * @brief Buffer a depth image and its camera model for frustum clearing.
+   */
+  void depthImageCallback(robot_sensor_msgs::DepthCameraData::ConstPtr message,
+                          const boost::shared_ptr<robot_costmap_2d::ObservationBuffer>& buffer);
+
  /**
   * @brief  Get the observations used to mark space
   * @param marking_observations A reference to a vector that will be populated with the observations
@@ -141,6 +151,13 @@ protected:
   */
  bool getClearingObservations(std::vector<robot_costmap_2d::Observation>& clearing_observations) const;

+  /**
+   * @brief Collect fresh depth frames from every configured frustum-clearing source.
+   * @return True when every configured depth source is current.
+   */
+  bool getFrustumClearingObservations(
+      std::vector<robot_costmap_2d::DepthCameraObservation>& frustum_clearing_observations) const;
+
  /**
   * @brief  Clear freespace based on one observation
   * @param clearing_observation The observation used to raytrace
@@ -169,6 +186,9 @@ protected:
  std::vector<boost::shared_ptr<robot_costmap_2d::ObservationBuffer> > marking_buffers_;  ///< @brief Used to store observation buffers used for marking obstacles
  std::vector<boost::shared_ptr<robot_costmap_2d::ObservationBuffer> > clearing_buffers_;  ///< @brief Used to store observation buffers used for clearing obstacles

+  std::vector<boost::shared_ptr<robot_costmap_2d::ObservationBuffer> > depth_observation_buffers_;
+  std::vector<boost::shared_ptr<robot_costmap_2d::ObservationBuffer> > depth_clearing_buffers_;
+
  // Used only for testing purposes
  std::vector<robot_costmap_2d::Observation> static_clearing_observations_, static_marking_observations_;

@@ -177,6 +197,10 @@ protected:

  int combination_method_;
  std::vector<CallBackInfo> callback_infos_;
+  std::vector<CallBackInfo> callback_depth_infos_;
+  std::string depth_camera_data_topic_;
+  mutable std::mutex depth_camera_data_mutex_;
+  robot_sensor_msgs::DepthCameraData::ConstPtr pending_depth_camera_data_;

 private:
  bool getParams(const std::string& config_file_name, robot::NodeHandle &nh);
--- a/include/robot_costmap_2d/voxel_layer.h
+++ b/include/robot_costmap_2d/voxel_layer.h
@@ -91,13 +91,68 @@ private:
  void clearNonLethal(double wx, double wy, double w_size_x, double w_size_y, bool clear_no_info);
  virtual void raytraceFreespace(const robot_costmap_2d::Observation& clearing_observation, double* min_x, double* min_y,
                                 double* max_x, double* max_y);
+  // bool raytraceDepthFrustum(double* min_x, double* min_y, double* max_x, double* max_y);
+  bool raytraceDepthFrustum(const robot_costmap_2d::DepthCameraObservation& observation,
+                            double* min_x, double* min_y, double* max_x, double* max_y);
+  bool readDepthMeters(const robot_sensor_msgs::Image& depth, unsigned int u, unsigned int v,
+                       double& depth_m, bool& is_valid) const;
+  void updateDepthRayCache(unsigned int width, unsigned int height, unsigned int pixel_step,
+                           double fx, double fy, double cx, double cy,
+                           unsigned int u_offset, unsigned int v_offset);
+  bool clipRaytraceEndpoint(double ox, double oy, double oz, double& wx, double& wy, double& wz);
+  bool clearVoxelRay(double ox, double oy, double oz, double wx, double wy, double wz,
+                     double raytrace_range, unsigned int cell_raytrace_range,
+                     double* min_x, double* min_y, double* max_x, double* max_y);
+  /// Frees LETHAL cells this layer marked that have not been re-marked within
+  /// obstacle_decay_time. Handles ghost cells frustum clearing can never
+  /// reach: cells that left the camera FOV, occlusion shadows, and cells
+  /// inside the skip band in front of a measured surface.
+  void decayStaleObstacles(double now_sec, double* min_x, double* min_y,
+                           double* max_x, double* max_y);


  bool publish_voxel_;
  robot_voxel_grid::VoxelGrid robot_voxel_grid_;
  double z_resolution_, origin_z_;
  unsigned int unknown_threshold_, mark_threshold_, size_z_;
+  /// Seconds a marked cell survives without being re-observed before it is
+  /// freed. <= 0 disables decay (default). Only enable on camera-only
+  /// costmaps: decay clears obstacles the sensor cannot currently see.
+  double obstacle_decay_time_{0.0};
+  /// Clearing rays stop this far [m] before the measured surface. < 0 keeps
+  /// the legacy 2 * resolution behaviour.
+  double frustum_skip_distance_{-1.0};
+  /// Per-cell robot::Time seconds of the last marking; 0 = no valid stamp.
+  std::vector<double> cell_last_marked_;
  robot_sensor_msgs::PointCloud clearing_endpoints_;
+  std::vector<unsigned char> rolling_costmap_scratch_;
+  std::vector<unsigned int> rolling_voxel_scratch_;
+  std::vector<double> rolling_stamp_scratch_;
+
+  struct DepthRay
+  {
+    unsigned int u;
+    unsigned int v;
+    double x;
+    double y;
+    double z;
+  };
+  std::vector<DepthRay> depth_ray_cache_;
+  unsigned int cached_depth_width_ = 0;
+  unsigned int cached_depth_height_ = 0;
+  unsigned int cached_depth_pixel_step_ = 0;
+  double cached_fx_ = 0.0;
+  double cached_fy_ = 0.0;
+  double cached_cx_ = 0.0;
+  double cached_cy_ = 0.0;
+  unsigned int cached_u_offset_ = 0;
+  unsigned int cached_v_offset_ = 0;
+  /// Advances every frustum pass to dither the sampled pixel grid. A static
+  /// camera otherwise re-traces the same fixed pixel_step subgrid each frame,
+  /// so cells between adjacent rays (angular gap pixel_step / fx) are never
+  /// crossed and stay marked until the robot moves. Cycling the grid phase
+  /// sweeps every pixel over pixel_step^2 frames at unchanged per-frame cost.
+  unsigned int frustum_phase_ = 0;

  inline bool worldToMap3DFloat(double wx, double wy, double wz, double& mx, double& my, double& mz)
  {
--- a/package.xml
+++ b/package.xml
@@ -46,9 +46,6 @@
  <build_depend>robot_voxel_grid</build_depend>
  <run_depend>robot_voxel_grid</run_depend>

-  <build_depend>tf3</build_depend>
-  <run_depend>tf3</run_depend>
-
  <build_depend>robot_tf3_geometry_msgs</build_depend>
  <run_depend>robot_tf3_geometry_msgs</run_depend>

--- a/plugins/directional_layer.cpp
+++ b/plugins/directional_layer.cpp
@@ -162,14 +162,6 @@ namespace robot_costmap_2d
      robot_nav_msgs::OccupancyGrid lanes;
      convertToMap(costmap_, lanes, 0.65, 0.196);

-      //////////////////////////////////
-      //////////////////////////////////
-      /////////THAY THẾ PUBLISH////////
-      // lane_mask_pub_.publish(lanes);
-      //////////////////////////////////
-      //////////////////////////////////
-      //////////////////////////////////
-
      
      return false;
    }
--- a/plugins/inflation_layer.cpp
+++ b/plugins/inflation_layer.cpp
@@ -58,7 +58,6 @@ InflationLayer::InflationLayer()
  , inflate_unknown_(false)
  , cell_inflation_radius_(0)
  , cached_cell_inflation_radius_(0)
-  , seen_(NULL)
  , cached_costs_(NULL)
  , cached_distances_(NULL)
  , last_min_x_(-std::numeric_limits<float>::max())
@@ -76,10 +75,8 @@ void InflationLayer::onInitialize()

  boost::unique_lock < boost::recursive_mutex > lock(*inflation_access_);
  current_ = true;
-  if (seen_)
-    delete[] seen_;
-  seen_ = NULL;
-  seen_size_ = 0;
+  seen_.clear();
+  seen_generation_ = 0;
  need_reinflation_ = false;
  std::string config_file_name = "inflation_layer_params.yaml";
  // std::cout << "InflationLayer: " << config_file_name << std::endl;
@@ -91,7 +88,13 @@ void InflationLayer::onInitialize()
 bool InflationLayer::getParams(const std::string& config_file_name, robot::NodeHandle &nh)
 {
    try {
-        std::string folder = ROBOT_COSTMAP_2D_DIR;
+        const char *env_config = std::getenv("PNKX_NAV_CORE_CONFIG_DIR");
+        std::string folder;
+        if (env_config && std::filesystem::exists(env_config))
+        {
+          folder = std::string(env_config);
+          // robot::log_error("config_directory: %s", folder.c_str());
+        }
        std::string path_source = getSourceFile(folder,config_file_name);

        YAML::Node config = YAML::LoadFile(path_source);
@@ -138,10 +141,8 @@ void InflationLayer::matchSize()
  computeCaches();

  unsigned int size_x = costmap->getSizeInCellsX(), size_y = costmap->getSizeInCellsY();
-  if (seen_)
-    delete[] seen_;
-  seen_size_ = size_x * size_y;
-  seen_ = new bool[seen_size_];
+  seen_.assign(static_cast<std::size_t>(size_x) * size_y, 0);
+  seen_generation_ = 0;
 }

 void InflationLayer::updateBounds(double robot_x, double robot_y, double robot_yaw, double* min_x,
@@ -197,26 +198,28 @@ void InflationLayer::updateCosts(robot_costmap_2d::Costmap2D& master_grid, int m
  if (cell_inflation_radius_ == 0)
    return;

-  // make sure the inflation list is empty at the beginning of the cycle (should always be true)
-  if(!inflation_cells_.empty())
-    robot::log_error("The inflation list must be empty at the beginning of inflation\n");
+  for (std::vector<CellData>& cells : inflation_cells_)
+    cells.clear();

  unsigned char* master_array = master_grid.getCharMap();
  unsigned int size_x = master_grid.getSizeInCellsX(), size_y = master_grid.getSizeInCellsY();

-  if (seen_ == NULL) {
-    robot::log_error("InflationLayer::updateCosts(): seen_ array is NULL\n");
-    seen_size_ = size_x * size_y;
-    seen_ = new bool[seen_size_];
-  }
-  else if (seen_size_ != size_x * size_y)
+  const std::size_t map_size = static_cast<std::size_t>(size_x) * size_y;
+  if (seen_.size() != map_size)
  {
-    robot::log_error("InflationLayer::updateCosts(): seen_ array size is wrong\n");
-    delete[] seen_;
-    seen_size_ = size_x * size_y;
-    seen_ = new bool[seen_size_];
+    seen_.assign(map_size, 0);
+    seen_generation_ = 0;
+  }
+
+  if (seen_generation_ == std::numeric_limits<std::uint32_t>::max())
+  {
+    std::fill(seen_.begin(), seen_.end(), 0);
+    seen_generation_ = 1;
+  }
+  else
+  {
+    ++seen_generation_;
  }
-  memset(seen_, false, size_x * size_y * sizeof(bool));

  // We need to include in the inflation cells outside the bounding
  // box min_i...max_j, by the amount cell_inflation_radius_.  Cells
@@ -232,11 +235,13 @@ void InflationLayer::updateCosts(robot_costmap_2d::Costmap2D& master_grid, int m
  max_i = std::min(int(size_x), max_i);
  max_j = std::min(int(size_y), max_j);

-  // Inflation list; we append cells to visit in a list associated with its distance to the nearest obstacle
-  // We use a map<distance, list> to emulate the priority queue used before, with a notable performance boost
+  // Precomputed distance buckets preserve priority ordering without a tree lookup
+  // for every enqueued cell.

  // Start with lethal obstacles: by definition distance is 0.0
-  std::vector<CellData>& obs_bin = inflation_cells_[0.0];
+  if (inflation_cells_.empty())
+    return;
+  std::vector<CellData>& obs_bin = inflation_cells_.front();
  for (int j = min_j; j < max_j; j++)
  {
    for (int i = min_i; i < max_i; i++)
@@ -252,23 +257,22 @@ void InflationLayer::updateCosts(robot_costmap_2d::Costmap2D& master_grid, int m

  // Process cells by increasing distance; new cells are appended to the corresponding distance bin, so they
  // can overtake previously inserted but farther away cells
-  std::map<double, std::vector<CellData> >::iterator bin;
-  for (bin = inflation_cells_.begin(); bin != inflation_cells_.end(); ++bin)
+  for (std::vector<CellData>& bin : inflation_cells_)
  {
-    for (int i = 0; i < bin->second.size(); ++i)
+    for (std::size_t i = 0; i < bin.size(); ++i)
    {
      // process all cells at distance dist_bin.first
-      const CellData& cell = bin->second[i];
+      const CellData& cell = bin[i];

      unsigned int index = cell.index_;

      // ignore if already visited
-      if (seen_[index])
+      if (seen_[index] == seen_generation_)
      {
        continue;
      }

-      seen_[index] = true;
+      seen_[index] = seen_generation_;

      unsigned int mx = cell.x_;
      unsigned int my = cell.y_;
@@ -295,7 +299,6 @@ void InflationLayer::updateCosts(robot_costmap_2d::Costmap2D& master_grid, int m
    }
  }

-  inflation_cells_.clear();
 }

 /**
@@ -310,7 +313,7 @@ void InflationLayer::updateCosts(robot_costmap_2d::Costmap2D& master_grid, int m
 inline void InflationLayer::enqueue(unsigned int index, unsigned int mx, unsigned int my,
                                    unsigned int src_x, unsigned int src_y)
 {
-  if (!seen_[index])
+  if (seen_[index] != seen_generation_)
  {
    // we compute our distance table one cell further than the inflation radius dictates so we can make the check below
    double distance = distanceLookup(mx, my, src_x, src_y);
@@ -319,8 +322,10 @@ inline void InflationLayer::enqueue(unsigned int index, unsigned int mx, unsigne
    if (distance > cell_inflation_radius_)
      return;

-    // push the cell data onto the inflation list and mark
-    inflation_cells_[distance].push_back(CellData(index, mx, my, src_x, src_y));
+    const unsigned int dx = std::abs(static_cast<int>(mx) - static_cast<int>(src_x));
+    const unsigned int dy = std::abs(static_cast<int>(my) - static_cast<int>(src_y));
+    const unsigned int bin_index = distance_bin_lookup_[dx * distance_lookup_size_ + dy];
+    inflation_cells_[bin_index].push_back(CellData(index, mx, my, src_x, src_y));
  }
 }

@@ -348,6 +353,38 @@ void InflationLayer::computeCaches()
    }

    cached_cell_inflation_radius_ = cell_inflation_radius_;
+
+    distance_lookup_size_ = cell_inflation_radius_ + 2;
+    distance_levels_.clear();
+    for (unsigned int i = 0; i < distance_lookup_size_; ++i)
+    {
+      for (unsigned int j = 0; j < distance_lookup_size_; ++j)
+      {
+        if (cached_distances_[i][j] <= cell_inflation_radius_)
+          distance_levels_.push_back(cached_distances_[i][j]);
+      }
+    }
+    std::sort(distance_levels_.begin(), distance_levels_.end());
+    distance_levels_.erase(
+        std::unique(distance_levels_.begin(), distance_levels_.end()), distance_levels_.end());
+
+    inflation_cells_.clear();
+    inflation_cells_.resize(distance_levels_.size());
+    distance_bin_lookup_.assign(
+        static_cast<std::size_t>(distance_lookup_size_) * distance_lookup_size_, 0);
+    for (unsigned int i = 0; i < distance_lookup_size_; ++i)
+    {
+      for (unsigned int j = 0; j < distance_lookup_size_; ++j)
+      {
+        const double distance = cached_distances_[i][j];
+        if (distance > cell_inflation_radius_)
+          continue;
+        distance_bin_lookup_[i * distance_lookup_size_ + j] =
+            static_cast<unsigned int>(
+                std::lower_bound(distance_levels_.begin(), distance_levels_.end(), distance) -
+                distance_levels_.begin());
+      }
+    }
  }

  for (unsigned int i = 0; i <= cell_inflation_radius_ + 1; ++i)
@@ -361,6 +398,10 @@ void InflationLayer::computeCaches()

 void InflationLayer::deleteKernels()
 {
+  inflation_cells_.clear();
+  distance_levels_.clear();
+  distance_bin_lookup_.clear();
+  distance_lookup_size_ = 0;
  if (cached_distances_ != NULL)
  {
    for (unsigned int i = 0; i <= cached_cell_inflation_radius_ + 1; ++i)
--- a/plugins/obstacle_layer.cpp
+++ b/plugins/obstacle_layer.cpp
@@ -75,7 +75,14 @@ ObstacleLayer::~ObstacleLayer()
 bool ObstacleLayer::getParams(const std::string& config_file_name, robot::NodeHandle &nh)
 {
  try {
-      std::string folder = ROBOT_COSTMAP_2D_DIR;
+      const char *env_config = std::getenv("PNKX_NAV_CORE_CONFIG_DIR");
+      std::string folder;
+      if (env_config && std::filesystem::exists(env_config))
+      {
+        folder = std::string(env_config);
+        // robot::log_error("config_directory: %s", folder.c_str());
+      }
+      // robot::log_error("folder: %s", folder.c_str());
      std::string path_source = getSourceFile(folder,config_file_name);
      
      YAML::Node config = YAML::LoadFile(path_source);
@@ -111,7 +118,7 @@ bool ObstacleLayer::getParams(const std::string& config_file_name, robot::NodeHa
      std::string topics_string = loadParam(layer,"observation_sources", std::string(""));
      if (nh.hasParam("observation_sources"))
        nh.getParam("observation_sources", topics_string);
-      robot::log_error("Subscribed to Topics: %s\n", topics_string.c_str());
+      robot::log_info("Subscribed to Topics: %s\n", topics_string.c_str());

      // now we need to split the topics based on whitespace which we can use a stringstream for
      std::stringstream ss(topics_string);
@@ -124,6 +131,10 @@ bool ObstacleLayer::getParams(const std::string& config_file_name, robot::NodeHa
        double observation_keep_time = 0, expected_update_rate = 0, min_obstacle_height = 0, max_obstacle_height = 2;
        std::string topic = "map", sensor_frame = "laser_frame", data_type = "PointCloud";
        bool inf_is_valid = false, clearing=false, marking=true;
+        bool frustum_clearing_enabled = false;
+        int frustum_pixel_step = 8;
+        double frustum_min_range = 0.2;
+        double frustum_max_range = 3.0;

        robot::NodeHandle priv_nh(nh, source);
        topic = loadParam(layer[source],"topic", topic);
@@ -136,6 +147,10 @@ bool ObstacleLayer::getParams(const std::string& config_file_name, robot::NodeHa
        inf_is_valid = loadParam(layer[source],"inf_is_valid", false);
        clearing = loadParam(layer[source],"clearing", false);
        marking = loadParam(layer[source],"marking", true);
+        frustum_clearing_enabled = loadParam(layer, "frustum_clearing_enabled", false);
+        frustum_pixel_step = loadParam(layer, "frustum_clearing_pixel_step", 8);
+        frustum_min_range = loadParam(layer, "frustum_min_range", 0.2);
+        frustum_max_range = loadParam(layer, "frustum_max_range", 3.0);

        if (priv_nh.hasParam("topic"))
          priv_nh.getParam("topic", topic);
@@ -157,52 +172,83 @@ bool ObstacleLayer::getParams(const std::string& config_file_name, robot::NodeHa
          priv_nh.getParam("clearing", clearing);
        if (priv_nh.hasParam("marking"))
          priv_nh.getParam("marking", marking);
-
-        if (!(data_type == "PointCloud2" || data_type == "PointCloud" || data_type == "LaserScan"))
+        if (priv_nh.hasParam("frustum_clearing_enabled"))
+          priv_nh.getParam("frustum_clearing_enabled", frustum_clearing_enabled);
+        if (priv_nh.hasParam("frustum_clearing_pixel_step"))
        {
-          robot::log_error("Only topics that use point clouds or laser scans are currently supported\n");
-          throw std::runtime_error("Only topics that use point clouds or laser scans are currently supported");
+          priv_nh.getParam("frustum_clearing_pixel_step", frustum_pixel_step);
+          frustum_pixel_step = std::max(1, frustum_pixel_step);
        }
+        if (priv_nh.hasParam("frustum_min_range"))
+          priv_nh.getParam("frustum_min_range", frustum_min_range);
+        if (priv_nh.hasParam("frustum_max_range"))
+          priv_nh.getParam("frustum_max_range", frustum_max_range);
+        if (priv_nh.hasParam("frustum_depth_camera_topic"))
+          priv_nh.getParam("frustum_depth_camera_topic", depth_camera_data_topic_);

-        CallBackInfo info_tmp;
-        info_tmp.observation_source = source;
-        info_tmp.data_type = data_type;
-        info_tmp.topic = topic;
-        info_tmp.inf_is_valid = inf_is_valid;
-        callback_infos_.push_back(info_tmp);
-
-        std::string raytrace_range_param_name, obstacle_range_param_name;
+        robot::log_info("frustum_clearing_enabled: %s, frustum_clearing_pixel_step: %d, frustum_min_range: %f, frustum_max_range: %f", frustum_clearing_enabled ? "true" : "false", frustum_pixel_step, frustum_min_range, frustum_max_range);

        double obstacle_range = 2.5;
        obstacle_range = loadParam(layer[source],"obstacle_range", obstacle_range);
        double raytrace_range = 3.0;
        raytrace_range = loadParam(layer[source],"raytrace_range", raytrace_range);

-        
-
        if (priv_nh.hasParam("obstacle_range"))
          priv_nh.getParam("obstacle_range", obstacle_range);
        if (priv_nh.hasParam("raytrace_range"))
          priv_nh.getParam("raytrace_range", raytrace_range);

-        // enabled_ = enabled;
+        if (!(data_type == "PointCloud2" || data_type == "PointCloud" || data_type == "LaserScan" || data_type == "DepthCameraData"))
+        {
+          robot::log_error("Only topics that use point clouds or laser scans are currently supported\n");
+          throw std::runtime_error("Only topics that use point clouds or laser scans are currently supported");
+        }

-        robot::log_info("Creating an observation buffer for topic %s, frame %s\n", topic.c_str(),
-                  sensor_frame.c_str());
+        if(!frustum_clearing_enabled)
+        {

-        // create an observation buffer
-        observation_buffers_.push_back(
-            boost::shared_ptr < ObservationBuffer
-                > (new ObservationBuffer(topic, observation_keep_time, expected_update_rate, min_obstacle_height,
-                                        max_obstacle_height, obstacle_range, raytrace_range, *tf_, global_frame_,
-                                        sensor_frame, transform_tolerance)));
-        if (marking)
-          marking_buffers_.push_back(observation_buffers_.back());
+          CallBackInfo info_tmp;
+          info_tmp.observation_source = source;
+          info_tmp.data_type = data_type;
+          info_tmp.topic = topic;
+          info_tmp.inf_is_valid = inf_is_valid;
+          callback_infos_.push_back(info_tmp);

-        // check if we'll also add this buffer to our clearing observation buffers
-        if (clearing)
-          clearing_buffers_.push_back(observation_buffers_.back());
+          // enabled_ = enabled;

+          robot::log_info("Creating an observation buffer for topic %s, frame %s\n", topic.c_str(),
+                    priv_nh.getNamespace().c_str());
+
+          // create an observation buffer
+          observation_buffers_.push_back(
+              boost::shared_ptr < ObservationBuffer
+                  > (new ObservationBuffer(topic, observation_keep_time, expected_update_rate, min_obstacle_height,
+                                          max_obstacle_height, obstacle_range, raytrace_range, *tf_, global_frame_,
+                                          sensor_frame, transform_tolerance)));
+          if (marking)
+            marking_buffers_.push_back(observation_buffers_.back());
+
+          // check if we'll also add this buffer to our clearing observation buffers
+          if (clearing)
+            clearing_buffers_.push_back(observation_buffers_.back());
+        }
+        else
+        {
+          CallBackInfo info_tmp;
+          info_tmp.observation_source = source;
+          info_tmp.data_type = data_type;
+          info_tmp.topic = topic;
+          info_tmp.inf_is_valid = inf_is_valid;
+          callback_depth_infos_.push_back(info_tmp);
+
+          depth_observation_buffers_.push_back(
+              boost::shared_ptr < ObservationBuffer
+                  > (new ObservationBuffer(topic, observation_keep_time, expected_update_rate, min_obstacle_height,
+                                          max_obstacle_height, obstacle_range, raytrace_range, frustum_pixel_step,
+                                          frustum_min_range, frustum_max_range, *tf_, global_frame_,
+                                          sensor_frame, transform_tolerance)));
+          
+        }
        robot::log_info(
            "Created an observation buffer for topic %s, global frame: %s, "
            "expected update rate: %.2f, observation persistence: %.2f\n",
@@ -223,13 +269,93 @@ void ObstacleLayer::handleImpl(const void* data,
                              const std::type_info& type,
                              const std::string& topic)  
 {
-  if(!stop_receiving_data_)
+  if (!enabled_ || stop_receiving_data_)
+    return;
+
+  if (type == typeid(robot_sensor_msgs::DepthCameraData::ConstPtr))
  {
-    
-    if(observation_buffers_.empty() || callback_infos_.empty()) return;
-    
+      const robot_sensor_msgs::DepthCameraData::ConstPtr& depth_camera_data_ptr =
+          *static_cast<const robot_sensor_msgs::DepthCameraData::ConstPtr*>(data);
+      if (!depth_camera_data_ptr)
+        return;
+
+      const robot_sensor_msgs::DepthCameraData& depth_camera_data =
+          *depth_camera_data_ptr;
+      const robot_sensor_msgs::Image& depth = depth_camera_data.depth;
+      const robot_sensor_msgs::CameraInfo& camera_info = depth_camera_data.camera_info;
+
+      std::size_t bytes_per_pixel = 0;
+      if (depth.encoding == "16UC1" || depth.encoding == "mono16")
+        bytes_per_pixel = 2;
+      else if (depth.encoding == "32FC1")
+        bytes_per_pixel = 4;
+      else
+      {
+        robot::log_error("ObstacleLayer received unsupported depth encoding: %s\n", depth.encoding.c_str());
+        return;
+      }
+
+      const bool invalid_dimensions = depth.width == 0 || depth.height == 0 ||
+          depth.step < static_cast<std::size_t>(depth.width) * bytes_per_pixel ||
+          depth.data.size() < static_cast<std::size_t>(depth.step) * depth.height;
+      if (invalid_dimensions)
+      {
+        robot::log_error("ObstacleLayer received malformed DepthCameraData image\n");
+        return;
+      }
+
+      if (camera_info.K[0] <= 0.0 || camera_info.K[4] <= 0.0)
+      {
+        robot::log_error("ObstacleLayer received invalid camera intrinsics for depth clearing\n");
+        return;
+      }
+
+      if ((camera_info.width != 0 && camera_info.width != depth.width) ||
+          (camera_info.height != 0 && camera_info.height != depth.height))
+      {
+        robot::log_error("ObstacleLayer received mismatched depth image and camera info dimensions\n");
+        return;
+      }
+
+      const std::string& depth_frame = depth.header.frame_id;
+      const std::string& camera_frame = camera_info.header.frame_id;
+      if (!depth_frame.empty() && !camera_frame.empty() && depth_frame != camera_frame)
+      {
+        robot::log_error("ObstacleLayer received mismatched depth and camera-info frames: %s != %s\n",
+                         depth_frame.c_str(), camera_frame.c_str());
+        return;
+      }
+
+      if (depth_camera_data.header.frame_id.empty() && depth_frame.empty() && camera_frame.empty())
+      {
+        robot::log_error("ObstacleLayer received DepthCameraData without an optical frame\n");
+        return;
+      }
+
+      // std::lock_guard<std::mutex> lock(depth_camera_data_mutex_);
+      // pending_depth_camera_data_ = depth_camera_data_ptr;
+      if (depth_observation_buffers_.empty() || callback_depth_infos_.empty())
+        return;
+
+      int size_callback_depth = static_cast<int>(callback_depth_infos_.size());
+      for(int i = 0; i < size_callback_depth; i++)
+      {
+        boost::shared_ptr<ObservationBuffer>& buffer = depth_observation_buffers_[i];
+        if (type == typeid(robot_sensor_msgs::DepthCameraData::ConstPtr) &&
+            topic == callback_depth_infos_[i].topic)
+        {
+          // robot::log_error_throttle(1.0,"TEST");
+          depthImageCallback(depth_camera_data_ptr, buffer);
+        }
+      }
+  }
+  else
+  {
+    if (observation_buffers_.empty() || callback_infos_.empty())
+      return;
+
    int size_callback = static_cast<int>(callback_infos_.size());
-    for(int i = 0; i < size_callback; i++)
+    for (int i = 0; i < size_callback; i++)
    {
      boost::shared_ptr<ObservationBuffer>& buffer = observation_buffers_[i];

@@ -280,11 +406,6 @@ void ObstacleLayer::handleImpl(const void* data,
      // }
    }
  }
-  else 
-  {
-    robot::log_info("Stop receiving data!\n");
-    return;
-  }
 }

 void ObstacleLayer::laserScanCallback(const robot_sensor_msgs::LaserScan& message,
@@ -385,6 +506,14 @@ void ObstacleLayer::pointCloud2Callback(const robot_sensor_msgs::PointCloud2& me
  buffer->unlock();
 }

+void ObstacleLayer::depthImageCallback(robot_sensor_msgs::DepthCameraData::ConstPtr message,
+                                       const boost::shared_ptr<ObservationBuffer>& buffer)
+{
+  buffer->lock();
+  buffer->bufferDepthCamera(std::move(message));
+  buffer->unlock();
+}
+
 void ObstacleLayer::updateBounds(double robot_x, double robot_y, double robot_yaw, double* min_x,
                                          double* min_y, double* max_x, double* max_y)
 {
@@ -423,6 +552,9 @@ void ObstacleLayer::updateBounds(double robot_x, double robot_y, double robot_ya
    robot_sensor_msgs::PointCloud2ConstIterator<float> iter_y(cloud, "y");
    robot_sensor_msgs::PointCloud2ConstIterator<float> iter_z(cloud, "z");

+    std::size_t rejected_height = 0;
+    std::size_t rejected_range = 0;
+    std::size_t rejected_bounds = 0;
    for (; iter_x !=iter_x.end(); ++iter_x, ++iter_y, ++iter_z)
    {
      double px = *iter_x, py = *iter_y, pz = *iter_z;
@@ -430,7 +562,7 @@ void ObstacleLayer::updateBounds(double robot_x, double robot_y, double robot_ya
      // if the obstacle is too high or too far away from the robot we won't add it
      if (pz > max_obstacle_height_)
      {
-        robot::log_error("The point is too high\n");
+        ++rejected_height;
        continue;
      }

@@ -441,7 +573,7 @@ void ObstacleLayer::updateBounds(double robot_x, double robot_y, double robot_ya
      // if the point is far enough away... we won't consider it
      if (sq_dist >= sq_obstacle_range)
      {
-        robot::log_error("The point is too far away\n");
+        ++rejected_range;
        continue;
      }

@@ -449,7 +581,7 @@ void ObstacleLayer::updateBounds(double robot_x, double robot_y, double robot_ya
      unsigned int mx, my;
      if (!worldToMap(px, py, mx, my))
      {
-        robot::log_error("Computing map coords failed\n");
+        ++rejected_bounds;
        continue;
      }

@@ -457,6 +589,14 @@ void ObstacleLayer::updateBounds(double robot_x, double robot_y, double robot_ya
      costmap_[index] = LETHAL_OBSTACLE;
      touch(px, py, min_x, min_y, max_x, max_y);
    }
+
+    if (rejected_height + rejected_range + rejected_bounds > 0)
+    {
+      robot::log_info_throttle(
+          5.0,
+          "ObstacleLayer filtered points: height=%zu range=%zu outside_map=%zu\n",
+          rejected_height, rejected_range, rejected_bounds);
+    }
  }

  updateFootprint(robot_x, robot_y, robot_yaw, min_x, min_y, max_x, max_y);
@@ -543,6 +683,23 @@ bool ObstacleLayer::getClearingObservations(std::vector<Observation>& clearing_o
  return current;
 }

+bool ObstacleLayer::getFrustumClearingObservations(std::vector<DepthCameraObservation>& frustum_clearing_observations) const
+{
+  bool current = true;
+  // DepthCameraObservation depth_obs;
+
+  for (const boost::shared_ptr<ObservationBuffer>& buffer : depth_observation_buffers_)
+  {
+    buffer->lock();
+    buffer->getDepthObservations(frustum_clearing_observations);
+    current = buffer->isCurrent() && current;
+    buffer->unlock();
+    // frustum_clearing_observations.push_back(depth_obs);
+  }
+
+  return current;
+}
+
 void ObstacleLayer::raytraceFreespace(const Observation& clearing_observation, double* min_x, double* min_y,
                                              double* max_x, double* max_y)
 {
--- a/plugins/static_layer.cpp
+++ b/plugins/static_layer.cpp
@@ -44,6 +44,7 @@

 #include <boost/dll/alias.hpp>
 #include <fstream>
+#include <cxxabi.h>


 using robot_costmap_2d::NO_INFORMATION;
@@ -71,12 +72,19 @@ void StaticLayer::onInitialize()
  global_frame_ = layered_costmap_->getGlobalFrameID();
  std::string config_file_name = "static_layer_params.yaml";
  getParams(config_file_name, priv_nh);
+  robot::log_warning("Initializing static layer with map topic \"%s\" in frame \"%s\"", map_topic_.c_str(), global_frame_.c_str());
 }

 bool StaticLayer::getParams(const std::string& config_file_name, robot::NodeHandle &nh)
 {
    try {
-        std::string folder = ROBOT_COSTMAP_2D_DIR;
+        const char *env_config = std::getenv("PNKX_NAV_CORE_CONFIG_DIR");
+        std::string folder;
+        if (env_config && std::filesystem::exists(env_config))
+        {
+          folder = std::string(env_config);
+          // robot::log_error("config_directory: %s", folder.c_str());
+        }
        std::string path_source = getSourceFile(folder,config_file_name);
        
        YAML::Node config = YAML::LoadFile(path_source);
@@ -185,12 +193,23 @@ void StaticLayer::handleImpl(const void* data,
                            const std::type_info& type,
                            const std::string& topic)
 {
-    if (type == typeid(robot_nav_msgs::OccupancyGrid) && topic == map_topic_) {
+    if (type == typeid(robot_nav_msgs::OccupancyGrid) && 
+        (topic == map_topic_ || topic == "/" + map_topic_)) {
        incomingMap(*static_cast<const robot_nav_msgs::OccupancyGrid*>(data));
-    } else if (type == typeid(robot_map_msgs::OccupancyGridUpdate) && topic == map_topic_ + "_updates") {
+    } else if (type == typeid(robot_map_msgs::OccupancyGridUpdate) && 
+        (topic == map_topic_ + "_updates" || topic == "/" + map_topic_ + "_updates")) {
        incomingUpdate(*static_cast<const robot_map_msgs::OccupancyGridUpdate*>(data));
    } else {
-        std::cout << "[Plugin] Unknown type: " << type.name() << std::endl;
+      std::string readable = boost::core::demangle(type.name());
+
+      size_t pos = readable.find("<");
+      if (pos != std::string::npos)
+      {
+          readable = readable.substr(0, pos);
+      }
+      robot::log_error("[] con1: %x, con2: %x ", type == typeid(robot_nav_msgs::OccupancyGrid), (topic == map_topic_ || topic == "/" + map_topic_));
+      robot::log_error("[StaticLayer] Received data of unknown type: %s on topic: %s, map_topic_: %s\n", readable.c_str(), topic.c_str(), map_topic_.c_str());
+        // std::cout << "[StaticLayer] Unknown type: " << type.name() << " on topic: " << topic << std::endl;
    }
 }

@@ -198,7 +217,7 @@ void StaticLayer::incomingMap(const robot_nav_msgs::OccupancyGrid& new_map)
 {
  if(!map_shutdown_)
  {
-    std::cout << "Received new map!" << std::endl;
+    std::cout << "[StaticLayer] Received new map!" << std::endl;
    unsigned int size_x = new_map.info.width, size_y = new_map.info.height;

    robot::log_info("Received a %d X %d map at %f m/pix\n", size_x, size_y, new_map.info.resolution);
@@ -375,7 +394,7 @@ void StaticLayer::updateCosts(robot_costmap_2d::Costmap2D& master_grid, int min_
    tf3::TransformStampedMsg transformMsg;
    try
    {
-      transformMsg = tf_->lookupTransform(map_frame_, global_frame_, tf3::Time::now());
+      transformMsg = tf_->lookupTransform(map_frame_, global_frame_, tf3::Time());
    }
    catch (tf3::TransformException ex)
    {
--- a/plugins/voxel_layer.cpp
+++ b/plugins/voxel_layer.cpp
@@ -37,7 +37,14 @@
 *********************************************************************/
 #include <robot_costmap_2d/voxel_layer.h>
 #include <robot_sensor_msgs/point_cloud2_iterator.h>
+#include <robot_tf3_geometry_msgs/tf3_geometry_msgs.h>
+#include <robot_geometry_msgs/Vector3.h>
+#include <tf3/exceptions.h>
 #include <boost/dll/alias.hpp>
+#include <algorithm>
+#include <cmath>
+#include <cstdint>
+#include <cstring>

 #define VOXEL_BITS 16

@@ -68,7 +75,14 @@ bool VoxelLayer::getParams(const std::string& config_file_name, robot::NodeHandl
 {
  try 
  {
-    std::string folder = ROBOT_COSTMAP_2D_DIR;
+    const char *env_config = std::getenv("PNKX_NAV_CORE_CONFIG_DIR");
+    std::string folder;
+    if (env_config && std::filesystem::exists(env_config))
+    {
+      folder = std::string(env_config);
+      // robot::log_error("config_directory: %s", folder.c_str());
+    }
+
    std::string path_source = getSourceFile(folder,config_file_name);
    
    YAML::Node config = YAML::LoadFile(path_source);
@@ -84,8 +98,10 @@ bool VoxelLayer::getParams(const std::string& config_file_name, robot::NodeHandl
    unknown_threshold_ = loadParam(layer, "unknown_threshold", 15.0) + (VOXEL_BITS - size_z_);
    mark_threshold_ = loadParam(layer, "mark_threshold", 0);
    combination_method_ = loadParam(layer, "combination_method", 0.0);
+    obstacle_decay_time_ = loadParam(layer, "obstacle_decay_time", 0.0);
+    frustum_skip_distance_ = loadParam(layer, "frustum_skip_distance", -1.0);

-    int size_z, unknown_threshold, mark_threshold;
+    int size_z, unknown_threshold, mark_threshold, frustum_pixel_step;
    if (nh.hasParam("enabled"))
      nh.getParam("enabled", enabled_);
    if (nh.hasParam("footprint_clearing_enabled"))
@@ -111,7 +127,18 @@ bool VoxelLayer::getParams(const std::string& config_file_name, robot::NodeHandl
    }
    if (nh.hasParam("combination_method"))
      nh.getParam("combination_method", combination_method_);
-    
+    if (nh.hasParam("obstacle_decay_time"))
+      nh.getParam("obstacle_decay_time", obstacle_decay_time_);
+    if (nh.hasParam("frustum_skip_distance"))
+      nh.getParam("frustum_skip_distance", frustum_skip_distance_);
+
+    if (obstacle_decay_time_ > 0.0)
+    {
+      robot::log_info(
+          "VoxelLayer obstacle decay enabled: LETHAL cells not re-observed for %.1f s are freed. "
+          "Intended for camera-only costmaps; decay clears obstacles outside the current FOV.\n",
+          obstacle_decay_time_);
+    }

    this->matchSize();
  } 
@@ -127,6 +154,7 @@ void VoxelLayer::matchSize()
 {
  ObstacleLayer::matchSize();
  robot_voxel_grid_.resize(size_x_, size_y_, size_z_);
+  cell_last_marked_.assign(static_cast<std::size_t>(size_x_) * size_y_, 0.0);
  if (!(robot_voxel_grid_.sizeX() == size_x_ && robot_voxel_grid_.sizeY() == size_y_)) 
  {
    std::cerr << "[FATAL] Voxel grid size mismatch: "
@@ -148,6 +176,7 @@ void VoxelLayer::resetMaps()
 {
  Costmap2D::resetMaps();
  robot_voxel_grid_.reset();
+  cell_last_marked_.assign(static_cast<std::size_t>(size_x_) * size_y_, 0.0);
 }

 void VoxelLayer::updateBounds(double robot_x, double robot_y, double robot_yaw, double* min_x,
@@ -159,6 +188,7 @@ void VoxelLayer::updateBounds(double robot_x, double robot_y, double robot_yaw,

  bool current = true;
  std::vector<Observation> observations, clearing_observations;
+  std::vector<DepthCameraObservation> depth_observations;

  // get the marking observations
  current = getMarkingObservations(observations) && current;
@@ -166,9 +196,16 @@ void VoxelLayer::updateBounds(double robot_x, double robot_y, double robot_yaw,
  // get the clearing observations
  current = getClearingObservations(clearing_observations) && current;

+  current = getFrustumClearingObservations(depth_observations) && current;
+
  // update the global current status
  current_ = current;

+  for (const DepthCameraObservation& depth_observation : depth_observations)
+  {
+    raytraceDepthFrustum(depth_observation, min_x, min_y, max_x, max_y);
+  }
+
  // raytrace freespace
  for (unsigned int i = 0; i < clearing_observations.size(); ++i)
  {
@@ -176,6 +213,8 @@ void VoxelLayer::updateBounds(double robot_x, double robot_y, double robot_yaw,
  }

  // place the new obstacles into a priority queue... each with a priority of zero to begin with
+  const double now_sec =
+      obstacle_decay_time_ > 0.0 ? robot::Time::now().toSec() : 0.0;
  for (std::vector<Observation>::const_iterator it = observations.begin(); it != observations.end(); ++it)
  {
    const Observation& obs = *it;
@@ -221,36 +260,56 @@ void VoxelLayer::updateBounds(double robot_x, double robot_y, double robot_yaw,
        unsigned int index = getIndex(mx, my);

        costmap_[index] = LETHAL_OBSTACLE;
+        if (obstacle_decay_time_ > 0.0)
+          cell_last_marked_[index] = now_sec;
        touch(double(*iter_x), double(*iter_y), min_x, min_y, max_x, max_y);
      }
    }
  }

-  // if (publish_voxel_)
-  // {
-  //   robot_costmap_2d::VoxelGrid grid_msg;
-  //   unsigned int size = robot_voxel_grid_.sizeX() * robot_voxel_grid_.sizeY();
-  //   grid_msg.size_x = robot_voxel_grid_.sizeX();
-  //   grid_msg.size_y = robot_voxel_grid_.sizeY();
-  //   grid_msg.size_z = robot_voxel_grid_.sizeZ();
-  //   grid_msg.data.resize(size);
-  //   memcpy(&grid_msg.data[0], robot_voxel_grid_.getData(), size * sizeof(unsigned int));
-
-  //   grid_msg.origin.x = origin_x_;
-  //   grid_msg.origin.y = origin_y_;
-  //   grid_msg.origin.z = origin_z_;
-
-  //   grid_msg.resolutions.x = resolution_;
-  //   grid_msg.resolutions.y = resolution_;
-  //   grid_msg.resolutions.z = z_resolution_;
-  //   grid_msg.header.frame_id = global_frame_;
-  //   grid_msg.header.stamp = robot::Time::now();
-  //   voxel_pub_.publish(grid_msg);
-  // }
+  if (obstacle_decay_time_ > 0.0)
+    decayStaleObstacles(now_sec, min_x, min_y, max_x, max_y);

  updateFootprint(robot_x, robot_y, robot_yaw, min_x, min_y, max_x, max_y);
 }

+void VoxelLayer::decayStaleObstacles(double now_sec, double* min_x, double* min_y,
+                                     double* max_x, double* max_y)
+{
+  const std::size_t map_size = static_cast<std::size_t>(size_x_) * size_y_;
+  if (cell_last_marked_.size() != map_size)
+    cell_last_marked_.assign(map_size, 0.0);
+
+  for (std::size_t index = 0; index < map_size; ++index)
+  {
+    if (costmap_[index] != LETHAL_OBSTACLE)
+      continue;
+
+    double& stamp = cell_last_marked_[index];
+    // A LETHAL cell without a stamp was marked before decay tracking covered
+    // it (e.g. right after a resize). Start its timer now instead of freeing
+    // an obstacle we have no age evidence for.
+    if (stamp <= 0.0)
+    {
+      stamp = now_sec;
+      continue;
+    }
+
+    if (now_sec - stamp <= obstacle_decay_time_)
+      continue;
+
+    costmap_[index] = FREE_SPACE;
+    robot_voxel_grid_.clearVoxelColumn(static_cast<unsigned int>(index));
+    stamp = 0.0;
+
+    unsigned int mx, my;
+    indexToCells(static_cast<unsigned int>(index), mx, my);
+    double wx, wy;
+    mapToWorld(mx, my, wx, wy);
+    touch(wx, wy, min_x, min_y, max_x, max_y);
+  }
+}
+
 void VoxelLayer::clearNonLethal(double wx, double wy, double w_size_x, double w_size_y, bool clear_no_info)
 {
  // get the cell coordinates of the center point of the window
@@ -321,14 +380,6 @@ void VoxelLayer::raytraceFreespace(const Observation& clearing_observation, doub
        ox, oy, oz);
    return;
  }
-
-  // bool publish_clearing_points = (clearing_endpoints_pub_.getNumSubscribers() > 0);
-  // if (publish_clearing_points)
-  // {
-    clearing_endpoints_.points.clear();
-    clearing_endpoints_.points.reserve(clearing_observation_cloud_size);
-  // }
-
  // we can pre-compute the enpoints of the map outside of the inner loop... we'll need these later
  double map_end_x = origin_x_ + getSizeInMetersX();
  double map_end_y = origin_y_ + getSizeInMetersY();
@@ -403,26 +454,305 @@ void VoxelLayer::raytraceFreespace(const Observation& clearing_observation, doub
                                      cell_raytrace_range);

      updateRaytraceBounds(ox, oy, wpx, wpy, clearing_observation.raytrace_range_, min_x, min_y, max_x, max_y);
-
-      // if (publish_clearing_points)
-      // {
-        robot_geometry_msgs::Point32 point;
-        point.x = wpx;
-        point.y = wpy;
-        point.z = wpz;
-        clearing_endpoints_.points.push_back(point);
-      // }
    }
  }
+}

-  // if (publish_clearing_points)
-  // {
-    clearing_endpoints_.header.frame_id = global_frame_;
-    clearing_endpoints_.header.stamp = clearing_observation.cloud_->header.stamp;
-    clearing_endpoints_.header.seq = clearing_observation.cloud_->header.seq;
+bool VoxelLayer::readDepthMeters(const robot_sensor_msgs::Image& depth, unsigned int u, unsigned int v,
+                                 double& depth_m, bool& is_valid) const
+{
+  depth_m = 0.0;
+  is_valid = false;

-    // clearing_endpoints_pub_.publish(clearing_endpoints_);
-  // }
+  if (u >= depth.width || v >= depth.height)
+    return false;
+
+  if (depth.encoding == "16UC1" || depth.encoding == "mono16")
+  {
+    const std::size_t offset = static_cast<std::size_t>(v) * depth.step + static_cast<std::size_t>(u) * 2;
+    if (offset + sizeof(std::uint16_t) > depth.data.size())
+      return false;
+
+    std::uint16_t raw = 0;
+    if (depth.is_bigendian)
+      raw = static_cast<std::uint16_t>((depth.data[offset] << 8) | depth.data[offset + 1]);
+    else
+      raw = static_cast<std::uint16_t>(depth.data[offset] | (depth.data[offset + 1] << 8));
+
+    if (raw == 0)
+      return true;
+
+    depth_m = static_cast<double>(raw) * 0.001;
+    is_valid = true;
+    return true;
+  }
+
+  if (depth.encoding == "32FC1")
+  {
+    const std::size_t offset = static_cast<std::size_t>(v) * depth.step + static_cast<std::size_t>(u) * 4;
+    if (offset + sizeof(float) > depth.data.size())
+      return false;
+
+    float raw = 0.0f;
+    if (depth.is_bigendian)
+    {
+      unsigned char bytes[sizeof(float)] = {
+          depth.data[offset + 3], depth.data[offset + 2], depth.data[offset + 1], depth.data[offset]};
+      std::memcpy(&raw, bytes, sizeof(float));
+    }
+    else
+    {
+      std::memcpy(&raw, &depth.data[offset], sizeof(float));
+    }
+
+    if (!std::isfinite(raw) || raw <= 0.0f)
+      return true;
+
+    depth_m = static_cast<double>(raw);
+    is_valid = true;
+    return true;
+  }
+
+  robot::log_error("VoxelLayer unsupported depth encoding for frustum clearing: %s\n", depth.encoding.c_str());
+  return false;
+}
+
+void VoxelLayer::updateDepthRayCache(unsigned int width, unsigned int height,
+                                     unsigned int pixel_step, double fx, double fy,
+                                     double cx, double cy,
+                                     unsigned int u_offset, unsigned int v_offset)
+{
+  if (cached_depth_width_ == width && cached_depth_height_ == height &&
+      cached_depth_pixel_step_ == pixel_step && cached_fx_ == fx && cached_fy_ == fy &&
+      cached_cx_ == cx && cached_cy_ == cy &&
+      cached_u_offset_ == u_offset && cached_v_offset_ == v_offset)
+  {
+    return;
+  }
+
+  cached_depth_width_ = width;
+  cached_depth_height_ = height;
+  cached_depth_pixel_step_ = pixel_step;
+  cached_fx_ = fx;
+  cached_fy_ = fy;
+  cached_cx_ = cx;
+  cached_cy_ = cy;
+  cached_u_offset_ = u_offset;
+  cached_v_offset_ = v_offset;
+
+  const std::size_t rows = (height + pixel_step - 1) / pixel_step;
+  const std::size_t columns = (width + pixel_step - 1) / pixel_step;
+  depth_ray_cache_.clear();
+  depth_ray_cache_.reserve(rows * columns);
+
+  for (unsigned int v = v_offset; v < height; v += pixel_step)
+  {
+    for (unsigned int u = u_offset; u < width; u += pixel_step)
+    {
+      const double x = (static_cast<double>(u) - cx) / fx;
+      const double y = (static_cast<double>(v) - cy) / fy;
+      const double inverse_norm = 1.0 / std::sqrt(x * x + y * y + 1.0);
+      depth_ray_cache_.push_back(
+          DepthRay{u, v, x * inverse_norm, y * inverse_norm, inverse_norm});
+    }
+  }
+}
+
+bool VoxelLayer::clipRaytraceEndpoint(double ox, double oy, double oz, double& wx, double& wy, double& wz)
+{
+  double a = wx - ox;
+  double b = wy - oy;
+  double c = wz - oz;
+  double t = 1.0;
+  constexpr double kEpsilon = 1e-9;
+
+  if (std::fabs(a) < kEpsilon && std::fabs(b) < kEpsilon && std::fabs(c) < kEpsilon)
+    return false;
+
+  if (wz > max_obstacle_height_ && std::fabs(c) > kEpsilon)
+    t = std::max(0.0, std::min(t, (max_obstacle_height_ - 0.01 - oz) / c));
+  else if (wz < origin_z_ && std::fabs(c) > kEpsilon)
+    t = std::min(t, (origin_z_ - oz) / c);
+
+  const double map_end_x = origin_x_ + getSizeInMetersX();
+  const double map_end_y = origin_y_ + getSizeInMetersY();
+
+  if (wx < origin_x_ && std::fabs(a) > kEpsilon)
+    t = std::min(t, (origin_x_ - ox) / a);
+  if (wy < origin_y_ && std::fabs(b) > kEpsilon)
+    t = std::min(t, (origin_y_ - oy) / b);
+  if (wx > map_end_x && std::fabs(a) > kEpsilon)
+    t = std::min(t, (map_end_x - ox) / a);
+  if (wy > map_end_y && std::fabs(b) > kEpsilon)
+    t = std::min(t, (map_end_y - oy) / b);
+
+  if (!std::isfinite(t) || t <= 0.0)
+    return false;
+
+  wx = ox + a * t;
+  wy = oy + b * t;
+  wz = oz + c * t;
+  return true;
+}
+
+bool VoxelLayer::clearVoxelRay(double ox, double oy, double oz, double wx, double wy, double wz,
+                               double raytrace_range, unsigned int cell_raytrace_range,
+                               double* min_x, double* min_y, double* max_x, double* max_y)
+{
+  double sensor_x, sensor_y, sensor_z;
+  if (!worldToMap3DFloat(ox, oy, oz, sensor_x, sensor_y, sensor_z))
+    return false;
+
+  if (!clipRaytraceEndpoint(ox, oy, oz, wx, wy, wz))
+    return false;
+
+  double point_x, point_y, point_z;
+  if (!worldToMap3DFloat(wx, wy, wz, point_x, point_y, point_z))
+    return false;
+
+  robot_voxel_grid_.clearVoxelLineInMap(sensor_x, sensor_y, sensor_z, point_x, point_y, point_z, costmap_,
+                                        unknown_threshold_, mark_threshold_, FREE_SPACE, NO_INFORMATION,
+                                        cell_raytrace_range);
+  updateRaytraceBounds(ox, oy, wx, wy, raytrace_range, min_x, min_y, max_x, max_y);
+  return true;
+}
+
+bool VoxelLayer::raytraceDepthFrustum(const DepthCameraObservation& observation,
+                                     double* min_x, double* min_y, double* max_x, double* max_y)
+{
+  if (!observation.data_)
+    return false;
+
+  const robot_sensor_msgs::DepthCameraData& depth_camera_data = *observation.data_;
+  const robot_sensor_msgs::Image& depth = depth_camera_data.depth;
+  const robot_sensor_msgs::CameraInfo& camera_info = depth_camera_data.camera_info;
+
+  if (depth.width == 0 || depth.height == 0 || depth.data.empty())
+    return false;
+
+  const double fx = camera_info.K[0];
+  const double fy = camera_info.K[4];
+  const double cx = camera_info.K[2];
+  const double cy = camera_info.K[5];
+  if (fx <= 0.0 || fy <= 0.0)
+    return false;
+
+  std::string depth_frame = depth.header.frame_id.empty() ? depth_camera_data.header.frame_id : depth.header.frame_id;
+  if (depth_frame.empty())
+    depth_frame = camera_info.header.frame_id;
+  if (depth_frame.empty() || tf_ == nullptr)
+    return false;
+
+  robot_geometry_msgs::PointStamped local_origin;
+  local_origin.header = depth.header;
+  local_origin.header.frame_id = depth_frame;
+  if (local_origin.header.stamp.isZero())
+    local_origin.header.stamp = depth_camera_data.header.stamp;
+  local_origin.point.x = 0.0;
+  local_origin.point.y = 0.0;
+  local_origin.point.z = 0.0;
+
+  robot_geometry_msgs::PointStamped global_origin;
+  tf3::TransformStampedMsg tfm;
+  try
+  {
+    tfm = tf_->lookupTransform(global_frame_, depth_frame, tf3::Time());
+    tf3::doTransform(local_origin, global_origin, tfm);
+  }
+  catch (tf3::TransformException& ex)
+  {
+    robot::log_error_throttle(
+        5.0, "VoxelLayer depth topic [%s] TF exception from %s to %s: %s\n",
+        observation.topic_.c_str(), depth_frame.c_str(), global_frame_.c_str(), ex.what());
+    return false;
+  }
+
+  const double ox = global_origin.point.x;
+  const double oy = global_origin.point.y;
+  const double oz = global_origin.point.z;
+
+  double sensor_x, sensor_y, sensor_z;
+  if (!worldToMap3DFloat(ox, oy, oz, sensor_x, sensor_y, sensor_z))
+  {
+    robot::log_error_throttle(
+        5.0, "VoxelLayer depth topic [%s] origin at (%.2f, %.2f, %.2f) is outside the voxel map\n",
+        observation.topic_.c_str(), ox, oy, oz);
+    return false;
+  }
+
+  const unsigned int step = std::max(1u, observation.pixel_step_);
+  const double min_range = observation.min_range_;
+  const double max_range = observation.max_range_;
+  const double skip_dist =
+      frustum_skip_distance_ >= 0.0 ? frustum_skip_distance_ : 2.0 * resolution_;
+  const unsigned int width = std::min(depth.width, camera_info.width == 0 ? depth.width : camera_info.width);
+  const unsigned int height = std::min(depth.height, camera_info.height == 0 ? depth.height : camera_info.height);
+
+  // Dither the sampled pixel grid every pass. With a fixed subgrid a static
+  // camera re-traces identical rays each frame; cells between adjacent rays
+  // (angular gap step / fx, ~5 cm at 2.5 m for step 8) are never crossed and
+  // stay marked until the robot moves. Cycling the phase sweeps every pixel
+  // column/row over step^2 frames at the same per-frame ray count.
+  const unsigned int u_offset = step > 1 ? frustum_phase_ % step : 0;
+  const unsigned int v_offset = step > 1 ? (frustum_phase_ / step) % step : 0;
+  ++frustum_phase_;
+  updateDepthRayCache(width, height, step, fx, fy, cx, cy, u_offset, v_offset);
+
+  double qx = tfm.transform.rotation.x;
+  double qy = tfm.transform.rotation.y;
+  double qz = tfm.transform.rotation.z;
+  double qw = tfm.transform.rotation.w;
+  const double quaternion_norm = std::sqrt(qx * qx + qy * qy + qz * qz + qw * qw);
+  if (quaternion_norm <= 0.0)
+    return false;
+  qx /= quaternion_norm;
+  qy /= quaternion_norm;
+  qz /= quaternion_norm;
+  qw /= quaternion_norm;
+
+  const double r00 = 1.0 - 2.0 * (qy * qy + qz * qz);
+  const double r01 = 2.0 * (qx * qy - qz * qw);
+  const double r02 = 2.0 * (qx * qz + qy * qw);
+  const double r10 = 2.0 * (qx * qy + qz * qw);
+  const double r11 = 1.0 - 2.0 * (qx * qx + qz * qz);
+  const double r12 = 2.0 * (qy * qz - qx * qw);
+  const double r20 = 2.0 * (qx * qz - qy * qw);
+  const double r21 = 2.0 * (qy * qz + qx * qw);
+  const double r22 = 1.0 - 2.0 * (qx * qx + qy * qy);
+  const unsigned int cell_raytrace_range = cellDistance(max_range);
+  bool cleared_any = false;
+
+  for (const DepthRay& local_ray : depth_ray_cache_)
+  {
+    double depth_m = 0.0;
+    bool valid = false;
+    if (!readDepthMeters(depth, local_ray.u, local_ray.v, depth_m, valid))
+      continue;
+
+    double ray_len = max_range;
+    if (valid && depth_m < max_range)
+      ray_len = std::max(0.0, depth_m - skip_dist);
+
+    if (ray_len <= min_range)
+      continue;
+
+    robot_geometry_msgs::Vector3 global_ray;
+    global_ray.x = r00 * local_ray.x + r01 * local_ray.y + r02 * local_ray.z;
+    global_ray.y = r10 * local_ray.x + r11 * local_ray.y + r12 * local_ray.z;
+    global_ray.z = r20 * local_ray.x + r21 * local_ray.y + r22 * local_ray.z;
+
+    const double sx = ox + global_ray.x * min_range;
+    const double sy = oy + global_ray.y * min_range;
+    const double sz = oz + global_ray.z * min_range;
+    const double wx = ox + global_ray.x * ray_len;
+    const double wy = oy + global_ray.y * ray_len;
+    const double wz = oz + global_ray.z * ray_len;
+
+    cleared_any = clearVoxelRay(sx, sy, sz, wx, wy, wz, ray_len, cell_raytrace_range,
+                                min_x, min_y, max_x, max_y) || cleared_any;
+  }
+
+  return cleared_any;
 }

 void VoxelLayer::updateOrigin(double new_origin_x, double new_origin_y)
@@ -432,6 +762,11 @@ void VoxelLayer::updateOrigin(double new_origin_x, double new_origin_y)
  cell_ox = int((new_origin_x - origin_x_) / resolution_);
  cell_oy = int((new_origin_y - origin_y_) / resolution_);

+  // Most update cycles do not cross a costmap cell boundary. Avoid copying and
+  // resetting the complete 2D/3D grids when the cell-aligned origin is unchanged.
+  if (cell_ox == 0 && cell_oy == 0)
+    return;
+
  // compute the associated world coordinates for the origin cell
  // beacuase we want to keep things grid-aligned
  double new_grid_ox, new_grid_oy;
@@ -452,15 +787,24 @@ void VoxelLayer::updateOrigin(double new_origin_x, double new_origin_y)
  unsigned int cell_size_x = upper_right_x - lower_left_x;
  unsigned int cell_size_y = upper_right_y - lower_left_y;

-  // we need a map to store the obstacles in the window temporarily
-  unsigned char* local_map = new unsigned char[cell_size_x * cell_size_y];
-  unsigned int* local_voxel_map = new unsigned int[cell_size_x * cell_size_y];
+  const std::size_t overlap_size = static_cast<std::size_t>(cell_size_x) * cell_size_y;
+  rolling_costmap_scratch_.resize(overlap_size);
+  rolling_voxel_scratch_.resize(overlap_size);
+  rolling_stamp_scratch_.resize(overlap_size);
+  unsigned char* local_map = rolling_costmap_scratch_.data();
+  unsigned int* local_voxel_map = rolling_voxel_scratch_.data();
+  double* local_stamp_map = rolling_stamp_scratch_.data();
  unsigned int* voxel_map = robot_voxel_grid_.getData();

-  // copy the local window in the costmap to the local map
-  copyMapRegion(costmap_, lower_left_x, lower_left_y, size_x_, local_map, 0, 0, cell_size_x, cell_size_x, cell_size_y);
-  copyMapRegion(voxel_map, lower_left_x, lower_left_y, size_x_, local_voxel_map, 0, 0, cell_size_x, cell_size_x,
-                cell_size_y);
+  if (overlap_size > 0)
+  {
+    copyMapRegion(costmap_, lower_left_x, lower_left_y, size_x_, local_map, 0, 0,
+                  cell_size_x, cell_size_x, cell_size_y);
+    copyMapRegion(voxel_map, lower_left_x, lower_left_y, size_x_, local_voxel_map, 0, 0,
+                  cell_size_x, cell_size_x, cell_size_y);
+    copyMapRegion(cell_last_marked_.data(), lower_left_x, lower_left_y, size_x_,
+                  local_stamp_map, 0, 0, cell_size_x, cell_size_x, cell_size_y);
+  }

  // we'll reset our maps to unknown space if appropriate
  resetMaps();
@@ -474,12 +818,16 @@ void VoxelLayer::updateOrigin(double new_origin_x, double new_origin_y)
  int start_y = lower_left_y - cell_oy;

  // now we want to copy the overlapping information back into the map, but in its new location
-  copyMapRegion(local_map, 0, 0, cell_size_x, costmap_, start_x, start_y, size_x_, cell_size_x, cell_size_y);
-  copyMapRegion(local_voxel_map, 0, 0, cell_size_x, voxel_map, start_x, start_y, size_x_, cell_size_x, cell_size_y);
+  if (overlap_size > 0)
+  {
+    copyMapRegion(local_map, 0, 0, cell_size_x, costmap_, start_x, start_y,
+                  size_x_, cell_size_x, cell_size_y);
+    copyMapRegion(local_voxel_map, 0, 0, cell_size_x, voxel_map, start_x, start_y,
+                  size_x_, cell_size_x, cell_size_y);
+    copyMapRegion(local_stamp_map, 0, 0, cell_size_x, cell_last_marked_.data(),
+                  start_x, start_y, size_x_, cell_size_x, cell_size_y);
+  }

-  // make sure to clean up
-  delete[] local_map;
-  delete[] local_voxel_map;
 }

 // Export factory function
--- a/src/costmap_2d.cpp
+++ b/src/costmap_2d.cpp
@@ -288,11 +288,17 @@ void Costmap2D::updateOrigin(double new_origin_x, double new_origin_y)
  unsigned int cell_size_x = upper_right_x - lower_left_x;
  unsigned int cell_size_y = upper_right_y - lower_left_y;

-  // we need a map to store the obstacles in the window temporarily
-  unsigned char* local_map = new unsigned char[cell_size_x * cell_size_y];
+  const std::size_t overlap_size = static_cast<std::size_t>(cell_size_x) * cell_size_y;

-  // copy the local window in the costmap to the local map
-  copyMapRegion(costmap_, lower_left_x, lower_left_y, size_x_, local_map, 0, 0, cell_size_x, cell_size_x, cell_size_y);
+  // Reuse the temporary window to avoid allocating on every rolling-window shift.
+  rolling_window_scratch_.resize(overlap_size);
+  unsigned char* local_map = rolling_window_scratch_.data();
+
+  if (overlap_size > 0)
+  {
+    copyMapRegion(costmap_, lower_left_x, lower_left_y, size_x_, local_map, 0, 0,
+                  cell_size_x, cell_size_x, cell_size_y);
+  }

  // now we'll set the costmap to be completely unknown if we track unknown space
  resetMaps();
@@ -306,10 +312,12 @@ void Costmap2D::updateOrigin(double new_origin_x, double new_origin_y)
  int start_y = lower_left_y - cell_oy;

  // now we want to copy the overlapping information back into the map, but in its new location
-  copyMapRegion(local_map, 0, 0, cell_size_x, costmap_, start_x, start_y, size_x_, cell_size_x, cell_size_y);
+  if (overlap_size > 0)
+  {
+    copyMapRegion(local_map, 0, 0, cell_size_x, costmap_, start_x, start_y,
+                  size_x_, cell_size_x, cell_size_y);
+  }

-  // make sure to clean up
-  delete[] local_map;
 }

 bool Costmap2D::setConvexPolygonCost(const std::vector<robot_geometry_msgs::Point>& polygon, unsigned char cost_value)
--- a/src/costmap_2d_robot.cpp
+++ b/src/costmap_2d_robot.cpp
@@ -55,6 +55,16 @@ using namespace std;

 namespace robot_costmap_2d
 {
+template <class T>
+void move_parameter(robot::NodeHandle& old_h, robot::NodeHandle& new_h, std::string name,T& value)
+{
+  if (!old_h.hasParam(name))
+    return;
+
+  old_h.getParam(name, value);
+  new_h.setParam(name, value);
+}
+
 Costmap2DROBOT::Costmap2DROBOT(const std::string& name, tf3::BufferCore& tf) :
    layered_costmap_(NULL),
    name_(name),
@@ -71,38 +81,45 @@ Costmap2DROBOT::Costmap2DROBOT(const std::string& name, tf3::BufferCore& tf) :
  robot::NodeHandle priv_nh(nh, name);
  name_ = name;
  std::string config_file_name = "costmap_params.yaml";
-  getParams(config_file_name, priv_nh);
-
+  getParams(config_file_name, name_, nh);
+ 
  // create a thread to handle updating the map
  stop_updates_ = false;
  initialized_ = true;
  stopped_ = false;
 }

-void Costmap2DROBOT::getParams(const std::string& config_file_name, robot::NodeHandle& nh)
+void Costmap2DROBOT::getParams(const std::string& config_file_name,const std::string& name, robot::NodeHandle& nh)
 {
  try 
  {
-    std::string folder = ROBOT_COSTMAP_2D_DIR;
+    const char *env_config = std::getenv("PNKX_NAV_CORE_CONFIG_DIR");
+    std::string folder;
+    if (env_config && std::filesystem::exists(env_config))
+    {
+      folder = std::string(env_config);
+      // robot::log_error("config_directory: %s", folder.c_str());
+    }
    std::string path_source = getSourceFile(folder,config_file_name);

    YAML::Node config = YAML::LoadFile(path_source);
    YAML::Node layer = config["robot_costmap_2d"];

+    robot::NodeHandle priv_nh(priv_nh, name);
+
    std::string global_frame =
    loadParam(layer, "global_frame", std::string("map"));
    std::string robot_base_frame =
        loadParam(layer, "robot_base_frame", std::string("base_link"));

-    if (nh.hasParam("global_frame"))
-      nh.getParam("global_frame", global_frame);
+    if (priv_nh.hasParam("global_frame"))
+      priv_nh.getParam("global_frame", global_frame);

    if (nh.hasParam("robot_base_frame"))
      nh.getParam("robot_base_frame", robot_base_frame);

    global_frame_ = global_frame;
    robot_base_frame_ = robot_base_frame;
-    robot::log_error("robot_base_frame: %s", robot_base_frame.c_str());

    robot::Time last_error = robot::Time::now();
    std::string tf_error;
@@ -112,6 +129,8 @@ void Costmap2DROBOT::getParams(const std::string& config_file_name, robot::NodeH
    {
      if (last_error + robot::Duration(5.0) < robot::Time::now())
      {
+        std::string all_frames_string = tf_.allFramesAsString();
+        robot::log_info("[%s:%d]\n INFO: tf allFramesAsString: %s", __FILE__, __LINE__, all_frames_string.c_str());
        // std::cout << std::fixed << std::setprecision(6) << robot::Time::now().toSec() << std::endl;
        robot::log_warning("[%s:%d] %0.6f: Timed out waiting for transform from %s to %s to become available before running costmap, tf error: %s\n",
                __FILE__, __LINE__, robot::Time::now().toSec(), robot_base_frame_.c_str(), global_frame_.c_str(), tf_error.c_str());
@@ -130,16 +149,26 @@ void Costmap2DROBOT::getParams(const std::string& config_file_name, robot::NodeH
    robot::PluginLoaderHelper loader;
    

-    if (nh.hasParam("rolling_window"))
-      nh.getParam("rolling_window", rolling_window);
+    if (priv_nh.hasParam("rolling_window"))
+      priv_nh.getParam("rolling_window", rolling_window);

-    if (nh.hasParam("track_unknown_space"))
-      nh.getParam("track_unknown_space", track_unknown_space);
+    if (priv_nh.hasParam("track_unknown_space"))
+      priv_nh.getParam("track_unknown_space", track_unknown_space);

-    if (nh.hasParam("library_path"))
+    bool performance_metrics_enabled =
+        loadParam(layer, "performance_metrics_enabled", false);
+    double performance_metrics_period =
+        loadParam(layer, "performance_metrics_period", 5.0);
+    if (priv_nh.hasParam("performance_metrics_enabled"))
+      priv_nh.getParam("performance_metrics_enabled", performance_metrics_enabled);
+    if (priv_nh.hasParam("performance_metrics_period"))
+      priv_nh.getParam("performance_metrics_period", performance_metrics_period);
+
+    if (priv_nh.hasParam("library_path"))
      path_plugins = loader.findLibraryPath(name_);
-    robot::log_error("name: %s | rolling_window: %x", name_.c_str(), rolling_window);
    layered_costmap_ = new LayeredCostmap(global_frame_, rolling_window, track_unknown_space);
+    layered_costmap_->setPerformanceMetrics(
+        performance_metrics_enabled, performance_metrics_period);

    // find size parameters
    double map_width_meters = loadParam(layer, "width", 0.0);
@@ -148,16 +177,16 @@ void Costmap2DROBOT::getParams(const std::string& config_file_name, robot::NodeH
    double origin_x = loadParam(layer, "origin_x", 0.0);
    double origin_y = loadParam(layer, "origin_y", 0.0);

-    if (nh.hasParam("width"))
-      nh.getParam("width", map_width_meters);
-    if (nh.hasParam("height"))
-      nh.getParam("height", map_height_meters);
-    if (nh.hasParam("resolution"))
-      nh.getParam("resolution", resolution);
-    if (nh.hasParam("origin_x"))
-      nh.getParam("origin_x", origin_x);
-    if (nh.hasParam("origin_y"))
-      nh.getParam("origin_y", origin_y);
+    if (priv_nh.hasParam("width"))
+      priv_nh.getParam("width", map_width_meters);
+    if (priv_nh.hasParam("height"))
+      priv_nh.getParam("height", map_height_meters);
+    if (priv_nh.hasParam("resolution"))
+      priv_nh.getParam("resolution", resolution);
+    if (priv_nh.hasParam("origin_x"))
+      priv_nh.getParam("origin_x", origin_x);
+    if (priv_nh.hasParam("origin_y"))
+      priv_nh.getParam("origin_y", origin_y);

    if (!layered_costmap_->isSizeLocked())
    {
@@ -173,10 +202,10 @@ void Costmap2DROBOT::getParams(const std::string& config_file_name, robot::NodeH
    };
    std::vector<PluginConfig> my_list;
      
-    if(nh.hasParam("plugins"))
+    if(priv_nh.hasParam("plugins"))
    {
      my_list.clear();
-      YAML::Node my_plugins = nh.getParamValue("plugins");
+      YAML::Node my_plugins = priv_nh.getParamValue("plugins");
      if (my_plugins && my_plugins.IsSequence())
      {
        for (size_t i = 0; i < my_plugins.size(); ++i)
@@ -230,12 +259,12 @@ void Costmap2DROBOT::getParams(const std::string& config_file_name, robot::NodeH
        }
      }
    }
-
+    robot::NodeHandle private_nh("~");
    for (auto& info : my_list)
    {
      try
      {
-        // copyParentParameters(pname, type, private_nh);
+        copyParentParameters(name_, info.name, info.type, private_nh);
        creators_.push_back(
          boost::dll::import_alias<PluginLayerPtr()>(
          path_plugins, info.type, boost::dll::load_mode::append_decorations)
@@ -257,14 +286,15 @@ void Costmap2DROBOT::getParams(const std::string& config_file_name, robot::NodeH
    new_footprint = loadFootprint(layer["footprint"], new_footprint);
    transform_tolerance_ = loadParam(layer, "transform_tolerance", 0.0);

-    if (nh.hasParam("footprint"))
+    if (priv_nh.hasParam("footprint"))
    {
      std::cout <<"FOOTPRINT ROBOT:"<<std::endl;
-      new_footprint = makeFootprintFromParams(nh);
+      new_footprint = makeFootprintFromParams(priv_nh);
    }

    if (nh.hasParam("transform_tolerance"))
      nh.getParam("transform_tolerance", transform_tolerance_);
+    // robot::log_info("transform_tolerance: %d", transform_tolerance_);
    
    setUnpaddedRobotFootprint(new_footprint);

@@ -280,8 +310,8 @@ void Costmap2DROBOT::getParams(const std::string& config_file_name, robot::NodeH
    map_update_thread_shutdown_ = false;
    double map_update_frequency = loadParam(layer, "update_frequency", 0.0);

-    if (nh.hasParam("update_frequency"))
-      nh.getParam("update_frequency", map_update_frequency);
+    if (priv_nh.hasParam("update_frequency"))
+      priv_nh.getParam("update_frequency", map_update_frequency);

    // If the padding has changed, call setUnpaddedRobotFootprint() to
    // re-apply the padding.
@@ -295,8 +325,8 @@ void Costmap2DROBOT::getParams(const std::string& config_file_name, robot::NodeH
    }

    double robot_radius = loadParam(layer, "robot_radius", 0.0);
-    if (nh.hasParam("robot_radius"))
-      nh.getParam("robot_radius", robot_radius);
+    if (priv_nh.hasParam("robot_radius"))
+      priv_nh.getParam("robot_radius", robot_radius);
    readFootprintFromConfig(new_footprint, unpadded_footprint_, robot_radius);

    // only construct the thread if the frequency is positive
@@ -310,6 +340,157 @@ void Costmap2DROBOT::getParams(const std::string& config_file_name, robot::NodeH
  }
 }

+void Costmap2DROBOT::copyParentParameters(const std::string& costmap_name, 
+                                          const std::string& plugin_name, 
+                                          const std::string& plugin_type, 
+                                          robot::NodeHandle& nh)
+{
+  robot::NodeHandle costmap_nh(nh, costmap_name);
+  robot::NodeHandle costmap_plugin_nh(costmap_nh, plugin_name);
+  robot::NodeHandle plugin_nh(nh, plugin_name);
+  if(plugin_type == "StaticLayer")
+  {
+    std::string map_topic;
+    int unknown_cost_value;
+    int lethal_cost_threshold;
+    bool track_unknown_space;
+    move_parameter(plugin_nh, costmap_plugin_nh, "map_topic", map_topic);
+    move_parameter(plugin_nh, costmap_plugin_nh, "unknown_cost_value", unknown_cost_value);
+    move_parameter(plugin_nh, costmap_plugin_nh, "lethal_cost_threshold", lethal_cost_threshold);
+    move_parameter(plugin_nh, costmap_plugin_nh, "track_unknown_space", track_unknown_space);
+  }
+  else if(plugin_type == "VoxelLayer")
+  {
+    double origin_z;
+    double z_resolution;
+    int z_voxels;
+    int mark_threshold;
+    int unknown_threshold;
+    bool publish_voxel_map;
+    move_parameter(plugin_nh, costmap_plugin_nh, "origin_z", origin_z);
+    move_parameter(plugin_nh, costmap_plugin_nh, "z_resolution", z_resolution);
+    move_parameter(plugin_nh, costmap_plugin_nh, "z_voxels", z_voxels);
+    move_parameter(plugin_nh, costmap_plugin_nh, "mark_threshold", mark_threshold);
+    move_parameter(plugin_nh, costmap_plugin_nh, "unknown_threshold", unknown_threshold);
+    move_parameter(plugin_nh, costmap_plugin_nh, "publish_voxel_map", publish_voxel_map);
+    if(plugin_nh.hasParam("observation_sources"))
+    {
+      std::string topics_string;
+      move_parameter(plugin_nh, costmap_plugin_nh, "observation_sources", topics_string);
+      robot::log_error("topics_string: %s", topics_string.c_str());
+      std::stringstream ss(topics_string);
+      std::string source;
+      while (ss >> source)
+      {
+        robot::NodeHandle plugin_nh_element(plugin_nh, source);
+        robot::NodeHandle costmap_plugin_nh_element(costmap_plugin_nh, source);
+        std::string topic;
+        std::string data_type;
+        bool clearing;
+        bool marking;
+        bool inf_is_valid;
+        std::string sensor_frame;
+        double observation_persistence;
+        double expected_update_rate;
+        double min_obstacle_height;
+        double max_obstacle_height;
+        double obstacle_range;
+        double raytrace_range;
+        bool frustum_clearing_enabled = false;
+        int frustum_clearing_pixel_step = 8;
+        double frustum_min_range = 0.2;
+        double frustum_max_range = 3.0;
+
+        move_parameter(plugin_nh_element, costmap_plugin_nh_element, "topic", topic);
+        move_parameter(plugin_nh_element, costmap_plugin_nh_element, "sensor_frame", sensor_frame);
+        move_parameter(plugin_nh_element, costmap_plugin_nh_element, "observation_persistence", observation_persistence);
+        move_parameter(plugin_nh_element, costmap_plugin_nh_element, "expected_update_rate", expected_update_rate);
+        move_parameter(plugin_nh_element, costmap_plugin_nh_element, "data_type", data_type);
+        move_parameter(plugin_nh_element, costmap_plugin_nh_element, "clearing", clearing);
+        move_parameter(plugin_nh_element, costmap_plugin_nh_element, "marking", marking);
+        move_parameter(plugin_nh_element, costmap_plugin_nh_element, "inf_is_valid", inf_is_valid);
+        move_parameter(plugin_nh_element, costmap_plugin_nh_element, "min_obstacle_height", min_obstacle_height);
+        move_parameter(plugin_nh_element, costmap_plugin_nh_element, "max_obstacle_height", max_obstacle_height);
+        move_parameter(plugin_nh_element, costmap_plugin_nh_element, "obstacle_range", obstacle_range);
+        move_parameter(plugin_nh_element, costmap_plugin_nh_element, "raytrace_range", raytrace_range);
+        move_parameter(plugin_nh_element, costmap_plugin_nh_element, "frustum_clearing_enabled", frustum_clearing_enabled);
+        move_parameter(plugin_nh_element, costmap_plugin_nh_element, "frustum_clearing_pixel_step", frustum_clearing_pixel_step);
+        move_parameter(plugin_nh_element, costmap_plugin_nh_element, "frustum_min_range", frustum_min_range);
+        move_parameter(plugin_nh_element, costmap_plugin_nh_element, "frustum_max_range", frustum_max_range);
+        robot::log_info("topic: %s data_type: %s clearing: %d marking: %d inf_is_valid: %d min_obstacle_height: %f max_obstacle_height: %f", topic.c_str(), data_type.c_str(), clearing, marking, inf_is_valid, min_obstacle_height, max_obstacle_height);
+        robot::log_info("frustum_clearing_enabled: %s, frustum_clearing_pixel_step: %d, frustum_min_range: %f, frustum_max_range: %f", frustum_clearing_enabled ? "true" : "false", frustum_clearing_pixel_step, frustum_min_range, frustum_max_range);
+      }
+    }
+  }
+  else if(plugin_type == "ObstacleLayer")
+  {
+    double max_obstacle_height;
+    double raytrace_range;
+    double obstacle_range;
+    bool track_unknown_space;
+    move_parameter(plugin_nh, costmap_plugin_nh, "max_obstacle_height", max_obstacle_height);
+    move_parameter(plugin_nh, costmap_plugin_nh, "raytrace_range", raytrace_range);
+    move_parameter(plugin_nh, costmap_plugin_nh, "obstacle_range", obstacle_range);
+    move_parameter(plugin_nh, costmap_plugin_nh, "track_unknown_space", track_unknown_space);
+    if(plugin_nh.hasParam("observation_sources"))
+    {
+      std::string topics_string;
+      move_parameter(plugin_nh, costmap_plugin_nh, "observation_sources", topics_string);
+      robot::log_error("topics_string: %s", topics_string.c_str());
+      std::stringstream ss(topics_string);
+      std::string source;
+      while (ss >> source)
+      {
+        robot::NodeHandle plugin_nh_element(plugin_nh, source);
+        robot::NodeHandle costmap_plugin_nh_element(costmap_plugin_nh, source);
+        std::string topic;
+        std::string data_type;
+        bool clearing;
+        bool marking;
+        bool inf_is_valid;
+        std::string sensor_frame;
+        double observation_persistence;
+        double expected_update_rate;
+        double min_obstacle_height;
+        double max_obstacle_height;
+        double obstacle_range;
+        double raytrace_range;
+        bool frustum_clearing_enabled = false;
+        int frustum_clearing_pixel_step = 8;
+        double frustum_min_range = 0.2;
+        double frustum_max_range = 3.0;
+
+        move_parameter(plugin_nh_element, costmap_plugin_nh_element, "topic", topic);
+        move_parameter(plugin_nh_element, costmap_plugin_nh_element, "sensor_frame", sensor_frame);
+        move_parameter(plugin_nh_element, costmap_plugin_nh_element, "observation_persistence", observation_persistence);
+        move_parameter(plugin_nh_element, costmap_plugin_nh_element, "expected_update_rate", expected_update_rate);
+        move_parameter(plugin_nh_element, costmap_plugin_nh_element, "data_type", data_type);
+        move_parameter(plugin_nh_element, costmap_plugin_nh_element, "clearing", clearing);
+        move_parameter(plugin_nh_element, costmap_plugin_nh_element, "marking", marking);
+        move_parameter(plugin_nh_element, costmap_plugin_nh_element, "inf_is_valid", inf_is_valid);
+        move_parameter(plugin_nh_element, costmap_plugin_nh_element, "min_obstacle_height", min_obstacle_height);
+        move_parameter(plugin_nh_element, costmap_plugin_nh_element, "max_obstacle_height", max_obstacle_height);
+        move_parameter(plugin_nh_element, costmap_plugin_nh_element, "obstacle_range", obstacle_range);
+        move_parameter(plugin_nh_element, costmap_plugin_nh_element, "raytrace_range", raytrace_range);
+        move_parameter(plugin_nh_element, costmap_plugin_nh_element, "frustum_clearing_enabled", frustum_clearing_enabled);
+        move_parameter(plugin_nh_element, costmap_plugin_nh_element, "frustum_clearing_pixel_step", frustum_clearing_pixel_step);
+        move_parameter(plugin_nh_element, costmap_plugin_nh_element, "frustum_min_range", frustum_min_range);
+        move_parameter(plugin_nh_element, costmap_plugin_nh_element, "frustum_max_range", frustum_max_range);
+        robot::log_info("topic: %s data_type: %s clearing: %d marking: %d inf_is_valid: %d min_obstacle_height: %f max_obstacle_height: %f", topic.c_str(), data_type.c_str(), clearing, marking, inf_is_valid, min_obstacle_height, max_obstacle_height);
+        robot::log_info("frustum_clearing_enabled: %s, frustum_clearing_pixel_step: %d, frustum_min_range: %f, frustum_max_range: %f", frustum_clearing_enabled ? "true" : "false", frustum_clearing_pixel_step, frustum_min_range, frustum_max_range);
+      }
+    }
+  }
+  else if(plugin_type == "InflationLayer")
+  {
+    double cost_scaling_factor;
+    double inflation_radius;
+    move_parameter(plugin_nh, costmap_plugin_nh, "cost_scaling_factor", cost_scaling_factor);
+    move_parameter(plugin_nh, costmap_plugin_nh, "inflation_radius", inflation_radius);
+  }
+
+}
+
 void Costmap2DROBOT::setUnpaddedRobotFootprintPolygon(const robot_geometry_msgs::Polygon& footprint)
 { 
  setUnpaddedRobotFootprint(toPointVector(footprint));
@@ -402,16 +583,17 @@ void Costmap2DROBOT::updateMap()
             yaw = data_convert::getYaw(pose.pose.orientation);
      // robot::log_error("ROBOT POSE: %f | %f | %f",x,y,yaw);
      layered_costmap_->updateMap(x, y, yaw);
-      robot_geometry_msgs::PolygonStamped footprint;
-      footprint.header.frame_id = global_frame_;
-      footprint.header.stamp = robot::Time::now();
-      transformFootprint(x, y, yaw, padded_footprint_, footprint);
+      
+      footprint_.header.frame_id = global_frame_;
+      footprint_.header.stamp = robot::Time::now();
+      transformFootprint(x, y, yaw, padded_footprint_, footprint_);

      initialized_ = true;
    }
  }
 }

+
 void Costmap2DROBOT::start()
 {
  // if(name_.find("global_costmap")) ROS_WARN("Costmap2DROBOT::start");
@@ -521,35 +703,9 @@ bool Costmap2DROBOT::getRobotPose(robot_geometry_msgs::PoseStamped& global_pose)
  // get the global pose of the robot
  try
  {
-    // use current time if possible (makes sure it's not in the future)
-    if (tf_.canTransform(global_frame_, robot_base_frame_, tf3::Time()))
-    {
-      tf3::TransformStampedMsg transform = tf_.lookupTransform(global_frame_, robot_base_frame_,tf3::Time());
-      tf3::doTransform(robot_pose, global_pose, transform);
-      // robot::log_error("%s ||| %f | %f | %f ||| %f | %f | %f | %f", transform.child_frame_id.c_str(),
-      // global_pose.pose.position.x,
-      // global_pose.pose.position.y,
-      // global_pose.pose.position.z,
-      // global_pose.pose.orientation.x,
-      // global_pose.pose.orientation.y,
-      // global_pose.pose.orientation.z,
-      // global_pose.pose.orientation.w);
-      // transform.transform.rotation.x,
-      // transform.transform.rotation.y,
-      // transform.transform.rotation.z,
-      // transform.transform.rotation.w);
-    }
-    // use the latest otherwise
-    else
-    {
-      // tf_.transform(robot_pose, global_pose, global_frame_);
-      tf3::TransformStampedMsg transform = tf_.lookupTransform(
-                    global_frame_,                      // frame đích
-                    robot_base_frame_,                      // frame nguồn
-                    tf3::Time()
-                );
-      tf3::doTransform(robot_pose, global_pose, transform);
-    }
+    const tf3::TransformStampedMsg transform =
+        tf_.lookupTransform(global_frame_, robot_base_frame_, tf3::Time());
+    tf3::doTransform(robot_pose, global_pose, transform);
  }
  catch (tf3::LookupException& ex)
  {
@@ -563,7 +719,7 @@ bool Costmap2DROBOT::getRobotPose(robot_geometry_msgs::PoseStamped& global_pose)
  }
  catch (tf3::ExtrapolationException& ex)
  {
-    robot::log_error("Costmap2DROBOT %s Extrapolation Error looking up robot pose: %s\n", name_.c_str(), ex.what());
+    // robot::log_error("Costmap2DROBOT %s Extrapolation Error looking up robot pose: %s\n", name_.c_str(), ex.what());
    return false;
  }
  // ROS_INFO_THROTTLE(1.0, "Time Delay %f , p %f %f", current_time.toSec() - global_pose.header.stamp.toSec(), global_pose.pose.position.x, global_pose.pose.position.y);
--- a/src/layered_costmap.cpp
+++ b/src/layered_costmap.cpp
@@ -69,6 +69,68 @@ namespace robot_costmap_2d
      costmap_.setDefaultValue(NO_INFORMATION);
    else
      costmap_.setDefaultValue(FREE_SPACE);
+    performance_window_start_ = std::chrono::steady_clock::now();
+  }
+
+  void LayeredCostmap::setPerformanceMetrics(bool enabled, double reporting_period_seconds)
+  {
+    performance_metrics_enabled_ = enabled;
+    performance_metrics_period_seconds_ = reporting_period_seconds > 0.0 ? reporting_period_seconds : 5.0;
+    resetPerformanceMetrics();
+  }
+
+  void LayeredCostmap::resetPerformanceMetrics()
+  {
+    performance_window_start_ = std::chrono::steady_clock::now();
+    performance_cycle_nanoseconds_ = 0;
+    performance_reset_nanoseconds_ = 0;
+    performance_cycles_ = 0;
+    performance_cycle_samples_.clear();
+    performance_cycle_samples_.reserve(128);
+    layer_performance_.assign(plugins_.size(), LayerPerformance());
+  }
+
+  void LayeredCostmap::maybeReportPerformance()
+  {
+    if (!performance_metrics_enabled_ || performance_cycles_ == 0)
+      return;
+
+    const auto now = std::chrono::steady_clock::now();
+    const double elapsed = std::chrono::duration<double>(now - performance_window_start_).count();
+    if (elapsed < performance_metrics_period_seconds_)
+      return;
+
+    const double average_cycle_ms =
+        static_cast<double>(performance_cycle_nanoseconds_) / performance_cycles_ / 1.0e6;
+    const double average_reset_ms =
+        static_cast<double>(performance_reset_nanoseconds_) / performance_cycles_ / 1.0e6;
+    std::sort(performance_cycle_samples_.begin(), performance_cycle_samples_.end());
+    const auto percentile_ms = [this](double percentile) {
+      if (performance_cycle_samples_.empty())
+        return 0.0;
+      const std::size_t index = static_cast<std::size_t>(
+          percentile * static_cast<double>(performance_cycle_samples_.size() - 1));
+      return static_cast<double>(performance_cycle_samples_[index]) / 1.0e6;
+    };
+    robot::log_info(
+        "Costmap performance: cycles=%llu avg_cycle_ms=%.3f p95_cycle_ms=%.3f "
+        "p99_cycle_ms=%.3f avg_reset_ms=%.3f\n",
+        static_cast<unsigned long long>(performance_cycles_), average_cycle_ms,
+        percentile_ms(0.95), percentile_ms(0.99), average_reset_ms);
+
+    for (std::size_t i = 0; i < plugins_.size() && i < layer_performance_.size(); ++i)
+    {
+      const LayerPerformance& stats = layer_performance_[i];
+      const double average_bounds_ms = stats.bounds_calls == 0 ? 0.0 :
+          static_cast<double>(stats.bounds_nanoseconds) / stats.bounds_calls / 1.0e6;
+      const double average_costs_ms = stats.costs_calls == 0 ? 0.0 :
+          static_cast<double>(stats.costs_nanoseconds) / stats.costs_calls / 1.0e6;
+      robot::log_info(
+          "Costmap layer [%s]: avg_bounds_ms=%.3f avg_costs_ms=%.3f\n",
+          plugins_[i]->getName().c_str(), average_bounds_ms, average_costs_ms);
+    }
+
+    resetPerformanceMetrics();
  }

  LayeredCostmap::~LayeredCostmap()
@@ -94,6 +156,8 @@ namespace robot_costmap_2d

  void LayeredCostmap::updateMap(double robot_x, double robot_y, double robot_yaw)
  {
+    const auto cycle_start = performance_metrics_enabled_ ? std::chrono::steady_clock::now() :
+        std::chrono::steady_clock::time_point();
    // Lock for the remainder of this function, some plugins (e.g. VoxelLayer)
    // implement thread unsafe updateBounds() functions.
    boost::unique_lock<Costmap2D::mutex_t> lock(*(costmap_.getMutex()));
@@ -111,23 +175,35 @@ namespace robot_costmap_2d

    minx_ = miny_ = 1e30;
    maxx_ = maxy_ = -1e30;
-    for (vector<boost::shared_ptr<Layer>>::iterator plugin = plugins_.begin(); plugin != plugins_.end();
-         ++plugin)
+    if (performance_metrics_enabled_ && layer_performance_.size() != plugins_.size())
+      layer_performance_.assign(plugins_.size(), LayerPerformance());
+
+    for (std::size_t plugin_index = 0; plugin_index < plugins_.size(); ++plugin_index)
    {
-      if (!(*plugin)->isEnabled())
+      const boost::shared_ptr<Layer>& plugin = plugins_[plugin_index];
+      if (!plugin->isEnabled())
        continue;
      double prev_minx = minx_;
      double prev_miny = miny_;
      double prev_maxx = maxx_;
      double prev_maxy = maxy_;
-      (*plugin)->updateBounds(robot_x, robot_y, robot_yaw, &minx_, &miny_, &maxx_, &maxy_);
+      const auto bounds_start = performance_metrics_enabled_ ? std::chrono::steady_clock::now() :
+          std::chrono::steady_clock::time_point();
+      plugin->updateBounds(robot_x, robot_y, robot_yaw, &minx_, &miny_, &maxx_, &maxy_);
+      if (performance_metrics_enabled_)
+      {
+        layer_performance_[plugin_index].bounds_nanoseconds +=
+            std::chrono::duration_cast<std::chrono::nanoseconds>(
+                std::chrono::steady_clock::now() - bounds_start).count();
+        ++layer_performance_[plugin_index].bounds_calls;
+      }
      if (minx_ > prev_minx || miny_ > prev_miny || maxx_ < prev_maxx || maxy_ < prev_maxy)
      {
        robot::log_error("Illegal bounds change, was [tl: (%f, %f), br: (%f, %f)], but "
                               "is now [tl: (%f, %f), br: (%f, %f)]. The offending layer is %s\n",
                          prev_minx, prev_miny, prev_maxx, prev_maxy,
                          minx_, miny_, maxx_, maxy_,
-                          (*plugin)->getName().c_str());
+                          plugin->getName().c_str());
      }
    }

@@ -143,13 +219,32 @@ namespace robot_costmap_2d
    if (xn < x0 || yn < y0)
      return;

+    const auto reset_start = performance_metrics_enabled_ ? std::chrono::steady_clock::now() :
+        std::chrono::steady_clock::time_point();
    costmap_.resetMap(x0, y0, xn, yn);
-    
-    for (vector<boost::shared_ptr<Layer>>::iterator plugin = plugins_.begin(); plugin != plugins_.end();
-         ++plugin)
+    if (performance_metrics_enabled_)
    {
-      if ((*plugin)->isEnabled())
-        (*plugin)->updateCosts(costmap_, x0, y0, xn, yn);
+      performance_reset_nanoseconds_ +=
+          std::chrono::duration_cast<std::chrono::nanoseconds>(
+              std::chrono::steady_clock::now() - reset_start).count();
+    }
+    
+    for (std::size_t plugin_index = 0; plugin_index < plugins_.size(); ++plugin_index)
+    {
+      const boost::shared_ptr<Layer>& plugin = plugins_[plugin_index];
+      if (!plugin->isEnabled())
+        continue;
+
+      const auto costs_start = performance_metrics_enabled_ ? std::chrono::steady_clock::now() :
+          std::chrono::steady_clock::time_point();
+      plugin->updateCosts(costmap_, x0, y0, xn, yn);
+      if (performance_metrics_enabled_)
+      {
+        layer_performance_[plugin_index].costs_nanoseconds +=
+            std::chrono::duration_cast<std::chrono::nanoseconds>(
+                std::chrono::steady_clock::now() - costs_start).count();
+        ++layer_performance_[plugin_index].costs_calls;
+      }
    }

    bx0_ = x0;
@@ -158,6 +253,17 @@ namespace robot_costmap_2d
    byn_ = yn;

    initialized_ = true;
+
+    if (performance_metrics_enabled_)
+    {
+      const std::uint64_t cycle_nanoseconds = static_cast<std::uint64_t>(
+          std::chrono::duration_cast<std::chrono::nanoseconds>(
+              std::chrono::steady_clock::now() - cycle_start).count());
+      performance_cycle_nanoseconds_ += cycle_nanoseconds;
+      performance_cycle_samples_.push_back(cycle_nanoseconds);
+      ++performance_cycles_;
+      maybeReportPerformance();
+    }
  }

  bool LayeredCostmap::isCurrent()
--- a/src/observation_buffer.cpp
+++ b/src/observation_buffer.cpp
@@ -40,6 +40,8 @@
 #include <robot_tf3_sensor_msgs/tf3_sensor_msgs.h>
 #include <robot_sensor_msgs/point_cloud2_iterator.h>

+#include <cstring>
+
 using namespace std;
 using namespace tf3;

@@ -56,6 +58,23 @@ ObservationBuffer::ObservationBuffer(string topic_name, double observation_keep_
 {
 }

+ObservationBuffer::ObservationBuffer(string topic_name, double observation_keep_time, double expected_update_rate,
+                                     double min_obstacle_height, double max_obstacle_height, double obstacle_range,
+                                     double raytrace_range, unsigned int frustum_pixel_step,
+                                     double frustum_min_range, double frustum_max_range,
+                                     tf3::BufferCore& tf3_buffer, string global_frame,
+                                     string sensor_frame, double tf_tolerance) :
+    tf3_buffer_(tf3_buffer), observation_keep_time_(observation_keep_time), expected_update_rate_(expected_update_rate),
+    last_updated_(robot::Time::now()), global_frame_(global_frame), sensor_frame_(sensor_frame), topic_name_(topic_name),
+    min_obstacle_height_(min_obstacle_height), max_obstacle_height_(max_obstacle_height),
+    obstacle_range_(obstacle_range), raytrace_range_(raytrace_range),
+    frustum_pixel_step_(std::max(1u, frustum_pixel_step)),
+    frustum_min_range_(std::max(0.0, frustum_min_range)),
+    frustum_max_range_(std::max(frustum_max_range, frustum_min_range_)),
+    tf_tolerance_(tf_tolerance)
+{
+}
+
 ObservationBuffer::~ObservationBuffer()
 {
 }
@@ -80,6 +99,12 @@ bool ObservationBuffer::setGlobalFrame(const std::string new_global_frame)
    {
      Observation& obs = *obs_it;

+      if (!obs.cloud_handle_.unique())
+      {
+        obs.cloud_handle_ = boost::make_shared<robot_sensor_msgs::PointCloud2>(*obs.cloud_);
+        obs.cloud_ = obs.cloud_handle_.get();
+      }
+
      robot_geometry_msgs::PointStamped origin;
      origin.header.frame_id = global_frame_;
      origin.header.stamp = data_convert::convertTime(transform_time);
@@ -120,9 +145,7 @@ bool ObservationBuffer::setGlobalFrame(const std::string new_global_frame)
 void ObservationBuffer::bufferCloud(const robot_sensor_msgs::PointCloud2& cloud)
 {
  robot_geometry_msgs::PointStamped global_origin;
-
-  // create a new observation on the list to be populated
-  observation_list_.push_front(Observation());
+  Observation observation;

  // check whether the origin frame has been set explicitly or whether we should get it from the cloud
  string origin_frame = sensor_frame_ == "" ? cloud.header.frame_id : sensor_frame_;
@@ -137,81 +160,68 @@ void ObservationBuffer::bufferCloud(const robot_sensor_msgs::PointCloud2& cloud)
    local_origin.point.y = 0;
    local_origin.point.z = 0;
    // tf3_buffer_.transform(local_origin, global_origin, global_frame_);
-    tf3::TransformStampedMsg tfm_1 = tf3_buffer_.lookupTransform(
-                        global_frame_,                      // frame đích
-                        local_origin.header.frame_id,                      // frame nguồn
-                        tf3::Time()
-                        // data_convert::convertTime(local_origin.header.stamp)
-                        );
-    tf3::doTransform(local_origin, global_origin, tfm_1);
+    const tf3::TransformStampedMsg cloud_transform = tf3_buffer_.lookupTransform(
+        global_frame_, cloud.header.frame_id, tf3::Time());
+    if (origin_frame == cloud.header.frame_id)
+      tf3::doTransform(local_origin, global_origin, cloud_transform);
+    else
+      tf3::doTransform(
+          local_origin, global_origin,
+          tf3_buffer_.lookupTransform(global_frame_, origin_frame, tf3::Time()));

-    /////////////////////////////////////////////////
-    ///////////chú ý hàm này/////////////////////////
-    tf3::convert(global_origin.point, observation_list_.front().origin_);
-    /////////////////////////////////////////////////
-    /////////////////////////////////////////////////
+    tf3::convert(global_origin.point, observation.origin_);
+    observation.raytrace_range_ = raytrace_range_;
+    observation.obstacle_range_ = obstacle_range_;

-    // make sure to pass on the raytrace/obstacle range of the observation buffer to the observations
-    observation_list_.front().raytrace_range_ = raytrace_range_;
-    observation_list_.front().obstacle_range_ = obstacle_range_;
+    robot_sensor_msgs::PointCloud2& observation_cloud = *observation.cloud_;
+    tf3::doTransform(cloud, observation_cloud, cloud_transform);
+    observation_cloud.header.stamp = cloud.header.stamp;

-    robot_sensor_msgs::PointCloud2 global_frame_cloud;
+    const std::size_t cloud_size =
+        static_cast<std::size_t>(observation_cloud.height) * observation_cloud.width;
+    const std::size_t point_step = observation_cloud.point_step;
+    std::size_t point_count = 0;
+    robot_sensor_msgs::PointCloud2Iterator<float> iter_z(observation_cloud, "z");

-    // transform the point cloud
-    // tf3_buffer_.transform(cloud, global_frame_cloud, global_frame_);
-    tf3::TransformStampedMsg tfm_2 = tf3_buffer_.lookupTransform(
-                        global_frame_,                      // frame đích
-                        cloud.header.frame_id,                      // frame nguồn
-                        tf3::Time()
-                        // data_convert::convertTime(cloud.header.stamp)
-                        );
-    tf3::doTransform(cloud, global_frame_cloud, tfm_2);
-    global_frame_cloud.header.stamp = cloud.header.stamp;
-
-    // now we need to remove observations from the cloud that are below or above our height thresholds
-    robot_sensor_msgs::PointCloud2& observation_cloud = *(observation_list_.front().cloud_);
-    observation_cloud.height = global_frame_cloud.height;
-    observation_cloud.width = global_frame_cloud.width;
-    observation_cloud.fields = global_frame_cloud.fields;
-    observation_cloud.is_bigendian = global_frame_cloud.is_bigendian;
-    observation_cloud.point_step = global_frame_cloud.point_step;
-    observation_cloud.row_step = global_frame_cloud.row_step;
-    observation_cloud.is_dense = global_frame_cloud.is_dense;
-
-    unsigned int cloud_size = global_frame_cloud.height*global_frame_cloud.width;
-    robot_sensor_msgs::PointCloud2Modifier modifier(observation_cloud);
-    modifier.resize(cloud_size);
-    unsigned int point_count = 0;
-
-    // copy over the points that are within our height bounds
-    robot_sensor_msgs::PointCloud2Iterator<float> iter_z(global_frame_cloud, "z");
-    std::vector<unsigned char>::const_iterator iter_global = global_frame_cloud.data.begin(), iter_global_end = global_frame_cloud.data.end();
-    std::vector<unsigned char>::iterator iter_obs = observation_cloud.data.begin();
-    for (; iter_global != iter_global_end; ++iter_z, iter_global += global_frame_cloud.point_step)
+    // Compact accepted points in-place. This avoids allocating and copying a
+    // second full-size filtered cloud after the TF transform.
+    for (std::size_t read_index = 0; read_index < cloud_size; ++read_index, ++iter_z)
    {
-      if ((*iter_z) <= max_obstacle_height_
-          && (*iter_z) >= min_obstacle_height_)
+      if ((*iter_z) > max_obstacle_height_ || (*iter_z) < min_obstacle_height_)
+        continue;
+
+      if (point_count != read_index)
      {
-        std::copy(iter_global, iter_global + global_frame_cloud.point_step, iter_obs);
-        iter_obs += global_frame_cloud.point_step;
-        ++point_count;
+        std::memmove(observation_cloud.data.data() + point_count * point_step,
+                     observation_cloud.data.data() + read_index * point_step,
+                     point_step);
      }
+      ++point_count;
    }

-    // resize the cloud for the number of legal points
-    modifier.resize(point_count);
-    observation_cloud.header.stamp = cloud.header.stamp;
-    observation_cloud.header.frame_id = global_frame_cloud.header.frame_id;
+    if (point_count != cloud_size)
+    {
+      robot_sensor_msgs::PointCloud2Modifier modifier(observation_cloud);
+      modifier.resize(point_count);
+    }
  }
  catch (TransformException& ex)
  {
-    // if an exception occurs, we need to remove the empty observation from the list
-    observation_list_.pop_front();
    robot::log_error("TF Exception that should never happen for sensor frame: %s, cloud frame: %s, %s\n", sensor_frame_.c_str(),
              cloud.header.frame_id.c_str(), ex.what());
    return;
  }

+  if (observation_keep_time_ == robot::Duration(0.0) && !observation_list_.empty())
+  {
+    observation_list_.front() = std::move(observation);
+    observation_list_.erase(++observation_list_.begin(), observation_list_.end());
+  }
+  else
+  {
+    observation_list_.push_front(std::move(observation));
+  }
+
  // if the update was successful, we want to update the last updated time
  last_updated_ = robot::Time::now();

@@ -219,6 +229,37 @@ void ObservationBuffer::bufferCloud(const robot_sensor_msgs::PointCloud2& cloud)
  purgeStaleObservations();
 }

+void ObservationBuffer::bufferDepthCamera(const robot_sensor_msgs::DepthCameraData& depth_camera_data)
+{
+  bufferDepthCamera(boost::make_shared<robot_sensor_msgs::DepthCameraData>(depth_camera_data));
+}
+
+void ObservationBuffer::bufferDepthCamera(robot_sensor_msgs::DepthCameraData::ConstPtr depth_camera_data)
+{
+  if (!depth_camera_data)
+    return;
+
+  DepthCameraObservation observation(
+      std::move(depth_camera_data), topic_name_, robot::Time::now(),
+      frustum_pixel_step_, frustum_min_range_, frustum_max_range_);
+
+  if (observation_keep_time_ == robot::Duration(0.0) && !depth_observation_list_.empty())
+  {
+    depth_observation_list_.front() = std::move(observation);
+    depth_observation_list_.erase(++depth_observation_list_.begin(), depth_observation_list_.end());
+  }
+  else
+  {
+    depth_observation_list_.push_front(std::move(observation));
+  }
+
+  // if the update was successful, we want to update the last updated time
+  last_updated_ = robot::Time::now();
+
+  // first... let's make sure that we don't have any stale observations
+  purgeStaleDepthObservations();
+}
+
 // returns a copy of the observations
 void ObservationBuffer::getObservations(vector<Observation>& observations)
 {
@@ -233,6 +274,26 @@ void ObservationBuffer::getObservations(vector<Observation>& observations)
  }
 }

+void ObservationBuffer::getDepthObservations(vector<DepthCameraObservation>& observations)
+{
+  // first... let's make sure that we don't have any stale observations
+  purgeStaleDepthObservations();
+
+  // now we'll just copy the observations for the caller
+  if (observation_keep_time_ == robot::Duration(0.0))
+  {
+    if (!depth_observation_list_.empty())
+    {
+      observations.push_back(std::move(depth_observation_list_.front()));
+      depth_observation_list_.clear();
+    }
+    return;
+  }
+
+  observations.insert(
+      observations.end(), depth_observation_list_.begin(), depth_observation_list_.end());
+}
+
 void ObservationBuffer::purgeStaleObservations()
 {
  if (!observation_list_.empty())
@@ -259,6 +320,30 @@ void ObservationBuffer::purgeStaleObservations()
  }
 }

+void ObservationBuffer::purgeStaleDepthObservations()
+{
+  if (depth_observation_list_.empty())
+    return;
+
+  if (observation_keep_time_ == robot::Duration(0.0))
+  {
+    auto observation = depth_observation_list_.begin();
+    depth_observation_list_.erase(++observation, depth_observation_list_.end());
+    return;
+  }
+
+  const robot::Time now = robot::Time::now();
+  for (auto observation = depth_observation_list_.begin(); observation != depth_observation_list_.end(); ++observation)
+  {
+    DepthCameraObservation& obs = *observation;
+    if ((last_updated_ - obs.data_->header.stamp) > observation_keep_time_)
+    {
+      depth_observation_list_.erase(observation, depth_observation_list_.end());
+      return;
+    }
+  }
+}
+
 bool ObservationBuffer::isCurrent() const
 {
  if (expected_update_rate_ == robot::Duration(0.0))
@@ -278,4 +363,3 @@ void ObservationBuffer::resetLastUpdated()
  last_updated_ = robot::Time::now();
 }
 }  // namespace robot_costmap_2d
-
--- a/test/coordinates_test.cpp
+++ b/test/coordinates_test.cpp
@@ -36,6 +36,14 @@

 #include <gtest/gtest.h>
 #include <robot_costmap_2d/costmap_2d.h>
+#include <robot_costmap_2d/cost_values.h>
+#include <robot_costmap_2d/inflation_layer.h>
+#include <robot_costmap_2d/layered_costmap.h>
+#include <robot_costmap_2d/observation_buffer.h>
+#include <robot_costmap_2d/voxel_layer.h>
+
+#include <boost/make_shared.hpp>
+#include <cstdlib>

 using namespace robot_costmap_2d;

@@ -124,9 +132,100 @@ TEST(CostmapCoordinates, hard_coordinates_test)
  EXPECT_EQ(my, 2);
 }

+TEST(CostmapPerformanceRegression, rolling_origin_preserves_overlap)
+{
+  Costmap2D costmap(4, 3, 1.0, 0.0, 0.0, FREE_SPACE);
+  costmap.setCost(1, 1, LETHAL_OBSTACLE);
+  costmap.setCost(3, 2, INSCRIBED_INFLATED_OBSTACLE);
+
+  costmap.updateOrigin(0.25, 0.25);
+  EXPECT_DOUBLE_EQ(costmap.getOriginX(), 0.0);
+  EXPECT_DOUBLE_EQ(costmap.getOriginY(), 0.0);
+  EXPECT_EQ(costmap.getCost(1, 1), LETHAL_OBSTACLE);
+
+  costmap.updateOrigin(1.0, 0.0);
+  EXPECT_DOUBLE_EQ(costmap.getOriginX(), 1.0);
+  EXPECT_EQ(costmap.getCost(0, 1), LETHAL_OBSTACLE);
+  EXPECT_EQ(costmap.getCost(3, 2), FREE_SPACE);
+}
+
+TEST(CostmapPerformanceRegression, voxel_origin_subcell_shift_is_noop)
+{
+  VoxelLayer layer;
+  layer.resizeMap(4, 3, 1.0, 0.0, 0.0);
+  layer.setCost(1, 1, LETHAL_OBSTACLE);
+
+  layer.updateOrigin(0.25, 0.25);
+
+  EXPECT_DOUBLE_EQ(layer.getOriginX(), 0.0);
+  EXPECT_DOUBLE_EQ(layer.getOriginY(), 0.0);
+  EXPECT_EQ(layer.getCost(1, 1), LETHAL_OBSTACLE);
+}
+
+TEST(CostmapPerformanceRegression, observation_copy_shares_cloud_payload)
+{
+  robot_geometry_msgs::Point origin;
+  robot_sensor_msgs::PointCloud2 cloud;
+  cloud.height = 1;
+  cloud.width = 1;
+  cloud.point_step = 4;
+  cloud.row_step = 4;
+  cloud.data = {1, 2, 3, 4};
+
+  Observation observation(origin, cloud, 2.5, 3.0);
+  Observation copied = observation;
+
+  EXPECT_EQ(copied.cloud_, observation.cloud_);
+  EXPECT_EQ(copied.cloud_handle_.use_count(), 2);
+  EXPECT_EQ(copied.cloud_->data, cloud.data);
+}
+
+TEST(CostmapPerformanceRegression, latest_depth_frame_is_consumed_once)
+{
+  tf3::BufferCore tf_buffer(tf3::Duration(10.0));
+  ObservationBuffer buffer(
+      "/camera/depth/data", 0.0, 0.5, 0.0, 2.0, 2.5, 3.0,
+      8, 0.2, 3.0, tf_buffer, "odom", "", 0.2);
+
+  robot_sensor_msgs::DepthCameraData::ConstPtr depth =
+      boost::make_shared<robot_sensor_msgs::DepthCameraData>();
+  buffer.bufferDepthCamera(depth);
+
+  std::vector<DepthCameraObservation> first_snapshot;
+  buffer.getDepthObservations(first_snapshot);
+  ASSERT_EQ(first_snapshot.size(), 1u);
+  EXPECT_EQ(first_snapshot.front().data_, depth.get());
+  EXPECT_EQ(first_snapshot.front().topic_, "/camera/depth/data");
+
+  std::vector<DepthCameraObservation> second_snapshot;
+  buffer.getDepthObservations(second_snapshot);
+  EXPECT_TRUE(second_snapshot.empty());
+}
+
+TEST(CostmapPerformanceRegression, inflation_buckets_preserve_radial_costs)
+{
+  ASSERT_EQ(setenv("PNKX_NAV_CORE_CONFIG_DIR", ROBOT_COSTMAP_2D_DIR, 1), 0);
+
+  LayeredCostmap layered_costmap("map", false, false);
+  layered_costmap.resizeMap(7, 7, 1.0, 0.0, 0.0, true);
+  tf3::BufferCore tf_buffer(tf3::Duration(10.0));
+  InflationLayer inflation;
+  inflation.initialize(&layered_costmap, "inflation", &tf_buffer);
+  inflation.setInflationParameters(2.0, 1.0);
+
+  Costmap2D& master = *layered_costmap.getCostmap();
+  master.setCost(3, 3, LETHAL_OBSTACLE);
+  inflation.updateCosts(master, 0, 0, 7, 7);
+
+  EXPECT_EQ(master.getCost(3, 3), LETHAL_OBSTACLE);
+  EXPECT_EQ(master.getCost(2, 3), master.getCost(4, 3));
+  EXPECT_EQ(master.getCost(3, 2), master.getCost(3, 4));
+  EXPECT_GT(master.getCost(4, 3), master.getCost(5, 3));
+  EXPECT_EQ(master.getCost(6, 3), FREE_SPACE);
+}
+
 int main(int argc, char** argv)
 {
  testing::InitGoogleTest( &argc, argv );
  return RUN_ALL_TESTS();
 }
-
--- a/test/static_layer_test.cpp
+++ b/test/static_layer_test.cpp
@@ -67,9 +67,9 @@ void CostmapTester::compareCells(robot_costmap_2d::Costmap2D& costmap, unsigned
    double furthest_valid_distance = 0; // ################costmap.cell_inscribed_radius_ + cell_distance + 1;
    unsigned char expected_lowest_cost = 0; // ################costmap.computeCost(furthest_valid_distance);
    if(neighbor_cost < expected_lowest_cost){
-      robot::log_error("Cell cost (%d, %d): %d, neighbor cost (%d, %d): %d, expected lowest cost: %d, cell distance: %.2f, furthest valid distance: %.2f",
+      printf("Cell cost (%d, %d): %d, neighbor cost (%d, %d): %d, expected lowest cost: %d, cell distance: %.2f, furthest valid distance: %.2f",
          x, y, cell_cost, nx, ny, neighbor_cost, expected_lowest_cost, cell_distance, furthest_valid_distance);
-      robot::log_error("Cell: (%d, %d), Neighbor: (%d, %d)", x, y, nx, ny);
+      printf("Cell: (%d, %d), Neighbor: (%d, %d)", x, y, nx, ny);
      costmap.saveMap("failing_costmap.pgm");
    }
    EXPECT_TRUE(neighbor_cost >= expected_lowest_cost || (furthest_valid_distance > 0/* costmap.cell_inflation_radius_ */&& neighbor_cost == robot_costmap_2d::FREE_SPACE));
Author	SHA1	Message	Date
duongtd	f0390fcc67	optimal voxel layer	2026-07-23 11:39:03 +07:00
duongtd	28a32ee67c	optimal voxel layer	2026-07-23 11:18:04 +07:00
duongtd	03b13f6936	add file readme	2026-07-14 11:37:35 +07:00
duongtd	bdbb03aa51	otimal deep coppy obj	2026-07-14 11:08:23 +07:00
duongtd	6a9834d3a8	add multi camera depth	2026-07-14 09:42:35 +07:00
duongtd	a2a021c114	add function computeCost file layer.h	2026-07-10 11:27:20 +07:00
hoangson02	2fcd211ccf	update	2026-03-03 07:26:47 +00:00
HiepLM	3d621de809	fix bugs	2026-02-26 14:54:23 +07:00
HiepLM	1f9e9f1398	update	2026-02-26 14:52:33 +07:00
duongtd	9208c8bcdc	WIP: fix costmap_2d_robot	2026-02-26 14:49:45 +07:00
duongtd	6c6e5b44f8	update copyParentParameters	2026-02-26 14:43:17 +07:00
HiepLM	eb52edc6e8	update tf3	2026-02-07 11:00:46 +07:00
hoangson02	ed43912c33	delete console_brigde	2026-02-06 10:14:35 +00:00
HiepLM	9026c03e1e	update	2026-01-13 14:30:13 +07:00
HiepLM	81e7874274	Duong update	2026-01-12 15:48:43 +07:00
HiepLM	9d3d31a4f9	include <robot/console.h>	2026-01-10 14:10:41 +07:00