test costmap + cam depth

config/costmap_params.yaml

@@ -0,0 +1,31 @@
+robot_costmap_2d:
+  global_frame: odom
+  robot_base_frame: base_link
+  rolling_window: true
+  track_unknown_space: false
+
+  plugins:
+    - name: voxel_layer
+      type: VoxelLayer
+    - name: inflation_layer
+      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
+  performance_metrics_period: 5.0
+  update_frequency: 5.0
+  width: 6.0
+  height: 6.0
+  resolution: 0.05
+  origin_x: 0.0
+  origin_y: 0.0
+  footprint_padding: 0.0
+  robot_radius: 0.0

config/inflation_layer_params.yaml

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

config/obstacle_layer_params.yaml

@@ -0,0 +1,39 @@
+obstacle_layer:
+  track_unknown_space: false
+  transform_tolerance: 0.5
+  footprint_clearing_enabled: true
+  max_obstacle_height: 1.5
+  combination_method: 1
+
+  observation_sources: pc_marking pc_clearing
+
+  pc_marking:
+    topic: /camera/depth/points_proc
+    data_type: PointCloud2
+    clearing: false
+    marking: true
+    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.0
+    frustum_clearing_enabled: false
+
+  pc_clearing:
+    topic: /camera/depth/data
+    data_type: DepthCameraData
+    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

config/voxel_layer_params.yaml

@@ -0,0 +1,15 @@
+voxel_layer:
+  enabled: true
+  footprint_clearing_enabled: true
+  max_obstacle_height: 1.5
+  origin_z: 0.0
+  z_resolution: 0.15
+  z_voxels: 8
+  unknown_threshold: 15.0
+  mark_threshold: 0
+  combination_method: 1
+  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