add new file nav_c_api2 and function

config/costmap_common_params.yaml

@@ -1,5 +1,7 @@
 robot_base_frame: base_link
 transform_tolerance: 1.0
+performance_metrics_enabled: true
+performance_metrics_period: 5.0
 obstacle_range: 3.0
 #mark_threshold: 1
 publish_voxel_map: true
@@ -19,7 +21,7 @@ virtual_walls_map:
   lethal_cost_threshold: 100
 
 obstacles:
-  observation_sources: b_scan_marking b_scan_clearing pc_marking pc_clearing
+  observation_sources: b_scan pc_marking pc_clearing pc_r_marking pc_r_clearing
   # f_scan_marking: f_scan_marking f_scan_clearing
   #   topic: /f_scan
   #   data_type: LaserScan
@@ -36,20 +38,13 @@ obstacles:
   #   inf_is_valid: true
   #   min_obstacle_height: 0.0
   #   max_obstacle_height: 0.25
-  b_scan_marking:
-    topic: /b_scan
-    data_type: LaserScan
-    clearing: false
-    marking: true
-    inf_is_valid: true
-    min_obstacle_height: 0.0
-    max_obstacle_height: 0.25
-  b_scan_clearing:
+  b_scan:
     topic: /b_scan
     data_type: LaserScan
     clearing: true
-    marking: false
+    marking: true
     inf_is_valid: true
+    frustum_clearing_enabled: false
     min_obstacle_height: 0.0
     max_obstacle_height: 0.25
 
@@ -59,6 +54,7 @@ obstacles:
     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
@@ -67,14 +63,38 @@ obstacles:
     max_obstacle_height: 1.0
 
   pc_clearing:
-    topic: /camera/depth/points_clear
+    topic: /camera/depth/data
+    data_type: DepthCameraData
+    clearing: false
+    marking: true
+    frustum_clearing_enabled: true
+    frustum_clearing_pixel_step: 8
+    frustum_min_range: 0.20
+    frustum_max_range: 3.5
+
+  pc_r_marking:
+    topic: /camera_right/depth/points_proc
     data_type: PointCloud2
-    clearing: true
-    marking: false
+    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.5
-    max_obstacle_height: 5.0
+    min_obstacle_height: 0.1
+    max_obstacle_height: 1.0
+
+  pc_r_clearing:
+    topic: /camera_right/depth/data
+    data_type: DepthCameraData
+    clearing: false
+    marking: true
+    frustum_clearing_enabled: true
+    frustum_clearing_pixel_step: 8
+    frustum_min_range: 0.20
+    frustum_max_range: 3.5
+
+  # Depth camera clearing is handled by VoxelLayer frustum clearing from
+  # /camera/depth/image_raw + /camera/depth/camera_info. /camera_right/depth/data