Added launch and configuration files of rthe 3D navigation.

launch/include/3d_nav_camera.launch

+<?xml version="1.0"?>
+<!-- -->
+<launch>
+  
+  <arg name="ns" default="ana" />
+  <arg name="output" default="log"/>
+  <arg name="launch_prefix" default=""/>
+  <arg name="use_nodelets" default="true"/>
+
+  <arg name="obstacle_nodelet_manager" default="obstacle_nodelet_manager" />
+  <arg name="hole_nodelet_manager" default="hole_nodelet_manager" />
+
+  <arg name="camera_cloud_in" default="sensors/nav_cam/depth_registered/points"/>
+  <arg name="average_node_name" default="average_point_cloud"/>
+  <arg name="average_config_file" default="$(find iri_average_point_cloud)/config/default_config.yaml"/>
+
+  <arg name="normals_node_name" default="obstacle_detection_normals"/>
+  <arg name="normals_config_file" default="$(find iri_nav_obstacle_detection_normals)/config/default_config.yaml"/>
+
+  <arg name="near_hole_detection_node_name" default="near_hole_detection"/>
+  <arg name="near_hole_detection_config_file" default="$(find iri_point_cloud_hole_detection)/config/near_config.yaml"/>
+  <arg name="hole_detection_cloud_in" default="radius_outlier_removal/output"/>
+
+  <arg name="far_hole_detection_node_name" default="far_hole_detection"/>
+  <arg name="far_hole_detection_config_file" default="$(find iri_point_cloud_hole_detection)/config/far_config.yaml"/>
+
+  <group if="$(arg use_nodelets)">
+    <include file="$(find iri_3dnav)/launch/include/camera_nodelets.launch">
+      <arg name="ns" value="$(arg ns)"/>
+      <arg name="obstacle_nodelet_manager" value="$(arg obstacle_nodelet_manager)" />
+      <arg name="hole_nodelet_manager" value="$(arg hole_nodelet_manager)" />
+      <arg name="camera_cloud_in" value="$(arg camera_cloud_in)"/>
+      <arg name="average_node_name" value="$(arg average_node_name)"/>
+      <arg name="average_config_file" value="$(arg average_config_file)"/>
+      <arg name="normals_node_name" value="$(arg normals_node_name)"/>
+      <arg name="normals_config_file" value="$(arg normals_config_file)"/>
+      <arg name="near_hole_detection_node_name" value="$(arg near_hole_detection_node_name)"/>
+      <arg name="near_hole_detection_config_file" value="$(arg near_hole_detection_config_file)"/>
+      <arg name="hole_detection_cloud_in" value="$(arg hole_detection_cloud_in)"/>
+      <arg name="far_hole_detection_node_name" value="$(arg far_hole_detection_node_name)"/>
+      <arg name="far_hole_detection_config_file" value="$(arg far_hole_detection_config_file)"/>
+      <arg name="output" value="$(arg output)"/>
+      <arg name="launch_prefix" value="$(arg launch_prefix)"/>
+    </include>
+  </group>
+
+  <group unless="$(arg use_nodelets)">
+    <include file="$(find iri_3dnav)/launch/include/camera_nodelets.launch">
+      <arg name="ns" value="$(arg ns)"/>
+      <arg name="camera_cloud_in" value="$(arg camera_cloud_in)"/>
+      <arg name="average_node_name" value="$(arg average_node_name)"/>
+      <arg name="average_config_file" value="$(arg average_config_file)"/>
+      <arg name="normals_node_name" value="$(arg normals_node_name)"/>
+      <arg name="normals_config_file" value="$(arg normals_config_file)"/>
+      <arg name="near_hole_detection_node_name" value="$(arg near_hole_detection_node_name)"/>
+      <arg name="near_hole_detection_config_file" value="$(arg near_hole_detection_config_file)"/>
+      <arg name="hole_detection_cloud_in" value="$(arg hole_detection_cloud_in)"/>
+      <arg name="far_hole_detection_node_name" value="$(arg far_hole_detection_node_name)"/>
+      <arg name="far_hole_detection_config_file" value="$(arg far_hole_detection_config_file)"/>
+      <arg name="output" value="$(arg output)"/>
+      <arg name="launch_prefix" value="$(arg launch_prefix)"/>
+    </include>
+  </group>
+</launch>
+

launch/include/3d_nav_lidar.launch

