test costmap + cam depth 14:38

2026-07-21 14:38:45 +07:00
parent 11164970e2
commit bb461b263d
9 changed files with 137 additions and 59 deletions
--- a/README.md
+++ b/README.md
@@ -80,24 +80,27 @@ Các topic RViz giống hướng `amr_control::AmrPublisher`:
 - `/local_costmap/costmap` kiểu `nav_msgs/OccupancyGrid`
 - `/local_costmap/costmap/footprint` kiểu `geometry_msgs/PolygonStamped`

-Config local cho `robot_costmap_2d` nằm trong thư mục `config/config/` của package này khi export:
+Config local chính của package này dùng cùng kiểu tách file với
+`pnkx_nav_core/config`:
+
+- `config/costmap_common_params.yaml`
+- `config/costmap_local_params_plugins_no_virtual_walls.yaml`
+- `config/costmap_local_params.yaml`
+
+Khi test trên NUC, export:

 ```bash
 export PNKX_NAV_CORE_CONFIG_DIR=/home/robotics/DuongTD/dev_ws/src/test/depth_local_costmap_noetic_test/config
 ```

-Lý do là `robot_costmap_2d::getSourceFile()` tự nối thêm `/config`, nên export tới package `config/` sẽ làm runtime đọc:
+`robot_cpp::NodeHandle` sẽ auto-load 3 file trên. Hiện `robot_costmap_2d`
+vẫn còn một số loader cũ dùng `getSourceFile()` và tự nối thêm `/config`, nên
+package cũng giữ các file shim tương thích trong:

 ```text
 /home/robotics/DuongTD/dev_ws/src/test/depth_local_costmap_noetic_test/config/config/*.yaml
 ```

