diff --git a/launch/include/3d_nav_camera.launch b/launch/include/3d_nav_camera.launch
new file mode 100644
index 0000000000000000000000000000000000000000..7fdeac86f077a808963eabeac208348795320a44
--- /dev/null
+++ b/launch/include/3d_nav_camera.launch
@@ -0,0 +1,65 @@
+<?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>
+
diff --git a/launch/include/3d_nav_lidar.launch b/launch/include/3d_nav_lidar.launch
new file mode 100644
index 0000000000000000000000000000000000000000..837ea11194c876950cf0dfa2581b7e425a4a46cf
--- /dev/null
+++ b/launch/include/3d_nav_lidar.launch
@@ -0,0 +1,47 @@
+<?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>
+
diff --git a/params/costmap/3d_camera_layers.yaml b/params/costmap/3d_camera_layers.yaml
new file mode 100755
index 0000000000000000000000000000000000000000..7bcef370683c3d4c9e6f8e0811944e4fed78194e
--- /dev/null
+++ b/params/costmap/3d_camera_layers.yaml
@@ -0,0 +1,96 @@
+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
+ }
diff --git a/params/costmap/3d_lidar_layers.yaml b/params/costmap/3d_lidar_layers.yaml
new file mode 100755
index 0000000000000000000000000000000000000000..49d7c8164baa58efbb53db07cfae44c4c7571936
--- /dev/null
+++ b/params/costmap/3d_lidar_layers.yaml
@@ -0,0 +1,21 @@
+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
+ }
diff --git a/params/costmap/3d_local_params.yaml b/params/costmap/3d_local_params.yaml
new file mode 100755
index 0000000000000000000000000000000000000000..c1af5d7a2837179562a8904fa4c8bf70dbd665db
--- /dev/null
+++ b/params/costmap/3d_local_params.yaml
@@ -0,0 +1,20 @@
+#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"}
+
+
diff --git a/params/costmap/map/3d_global_params.yaml b/params/costmap/map/3d_global_params.yaml
new file mode 100644
index 0000000000000000000000000000000000000000..09fc474b71a4643b2f55a495eaafb61aac8b4b61
--- /dev/null
+++ b/params/costmap/map/3d_global_params.yaml
@@ -0,0 +1,24 @@
+#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
diff --git a/params/costmap/no_map/3d_global_params.yaml b/params/costmap/no_map/3d_global_params.yaml
new file mode 100644
index 0000000000000000000000000000000000000000..f278ca4772626a74ba82a7288019c190d9bbb078
--- /dev/null
+++ b/params/costmap/no_map/3d_global_params.yaml
@@ -0,0 +1,18 @@
+#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"}