+<?xml version="1.0"?>
+<!-- -->
+<launch>
+  
+  <arg name="ns" default="ana" />
+  <arg name="output" default="log"/>
+  <arg name="launch_prefix" default=""/>
+  <arg name="use_nodelets" default="true"/>
+
+  <arg name="lidar_nodelet_manager" default="lidar_nodelet_manager"/>
+
+  <arg name="lidar_filter_node_name" default="lidar_filter"/>
+  <arg name="lidar_filter_config_file" default="$(find iri_point_cloud_angle_filter)/config/default_config.yaml"/>
+  <arg name="lidar_filter_cloud_in" default="/ana/sensors/velodyne_points" />
+
+  <arg name="lidar_detector_node_name" default="lidar_detector"/>
+  <arg name="lidar_detector_config_file" default="$(find iri_lidar_obstacle_detector)/config/default_config.yaml"/>
+
+  <group if="$(arg use_nodelets)">
+    <include file="$(find iri_3dnav)/launch/include/lidar_nodelets.launch">
+      <arg name="ns" value="$(arg ns)"/>
+      <arg name="output" value="$(arg output)"/>
+      <arg name="launch_prefix" value="$(arg launch_prefix)"/>
+      <arg name="lidar_nodelet_manager" value="$(arg lidar_nodelet_manager)"/>
+      <arg name="lidar_filter_node_name" value="$(arg lidar_filter_node_name)"/>
+      <arg name="lidar_filter_config_file" value="$(arg lidar_filter_config_file)"/>
+      <arg name="lidar_filter_cloud_in" value="$(arg lidar_filter_cloud_in)" />
+      <arg name="lidar_detector_node_name" value="$(arg lidar_detector_node_name)"/>
+      <arg name="lidar_detector_config_file" value="$(arg lidar_detector_config_file)"/>
+    </include>
+  </group>
+
+  <group unless="$(arg use_nodelets)">
+    <include file="$(find iri_3dnav)/launch/include/lidar_nodes.launch">
+      <arg name="ns" value="$(arg ns)"/>
+      <arg name="output" value="$(arg output)"/>
+      <arg name="launch_prefix" value="$(arg launch_prefix)"/>
+      <arg name="lidar_filter_node_name" value="$(arg lidar_filter_node_name)"/>
+      <arg name="lidar_filter_config_file" value="$(arg lidar_filter_config_file)"/>
+      <arg name="lidar_filter_cloud_in" value="$(arg lidar_filter_cloud_in)" />
+      <arg name="lidar_detector_node_name" value="$(arg lidar_detector_node_name)"/>
+      <arg name="lidar_detector_config_file" value="$(arg lidar_detector_config_file)"/>
+    </include>
+  </group>
+
+</launch>
+

params/costmap/3d_camera_layers.yaml

+obstacle_layer_camera:
+  enabled: true
+  track_unknown_space: true
+  transform_tolerance: 0.2
+  max_obstacle_height: 100.0 #1.0
+  footprint_clearing_enabled: false
+  combination_method: 1
+  observation_sources:  camera_marking
+
+  camera_marking: {
+    sensor_frame: /robot/base_footprint,
+    data_type: PointCloud,
+    topic: /robot/point_cloud_obs,
+    observation_persistence: 0.0,
+    marking: true,
+    clearing: true,
+    min_obstacle_height: -100.0,
+    max_obstacle_height: 100.0,
+    expected_update_rate: 1,
+    obstacle_range: 5.0,
+    raytrace_range: 10.0
+  }
+
+hole_layer_near_camera:
+  enabled: true
+  track_unknown_space: true
+  transform_tolerance: 0.2
+  max_obstacle_height: 100.0 #1.0
+  footprint_clearing_enabled: false
+  combination_method: 1
+  observation_sources: hole_near_marking hole_near_clearing
+
+  hole_near_marking: {
+    sensor_frame: /robot/base_footprint,
+    data_type: PointCloud2,
+    topic: /robot/point_cloud_mark_near,
+    observation_persistence: 0.0,
+    marking: true,
+    clearing: false,
+    min_obstacle_height: -100.0,
+    max_obstacle_height: 100.0,
+    expected_update_rate: 1,
+    obstacle_range: 5.0,
+    raytrace_range: 10.0
+  }
+
+  hole_near_clearing: {
+    sensor_frame: /robot/base_footprint,
+    data_type: PointCloud2,
+    topic: /robot/point_cloud_clear_near,
+    observation_persistence: 0.0,
+    marking: false,
+    clearing: true,
+    min_obstacle_height: -100.0,
+    max_obstacle_height: 100.0,
+    expected_update_rate: 1,
+    obstacle_range: 5.0,
+    raytrace_range: 10.0
+  }
+
+hole_layer_far_camera:
+  enabled: true
+  track_unknown_space: true
+  transform_tolerance: 0.2
+  max_obstacle_height: 100.0 #1.0
+  footprint_clearing_enabled: false
+  combination_method: 1
+  observation_sources: hole_far_marking hole_far_clearing
+
+  hole_far_marking: {
+    sensor_frame: /robot/base_footprint,
+    data_type: PointCloud2,
+    topic: /robot/point_cloud_mark_far,
+    observation_persistence: 0.0,
+    marking: true,
+    clearing: false,
+    min_obstacle_height: -100.0,
+    max_obstacle_height: 100.0,
+    expected_update_rate: 1,
+    obstacle_range: 5.0,
+    raytrace_range: 10.0
+  }
+
+  hole_far_clearing: {
+    sensor_frame: /robot/base_footprint,
+    data_type: PointCloud2,
+    topic: /robot/point_cloud_clear_far,
+    observation_persistence: 0.0,
+    marking: false,
+    clearing: true,
+    min_obstacle_height: -100.0,
+    max_obstacle_height: 100.0,
+    expected_update_rate: 1,
+    obstacle_range: 5.0,
+    raytrace_range: 10.0
+  }

