test costmap + cam depth

2026-07-21 12:26:51 +07:00
parent 5d734a68fd
commit af99998d2e
11 changed files with 183 additions and 42 deletions
--- a/README.md
+++ b/README.md
@@ -66,8 +66,26 @@ Log runtime cần thấy:
 - `Fed depth cloud to robot_costmap_2d`
 - `Fed DepthCameraData to robot_costmap_2d`

-Config local cho `robot_costmap_2d` nằm trong thư mục `config/` của package này. Launch file set:
+Config local cho `robot_costmap_2d` nằm trong thư mục `config/config/` của package này khi export:

-```xml
-<env name="PNKX_NAV_CORE_CONFIG_DIR" value="$(find depth_local_costmap_noetic_test)"/>
+```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:
+
+```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
+library_path: /home/robotics/DuongTD/dev_ws/devel/lib/libplugins.so
 ```
--- a/config/config/costmap_params.yaml
+++ b/config/config/costmap_params.yaml
@@ -0,0 +1,33 @@
+robot_costmap_2d:
+  # Derived from pnkx_nav_core/config/costmap_local_params.yaml.
+  # NUC path is intentional for the user's local test workspace.
+  library_path: /home/robotics/DuongTD/dev_ws/devel/lib/libplugins.so
+  global_frame: odom
+  robot_base_frame: base_link
+  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
+
+  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
--- a/config/config/inflation_layer_params.yaml
+++ b/config/config/inflation_layer_params.yaml
@@ -0,0 +1,5 @@
+inflation_layer:
+  enabled: true
+  inflate_unknown: false
+  cost_scaling_factor: 10.0
+  inflation_radius: 0.30
--- a/config/config/obstacle_layer_params.yaml
+++ b/config/config/obstacle_layer_params.yaml
@@ -0,0 +1,41 @@
+obstacle_layer:
+  enabled: true
+  track_unknown_space: false
+  transform_tolerance: 1.0
+  footprint_clearing_enabled: true
+  combination_method: 1
+
+  # Subset of costmap_common_params.yaml for the topics available in this
+  # local depth-camera test.
+  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/config/static_layer_params.yaml
+++ b/config/config/static_layer_params.yaml
@@ -0,0 +1,11 @@
+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"
--- a/config/config/voxel_layer_params.yaml
+++ b/config/config/voxel_layer_params.yaml
@@ -0,0 +1,16 @@
+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
--- a/config/costmap_params.yaml
+++ b/config/costmap_params.yaml
@@ -1,29 +1,31 @@
 robot_costmap_2d:
+  # Derived from pnkx_nav_core/config/costmap_local_params.yaml.
+  # NUC path is intentional for the user's local test workspace.
+  library_path: /home/robotics/DuongTD/dev_ws/devel/lib/libplugins.so
  global_frame: odom
  robot_base_frame: base_link
  rolling_window: true
  track_unknown_space: false

  plugins:
-    - name: voxel_layer
+    # Derived from costmap_local_params_plugins_no_virtual_walls.yaml.
+    - name: obstacles
      type: VoxelLayer
-    - name: inflation_layer
+    - name: inflation
      type: InflationLayer

-  library_path: /home/duongtd/T800_ws/devel/lib/libplugins.so
-
  footprint:
    - [0.30, 0.30]
    - [0.30, -0.30]
    - [-0.30, -0.30]
    - [-0.30, 0.30]

-  transform_tolerance: 0.5
-  performance_metrics_enabled: false
+  transform_tolerance: 1.0
+  performance_metrics_enabled: true
  performance_metrics_period: 5.0
-  update_frequency: 5.0
-  width: 6.0
-  height: 6.0
+  update_frequency: 6.0
+  width: 8.0
+  height: 8.0
  resolution: 0.05
  origin_x: 0.0
  origin_y: 0.0
--- a/config/inflation_layer_params.yaml
+++ b/config/inflation_layer_params.yaml
@@ -1,5 +1,5 @@
 inflation_layer:
  enabled: true
  inflate_unknown: false
-  cost_scaling_factor: 3.0
-  inflation_radius: 0.45
+  cost_scaling_factor: 10.0
+  inflation_radius: 0.30
--- a/config/obstacle_layer_params.yaml
+++ b/config/obstacle_layer_params.yaml
@@ -1,10 +1,12 @@
 obstacle_layer:
+  enabled: true
  track_unknown_space: false
-  transform_tolerance: 0.5
+  transform_tolerance: 1.0
  footprint_clearing_enabled: true
-  max_obstacle_height: 1.5
  combination_method: 1

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

  pc_marking:
@@ -13,13 +15,13 @@ obstacle_layer:
    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.10
+    min_obstacle_height: 0.1
    max_obstacle_height: 1.0
-    frustum_clearing_enabled: false

  pc_clearing:
    topic: /camera/depth/data
@@ -27,13 +29,13 @@ obstacle_layer:
    clearing: false
    marking: true
    inf_is_valid: false
-    observation_persistence: 0.0
-    expected_update_rate: 0.5
-    obstacle_range: 2.5
-    raytrace_range: 3.5
-    min_obstacle_height: 0.10
-    max_obstacle_height: 1.0
    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/voxel_layer_params.yaml
+++ b/config/voxel_layer_params.yaml
@@ -1,13 +1,14 @@
 voxel_layer:
  enabled: true
  footprint_clearing_enabled: true
