test costmap + cam depth

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

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

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