-File test đang bám theo:
-
- `costmap_common_params.yaml`: source `pc_marking` và `pc_clearing`
- `costmap_local_params_plugins_no_virtual_walls.yaml`: plugin `obstacles` kiểu `VoxelLayer`, `inflation` kiểu `InflationLayer`
- `costmap_local_params.yaml`: `global_frame=odom`, `rolling_window=true`, `width/height=8.0`, `resolution=0.05`
-
 Với `robot_costmap_2d` chưa patch resolver plugin path, `library_path` trong config được đặt thẳng tới NUC:

 ```yaml
--- a/config/config/costmap_common_params.yaml
+++ b/config/config/costmap_common_params.yaml
@@ -1,12 +1,12 @@
-obstacle_layer:
-  enabled: true
-  track_unknown_space: false
-  transform_tolerance: 1.0
-  footprint_clearing_enabled: true
-  combination_method: 1
+robot_base_frame: base_link
+transform_tolerance: 1.0
+performance_metrics_enabled: true
+performance_metrics_period: 5.0
+obstacle_range: 3.0
+publish_voxel_map: true
+footprint_padding: 0.0

-  # Subset of costmap_common_params.yaml for the topics available in this
-  # local depth-camera test.
+obstacles:
  observation_sources: pc_marking pc_clearing

  pc_marking:
--- a/config/config/costmap_local_params.yaml
+++ b/config/config/costmap_local_params.yaml
@@ -1,33 +1,32 @@
-robot_costmap_2d:
-  # Derived from pnkx_nav_core/config/costmap_local_params.yaml.
-  # NUC path is intentional for the user's local test workspace.
+local_costmap:
+  # Absolute NUC path is intentional for this local test package. It avoids
+  # boost::dll resolving a relative value such as "libplugins" to ./libplugins.
  library_path: /home/robotics/DuongTD/dev_ws/devel/lib/libplugins.so
  global_frame: odom
  robot_base_frame: base_link
+  update_frequency: 6.0
+  publish_frequency: 6.0
  rolling_window: true
  track_unknown_space: false
-
-  plugins:
-    # Derived from costmap_local_params_plugins_no_virtual_walls.yaml.
-    - name: obstacles
-      type: VoxelLayer
-    - name: inflation
-      type: InflationLayer
-
+  raytrace_range: 3.0
+  resolution: 0.05
+  z_resolution: 0.15
+  z_voxels: 8
+  width: 8.0
+  height: 8.0
+  origin_x: 0.0
+  origin_y: 0.0
+  transform_tolerance: 1.0
+  performance_metrics_enabled: true
+  performance_metrics_period: 5.0
+  footprint_padding: 0.0
+  robot_radius: 0.0
  footprint:
    - [0.30, 0.30]
    - [0.30, -0.30]
    - [-0.30, -0.30]
    - [-0.30, 0.30]

-  transform_tolerance: 1.0
-  performance_metrics_enabled: true
-  performance_metrics_period: 5.0
-  update_frequency: 6.0
-  width: 8.0
-  height: 8.0
-  resolution: 0.05
-  origin_x: 0.0
-  origin_y: 0.0
-  footprint_padding: 0.0
-  robot_radius: 0.0
+  inflation:
+    cost_scaling_factor: 10.0
+    inflation_radius: 0.3
--- a/config/config/costmap_local_params_plugins_no_virtual_walls.yaml
+++ b/config/config/costmap_local_params_plugins_no_virtual_walls.yaml
@@ -0,0 +1,12 @@
+local_costmap:
+  frame_id: odom
+  plugins:
+    - {name: obstacles, type: "VoxelLayer"}
+    - {name: inflation, type: "InflationLayer"}
+
+  obstacles:
+    enabled: true
+    footprint_clearing_enabled: true
+
+  inflation:
+    enabled: true
--- a/config/costmap_common_params.yaml
+++ b/config/costmap_common_params.yaml
@@ -0,0 +1,41 @@
+robot_base_frame: base_link
+transform_tolerance: 1.0
+performance_metrics_enabled: true
+performance_metrics_period: 5.0
+obstacle_range: 3.0
+publish_voxel_map: true
+footprint_padding: 0.0
+
+obstacles:
+  observation_sources: pc_marking pc_clearing
+
+  pc_marking:
+    topic: /camera/depth/points_proc
+    data_type: PointCloud2
+    clearing: false
+    marking: true
+    inf_is_valid: false
+    frustum_clearing_enabled: false
+    observation_persistence: 0.0
+    expected_update_rate: 0.5
+    obstacle_range: 2.5
+    raytrace_range: 3.0
+    min_obstacle_height: 0.1
+    max_obstacle_height: 1.0
+
+  pc_clearing:
+    topic: /camera/depth/data
+    data_type: DepthCameraData
+    clearing: false
+    marking: true
+    inf_is_valid: false
+    frustum_clearing_enabled: true
+    frustum_clearing_pixel_step: 8
+    frustum_min_range: 0.20
+    frustum_max_range: 3.5
+    observation_persistence: 0.0
+    expected_update_rate: 0.5
+    obstacle_range: 2.5
+    raytrace_range: 3.5
+    min_obstacle_height: 0.1
+    max_obstacle_height: 1.0
--- a/config/costmap_local_params.yaml
+++ b/config/costmap_local_params.yaml
@@ -0,0 +1,32 @@
+local_costmap:
+  # Absolute NUC path is intentional for this local test package. It avoids
+  # boost::dll resolving a relative value such as "libplugins" to ./libplugins.
+  library_path: /home/robotics/DuongTD/dev_ws/devel/lib/libplugins.so
+  global_frame: odom
+  robot_base_frame: base_link
+  update_frequency: 6.0
+  publish_frequency: 6.0
+  rolling_window: true
+  track_unknown_space: false
+  raytrace_range: 3.0
+  resolution: 0.05
+  z_resolution: 0.15
+  z_voxels: 8
+  width: 8.0
+  height: 8.0
+  origin_x: 0.0
+  origin_y: 0.0
+  transform_tolerance: 1.0
+  performance_metrics_enabled: true
+  performance_metrics_period: 5.0
+  footprint_padding: 0.0
+  robot_radius: 0.0
+  footprint:
+    - [0.30, 0.30]
+    - [0.30, -0.30]
+    - [-0.30, -0.30]
+    - [-0.30, 0.30]
+
+  inflation:
+    cost_scaling_factor: 10.0
+    inflation_radius: 0.3
--- a/config/costmap_local_params_plugins_no_virtual_walls.yaml
+++ b/config/costmap_local_params_plugins_no_virtual_walls.yaml
@@ -0,0 +1,12 @@
+local_costmap:
+  frame_id: odom
+  plugins:
+    - {name: obstacles, type: "VoxelLayer"}
+    - {name: inflation, type: "InflationLayer"}
+
+  obstacles:
+    enabled: true
+    footprint_clearing_enabled: true
+
+  inflation:
+    enabled: true
--- a/config/inflation_layer_params.yaml
+++ b/config/inflation_layer_params.yaml
@@ -1,5 +0,0 @@
-inflation_layer:
-  enabled: true
-  inflate_unknown: false
-  cost_scaling_factor: 10.0
-  inflation_radius: 0.30
--- a/config/voxel_layer_params.yaml
+++ b/config/voxel_layer_params.yaml
@@ -1,16 +0,0 @@
-voxel_layer:
-  enabled: true
-  footprint_clearing_enabled: true
-  max_obstacle_height: 1.0
-  origin_z: 0.0
-  z_resolution: 0.15
-  z_voxels: 8
-  unknown_threshold: 15.0
-  mark_threshold: 0
-  combination_method: 1
-  publish_voxel_map: true
-  frustum_clearing_enabled: false
-  frustum_clearing_pixel_step: 8
-  frustum_min_range: 0.20
-  frustum_max_range: 3.5
-  frustum_depth_camera_topic: /camera/depth/data