:optimal test NUC 27/7 15:14

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

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
+```

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

config/static_layer_params.yaml

@@ -1,5 +1,6 @@
 static_layer:
   enabled: true
+  map_topic: "map"
   first_map_only: false
   subscribe_to_updates: false
   track_unknown_space: true

config/voxel_layer_params.yaml

@@ -7,5 +7,4 @@ voxel_layer:
   z_voxels: 16
   unknown_threshold: 15.0
   mark_threshold: 0
-  combination_method: 3
-
+  combination_method: 1

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
   {

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

include/robot_costmap_2d/critical_layer.h

@@ -10,6 +10,12 @@ class CriticalLayer : public StaticLayer
 public:
   CriticalLayer();
   virtual ~CriticalLayer();
+
+  LayerType getType() const override
+  {
+      return LayerType::CRITICAL_LAYER;
+  }
+  
 private:
   unsigned char interpretValue(unsigned char value) override;
   void updateCosts(robot_costmap_2d::Costmap2D& master_grid, int min_i, int min_j, int max_i, int max_j) override;

include/robot_costmap_2d/directional_layer.h

@@ -14,6 +14,11 @@ namespace robot_costmap_2d
     bool laneFilter(const std::vector<robot_geometry_msgs::PoseStamped> plan);
     void resetMap();
 
+    LayerType getType() const override
+    {
+        return LayerType::DIRECTIONAL_LAYER;
+    }
+    
 	private:
     void incomingMap(const robot_nav_msgs::OccupancyGrid &new_map);
 		bool laneFilter(std::vector<std::array<uint16_t, 2>> new_map, const robot_nav_msgs::Path path);

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

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;
@@ -120,6 +123,11 @@ public:
    */
   void setInflationParameters(double inflation_radius, double cost_scaling_factor);
 
+  LayerType getType() const override
+  {
+      return LayerType::INFLATION_LAYER;
+  }
+  
 protected:
   virtual void onFootprintChanged();
   boost::recursive_mutex* inflation_access_;
@@ -178,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_;

include/robot_costmap_2d/layer.h

@@ -42,9 +42,23 @@
 #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
 {
+
+enum class LayerType
+{
+    UNKNOWN,
+    STATIC_LAYER,
+    OBSTACLE_LAYER,
+    INFLATION_LAYER,
+    CRITICAL_LAYER,
+    DIRECTIONAL_LAYER,
+    PREFERRED_LAYER,
+    UNPREFERRED_LAYER,
+    VOXEL_LAYER
+};
+
 class LayeredCostmap;
 
 class Layer
@@ -71,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() {}
 
@@ -134,6 +150,9 @@ public:
   void dataCallBack(const T& value, const std::string& topic) {
       handle(value, topic);
   }
+
+  virtual LayerType getType() const { return LayerType::UNKNOWN; }
+  
 protected:
 
   // Hàm template public, dùng để gửi dữ liệu

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

include/robot_costmap_2d/observation.h

@@ -34,10 +34,150 @@
 
 #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.
+ */
+/// Per-observation-source configuration of the depth-image frustum clearing,
+/// loaded by ObstacleLayer from the source's YAML/ROS params and carried with
+/// each DepthCameraObservation.
+struct DepthFrustumConfig
+{
+  unsigned int pixel_step = 0;
+  double min_range = 0.0;
+  double max_range = 0.0;
+  /// 3D clearing rays stop this far [m] before the measured surface.
+  /// Negative: legacy 2 * costmap resolution.
+  double skip_distance = -1.0;
+  /// Full-column clearing from the per-pixel-column nearest in-band return.
+  bool column_clearing = false;
+  /// Height band [m] used for the in-band test; floor returns below min do
+  /// not shorten the beam. max < 0: use the layer max_obstacle_height.
+  double column_min_height = 0.10;
+  double column_max_height = -1.0;
+  /// Column beams stop this far [m] before the nearest in-band return.
+  double column_skip_distance = 0.02;
+  /// Full columns are only cleared beyond this distance [m]. Negative:
+  /// derive each frame from the camera intrinsics and mounting pose.
+  double column_cover_distance = -1.0;
+  /// Depth-image columns at the LEFT edge excluded from clearing [px]. Covers
+  /// the stereo no-disparity strip that is permanently invalid there: those
+  /// pixels carry no free-space evidence, so clearing through them erases
+  /// obstacles that rotate out of the FOV on that side. Set to the measured
+  /// width of the black strip in the raw depth image (a few px margin). 0
+  /// disables. Invalid pixels ELSEWHERE still clear (needed for ghost removal).
+  unsigned int clear_left_border_px = 0;
+  /// Same as clear_left_border_px but for the RIGHT edge, for cameras whose
+  /// stereo no-disparity strip sits on the right instead of the left. Measured
+  /// from the last image column inward. 0 disables.
+  unsigned int clear_right_border_px = 0;
+};
+
+class DepthCameraObservation
+{
+public:
+  DepthCameraObservation()
+    : data_handle_(),
+      data_(nullptr),
+      topic_()
+  {
+  }
+
+  DepthCameraObservation(
+      const robot_sensor_msgs::DepthCameraData& data,
+      std::string topic,
+      const robot::Time& received_time,
+      const DepthFrustumConfig& frustum)
+    : data_handle_(boost::make_shared<robot_sensor_msgs::DepthCameraData>(data)),
+      data_(data_handle_.get()),
+      topic_(std::move(topic)),
+      received_time_(received_time),
+      frustum_(frustum)
+  {
+  }
+
+  DepthCameraObservation(
+      robot_sensor_msgs::DepthCameraData::ConstPtr data,
+      std::string topic,
+      const robot::Time& received_time,
+      const DepthFrustumConfig& frustum)
+    : data_handle_(std::move(data)),
+      data_(data_handle_.get()),
+      topic_(std::move(topic)),
+      received_time_(received_time),
+      frustum_(frustum)
+  {
+  }
+
+  DepthCameraObservation(const DepthCameraObservation& other)
+    : data_handle_(other.data_handle_),
+      data_(data_handle_.get()),
+      topic_(other.topic_),
+      received_time_(other.received_time_),
+      frustum_(other.frustum_)
+  {
+  }
+
+  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_),
+      frustum_(other.frustum_)
+  {
+    other.data_ = nullptr;
+    other.frustum_ = DepthFrustumConfig();
+  }
+
+  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_;
+    frustum_ = other.frustum_;
+    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_;
+    frustum_ = other.frustum_;
+
+    other.data_ = nullptr;
+    other.frustum_ = DepthFrustumConfig();
+
+    return *this;
+  }
+
+  ~DepthCameraObservation() = default;
+
+  robot_sensor_msgs::DepthCameraData::ConstPtr data_handle_;
+  const robot_sensor_msgs::DepthCameraData* data_;
+  std::string topic_;
+  robot::Time received_time_;
+  DepthFrustumConfig frustum_;
+};
+
 /**
  * @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 +190,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 +206,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 +226,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_;
 };

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
@@ -77,6 +76,13 @@ public:
                     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, const DepthFrustumConfig& frustum_config,
+                    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,14 @@ 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_;
+  DepthFrustumConfig frustum_config_;
 };
 }  // namespace robot_costmap_2d
 #endif  // ROBOT_COSTMAP_2D_OBSERVATION_BUFFER_H_

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,18 +46,22 @@
 
 #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
 {
 struct CallBackInfo
 {
+  std::string observation_source;
   std::string data_type;
   std::string topic;
   bool inf_is_valid;
@@ -84,6 +89,11 @@ public:
   void addStaticObservation(robot_costmap_2d::Observation& obs, bool marking, bool clearing);
   void clearStaticObservations(bool marking, bool clearing);
 
+  LayerType getType() const override
+  {
+      return LayerType::OBSTACLE_LAYER;
+  }
+  
 protected:
   void handleImpl(const void* data, 
                   const std::type_info&, 
@@ -121,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
@@ -135,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
@@ -163,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_;
 
@@ -171,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);

include/robot_costmap_2d/preferred_layer.h

@@ -10,6 +10,12 @@ class PreferredLayer : public StaticLayer
 public:
   PreferredLayer();
   virtual ~PreferredLayer();
+
+  LayerType getType() const override
+  {
+      return LayerType::PREFERRED_LAYER;
+  }
+  
 private:
   unsigned char interpretValue(unsigned char value);
 };

include/robot_costmap_2d/static_layer.h

@@ -63,6 +63,12 @@ public:
   virtual void updateCosts(robot_costmap_2d::Costmap2D& master_grid, int min_i, int min_j, int max_i, int max_j);
 
   virtual void matchSize();
+
+  LayerType getType() const override
+  {
+      return LayerType::STATIC_LAYER;
+  }
+  
 protected:
   void handleImpl(const void* data,
                   const std::type_info& type,

include/robot_costmap_2d/testing_helper.h

@@ -25,12 +25,12 @@ char printableCost(unsigned char cost)
 
 void printMap(robot_costmap_2d::Costmap2D& costmap)
 {
-  printf("map:\n");
+  robot::log_info("map:\n");
   for (int i = 0; i < costmap.getSizeInCellsY(); i++){
     for (int j = 0; j < costmap.getSizeInCellsX(); j++){
-      printf("%4d", int(costmap.getCost(j, i)));
+      robot::log_info("%4d", int(costmap.getCost(j, i)));
     }
-    printf("\n\n");
+    robot::log_info("\n\n");
   }
 }
 

include/robot_costmap_2d/unpreferred_layer.h

@@ -11,6 +11,11 @@ public:
   UnPreferredLayer();
   virtual ~UnPreferredLayer();
 
+  LayerType getType() const override
+  {
+      return LayerType::UNPREFERRED_LAYER;
+  }
+  
 private:
   unsigned char interpretValue(unsigned char value);
 

include/robot_costmap_2d/voxel_layer.h

@@ -51,6 +51,9 @@
 #include <robot_costmap_2d/obstacle_layer.h>
 #include <robot_voxel_grid/voxel_grid.h>
 
+#include <limits>
+#include <vector>
+
 namespace robot_costmap_2d
 {
 
@@ -77,6 +80,10 @@ public:
   virtual void matchSize();
   virtual void reset();
 
+  LayerType getType() const override
+  {
+      return LayerType::VOXEL_LAYER;
+  }
 
 protected:
 
@@ -87,13 +94,64 @@ 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);
+  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);
+  bool clearDepthColumns(double ox, double oy, double cover_distance, double far_distance,
+                         double min_range, double max_range, double skip_dist,
+                         double* min_x, double* min_y, double* max_x, double* max_y);
+  bool clipColumnSegment(double& sx, double& sy, double& ex, double& ey) const;
 
 
   bool publish_voxel_;
   robot_voxel_grid::VoxelGrid robot_voxel_grid_;
   double z_resolution_, origin_z_;
+  /// Scratch for the full-column clearing pass (config lives per observation
+  /// source in DepthFrustumConfig): per depth-image pixel column, the nearest
+  /// return inside the obstacle height band certifies "no obstacle in this
+  /// direction closer than d". Cells along that 2D beam get their whole voxel
+  /// column cleared, removing marked voxels the per-pixel 3D rays cannot
+  /// reach (above the vertical FOV at close range).
+  struct DepthColumnStat
+  {
+    double min_band_dist = -1.0;  ///< horizontal distance of nearest in-band return; < 0 = none
+    double azimuth = 0.0;         ///< beam direction in the global frame
+    double best_row_delta = std::numeric_limits<double>::infinity();
+    bool has_ray = false;         ///< column had at least one readable pixel
+    bool in_border = false;       ///< column lies in a left/right no-disparity strip
+  };
+  std::vector<DepthColumnStat> depth_column_stats_;
   unsigned int unknown_threshold_, mark_threshold_, size_z_;
   robot_sensor_msgs::PointCloud clearing_endpoints_;
+  std::vector<unsigned char> rolling_costmap_scratch_;
+  std::vector<unsigned int> rolling_voxel_scratch_;
+
+  struct DepthRay
+  {
+    unsigned int u;
+    unsigned int v;
+    unsigned int col;  ///< pixel-column index in the cache (border column included)
+    double x;
+    double y;
+    double z;
+  };
+  std::vector<DepthRay> depth_ray_cache_;
+  unsigned int cached_column_count_ = 0;
+  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;
 
   inline bool worldToMap3DFloat(double wx, double wy, double wz, double& mx, double& my, double& mz)
   {

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>
 

plugins/critical_layer.cpp

@@ -13,7 +13,6 @@ CriticalLayer::~CriticalLayer(){}
 
 unsigned char CriticalLayer::interpretValue(unsigned char value) 
 {
-  // printf("TEST PLUGIN CRITICAL\n");
   // check if the static value is above the unknown or lethal thresholds
   if(value >= *this->threshold_)
     return CRITICAL_SPACE;
@@ -23,7 +22,6 @@ unsigned char CriticalLayer::interpretValue(unsigned char value)
 
 void CriticalLayer::updateCosts(robot_costmap_2d::Costmap2D& master_grid, int min_i, int min_j, int max_i, int max_j)
 {
-  // printf("TEST PLUGIN CRITICAL\n");
   if (!map_received_)
     return;
     

plugins/directional_layer.cpp

@@ -36,7 +36,7 @@ namespace robot_costmap_2d
     {
       unsigned int size_x = new_map.info.width, size_y = new_map.info.height;
 
-      printf("Received a %d X %d map at %f m/pix\n", size_x / 2, size_y / 2, new_map.info.resolution);
+      robot::log_info("Received a %d X %d map at %f m/pix\n", size_x / 2, size_y / 2, new_map.info.resolution);
 
       // resize costmap if size, resolution or origin do not match
       Costmap2D *master = layered_costmap_->getCostmap();
@@ -48,7 +48,7 @@ namespace robot_costmap_2d
           master->getOriginY() != new_map.info.origin.position.y))
       {
         // Update the size of the layered costmap (and all layers, including this one)
-        printf("Resizing costmap to %d X %d at %f m/pix\n", size_x / 2, size_y / 2, new_map.info.resolution);
+        robot::log_info("Resizing costmap to %d X %d at %f m/pix\n", size_x / 2, size_y / 2, new_map.info.resolution);
         layered_costmap_->resizeMap(size_x / 2, size_y / 2, new_map.info.resolution, new_map.info.origin.position.x,
                                     new_map.info.origin.position.y,
                                     true /* set size_locked to true, prevents reconfigureCb from overriding map size*/);
@@ -59,7 +59,7 @@ namespace robot_costmap_2d
               origin_y_ != new_map.info.origin.position.y)
       {
         // only update the size of the costmap stored locally in this layer
-        printf("Resizing static layer to %d X %d at %f m/pix\n", size_x / 2, size_y / 2, new_map.info.resolution);
+        robot::log_info("Resizing static layer to %d X %d at %f m/pix\n", size_x / 2, size_y / 2, new_map.info.resolution);
         resizeMap(size_x / 2, size_y / 2, new_map.info.resolution,
                   new_map.info.origin.position.x, new_map.info.origin.position.y);
       }
@@ -99,12 +99,12 @@ namespace robot_costmap_2d
     }
     else
     {
-      printf("Stop receive new map!\n");
+      robot::log_info("Stop receive new map!\n");
     }
     // shutdown the map subscrber if firt_map_only_ flag is on
     if (first_map_only_)
     {
-      printf("Shutting down the map subscriber. first_map_only flag is on\n");
+      robot::log_info("Shutting down the map subscriber. first_map_only flag is on\n");
       map_shutdown_ = true;
       // map_sub_.shutdown();
     }
@@ -129,7 +129,7 @@ namespace robot_costmap_2d
       unsigned int mx, my;
       if (!worldToMap(p.pose.position.x, p.pose.position.y, mx, my))
       {
-        printf("ERROR: trying to set a start cell %f %f that is outside of map\n", p.pose.position.x, p.pose.position.y);
+        robot::log_error("ERROR: trying to set a start cell %f %f that is outside of map\n", p.pose.position.x, p.pose.position.y);
         return false;
       }
       // Convert to yaw
@@ -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;
     }
@@ -263,8 +255,7 @@ namespace robot_costmap_2d
       y_max_w = std::max(y_max_w, y[i].second);
 
     }
-    // printf("%d %d %d %d", x_min, y_min, x_max, y_max);
-    // printf("%f %f %f %f", x_min_w, y_min_w, x_max_w, y_max_w);
+
     for (int i = 0; i < yaw_robot.size(); i++)
     {
       if(inSkipErea(pose_x_, pose_y_, x[i].second, y[i].second, distance_skip_))
@@ -407,7 +398,7 @@ namespace robot_costmap_2d
     }
     catch (tf3::TransformException &ex)
     {
-      printf("%s\n", ex.what());
+      robot::log_error("%s\n", ex.what());
       return false;
     }
     // Copy map data given proper transformations

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,
@@ -186,7 +187,7 @@ void InflationLayer::onFootprintChanged()
   computeCaches();
   need_reinflation_ = true;
 
-  printf("InflationLayer::onFootprintChanged(): num footprint points: %lu,"
+  robot::log_info("InflationLayer::onFootprintChanged(): num footprint points: %lu,"
             " inscribed_radius_ = %.3f, inflation_radius_ = %.3f\n",
             layered_costmap_->getFootprint().size(), inscribed_radius_, inflation_radius_);
 }
@@ -197,25 +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)
-  printf("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) {
-    printf("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)
   {
-    printf("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
@@ -231,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++)
@@ -251,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_;
@@ -294,7 +299,6 @@ void InflationLayer::updateCosts(robot_costmap_2d::Costmap2D& master_grid, int m
     }
   }
 
-  inflation_cells_.clear();
 }
 
 /**
@@ -309,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);
@@ -318,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));
   }
 }
 
@@ -347,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)
@@ -360,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)
@@ -404,7 +446,7 @@ void InflationLayer::handleImpl(const void* data,
                               const std::type_info& info,
                               const std::string& source)
 {
-  printf("This function is not available!\n");
+  robot::log_error("This function is not available!\n");
 }
 
 

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,9 @@ 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;
+        DepthFrustumConfig frustum_config;
 
         robot::NodeHandle priv_nh(nh, source);
         topic = loadParam(layer[source],"topic", topic);
@@ -136,6 +146,34 @@ 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 params are per-source; the layer-level key is kept as a
+        // fallback for older YAMLs
+        frustum_clearing_enabled = loadParam(layer[source], "frustum_clearing_enabled",
+                                             loadParam(layer, "frustum_clearing_enabled", false));
+        frustum_pixel_step = loadParam(layer[source], "frustum_clearing_pixel_step",
+                                       loadParam(layer, "frustum_clearing_pixel_step", 8));
+        frustum_config.min_range = loadParam(layer[source], "frustum_min_range",
+                                             loadParam(layer, "frustum_min_range", 0.2));
+        frustum_config.max_range = loadParam(layer[source], "frustum_max_range",
+                                             loadParam(layer, "frustum_max_range", 3.0));
+        frustum_config.skip_distance =
+            loadParam(layer[source], "frustum_skip_distance", frustum_config.skip_distance);
+        frustum_config.column_clearing =
+            loadParam(layer[source], "frustum_column_clearing", frustum_config.column_clearing);
+        frustum_config.column_min_height =
+            loadParam(layer[source], "column_clear_min_height", frustum_config.column_min_height);
+        frustum_config.column_max_height =
+            loadParam(layer[source], "column_clear_max_height", frustum_config.column_max_height);
+        frustum_config.column_skip_distance =
+            loadParam(layer[source], "column_skip_distance", frustum_config.column_skip_distance);
+        frustum_config.column_cover_distance =
+            loadParam(layer[source], "column_cover_distance", frustum_config.column_cover_distance);
+        int frustum_clear_left_border =
+            loadParam(layer[source], "frustum_clear_left_border_px",
+                      loadParam(layer, "frustum_clear_left_border_px", 0));
+        int frustum_clear_right_border =
+            loadParam(layer[source], "frustum_clear_right_border_px",
+                      loadParam(layer, "frustum_clear_right_border_px", 0));
 
         if (priv_nh.hasParam("topic"))
           priv_nh.getParam("topic", topic);
@@ -157,37 +195,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_config.min_range);
+        if (priv_nh.hasParam("frustum_max_range"))
+          priv_nh.getParam("frustum_max_range", frustum_config.max_range);
+        if (priv_nh.hasParam("frustum_skip_distance"))
+          priv_nh.getParam("frustum_skip_distance", frustum_config.skip_distance);
+        if (priv_nh.hasParam("frustum_column_clearing"))
+          priv_nh.getParam("frustum_column_clearing", frustum_config.column_clearing);
+        if (priv_nh.hasParam("column_clear_min_height"))
+          priv_nh.getParam("column_clear_min_height", frustum_config.column_min_height);
+        if (priv_nh.hasParam("column_clear_max_height"))
+          priv_nh.getParam("column_clear_max_height", frustum_config.column_max_height);
+        if (priv_nh.hasParam("column_skip_distance"))
+          priv_nh.getParam("column_skip_distance", frustum_config.column_skip_distance);
+        if (priv_nh.hasParam("column_cover_distance"))
+          priv_nh.getParam("column_cover_distance", frustum_config.column_cover_distance);
+        if (priv_nh.hasParam("frustum_clear_left_border_px"))
+          priv_nh.getParam("frustum_clear_left_border_px", frustum_clear_left_border);
+        if (priv_nh.hasParam("frustum_clear_right_border_px"))
+          priv_nh.getParam("frustum_clear_right_border_px", frustum_clear_right_border);
+        if (priv_nh.hasParam("frustum_depth_camera_topic"))
+          priv_nh.getParam("frustum_depth_camera_topic", depth_camera_data_topic_);
 
-        CallBackInfo info_tmp;
-        info_tmp.data_type = data_type;
-        info_tmp.topic = topic;
-        info_tmp.inf_is_valid = inf_is_valid;
-        callback_infos_.push_back(info_tmp);
+        frustum_config.pixel_step = static_cast<unsigned int>(std::max(1, frustum_pixel_step));
+        frustum_config.clear_left_border_px =
+            static_cast<unsigned int>(std::max(0, frustum_clear_left_border));
+        frustum_config.clear_right_border_px =
+            static_cast<unsigned int>(std::max(0, frustum_clear_right_border));
 
-        std::string raytrace_range_param_name, obstacle_range_param_name;
+        robot::log_info("source %s: frustum_clearing_enabled: %s, pixel_step: %u, range: [%.2f, %.2f] m, "
+                        "skip: %.3f m, column_clearing: %s, column_band: [%.2f, %.2f] m, "
+                        "column_skip: %.3f m, column_cover: %.2f m, clear_left_border_px: %u px, "
+                        "clear_right_border_px: %u px\n",
+                        source.c_str(), frustum_clearing_enabled ? "true" : "false",
+                        frustum_config.pixel_step, frustum_config.min_range, frustum_config.max_range,
+                        frustum_config.skip_distance, frustum_config.column_clearing ? "true" : "false",
+                        frustum_config.column_min_height, frustum_config.column_max_height,
+                        frustum_config.column_skip_distance, frustum_config.column_cover_distance,
+                        frustum_config.clear_left_border_px, frustum_config.clear_right_border_px);
 
         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);
 
+        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");
+        }
+
+        if(!frustum_clearing_enabled)
+        {
+
+          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);
+
           // enabled_ = enabled;
 
-        printf("Creating an observation buffer for topic %s, frame %s\n", topic.c_str(),
-                  sensor_frame.c_str());
+          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(
@@ -201,8 +285,25 @@ bool ObstacleLayer::getParams(const std::string& config_file_name, robot::NodeHa
           // 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);
 
-        printf(
+          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_config,
+                                          *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",
             topic.c_str(), global_frame_.c_str(), expected_update_rate, observation_keep_time);
@@ -210,7 +311,7 @@ bool ObstacleLayer::getParams(const std::string& config_file_name, robot::NodeHa
       } 
     }
     catch (const YAML::BadFile& e) {
-        std::cerr << "Cannot open YAML file: " << e.what() << std::endl;
+        robot::log_error("Cannot open YAML file: %s\n", e.what());
         return false;
     }
     
@@ -222,13 +323,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(observation_buffers_.empty() || callback_infos_.empty()) return;
+  if (type == typeid(robot_sensor_msgs::DepthCameraData::ConstPtr))
+  {
+      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];
 
@@ -237,6 +418,8 @@ void ObstacleLayer::handleImpl(const void* data,
           topic == callback_infos_[i].topic &&
           !callback_infos_[i].inf_is_valid)
       {
+        // if(topic == "/f_scan") robot::log_error("DATA front laser! %d",i);
+        // if(topic == "/b_scan") robot::log_error("DATA back laser! %d",i);
         laserScanCallback(*static_cast<const robot_sensor_msgs::LaserScan*>(data), buffer);
       }
       else if (type == typeid(robot_sensor_msgs::LaserScan) &&
@@ -244,6 +427,7 @@ void ObstacleLayer::handleImpl(const void* data,
               topic == callback_infos_[i].topic &&
               callback_infos_[i].inf_is_valid)
       {
+        
         laserScanValidInfCallback(*static_cast<const robot_sensor_msgs::LaserScan*>(data), buffer);
       }
       else if (type == typeid(robot_sensor_msgs::PointCloud) &&
@@ -252,7 +436,7 @@ void ObstacleLayer::handleImpl(const void* data,
       {
         if (callback_infos_[i].inf_is_valid)
         {
-          printf("obstacle_layer: inf_is_valid option is not applicable to PointCloud observations.");
+          robot::log_error("obstacle_layer: inf_is_valid option is not applicable to PointCloud observations.\n");
         }
         pointCloudCallback(*static_cast<const robot_sensor_msgs::PointCloud*>(data), buffer);
       }
@@ -262,21 +446,20 @@ void ObstacleLayer::handleImpl(const void* data,
       {
         if (callback_infos_[i].inf_is_valid)
         {
-          printf("obstacle_layer: inf_is_valid option is not applicable to PointCloud observations.");
+          robot::log_error("obstacle_layer: inf_is_valid option is not applicable to PointCloud observations.\n");
         }
         pointCloud2Callback(*static_cast<const robot_sensor_msgs::PointCloud2*>(data), buffer);
       }
-      else 
-      {
-          std::cout << "obstacle_layer: Unknown type: " << type.name() << std::endl;
+      // else 
+      // {
+      //     std::cout << "obstacle_layer: check type: " << (type == typeid(robot_sensor_msgs::LaserScan)) << std::endl 
+      //     << "obstacle_layer: inf_is_valid: " << callback_infos_[i].inf_is_valid << std::endl
+      //     << "data type: " << callback_infos_[i].data_type << std::endl
+      //     << "topic: " << topic << std::endl
+      //     << "topic check: " << callback_infos_[i].topic << std::endl << std::endl; 
+      // }
     }
   }
-  }
-  else 
-  {
-    std::cout << "Stop receiving data!" << std::endl;
-    return;
-  }
 }
 
 void ObstacleLayer::laserScanCallback(const robot_sensor_msgs::LaserScan& message,
@@ -293,13 +476,13 @@ void ObstacleLayer::laserScanCallback(const robot_sensor_msgs::LaserScan& messag
   }
   catch (tf3::TransformException &ex)
   {
-    printf("High fidelity enabled, but TF returned a transform exception to frame %s: %s\n", global_frame_.c_str(),
+    robot::log_error("High fidelity enabled, but TF returned a transform exception to frame %s: %s\n", global_frame_.c_str(),
              ex.what());
     projector_.projectLaser(message, cloud);
   }
   catch (std::runtime_error &ex)
   {
-    printf("transformLaserScanToPointCloud error, it seems the message from laser sensor is malformed. Ignore this laser scan. what(): %s\n", ex.what());
+    robot::log_error("transformLaserScanToPointCloud error, it seems the message from laser sensor is malformed. Ignore this laser scan. what(): %s\n", ex.what());
     return; //ignore this message
   }
 
@@ -335,13 +518,13 @@ void ObstacleLayer::laserScanValidInfCallback(const robot_sensor_msgs::LaserScan
   }
   catch (tf3::TransformException &ex)
   {
-    printf("High fidelity enabled, but TF returned a transform exception to frame %s: %s\n",
+    robot::log_error("High fidelity enabled, but TF returned a transform exception to frame %s: %s\n",
              global_frame_.c_str(), ex.what());
     projector_.projectLaser(message, cloud);
   }
   catch (std::runtime_error &ex)
   {
-    printf("transformLaserScanToPointCloud error, it seems the message from laser sensor is malformed. Ignore this laser scan. what(): %s\n", ex.what());
+    robot::log_error("transformLaserScanToPointCloud error, it seems the message from laser sensor is malformed. Ignore this laser scan. what(): %s\n", ex.what());
     return; //ignore this message
   }
 
@@ -358,7 +541,7 @@ void ObstacleLayer::pointCloudCallback(const robot_sensor_msgs::PointCloud& mess
 
   if (!robot_sensor_msgs::convertPointCloudToPointCloud2(message, cloud2))
   {
-    printf("Failed to convert a PointCloud to a PointCloud2, dropping message\n");
+    robot::log_error("Failed to convert a PointCloud to a PointCloud2, dropping message\n");
     return;
   }
 
@@ -377,6 +560,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)
 {
@@ -415,6 +606,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;
@@ -422,7 +616,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_)
       {
-        printf("The point is too high\n");
+        ++rejected_height;
         continue;
       }
 
@@ -433,7 +627,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)
       {
-        printf("The point is too far away\n");
+        ++rejected_range;
         continue;
       }
 
@@ -441,7 +635,7 @@ void ObstacleLayer::updateBounds(double robot_x, double robot_y, double robot_ya
       unsigned int mx, my;
       if (!worldToMap(px, py, mx, my))
       {
-        printf("Computing map coords failed\n");
+        ++rejected_bounds;
         continue;
       }
 
@@ -449,6 +643,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);
@@ -535,6 +737,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)
 {
@@ -546,7 +765,7 @@ void ObstacleLayer::raytraceFreespace(const Observation& clearing_observation, d
   unsigned int x0, y0;
   if (!worldToMap(ox, oy, x0, y0))
   {
-    printf(
+    robot::log_error(
         "The origin for the sensor at (%.2f, %.2f) is out of map bounds. So, the costmap cannot raytrace for it.\n",
         ox, oy);
     return;

plugins/preferred_layer.cpp

@@ -14,7 +14,6 @@ PreferredLayer::~PreferredLayer(){}
 
 unsigned char PreferredLayer::interpretValue(unsigned char value) 
 {
-  printf("TEST PLUGIN !!!\n");
   // check if the static value is above the unknown or lethal thresholds
   if(value == 0) return NO_INFORMATION;
   else if (value >= *this->threshold_)

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,10 +217,10 @@ 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;
 
-    printf("Received a %d X %d map at %f m/pix\n", size_x, size_y, new_map.info.resolution);
+    robot::log_info("Received a %d X %d map at %f m/pix\n", size_x, size_y, new_map.info.resolution);
 
     // resize costmap if size, resolution or origin do not match
     Costmap2D* master = layered_costmap_->getCostmap();
@@ -213,7 +232,7 @@ void StaticLayer::incomingMap(const robot_nav_msgs::OccupancyGrid& new_map)
         master->getOriginY() != new_map.info.origin.position.y))
     {
       // Update the size of the layered costmap (and all layers, including this one)
-      printf("Resizing costmap to %d X %d at %f m/pix\n", size_x, size_y, new_map.info.resolution);
+      robot::log_info("Resizing costmap to %d X %d at %f m/pix\n", size_x, size_y, new_map.info.resolution);
       layered_costmap_->resizeMap(size_x, size_y, new_map.info.resolution, new_map.info.origin.position.x,
                                   new_map.info.origin.position.y,
                                   true /* set size_locked to true, prevents reconfigureCb from overriding map size*/);
@@ -224,7 +243,7 @@ void StaticLayer::incomingMap(const robot_nav_msgs::OccupancyGrid& new_map)
             origin_y_ != new_map.info.origin.position.y)
     {
       // only update the size of the costmap stored locally in this layer
-      printf("Resizing static layer to %d X %d at %f m/pix\n", size_x, size_y, new_map.info.resolution);
+      robot::log_info("Resizing static layer to %d X %d at %f m/pix\n", size_x, size_y, new_map.info.resolution);
       resizeMap(size_x, size_y, new_map.info.resolution,
                 new_map.info.origin.position.x, new_map.info.origin.position.y);
     }
@@ -247,11 +266,9 @@ void StaticLayer::incomingMap(const robot_nav_msgs::OccupancyGrid& new_map)
       {
         unsigned char value = new_map.data[index];
         costmap_[index] = interpretValue(value);
-        // printf("%d , ",costmap_[index]);
         // 3. Ghi giá trị biến
         // file << static_cast<int>(costmap_[index]) << " , ";
         ++index;
-        // printf("%d , ",costmap_[index]);
       }
       // file << std::endl;
     }
@@ -268,13 +285,13 @@ void StaticLayer::incomingMap(const robot_nav_msgs::OccupancyGrid& new_map)
   }
   else
   {
-    printf("Stop receive new map!");
+    robot::log_info("Stop receive new map!\n");
   }
 
   // shutdown the map subscrber if firt_map_only_ flag is on
   if (first_map_only_)
   {
-    printf("Shutting down the map subscriber. first_map_only flag is on\n");
+    robot::log_info("Shutting down the map subscriber. first_map_only flag is on\n");
     map_shutdown_ = true;
   }
 }
@@ -283,7 +300,7 @@ void StaticLayer::incomingUpdate(const robot_map_msgs::OccupancyGridUpdate& upda
 {
   if(!map_update_shutdown_)
   {
-    std::cout << "Update new map!" << std::endl;
+    robot::log_info("Update new map!\n");
     unsigned int di = 0;
     for (unsigned int y = 0; y < update.height ; y++)
     {
@@ -328,7 +345,6 @@ void StaticLayer::reset()
   {
     onInitialize();
   }
-  printf("RESET MAP");
 }
 
 void StaticLayer::updateBounds(double robot_x, double robot_y, double robot_yaw, double* min_x, double* min_y,
@@ -378,11 +394,11 @@ 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)
     {
-      printf("%s", ex.what());
+      robot::log_error("StaticLayer::updateCosts(): %s \n", ex.what());
       return;
     }
     // Copy map data given proper transformations

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);
@@ -85,7 +99,7 @@ bool VoxelLayer::getParams(const std::string& config_file_name, robot::NodeHandl
     mark_threshold_ = loadParam(layer, "mark_threshold", 0);
     combination_method_ = loadParam(layer, "combination_method", 0.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"))
@@ -112,7 +126,6 @@ bool VoxelLayer::getParams(const std::string& config_file_name, robot::NodeHandl
     if (nh.hasParam("combination_method"))
       nh.getParam("combination_method", combination_method_);
 
-
     this->matchSize();
   } 
   catch (const YAML::BadFile& e) {
@@ -159,6 +172,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 +180,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)
   {
@@ -225,29 +246,6 @@ void VoxelLayer::updateBounds(double robot_x, double robot_y, double robot_yaw,
       }
     }
   }
-
-  // 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);
-  // }
-
   updateFootprint(robot_x, robot_y, robot_yaw, min_x, min_y, max_x, max_y);
 }
 
@@ -316,19 +314,11 @@ void VoxelLayer::raytraceFreespace(const Observation& clearing_observation, doub
 
   if (!worldToMap3DFloat(ox, oy, oz, sensor_x, sensor_y, sensor_z))
   {
-    printf(
+    robot::log_error(
         "The origin for the sensor at (%.2f, %.2f, %.2f) is out of map bounds. So, the costmap cannot raytrace for it.\n",
         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 +393,556 @@ 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);
-      // }
+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;
+
+  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)
+{
+  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)
+  {
+    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;
+
+  // Sample every pixel_step-th row/column and always include the last image
+  // row/column, so cells marked from border pixels stay inside the swept
+  // clearing fan.
+  std::vector<unsigned int> u_samples, v_samples;
+  u_samples.reserve(width / pixel_step + 2);
+  v_samples.reserve(height / pixel_step + 2);
+  for (unsigned int u = 0; u < width; u += pixel_step)
+    u_samples.push_back(u);
+  if (width > 0 && u_samples.back() != width - 1)
+    u_samples.push_back(width - 1);
+  for (unsigned int v = 0; v < height; v += pixel_step)
+    v_samples.push_back(v);
+  if (height > 0 && v_samples.back() != height - 1)
+    v_samples.push_back(height - 1);
+
+  cached_column_count_ = static_cast<unsigned int>(u_samples.size());
+  depth_ray_cache_.clear();
+  depth_ray_cache_.reserve(u_samples.size() * v_samples.size());
+
+  for (const unsigned int v : v_samples)
+  {
+    for (unsigned int col = 0; col < u_samples.size(); ++col)
+    {
+      const unsigned int u = u_samples[col];
+      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, col, 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;
+
+  // Look up the sensor pose at the depth image's CAPTURE time, not the latest
+  // transform. The costmap update runs later than the frame was captured, so
+  // during rotation the latest pose orients the clearing frustum where the depth
+  // pixels were never measured from; the fan's free rays then sweep across and
+  // erase freshly marked cells, and the trailing side that gets erased flips
+  // with rotation direction. A stamped lookup keeps the frustum geometrically
+  // consistent with its own pixels. If the transform at that stamp is
+  // unavailable (stale / would extrapolate), skip clearing this cycle instead of
+  // clearing from a wrong pose. Falls back to latest only when the frame carries
+  // no stamp.
+  const robot::Time& depth_stamp = local_origin.header.stamp;
+  const tf3::Time query_time =
+      depth_stamp.isZero() ? tf3::Time() : tf3::Time(depth_stamp.sec, depth_stamp.nsec);
+
+  robot_geometry_msgs::PointStamped global_origin;
+  tf3::TransformStampedMsg tfm;
+  try
+  {
+    tfm = tf_->lookupTransform(global_frame_, depth_frame, query_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 at t=%.3f: %s\n",
+        observation.topic_.c_str(), depth_frame.c_str(), global_frame_.c_str(),
+        query_time.toSec(), 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 DepthFrustumConfig& frustum = observation.frustum_;
+  const unsigned int step = std::max(1u, frustum.pixel_step);
+  const double min_range = frustum.min_range;
+  const double max_range = frustum.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);
+  updateDepthRayCache(width, height, step, fx, fy, cx, cy);
+
+  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;
+
+  // Column clearing: certify the beam length per pixel column and the distance
+  // window [cover, far] where the vertical FOV spans the whole height band.
+  // Outside that window a real obstacle could sit above/below the FOV, so only
+  // the per-pixel 3D rays may clear there.
+  const double band_min_h = frustum.column_min_height;
+  const double band_max_h =
+      frustum.column_max_height >= 0.0 ? frustum.column_max_height : max_obstacle_height_;
+  double cover_dist = frustum.column_cover_distance;
+  double far_dist = std::numeric_limits<double>::infinity();
+  bool column_pass = frustum.column_clearing && band_max_h > band_min_h;
+
+  if (column_pass && cover_dist < 0.0)
+  {
+    const double up_half = std::atan2(cy, fy);
+    const double down_half = std::atan2(static_cast<double>(height) - 1.0 - cy, fy);
+    const double axis_elev = std::atan2(r22, std::hypot(r02, r12));
+    const double alpha_top = axis_elev + up_half;
+    const double alpha_bot = axis_elev - down_half;
+    const double band_top = band_max_h - oz;
+    const double band_bot = band_min_h - oz;
+    constexpr double kMinSlope = 1e-3;
+
+    cover_dist = 0.0;
+    if (band_top > 0.0)
+    {
+      if (alpha_top <= kMinSlope)
+        column_pass = false;  // camera can never look up to the band top
+      else
+        cover_dist = std::max(cover_dist, band_top / std::tan(alpha_top));
+    }
+    else if (alpha_top < -kMinSlope)
+    {
+      far_dist = std::min(far_dist, band_top / std::tan(alpha_top));
+    }
+    if (band_bot < 0.0)
+    {
+      if (alpha_bot >= -kMinSlope)
+        column_pass = false;  // camera can never look down to the band bottom
+      else
+        cover_dist = std::max(cover_dist, band_bot / std::tan(alpha_bot));
+    }
+    else if (alpha_bot > kMinSlope)
+    {
+      far_dist = std::min(far_dist, band_bot / std::tan(alpha_bot));
+    }
+
+    if (!column_pass)
+    {
+      robot::log_warning_throttle(
+          10.0, "VoxelLayer column clearing disabled: vertical FOV [%.1f, %.1f] deg at camera "
+          "height %.2f m never covers band [%.2f, %.2f] m\n",
+          alpha_bot * 180.0 / M_PI, alpha_top * 180.0 / M_PI, oz, band_min_h, band_max_h);
     }
   }
 
-  // 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;
+  if (column_pass)
+    depth_column_stats_.assign(cached_column_count_, DepthColumnStat());
 
-    // clearing_endpoints_pub_.publish(clearing_endpoints_);
-  // }
+  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;
+
+    // Edge stereo no-disparity strips (left and/or right, depending on the
+    // camera). An INVALID pixel in such a strip is structurally invalid (carries
+    // no free-space evidence), so clearing it out to max_range erases obstacles
+    // rotating out of the FOV on that side (the turn bug) — skip only those. A
+    // VALID return there is a real measured surface, so it must still clear
+    // normally; otherwise the border becomes a clearing dead zone and obstacles
+    // there never get cleared. Invalid pixels OUTSIDE the strips still clear to
+    // max_range (ghost removal). Right edge measured inward from the last column;
+    // the unsigned test avoids underflow when the border exceeds the width.
+    const bool in_left_border = local_ray.u < frustum.clear_left_border_px;
+    const bool in_right_border =
+        frustum.clear_right_border_px > 0 &&
+        local_ray.u + frustum.clear_right_border_px >= width;
+    const bool in_border = in_left_border || in_right_border;
+    if (in_border && !valid)
+      continue;
+
+    // depth images store z-depth; local_ray.z is the unit ray's optical axis
+    // component, so depth / z is the Euclidean range
+    const double euclid_range = valid ? depth_m / local_ray.z : 0.0;
+
+    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;
+
+    if (column_pass && local_ray.col < depth_column_stats_.size())
+    {
+      const double horiz_norm = std::hypot(global_ray.x, global_ray.y);
+      if (horiz_norm > 1e-6)
+      {
+        DepthColumnStat& stat = depth_column_stats_[local_ray.col];
+        const double row_delta = std::fabs(static_cast<double>(local_ray.v) - cy);
+        if (stat.min_band_dist < 0.0 && row_delta < stat.best_row_delta)
+        {
+          // no in-band return yet: aim the beam along the ray nearest the
+          // principal row
+          stat.azimuth = std::atan2(global_ray.y, global_ray.x);
+          stat.best_row_delta = row_delta;
+        }
+        stat.has_ray = true;
+        if (in_border)
+          stat.in_border = true;
+
+        if (valid)
+        {
+          const double pz = oz + global_ray.z * euclid_range;
+          if (pz >= band_min_h && pz <= band_max_h)
+          {
+            const double dist_h = horiz_norm * euclid_range;
+            if (stat.min_band_dist < 0.0 || dist_h < stat.min_band_dist)
+            {
+              stat.min_band_dist = dist_h;
+              stat.azimuth = std::atan2(global_ray.y, global_ray.x);
+            }
+          }
+        }
+      }
+    }
+
+    double ray_len = max_range;
+    if (valid && euclid_range < max_range)
+      ray_len = std::max(0.0, euclid_range - skip_dist);
+
+    if (ray_len <= min_range)
+      continue;
+
+    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;
+  }
+
+  if (column_pass)
+  {
+    cleared_any = clearDepthColumns(ox, oy, cover_dist, far_dist, min_range, max_range,
+                                    std::max(0.0, frustum.column_skip_distance),
+                                    min_x, min_y, max_x, max_y) ||
+                  cleared_any;
+  }
+
+  return cleared_any;
+}
+
+namespace
+{
+/// raytraceLine action: frees the 2D cell and wipes its whole voxel column.
+class ClearFullColumn
+{
+public:
+  ClearFullColumn(unsigned char* costmap, robot_voxel_grid::VoxelGrid& voxel_grid)
+    : costmap_(costmap), voxel_grid_(voxel_grid)
+  {
+  }
+
+  inline void operator()(unsigned int offset)
+  {
+    costmap_[offset] = FREE_SPACE;
+    voxel_grid_.clearVoxelColumn(offset);
+  }
+
+private:
+  unsigned char* costmap_;
+  robot_voxel_grid::VoxelGrid& voxel_grid_;
+};
+}  // namespace
+
+bool VoxelLayer::clipColumnSegment(double& sx, double& sy, double& ex, double& ey) const
+{
+  // Liang-Barsky clip against the map interior; the half-resolution margin
+  // keeps clipped endpoints valid for worldToMap.
+  const double min_wx = origin_x_;
+  const double min_wy = origin_y_;
+  const double max_wx = origin_x_ + getSizeInMetersX() - 0.5 * resolution_;
+  const double max_wy = origin_y_ + getSizeInMetersY() - 0.5 * resolution_;
+  const double dx = ex - sx;
+  const double dy = ey - sy;
+  const double p[4] = {-dx, dx, -dy, dy};
+  const double q[4] = {sx - min_wx, max_wx - sx, sy - min_wy, max_wy - sy};
+
+  double t0 = 0.0;
+  double t1 = 1.0;
+  for (int i = 0; i < 4; ++i)
+  {
+    if (std::fabs(p[i]) < 1e-12)
+    {
+      if (q[i] < 0.0)
+        return false;
+      continue;
+    }
+    const double r = q[i] / p[i];
+    if (p[i] < 0.0)
+      t0 = std::max(t0, r);
+    else
+      t1 = std::min(t1, r);
+  }
+  if (t0 > t1)
+    return false;
+
+  const double bx = sx;
+  const double by = sy;
+  sx = bx + t0 * dx;
+  sy = by + t0 * dy;
+  ex = bx + t1 * dx;
+  ey = by + t1 * dy;
+  return true;
+}
+
+bool VoxelLayer::clearDepthColumns(double ox, double oy, double cover_distance,
+                                   double far_distance, double min_range, double max_range,
+                                   double skip_dist, double* min_x, double* min_y,
+                                   double* max_x, double* max_y)
+{
+  const double start_dist = std::max(cover_distance, min_range);
+  bool cleared_any = false;
+
+  for (const DepthColumnStat& stat : depth_column_stats_)
+  {
+    if (!stat.has_ray)
+      continue;
+
+    // In an edge stereo strip, never clear a whole column out to max_range on a
+    // missing in-band return: that is exactly the no-free-space-evidence case that
+    // erases obstacles turning out of view. Only an in-band measured surface may
+    // shorten (and thus clear) a border column.
+    if (stat.in_border && stat.min_band_dist < 0.0)
+      continue;
+
+    double end_dist = stat.min_band_dist >= 0.0 ? stat.min_band_dist - skip_dist : max_range;
+    end_dist = std::min(std::min(end_dist, max_range), far_distance);
+    if (end_dist <= start_dist)
+      continue;
+
+    const double cos_az = std::cos(stat.azimuth);
+    const double sin_az = std::sin(stat.azimuth);
+    double sx = ox + cos_az * start_dist;
+    double sy = oy + sin_az * start_dist;
+    double ex = ox + cos_az * end_dist;
+    double ey = oy + sin_az * end_dist;
+    if (!clipColumnSegment(sx, sy, ex, ey))
+      continue;
+
+    unsigned int sx_m, sy_m, ex_m, ey_m;
+    if (!worldToMap(sx, sy, sx_m, sy_m) || !worldToMap(ex, ey, ex_m, ey_m))
+      continue;
+
+    ClearFullColumn clearer(costmap_, robot_voxel_grid_);
+    raytraceLine(clearer, sx_m, sy_m, ex_m, ey_m);
+    touch(sx, sy, min_x, min_y, max_x, max_y);
+    touch(ex, ey, min_x, min_y, max_x, max_y);
+    cleared_any = true;
+  }
+
+  return cleared_any;
 }
 
 void VoxelLayer::updateOrigin(double new_origin_x, double new_origin_y)
@@ -432,6 +952,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 +977,20 @@ 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);
+  unsigned char* local_map = rolling_costmap_scratch_.data();
+  unsigned int* local_voxel_map = rolling_voxel_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);
+  }
 
   // we'll reset our maps to unknown space if appropriate
   resetMaps();
@@ -474,12 +1004,14 @@ 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);
+  }
 
-  // make sure to clean up
-  delete[] local_map;
-  delete[] local_voxel_map;
 }
 
 // Export factory function

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)
@@ -471,15 +479,12 @@ bool Costmap2D::saveMap(std::string file_name)
     return false;
   }
 
-  fprintf(fp, "P2\n%u\n%u\n%u\n", size_x_, size_y_, 0xff);
   for (unsigned int iy = 0; iy < size_y_; iy++)
   {
     for (unsigned int ix = 0; ix < size_x_; ix++)
     {
       unsigned char cost = getCost(ix, iy);
-      fprintf(fp, "%d ", cost);
     }
-    fprintf(fp, "\n");
   }
   fclose(fp);
   return true;

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,7 +81,7 @@ 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;
@@ -79,23 +89,31 @@ Costmap2DROBOT::Costmap2DROBOT(const std::string& name, tf3::BufferCore& tf) :
   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);
@@ -111,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());
@@ -128,16 +148,52 @@ 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 (nh.hasParam("track_unknown_space"))
-      nh.getParam("track_unknown_space", track_unknown_space);
+    if (priv_nh.hasParam("rolling_window"))
+      priv_nh.getParam("rolling_window", rolling_window);
 
-    if (nh.hasParam("library_path"))
+    if (priv_nh.hasParam("track_unknown_space"))
+      priv_nh.getParam("track_unknown_space", track_unknown_space);
+
+    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_);
-
     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);
+    double map_height_meters = loadParam(layer, "height", 0.0);
+    double resolution = loadParam(layer, "resolution", 0.0);
+    double origin_x = loadParam(layer, "origin_x", 0.0);
+    double origin_y = loadParam(layer, "origin_y", 0.0);
+
+    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())
+    {
+      // robot::log_warning("ROBOT origin_x: %f | origin_y: %f", origin_x, origin_y);
+      layered_costmap_->resizeMap((unsigned int)(map_width_meters / resolution),
+                                  (unsigned int)(map_height_meters / resolution), resolution, origin_x, origin_y);
+    }
     
     struct PluginConfig 
     {
@@ -146,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)
@@ -203,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)
@@ -230,13 +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);
 
@@ -252,31 +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);
 
-    // find size parameters
-    double map_width_meters = loadParam(layer, "width", 0.0);
-    double map_height_meters = loadParam(layer, "height", 0.0);
-    double resolution = loadParam(layer, "resolution", 0.0);
-    double origin_x = loadParam(layer, "origin_x", 0.0);
-    double origin_y = loadParam(layer, "origin_y", 0.0);
-
-    if (nh.hasParam("update_frequency"))
-      nh.getParam("update_frequency", map_update_frequency);
-    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 (!layered_costmap_->isSizeLocked())
-    {
-      layered_costmap_->resizeMap((unsigned int)(map_width_meters / resolution),
-                                  (unsigned int)(map_height_meters / resolution), resolution, origin_x, origin_y);
-    }
+    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.
@@ -290,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
@@ -305,6 +340,189 @@ 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;
+        double frustum_skip_distance = -1.0;
+        bool frustum_column_clearing = false;
+        double column_clear_min_height = 0.10;
+        double column_clear_max_height = -1.0;
+        double column_skip_distance = 0.02;
+        double column_cover_distance = -1.0;
+        int frustum_clear_left_border_px = 0;
+        int frustum_clear_right_border_px = 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);
+        move_parameter(plugin_nh_element, costmap_plugin_nh_element, "frustum_skip_distance", frustum_skip_distance);
+        move_parameter(plugin_nh_element, costmap_plugin_nh_element, "frustum_column_clearing", frustum_column_clearing);
+        move_parameter(plugin_nh_element, costmap_plugin_nh_element, "column_clear_min_height", column_clear_min_height);
+        move_parameter(plugin_nh_element, costmap_plugin_nh_element, "column_clear_max_height", column_clear_max_height);
+        move_parameter(plugin_nh_element, costmap_plugin_nh_element, "column_skip_distance", column_skip_distance);
+        move_parameter(plugin_nh_element, costmap_plugin_nh_element, "column_cover_distance", column_cover_distance);
+        move_parameter(plugin_nh_element, costmap_plugin_nh_element, "frustum_clear_left_border_px", frustum_clear_left_border_px);
+        move_parameter(plugin_nh_element, costmap_plugin_nh_element, "frustum_clear_right_border_px", frustum_clear_right_border_px);
+        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_clear_left_border_px: %d, frustum_clear_right_border_px: %d", frustum_clearing_enabled ? "true" : "false", frustum_clearing_pixel_step, frustum_min_range, frustum_max_range, frustum_clear_left_border_px, frustum_clear_right_border_px);
+      }
+    }
+  }
+  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;
+        double frustum_skip_distance = -1.0;
+        bool frustum_column_clearing = false;
+        double column_clear_min_height = 0.10;
+        double column_clear_max_height = -1.0;
+        double column_skip_distance = 0.02;
+        double column_cover_distance = -1.0;
+        int frustum_clear_left_border_px = 0;
+        int frustum_clear_right_border_px = 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);
+        move_parameter(plugin_nh_element, costmap_plugin_nh_element, "frustum_skip_distance", frustum_skip_distance);
+        move_parameter(plugin_nh_element, costmap_plugin_nh_element, "frustum_column_clearing", frustum_column_clearing);
+        move_parameter(plugin_nh_element, costmap_plugin_nh_element, "column_clear_min_height", column_clear_min_height);
+        move_parameter(plugin_nh_element, costmap_plugin_nh_element, "column_clear_max_height", column_clear_max_height);
+        move_parameter(plugin_nh_element, costmap_plugin_nh_element, "column_skip_distance", column_skip_distance);
+        move_parameter(plugin_nh_element, costmap_plugin_nh_element, "column_cover_distance", column_cover_distance);
+        move_parameter(plugin_nh_element, costmap_plugin_nh_element, "frustum_clear_left_border_px", frustum_clear_left_border_px);
+        move_parameter(plugin_nh_element, costmap_plugin_nh_element, "frustum_clear_right_border_px", frustum_clear_right_border_px);
+        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_clear_left_border_px: %d, frustum_clear_right_border_px: %d", frustum_clearing_enabled ? "true" : "false", frustum_clearing_pixel_step, frustum_min_range, frustum_max_range, frustum_clear_left_border_px, frustum_clear_right_border_px);
+      }
+    }
+  }
+  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));
@@ -379,7 +597,7 @@ void Costmap2DROBOT::mapUpdateLoop(double frequency)
     r.sleep();
     // make sure to sleep for the remainder of our cycle time
     if (r.cycleTime() > robot::Duration(1 / frequency))
-      printf("Map update loop missed its desired rate of %.4fHz... the loop actually took %.4f seconds\n", frequency,
+      robot::log_warning("Map update %s loop missed its desired rate of %.4fHz... the loop actually took %.4f seconds\n", name_.c_str(), frequency,
                r.cycleTime().toSec());
   }
 }
@@ -395,17 +613,19 @@ void Costmap2DROBOT::updateMap()
       double x = pose.pose.position.x,
              y = pose.pose.position.y,
              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");
@@ -429,11 +649,11 @@ void Costmap2DROBOT::start()
   robot::Rate r(100.0);
   while (!initialized_ && map_update_thread_)
   {
-    // if (robot::Time::now() - start_time > robot::Duration(5.0))
-    // {
-    //   printf("Costmap2DROBOT::start() timed out waiting for costmap to initialize or map_update_thread\n");
-    //   break;
-    // }
+    if (robot::Time::now() - start_time > robot::Duration(5.0))
+    {
+      robot::log_warning_throttle(3.0, "Costmap2DROBOT::start() timed out waiting for costmap to initialize or map_update_thread\n");
+      break;
+    }
     r.sleep();
   }
 }
@@ -472,7 +692,7 @@ void Costmap2DROBOT::resume()
   {
     if (robot::Time::now() - start_time > robot::Duration(5.0))
     {
-      printf("Costmap2DROBOT::resume() timed out waiting for costmap to initialize\n");
+      robot::log_warning("Costmap2DROBOT::resume() timed out waiting for costmap to initialize\n");
       break;
     }
     r.sleep();
@@ -498,6 +718,13 @@ bool Costmap2DROBOT::getRobotPose(robot_geometry_msgs::PoseStamped& global_pose)
 {
   robot_geometry_msgs::PoseStamped robot_pose;
   robot_geometry_msgs::Pose pose_default;
+  pose_default.orientation.x = 0;
+  pose_default.orientation.y = 0;
+  pose_default.orientation.z = 0;
+  pose_default.orientation.w = 1;
+  pose_default.position.x = 0;
+  pose_default.position.y = 0;
+  pose_default.position.z = 0;
   global_pose.pose = pose_default;
   robot_pose.pose = pose_default;
 
@@ -508,44 +735,30 @@ 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_, data_convert::convertTime(current_time)))
-    {
-      tf3::TransformStampedMsg transform = tf_.lookupTransform(global_frame_, robot_base_frame_, data_convert::convertTime(current_time));
+    const tf3::TransformStampedMsg transform =
+        tf_.lookupTransform(global_frame_, robot_base_frame_, tf3::Time());
     tf3::doTransform(robot_pose, global_pose, transform);
   }
-    // 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
-                    data_convert::convertTime(robot_pose.header.stamp)
-                );
-      tf3::doTransform(robot_pose, global_pose, transform);
-    }
-  }
   catch (tf3::LookupException& ex)
   {
-    printf("Cost Map No Transform available Error looking up robot pose: %s\n", ex.what());
+    robot::log_error("Costmap2DROBOT %s No Transform available Error looking up robot pose: %s\n", name_.c_str(), ex.what());
     return false;
   }
   catch (tf3::ConnectivityException& ex)
   {
-    printf("Connectivity Error looking up robot pose: %s\n", ex.what());
+    robot::log_error("Costmap2DROBOT %s Connectivity Error looking up robot pose: %s\n", name_.c_str(), ex.what());
     return false;
   }
   catch (tf3::ExtrapolationException& ex)
   {
-    printf("Extrapolation Error looking up robot pose: %s\n", 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);
   // check global_pose timeout
   if (!global_pose.header.stamp.isZero() && current_time.toSec() - global_pose.header.stamp.toSec() > transform_tolerance_)
   {
-    printf("Costmap2DROBOT transform timeout. Current time: %.4f, global_pose stamp: %.4f, tolerance: %.4f\n",
+    robot::log_warning("Costmap2DROBOT %s transform timeout. Current time: %.4f, global_pose stamp: %.4f, tolerance: %.4f\n", name_.c_str(),
             current_time.toSec(), global_pose.header.stamp.toSec(), transform_tolerance_);
     return false;
   }

src/footprint.cpp

@@ -174,15 +174,15 @@ bool makeFootprintFromString(const std::string& footprint_string, std::vector<ro
 
   if (error != "")
   {
-    printf("Error parsing footprint parameter: '%s'\n", error.c_str());
-    printf("  Footprint string was '%s'.\n", footprint_string.c_str());
+    robot::log_error("Error parsing footprint parameter: '%s'\n", error.c_str());
+    robot::log_error("  Footprint string was '%s'.\n", footprint_string.c_str());
     return false;
   }
 
   // convert vvf into points.
   if (vvf.size() < 3)
   {
-    printf("You must specify at least three points for the robot footprint, reverting to previous footprint.\n");
+    robot::log_error("You must specify at least three points for the robot footprint, reverting to previous footprint.\n");
     return false;
   }
   footprint.reserve(vvf.size());
@@ -198,7 +198,7 @@ bool makeFootprintFromString(const std::string& footprint_string, std::vector<ro
     }
     else
     {
-      printf("Points in the footprint specification must be pairs of numbers.  Found a point with %d numbers.\n",
+      robot::log_error("Points in the footprint specification must be pairs of numbers.  Found a point with %d numbers.\n",
                  int(vvf[ i ].size()));
       return false;
     }
@@ -256,7 +256,7 @@ double getNumberFromXMLRPC(robot_xmlrpcpp::XmlRpcValue& value, const std::string
       value.getType() != robot_xmlrpcpp::XmlRpcValue::TypeDouble)
   {
     std::string& value_string = value;
-    printf("Values in the footprint specification (param %s) must be numbers. Found value %s.\n",
+    robot::log_error("Values in the footprint specification (param %s) must be numbers. Found value %s.\n",
                full_param_name.c_str(), value_string.c_str());
     throw std::runtime_error("Values in the footprint specification must be numbers");
   }
@@ -270,7 +270,7 @@ std::vector<robot_geometry_msgs::Point> makeFootprintFromXMLRPC(robot_xmlrpcpp::
   if (footprint_xmlrpc.getType() != robot_xmlrpcpp::XmlRpcValue::TypeArray ||
       footprint_xmlrpc.size() < 3)
   {
-    printf("The footprint must be specified as list of lists on the parameter server, %s was specified as %s\n",
+    robot::log_error("The footprint must be specified as list of lists on the parameter server, %s was specified as %s\n",
                full_param_name.c_str(), std::string(footprint_xmlrpc).c_str());
     throw std::runtime_error("The footprint must be specified as list of lists on the parameter server with at least "
                              "3 points eg: [[x1, y1], [x2, y2], ..., [xn, yn]]");
@@ -286,7 +286,7 @@ std::vector<robot_geometry_msgs::Point> makeFootprintFromXMLRPC(robot_xmlrpcpp::
     if (point.getType() != robot_xmlrpcpp::XmlRpcValue::TypeArray ||
         point.size() != 2)
     {
-      printf("The footprint (parameter %s) must be specified as list of lists on the parameter server eg: "
+      robot::log_error("The footprint (parameter %s) must be specified as list of lists on the parameter server eg: "
                 "[[x1, y1], [x2, y2], ..., [xn, yn]], but this spec is not of that form.\n",
                  full_param_name.c_str());
       throw std::runtime_error("The footprint must be specified as list of lists on the parameter server eg: "

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,24 +175,35 @@ namespace robot_costmap_2d
 
     minx_ = miny_ = 1e30;
     maxx_ = maxy_ = -1e30;
+    if (performance_metrics_enabled_ && layer_performance_.size() != plugins_.size())
+      layer_performance_.assign(plugins_.size(), LayerPerformance());
 
-    for (vector<boost::shared_ptr<Layer>>::iterator plugin = plugins_.begin(); plugin != plugins_.end();
-         ++plugin)
+    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)
       {
-        printf("Illegal bounds change, was [tl: (%f, %f), br: (%f, %f)], but "
+        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());
       }
     }
 
@@ -141,18 +216,35 @@ namespace robot_costmap_2d
     y0 = std::max(0, y0);
     yn = std::min(int(costmap_.getSizeInCellsY()), yn + 1);
 
-    printf("Updating area x: [%d, %d] y: [%d, %d]\n", x0, xn, y0, yn);
-    
     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;
@@ -161,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()

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, const DepthFrustumConfig& frustum_config,
+                                     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),
+    tf_tolerance_(tf_tolerance),
+    frustum_config_(frustum_config)
+{
+  frustum_config_.pixel_step = std::max(1u, frustum_config_.pixel_step);
+  frustum_config_.min_range = std::max(0.0, frustum_config_.min_range);
+  frustum_config_.max_range = std::max(frustum_config_.max_range, frustum_config_.min_range);
+}
+
 ObservationBuffer::~ObservationBuffer()
 {
 }
@@ -68,7 +87,7 @@ bool ObservationBuffer::setGlobalFrame(const std::string new_global_frame)
   robot_geometry_msgs::TransformStamped transformStamped;
   if (!tf3_buffer_.canTransform(new_global_frame, global_frame_, transform_time, &tf_error))
   {
-    printf("Transform between %s and %s with tolerance %.2f failed: %s.\n", new_global_frame.c_str(),
+    robot::log_error("Transform between %s and %s with tolerance %.2f failed: %s.\n", new_global_frame.c_str(),
               global_frame_.c_str(), tf_tolerance_, tf_error.c_str());
     return false;
   }
@@ -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);
@@ -106,7 +131,7 @@ bool ObservationBuffer::setGlobalFrame(const std::string new_global_frame)
     }
     catch (TransformException& ex)
     {
-      printf("TF Error attempting to transform an observation from %s to %s: %s\n", global_frame_.c_str(),
+      robot::log_error("TF Error attempting to transform an observation from %s to %s: %s\n", global_frame_.c_str(),
                 new_global_frame.c_str(), ex.what());
       return false;
     }
@@ -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,79 +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
-                        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
-                        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;
+        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
+    if (point_count != cloud_size)
+    {
+      robot_sensor_msgs::PointCloud2Modifier modifier(observation_cloud);
       modifier.resize(point_count);
-    observation_cloud.header.stamp = cloud.header.stamp;
-    observation_cloud.header.frame_id = global_frame_cloud.header.frame_id;
+    }
   }
   catch (TransformException& ex)
   {
-    // if an exception occurs, we need to remove the empty observation from the list
-    observation_list_.pop_front();
-    printf("TF Exception that should never happen for sensor frame: %s, cloud frame: %s, %s\n", sensor_frame_.c_str(),
+    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();
 
@@ -217,6 +229,36 @@ 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_config_);
+
+  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)
 {
@@ -231,6 +273,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())
@@ -257,6 +319,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))
@@ -265,8 +351,7 @@ bool ObservationBuffer::isCurrent() const
   bool current = (robot::Time::now() - last_updated_).toSec() <= expected_update_rate_.toSec();
   if (!current)
   {
-    printf(
-        "The %s observation buffer has not been updated for %.2f seconds, and it should be updated every %.2f seconds.\n",
+    robot::log_error("The %s observation buffer has not been updated for %.2f seconds, and it should be updated every %.2f seconds.\n",
         topic_name_.c_str(), (robot::Time::now() - last_updated_).toSec(), expected_update_rate_.toSec());
   }
   return current;
@@ -277,4 +362,3 @@ void ObservationBuffer::resetLastUpdated()
   last_updated_ = robot::Time::now();
 }
 }  // namespace robot_costmap_2d
-

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();
 }
-