params/costmap/3d_lidar_layers.yaml

+obstacle_layer_lidar:
+  enabled: true
+  track_unknown_space: true
+  transform_tolerance: 0.2
+  max_obstacle_height: 10.0
+  footprint_clearing_enabled: false
+  combination_method: 1 
+  observation_sources:  lidar_marking
+  lidar_marking: {
+    sensor_frame: /robot/base_footprint,
+    data_type: LaserScan,
+    topic: /robot/scan,
+    observation_persistence: 0.0,
+    marking: true,
+    clearing: true,
+    min_obstacle_height: -10.0,
+    max_obstacle_height: 10.0,
+    expected_update_rate: 1,
+    obstacle_range: 5.0,
+    raytrace_range: 200.0
+  }

params/costmap/3d_local_params.yaml

+#local_costmap:  ###namespace added when loading parameters in move_base.launch
+global_frame: robot/odom
+update_frequency: 5.0
+publish_frequency: 2.0
+rolling_window: true
+width: 10.0
+height: 10.0
+resolution: 0.1
+#origin_x: 0.0
+#origin_y: 0.0
+
+#costmap plugins
+plugins:
+ - {name: obstacle_layer_lidar,   type: "costmap_2d::ObstacleLayer"}
+ - {name: obstacle_layer_camera,  type: "costmap_2d::ObstacleLayer"}
+ - {name: hole_layer_near_camera, type: "costmap_2d::ObstacleLayer"}
+ - {name: hole_layer_far_camera,  type: "costmap_2d::ObstacleLayer"}
+ - {name: inflation_layer,        type: "costmap_2d::InflationLayer"}
+
+

params/costmap/map/3d_global_params.yaml

+#global_costmap: ###namespace added when loading parameters in move_base.launch
+global_frame: map
+update_frequency: 1.0
+publish_frequency: 1.0
+rolling_window: false
+
+plugins:
+  - {name: static_layer,          type: "costmap_2d::StaticLayer"}
+  - {name: obstacle_layer_lidar,  type: "costmap_2d::ObstacleLayer"}
+  - {name: obstacle_layer_camera, type: "costmap_2d::ObstacleLayer"}
+  - {name: hole_layer_near_camera,type: "costmap_2d::ObstacleLayer"}
+  - {name: hole_layer_far_camera, type: "costmap_2d::ObstacleLayer"}
+  - {name: inflation_layer,       type: "costmap_2d::InflationLayer"}
+    
+static_layer:
+  enabled: true
+  map_topic: /map
+  first_map_only: false
+  subscribe_to_updates: false
+  unknown_cost_value: -1
+  use_maximum: false
+  lethal_cost_threshold: 100
+  track_unknown_space: true
+  trinary_costmap: true

params/costmap/no_map/3d_global_params.yaml

+#global_costmap: ###namespace added when loading parameters in move_base.launch
+global_frame: /robot/odom
+update_frequency: 1.0
+publish_frequency: 0.0
+rolling_window: true
+transform_tolerance: 1.0
+width: 20.0
+height: 20.0
+resolution: 0.2
+origin_x: 0.0
+origin_y: 0.0
+
+plugins:
+  - {name: obstacle_layer_lidar,   type: "costmap_2d::ObstacleLayer"}
+  - {name: obstacle_layer_camera,  type: "costmap_2d::ObstacleLayer"}
+  - {name: hole_layer_near_camera, type: "costmap_2d::ObstacleLayer"}
+  - {name: hole_layer_far_camera,  type: "costmap_2d::ObstacleLayer"}
+  - {name: inflation_layer,        type: "costmap_2d::InflationLayer"}