-  max_obstacle_height: 1.5
+  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
--- a/launch/depth_local_costmap_test.launch
+++ b/launch/depth_local_costmap_test.launch
@@ -13,8 +13,6 @@
        type="depth_local_costmap_noetic_test_node"
        name="depth_local_costmap_noetic_test"
        output="screen">
-    <env name="PNKX_NAV_CORE_CONFIG_DIR" value="$(find depth_local_costmap_noetic_test)"/>
-
    <param name="base_frame" value="$(arg base_frame)"/>
    <param name="publish_odom_tf" value="$(arg publish_odom_tf)"/>
    <param name="odom_topic" value="$(arg odom_topic)"/>
@@ -26,22 +24,36 @@

    <param name="local_costmap/robot_base_frame" value="$(arg base_frame)"/>
    <param name="local_costmap/global_frame" value="$(arg global_frame)"/>
-    <param name="local_costmap/voxel_layer/observation_sources" value="pc_marking pc_clearing"/>
+    <param name="local_costmap/obstacles/observation_sources" value="pc_marking pc_clearing"/>

-    <param name="local_costmap/voxel_layer/pc_marking/topic" value="$(arg depth_cloud_topic)"/>
-    <param name="local_costmap/voxel_layer/pc_marking/data_type" value="PointCloud2"/>
-    <param name="local_costmap/voxel_layer/pc_marking/clearing" value="false"/>
-    <param name="local_costmap/voxel_layer/pc_marking/marking" value="true"/>
-    <param name="local_costmap/voxel_layer/pc_marking/frustum_clearing_enabled" value="false"/>
+    <param name="local_costmap/obstacles/pc_marking/topic" value="$(arg depth_cloud_topic)"/>
+    <param name="local_costmap/obstacles/pc_marking/data_type" value="PointCloud2"/>
+    <param name="local_costmap/obstacles/pc_marking/clearing" value="false"/>
+    <param name="local_costmap/obstacles/pc_marking/marking" value="true"/>
+    <param name="local_costmap/obstacles/pc_marking/inf_is_valid" value="false"/>
+    <param name="local_costmap/obstacles/pc_marking/frustum_clearing_enabled" value="false"/>
+    <param name="local_costmap/obstacles/pc_marking/observation_persistence" value="0.0"/>
+    <param name="local_costmap/obstacles/pc_marking/expected_update_rate" value="0.5"/>
+    <param name="local_costmap/obstacles/pc_marking/obstacle_range" value="2.5"/>
+    <param name="local_costmap/obstacles/pc_marking/raytrace_range" value="3.0"/>
+    <param name="local_costmap/obstacles/pc_marking/min_obstacle_height" value="0.1"/>
+    <param name="local_costmap/obstacles/pc_marking/max_obstacle_height" value="1.0"/>

-    <param name="local_costmap/voxel_layer/pc_clearing/topic" value="$(arg depth_camera_data_topic)"/>
-    <param name="local_costmap/voxel_layer/pc_clearing/data_type" value="DepthCameraData"/>
-    <param name="local_costmap/voxel_layer/pc_clearing/clearing" value="false"/>
-    <param name="local_costmap/voxel_layer/pc_clearing/marking" value="true"/>
-    <param name="local_costmap/voxel_layer/pc_clearing/frustum_clearing_enabled" value="true"/>
-    <param name="local_costmap/voxel_layer/pc_clearing/frustum_clearing_pixel_step" value="8"/>
-    <param name="local_costmap/voxel_layer/pc_clearing/frustum_min_range" value="0.20"/>
-    <param name="local_costmap/voxel_layer/pc_clearing/frustum_max_range" value="3.5"/>
-    <param name="local_costmap/voxel_layer/frustum_depth_camera_topic" value="$(arg depth_camera_data_topic)"/>
+    <param name="local_costmap/obstacles/pc_clearing/topic" value="$(arg depth_camera_data_topic)"/>
+    <param name="local_costmap/obstacles/pc_clearing/data_type" value="DepthCameraData"/>
+    <param name="local_costmap/obstacles/pc_clearing/clearing" value="false"/>
+    <param name="local_costmap/obstacles/pc_clearing/marking" value="true"/>
+    <param name="local_costmap/obstacles/pc_clearing/inf_is_valid" value="false"/>
+    <param name="local_costmap/obstacles/pc_clearing/frustum_clearing_enabled" value="true"/>
+    <param name="local_costmap/obstacles/pc_clearing/frustum_clearing_pixel_step" value="8"/>
+    <param name="local_costmap/obstacles/pc_clearing/frustum_min_range" value="0.20"/>
+    <param name="local_costmap/obstacles/pc_clearing/frustum_max_range" value="3.5"/>
+    <param name="local_costmap/obstacles/pc_clearing/observation_persistence" value="0.0"/>
+    <param name="local_costmap/obstacles/pc_clearing/expected_update_rate" value="0.5"/>
+    <param name="local_costmap/obstacles/pc_clearing/obstacle_range" value="2.5"/>
+    <param name="local_costmap/obstacles/pc_clearing/raytrace_range" value="3.5"/>
+    <param name="local_costmap/obstacles/pc_clearing/min_obstacle_height" value="0.1"/>
+    <param name="local_costmap/obstacles/pc_clearing/max_obstacle_height" value="1.0"/>
+    <param name="local_costmap/obstacles/frustum_depth_camera_topic" value="$(arg depth_camera_data_topic)"/>
  </node>
 </launch>