test costmap + cam depth

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
+```bash
-<env name="PNKX_NAV_CORE_CONFIG_DIR" value="$(find depth_local_costmap_noetic_test)"/>
+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
 ```

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

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

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

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"

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

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
+  transform_tolerance: 1.0
-  performance_metrics_enabled: false
+  performance_metrics_enabled: true
   performance_metrics_period: 5.0
-  update_frequency: 5.0
+  update_frequency: 6.0
-  width: 6.0
+  width: 8.0
-  height: 6.0
+  height: 8.0
   resolution: 0.05
   origin_x: 0.0
   origin_y: 0.0

config/inflation_layer_params.yaml

@@ -1,5 +1,5 @@
 inflation_layer:
   enabled: true
   inflate_unknown: false
-  cost_scaling_factor: 3.0
+  cost_scaling_factor: 10.0
-  inflation_radius: 0.45
+  inflation_radius: 0.30

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

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

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/obstacles/pc_marking/topic" value="$(arg depth_cloud_topic)"/>
-    <param name="local_costmap/voxel_layer/pc_marking/data_type" value="PointCloud2"/>
+    <param name="local_costmap/obstacles/pc_marking/data_type" value="PointCloud2"/>
-    <param name="local_costmap/voxel_layer/pc_marking/clearing" value="false"/>
+    <param name="local_costmap/obstacles/pc_marking/clearing" value="false"/>
-    <param name="local_costmap/voxel_layer/pc_marking/marking" value="true"/>
+    <param name="local_costmap/obstacles/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/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/obstacles/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/obstacles/pc_clearing/data_type" value="DepthCameraData"/>
-    <param name="local_costmap/voxel_layer/pc_clearing/clearing" value="false"/>
+    <param name="local_costmap/obstacles/pc_clearing/clearing" value="false"/>
-    <param name="local_costmap/voxel_layer/pc_clearing/marking" value="true"/>
+    <param name="local_costmap/obstacles/pc_clearing/marking" value="true"/>
-    <param name="local_costmap/voxel_layer/pc_clearing/frustum_clearing_enabled" value="true"/>
+    <param name="local_costmap/obstacles/pc_clearing/inf_is_valid" value="false"/>
-    <param name="local_costmap/voxel_layer/pc_clearing/frustum_clearing_pixel_step" value="8"/>
+    <param name="local_costmap/obstacles/pc_clearing/frustum_clearing_enabled" value="true"/>
-    <param name="local_costmap/voxel_layer/pc_clearing/frustum_min_range" value="0.20"/>
+    <param name="local_costmap/obstacles/pc_clearing/frustum_clearing_pixel_step" value="8"/>
-    <param name="local_costmap/voxel_layer/pc_clearing/frustum_max_range" value="3.5"/>
+    <param name="local_costmap/obstacles/pc_clearing/frustum_min_range" value="0.20"/>
-    <param name="local_costmap/voxel_layer/frustum_depth_camera_topic" value="$(arg depth_camera_data_topic)"/>
+    <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>