diff --git a/config/default_outlier_removal_config.yaml b/config/default_outlier_removal_config.yaml
new file mode 100644
index 0000000000000000000000000000000000000000..2ff09bbb0314621e5ba3ce0788d65a350e09e390
--- /dev/null
+++ b/config/default_outlier_removal_config.yaml
@@ -0,0 +1,4 @@
+mean_k: 20
+stddev: 2.0
+negative: False
+
diff --git a/config/default_passthrough_config.yaml b/config/default_passthrough_config.yaml
new file mode 100644
index 0000000000000000000000000000000000000000..a81499adbfd4665627117336afef6bba46c13a5c
--- /dev/null
+++ b/config/default_passthrough_config.yaml
@@ -0,0 +1,4 @@
+filter_field_name: x
+filter_limit_min: 1.1
+filter_limit_max: 2.2
+filter_limit_negative: False
diff --git a/config/default_radius_outlier_removal_config.yaml b/config/default_radius_outlier_removal_config.yaml
new file mode 100644
index 0000000000000000000000000000000000000000..23952938277fae7cc9492e32647da98c9ecba5d4
--- /dev/null
+++ b/config/default_radius_outlier_removal_config.yaml
@@ -0,0 +1,3 @@
+radius_search: 0.1
+min_neighbors: 15
+
diff --git a/config/default_voxels_config.yaml b/config/default_voxels_config.yaml
new file mode 100644
index 0000000000000000000000000000000000000000..0c335082b5364ba9d4b67ea7ace488b13481d116
--- /dev/null
+++ b/config/default_voxels_config.yaml
@@ -0,0 +1,5 @@
+leaf_size: 0.03
+filter_field_name: z
+filter_limit_min: -0.5
+filter_limit_max: 3.0
+filter_limit_negative: False
diff --git a/launch/include/3d_nav_camera.launch b/launch/include/3d_nav_camera.launch
index 6e6b6dd6912a8eeeb09766c0705f96861d31423c..b117dd54a605e1435886fc92102337179bc996ad 100644
--- a/launch/include/3d_nav_camera.launch
+++ b/launch/include/3d_nav_camera.launch
@@ -2,63 +2,73 @@
<!-- -->
<launch>
- <arg name="ns" default="ana" />
- <arg name="output" default="log"/>
- <arg name="launch_prefix" default=""/>
- <arg name="use_nodelets" default="true"/>
+ <arg name="ns" default="ana" />
+ <arg name="camera_name" default="camera" />
+ <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="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="camera_cloud_in" default="sensors/nav_cam/depth_registered/points"/>
- <arg name="normals_node_name" default="obstacle_detection_normals"/>
- <arg name="normals_config_file" default="$(find iri_obstacle_detection_normals)/config/default_config.yaml"/>
+ <arg name="average_config_file" default="$(find iri_average_point_cloud)/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="voxels_config_file" default="$(find iri_rosnav)/config/default_voxels_config.yaml"/>
- <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"/>
+ <arg name="passthrough_config_file" default="$(find iri_rosnav)/config/default_passthrough_config.yaml"/>
+
+ <arg name="outlier_removal_config_file" default="$(find iri_rosnav)/config/default_outlier_removal_config.yaml"/>
+
+ <arg name="radius_outlier_removal_config_file" default="$(find iri_rosnav)/config/default_radius_outlier_removal_config.yaml"/>
+
+ <arg name="normals_config_file" default="$(find iri_obstacle_detection_normals)/config/default_config.yaml"/>
+
+ <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_config_file" default="$(find iri_point_cloud_hole_detection)/config/far_config.yaml"/>
+
+ <arg name="base_frame_id" default="$(arg ns)/base_footprint"/>
<group if="$(arg use_nodelets)">
<include file="$(find iri_rosnav)/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)"/>
+ <arg name="ns" value="$(arg ns)"/>
+ <arg name="camera_name" value="$(arg camera_name)"/>
+ <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_config_file" value="$(arg average_config_file)"/>
+ <arg name="voxels_config_file" value="$(arg voxels_config_file)"/>
+ <arg name="passthrough_config_file" value="$(arg passthrough_config_file)"/>
+ <arg name="outlier_removal_config_file" value="$(arg outlier_removal_config_file)"/>
+ <arg name="radius_outlier_removal_config_file" value="$(arg radius_outlier_removal_config_file)"/>
+ <arg name="normals_config_file" value="$(arg normals_config_file)"/>
+ <arg name="near_hole_detection_config_file" value="$(arg near_hole_detection_config_file)"/>
+ <arg name="far_hole_detection_config_file" value="$(arg far_hole_detection_config_file)"/>
+ <arg name="base_frame_id" value="$(arg base_frame_id)"/>
+ <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_rosnav)/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 file="$(find iri_rosnav)/launch/include/camera_nodes.launch">
+ <arg name="ns" value="$(arg ns)"/>
+ <arg name="camera_name" value="$(arg camera_name)"/>
+ <arg name="camera_cloud_in" value="$(arg camera_cloud_in)"/>
+ <arg name="average_config_file" value="$(arg average_config_file)"/>
+ <arg name="voxels_config_file" value="$(arg voxels_config_file)"/>
+ <arg name="passthrough_config_file" value="$(arg passthrough_config_file)"/>
+ <arg name="outlier_removal_config_file" value="$(arg outlier_removal_config_file)"/>
+ <arg name="radius_outlier_removal_config_file" value="$(arg radius_outlier_removal_config_file)"/>
+ <arg name="normals_config_file" value="$(arg normals_config_file)"/>
+ <arg name="near_hole_detection_config_file" value="$(arg near_hole_detection_config_file)"/>
+ <arg name="far_hole_detection_config_file" value="$(arg far_hole_detection_config_file)"/>
+ <arg name="base_frame_id" value="$(arg base_frame_id)"/>
+ <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
index 742cb510d5ff10b78bf8fe44fa08b9985eca95d7..40fcabb2b1eea182705522da5e5ba6ab7cca56a9 100644
--- a/launch/include/3d_nav_lidar.launch
+++ b/launch/include/3d_nav_lidar.launch
@@ -2,43 +2,40 @@
<!-- -->
<launch>
- <arg name="ns" default="ana" />
- <arg name="output" default="log"/>
- <arg name="launch_prefix" default=""/>
- <arg name="use_nodelets" default="true"/>
+ <arg name="ns" default="ana" />
+ <arg name="lidar_name" default="lidar" />
+ <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="/$(arg ns)/sensors/velodyne_points" />
+ <arg name="lidar_filter_cloud_in" default="/$(arg ns)/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_rosnav)/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="ns" value="$(arg ns)"/>
+ <arg name="lidar_name" value="$(arg lidar_name)"/>
+ <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_config_file" value="$(arg lidar_filter_config_file)"/>
+ <arg name="lidar_filter_cloud_in" value="$(arg lidar_filter_cloud_in)" />
<arg name="lidar_detector_config_file" value="$(arg lidar_detector_config_file)"/>
</include>
</group>
<group unless="$(arg use_nodelets)">
<include file="$(find iri_rosnav)/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="ns" value="$(arg ns)"/>
+ <arg name="lidar_name" value="$(arg lidar_name)"/>
+ <arg name="output" value="$(arg output)"/>
+ <arg name="launch_prefix" value="$(arg launch_prefix)"/>
+ <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_config_file" value="$(arg lidar_detector_config_file)"/>
</include>
</group>
diff --git a/launch/include/camera_nodelets.launch b/launch/include/camera_nodelets.launch
index ab5c44eac1399d0a81c76f9fa7660cd49c109b83..e44198452b9fe6210e5e4308d681ffa7e40083f3 100644
--- a/launch/include/camera_nodelets.launch
+++ b/launch/include/camera_nodelets.launch
@@ -2,30 +2,38 @@
<launch>
- <arg name="ns" default="ana"/>
+ <arg name="ns" default="robot"/>
+ <arg name="camera_name" default="camera"/>
+ <arg name="ns2" default="nav3d/$(arg camera_name)"/>
+ <arg name="ns_nav3d" default="$(arg ns)/$(arg ns2)"/>
- <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="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="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_obstacle_detection_normals)/config/default_config.yaml"/>
+ <arg name="voxels_config_file" default="$(find iri_rosnav)/config/default_voxels_config.yaml"/>
+
+ <arg name="passthrough_config_file" default="$(find iri_rosnav)/config/default_passthrough_config.yaml"/>
+
+ <arg name="outlier_removal_config_file" default="$(find iri_rosnav)/config/default_outlier_removal_config.yaml"/>
+
+ <arg name="radius_outlier_removal_config_file" default="$(find iri_rosnav)/config/default_radius_outlier_removal_config.yaml"/>
+
+ <arg name="normals_config_file" default="$(find iri_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"/>
+ <arg name="far_hole_detection_config_file" default="$(find iri_point_cloud_hole_detection)/config/far_config.yaml"/>
+
+ <arg name="base_frame_id" default="$(arg ns)/base_footprint"/>
- <arg name="output" default="screen"/>
- <arg name="launch_prefix" default=""/>
+ <arg name="output" default="screen"/>
+ <arg name="launch_prefix" default=""/>
<!-- ******************* OBSTACLE DETECTION ********************************** -->
- <group ns="$(arg ns)">
+ <group ns="$(arg ns_nav3d)">
<!-- Obstacle nodelets manager -->
<node name="$(arg obstacle_nodelet_manager)"
@@ -36,98 +44,87 @@
<node pkg="nodelet"
type="nodelet"
- name="pc_throttle" args="load iri_average_point_cloud/NodeletThrottlePC $(arg obstacle_nodelet_manager)" output="$(arg output)">
- <remap from="topic_in" to="$(arg camera_cloud_in)"/>
- <remap from="topic_out" to="$(arg camera_cloud_in)_throttle"/>
+ name="$(arg camera_name)_throttle" args="load iri_average_point_cloud/NodeletThrottlePC $(arg obstacle_nodelet_manager)" output="$(arg output)">
+ <remap from="topic_in" to="/$(arg camera_cloud_in)"/>
+ <remap from="topic_out" to="/$(arg ns_nav3d)/throttle"/>
<param name="update_rate" value="15"/>
</node>
</group>
<include file="$(find iri_average_point_cloud)/launch/nodelet.launch">
- <arg name="ns" value="$(arg ns)"/>
- <arg name="node_name" value="$(arg average_node_name)"/>
+ <arg name="ns" value="$(arg ns_nav3d)"/>
+ <arg name="node_name" value="$(arg camera_name)_average"/>
<arg name="camera_nodelet_manager" value="$(arg obstacle_nodelet_manager)"/>
- <arg name="config_file" value="$(arg average_config_file)"/>
- <arg name="output" value="$(arg output)"/>
- <arg name="launch_prefix" value="$(arg launch_prefix)"/>
- <arg name="cloud_in" value="$(arg camera_cloud_in)_throttle"/>
+ <arg name="config_file" value="$(arg average_config_file)"/>
+ <arg name="cloud_in" value="/$(arg ns_nav3d)/throttle"/>
+ <arg name="cloud_out" value="/$(arg ns_nav3d)/average"/>
+ <arg name="output" value="$(arg output)"/>
+ <arg name="launch_prefix" value="$(arg launch_prefix)"/>
</include>
- <group ns="$(arg ns)">
+ <group ns="$(arg ns_nav3d)">
<!-- Downsampling points -->
- <node name="voxel_grid_obs"
+ <node name="$(arg camera_name)_voxel_grid_obs"
pkg="nodelet"
type="nodelet"
args="load pcl/VoxelGrid $(arg obstacle_nodelet_manager)"
output="$(arg output)">
- <remap from="~input" to="average_point_cloud/output" />
- <param name="input_frame" value="$(arg ns)/base_footprint"/>
- <param name="output_frame" value="$(arg ns)/base_footprint"/>
- <rosparam>
- leaf_size: 0.03
- filter_field_name: z
- filter_limit_min: -0.5
- filter_limit_max: 5.0
- filter_limit_negative: False
- </rosparam>
+ <remap from="~input" to="/$(arg ns_nav3d)/average" />
+ <remap from="~output" to="/$(arg ns_nav3d)/voxels" />
+ <param name="input_frame" value="$(arg base_frame_id)"/>
+ <param name="output_frame" value="$(arg base_frame_id)"/>
+ <rosparam file="$(arg voxels_config_file)" command="load"/>
</node>
<!-- Run a passthrough filter -->
- <node name="passthrough"
+ <node name="$(arg camera_name)_passthrough"
pkg="nodelet"
type="nodelet"
args="load pcl/PassThrough $(arg obstacle_nodelet_manager)"
output="$(arg output)">
- <remap from="~input" to="voxel_grid_obs/output" />
- <rosparam>
- filter_field_name: x
- filter_limit_min: 0.01
- filter_limit_max: 1.5
- filter_limit_negative: False
- </rosparam>
+ <remap from="~input" to="/$(arg ns_nav3d)/voxels" />
+ <remap from="~output" to="/$(arg ns_nav3d)/passthrough" />
+ <rosparam file="$(arg passthrough_config_file)" command="load"/>
</node>
<!-- Statistical outlier removal -->
- <node name="outlier_removal"
+ <node name="$(arg camera_name)_outlier_removal"
pkg="nodelet"
type="nodelet"
args="load pcl/StatisticalOutlierRemoval $(arg obstacle_nodelet_manager)"
output="$(arg output)" >
- <remap from="~input" to="passthrough/output" />
- <rosparam>
- mean_k: 20
- stddev: 2.0
- negative: False
- </rosparam>
+ <remap from="~input" to="/$(arg ns_nav3d)/passthrough" />
+ <remap from="~output" to="/$(arg ns_nav3d)/outlier" />
+ <rosparam file="$(arg outlier_removal_config_file)" command="load"/>
</node>
<!-- Radius outlier removal -->
- <node name="radius_outlier_removal"
+ <node name="$(arg camera_name)_radius_outlier_removal"
pkg="nodelet"
type="nodelet"
args="load pcl/RadiusOutlierRemoval $(arg obstacle_nodelet_manager)"
output="$(arg output)" >
- <remap from="~input" to="outlier_removal/output" />
- <rosparam>
- radius_search: 0.1
- min_neighbors: 15
- </rosparam>
+ <remap from="~input" to="/$(arg ns_nav3d)/outlier" />
+ <remap from="~output" to="/$(arg ns_nav3d)/radius_outlier" />
+ <rosparam file="$(arg radius_outlier_removal_config_file)" command="load"/>
</node>
</group>
<include file="$(find iri_obstacle_detection_normals)/launch/nodelet.launch">
- <arg name="ns" value="$(arg ns)"/>
- <arg name="node_name" value="$(arg normals_node_name)"/>
+ <arg name="ns" value="$(arg ns_nav3d)"/>
+ <arg name="node_name" value="$(arg camera_name)_normals"/>
<arg name="camera_nodelet_manager" value="$(arg obstacle_nodelet_manager)"/>
- <arg name="config_file" value="$(arg normals_config_file)"/>
- <arg name="output" value="$(arg output)"/>
- <arg name="launch_prefix" value="$(arg launch_prefix)"/>
- <arg name="cloud_in" value="radius_outlier_removal/output"/>
+ <arg name="config_file" value="$(arg normals_config_file)"/>
+ <arg name="output" value="$(arg output)"/>
+ <arg name="launch_prefix" value="$(arg launch_prefix)"/>
+ <arg name="cloud_in" value="/$(arg ns_nav3d)/radius_outlier"/>
+ <arg name="obstacles_cloud_out" value="/$(arg ns_nav3d)/normals_obs"/>
+ <arg name="free_space_cloud_out" value="/$(arg ns_nav3d)/normals_free"/>
</include>
<!-- **************************** HOLE DETECTION ******************************* -->
- <group ns="$(arg ns)">
+ <group ns="$(arg ns_nav3d)">
<node name="$(arg hole_nodelet_manager)"
pkg="nodelet"
type="nodelet"
@@ -136,23 +133,27 @@
</group>
<include file="$(find iri_point_cloud_hole_detection)/launch/nodelet.launch">
- <arg name="ns" value="$(arg ns)"/>
- <arg name="node_name" value="$(arg near_hole_detection_node_name)"/>
+ <arg name="ns" value="$(arg ns_nav3d)"/>
+ <arg name="node_name" value="$(arg camera_name)_hear_hole_detection"/>
<arg name="camera_nodelet_manager" value="$(arg hole_nodelet_manager)"/>
- <arg name="config_file" value="$(arg near_hole_detection_config_file)"/>
- <arg name="output" value="$(arg output)"/>
- <arg name="launch_prefix" default="$(arg launch_prefix)"/>
- <arg name="cloud_in" default="$(arg hole_detection_cloud_in)"/>
+ <arg name="config_file" value="$(arg near_hole_detection_config_file)"/>
+ <arg name="cloud_in" value="/$(arg ns_nav3d)/radius_outlier"/>
+ <arg name="hole_zone_cloud_out" value="/$(arg ns_nav3d)/near_hole_zone"/>
+ <arg name="hole_obs_cloud_out" value="/$(arg ns_nav3d)/near_hole_obs"/>
+ <arg name="output" value="$(arg output)"/>
+ <arg name="launch_prefix" value="$(arg launch_prefix)"/>
</include>
<include file="$(find iri_point_cloud_hole_detection)/launch/nodelet.launch">
- <arg name="ns" value="$(arg ns)"/>
- <arg name="node_name" value="$(arg far_hole_detection_node_name)"/>
+ <arg name="ns" value="$(arg ns_nav3d)"/>
+ <arg name="node_name" value="$(arg camera_name)_far_hole_detection"/>
<arg name="camera_nodelet_manager" value="$(arg hole_nodelet_manager)"/>
- <arg name="config_file" value="$(arg far_hole_detection_config_file)"/>
- <arg name="output" value="$(arg output)"/>
- <arg name="launch_prefix" default="$(arg launch_prefix)"/>
- <arg name="cloud_in" default="$(arg hole_detection_cloud_in)"/>
+ <arg name="config_file" value="$(arg far_hole_detection_config_file)"/>
+ <arg name="cloud_in" value="/$(arg ns_nav3d)/radius_outlier"/>
+ <arg name="hole_zone_cloud_out" value="/$(arg ns_nav3d)/far_hole_zone"/>
+ <arg name="hole_obs_cloud_out" value="/$(arg ns_nav3d)/far_hole_obs"/>
+ <arg name="output" value="$(arg output)"/>
+ <arg name="launch_prefix" value="$(arg launch_prefix)"/>
</include>
</launch>
diff --git a/launch/include/camera_nodes.launch b/launch/include/camera_nodes.launch
index e7b56f821695272e131d03b5a6f6eaf979f360e9..09c20e41afcb733e35f411e055063ebf10c5187d 100644
--- a/launch/include/camera_nodes.launch
+++ b/launch/include/camera_nodes.launch
@@ -2,49 +2,56 @@
<launch>
- <arg name="ns" default="ana"/>
+ <arg name="ns" default="ana"/>
+ <arg name="camera_name" default="camera"/>
+ <arg name="ns2" default="nav3d/$(arg camera_name)"/>
+ <arg name="ns_nav3d" default="$(arg ns)/$(arg ns2)"/>
+
+ <arg name="obstacle_nodelet_manager" default="obstacle_nodelet_manager" />
- <arg name="obstacle_nodelet_manager" default="obstacle_nodelet_manager" />
+ <arg name="camera_cloud_in" default="sensors/nav_cam/depth_registered/points"/>
- <arg name="camera_cloud_in" default="sensors/nav_cam/depth_registered/points"/>
+ <arg name="average_config_file" default="$(find iri_average_point_cloud)/config/default_config.yaml"/>
+
+ <arg name="voxels_config_file" default="$(find iri_rosnav)/config/default_voxels_config.yaml"/>
- <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="average_cloud_in" default="sensors/nav_cam/depth_registered/points_throttle"/>
+ <arg name="passthrough_config_file" default="$(find iri_rosnav)/config/default_passthrough_config.yaml"/>
- <arg name="normals_node_name" default="obstacle_detection_normals"/>
- <arg name="normals_config_file" default="$(find iri_obstacle_detection_normals)/config/default_config.yaml"/>
- <arg name="normals_cloud_in" default="radius_outlier_removal/output"/>
+ <arg name="outlier_removal_config_file" default="$(find iri_rosnav)/config/default_outlier_removal_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="radius_outlier_removal_config_file" default="$(find iri_rosnav)/config/default_radius_outlier_removal_config.yaml"/>
- <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"/>
+ <arg name="normals_config_file" default="$(find iri_obstacle_detection_normals)/config/default_config.yaml"/>
- <arg name="output" default="screen"/>
- <arg name="launch_prefix" default=""/>
+ <arg name="near_hole_detection_config_file" default="$(find iri_point_cloud_hole_detection)/config/near_config.yaml"/>
+
+ <arg name="far_hole_detection_config_file" default="$(find iri_point_cloud_hole_detection)/config/far_config.yaml"/>
+
+ <arg name="base_frame_id" default="$(arg ns)/base_footprint"/>
+
+ <arg name="output" default="screen"/>
+ <arg name="launch_prefix" default=""/>
<!-- ******************* OBSTACLE DETECTION ********************************** -->
- <group ns="$(arg ns)">
- <node name="pc_throttler"
+ <group ns="$(arg ns_nav3d)">
+ <node name="$(arg camera_name)_throttle"
type="throttle"
pkg="topic_tools"
- args="messages $(arg camera_cloud_in) 15 $(arg camera_cloud_in)_throttle" />
+ args="messages $(arg camera_cloud_in) 15 /$(arg ns_nav3d)/throttle" />
</group>
<include file="$(find iri_average_point_cloud)/launch/node.launch">
- <arg name="ns" value="$(arg ns)"/>
- <arg name="node_name" value="$(arg average_node_name)"/>
- <arg name="config_file" value="$(arg average_config_file)"/>
- <arg name="output" value="$(arg output)"/>
- <arg name="launch_prefix" default="$(arg launch_prefix)"/>
- <arg name="cloud_in" default="$(arg average_cloud_in)"/>
+ <arg name="ns" value="$(arg ns_nav3d)"/>
+ <arg name="node_name" value="$(arg camera_name)_average"/>
+ <arg name="config_file" value="$(arg average_config_file)"/>
+ <arg name="cloud_in" value="/$(arg ns_nav3d)/throttle"/>
+ <arg name="cloud_out" value="/$(arg ns_nav3d)/average"/>
+ <arg name="output" value="$(arg output)"/>
+ <arg name="launch_prefix" value="$(arg launch_prefix)"/>
</include>
- <group ns="$(arg ns)">
+ <group ns="$(arg ns_nav3d)">
<node name="$(arg obstacle_nodelet_manager)"
pkg="nodelet"
@@ -53,93 +60,85 @@
output="screen"/>
<!-- Downsampling points -->
- <node name="voxel_grid_obs"
+ <node name="$(arg camera_name)_voxel_grid_obs"
pkg="nodelet"
type="nodelet"
args="load pcl/VoxelGrid $(arg obstacle_nodelet_manager)"
output="$(arg output)">
- <remap from="~input" to="average_point_cloud/output" />
- <rosparam>
- input_frame: 'ana/base_footprint'
- output_frame: 'ana/base_footprint'
- leaf_size: 0.03
- filter_field_name: z
- filter_limit_min: -0.5
- filter_limit_max: 5.0
- filter_limit_negative: False
- </rosparam>
+ <remap from="~input" to="/$(arg ns_nav3d)/average" />
+ <remap from="~output" to="/$(arg ns_nav3d)/voxels" />
+ <param name="input_frame" value="$(arg base_frame_id)"/>
+ <param name="output_frame" value="$(arg base_frame_id)"/>
+ <rosparam file="$(arg voxels_config_file)" command="load"/>
</node>
<!-- Run a passthrough filter -->
- <node name="passthrough"
+ <node name="$(arg camera_name)_passthrough"
pkg="nodelet"
type="nodelet"
args="load pcl/PassThrough $(arg obstacle_nodelet_manager)"
output="$(arg output)">
- <remap from="~input" to="voxel_grid_obs/output" />
- <rosparam>
- filter_field_name: x
- filter_limit_min: 0.01
- filter_limit_max: 1.5
- filter_limit_negative: False
- </rosparam>
+ <remap from="~input" to="/$(arg ns_nav3d)/voxels" />
+ <remap from="~output" to="/$(arg ns_nav3d)/passthrough" />
+ <rosparam file="$(arg passthrough_config_file)" command="load"/>
</node>
<!-- Statistical outlier removal -->
- <node name="outlier_removal"
+ <node name="$(arg camera_name)_outlier_removal"
pkg="nodelet"
type="nodelet"
args="load pcl/StatisticalOutlierRemoval $(arg obstacle_nodelet_manager)"
output="$(arg output)" >
- <remap from="~input" to="passthrough/output" />
- <rosparam>
- mean_k: 20
- stddev: 2.0
- negative: False
- </rosparam>
+ <remap from="~input" to="/$(arg ns_nav3d)/passthrough" />
+ <remap from="~output" to="/$(arg ns_nav3d)/outlier" />
+ <rosparam file="$(arg outlier_removal_config_file)" command="load"/>
</node>
<!-- Radius outlier removal -->
- <node name="radius_outlier_removal"
+ <node name="$(arg camera_name)_radius_outlier_removal"
pkg="nodelet"
type="nodelet"
args="load pcl/RadiusOutlierRemoval $(arg obstacle_nodelet_manager)"
output="$(arg output)" >
- <remap from="~input" to="outlier_removal/output" />
- <rosparam>
- radius_search: 0.1
- min_neighbors: 15
- </rosparam>
+ <remap from="~input" to="/$(arg ns_nav3d)/outlier" />
+ <remap from="~output" to="/$(arg ns_nav3d)/radius_outlier" />
+ <rosparam file="$(arg radius_outlier_removal_config_file)" command="load"/>
</node>
</group>
<include file="$(find iri_obstacle_detection_normals)/launch/node.launch">
- <arg name="ns" value="$(arg ns)"/>
- <arg name="node_name" value="$(arg normals_node_name)"/>
- <arg name="config_file" value="$(arg normals_config_file)"/>
- <arg name="output" value="$(arg output)"/>
- <arg name="launch_prefix" default="$(arg launch_prefix)"/>
- <arg name="cloud_in" default="$(arg normals_cloud_in)"/>
+ <arg name="ns" value="$(arg ns_nav3d)"/>
+ <arg name="node_name" value="$(arg camera_name)_normals"/>
+ <arg name="config_file" value="$(arg normals_config_file)"/>
+ <arg name="output" value="$(arg output)"/>
+ <arg name="launch_prefix" value="$(arg launch_prefix)"/>
+ <arg name="cloud_in" value="/$(arg ns_nav3d)/radius_outlier"/>
+ <arg name="obstacles_cloud_out" value="/$(arg ns_nav3d)/normals_obs"/>
+ <arg name="free_space_cloud_out" value="/$(arg ns_nav3d)/normals_free"/>
</include>
<!-- **************************** HOLE DETECTION ******************************* -->
<include file="$(find iri_point_cloud_hole_detection)/launch/node.launch">
- <arg name="ns" value="$(arg ns)"/>
- <arg name="node_name" value="$(arg near_hole_detection_node_name)"/>
- <arg name="config_file" value="$(arg near_hole_detection_config_file)"/>
- <arg name="output" value="$(arg output)"/>
- <arg name="launch_prefix" default="$(arg launch_prefix)"/>
- <arg name="cloud_in" default="$(arg hole_detection_cloud_in)"/>
+ <arg name="ns" value="$(arg ns_nav3d)"/>
+ <arg name="node_name" value="$(arg camera_name)_near_hole_detection"/>
+ <arg name="config_file" value="$(arg near_hole_detection_config_file)"/>
+ <arg name="cloud_in" value="/$(arg ns_nav3d)/radius_outlier"/>
+ <arg name="hole_zone_cloud_out" value="/$(arg ns_nav3d)/near_hole_zone"/>
+ <arg name="hole_obs_cloud_out" value="/$(arg ns_nav3d)/near_hole_obs"/>
+ <arg name="output" value="$(arg output)"/>
+ <arg name="launch_prefix" value="$(arg launch_prefix)"/>
</include>
<include file="$(find iri_point_cloud_hole_detection)/launch/node.launch">
- <arg name="ns" value="$(arg ns)"/>
- <arg name="node_name" value="$(arg far_hole_detection_node_name)"/>
- <arg name="config_file" value="$(arg far_hole_detection_config_file)"/>
- <arg name="output" value="$(arg output)"/>
- <arg name="launch_prefix" default="$(arg launch_prefix)"/>
- <arg name="cloud_in" default="$(arg hole_detection_cloud_in)"/>
+ <arg name="ns" value="$(arg ns_nav3d)"/>
+ <arg name="node_name" value="$(arg camera_name)_far_hole_detection"/>
+ <arg name="config_file" value="$(arg far_hole_detection_config_file)"/>
+ <arg name="cloud_in" value="/$(arg ns_nav3d)/radius_outlier"/>
+ <arg name="hole_zone_cloud_out" value="/$(arg ns_nav3d)/far_hole_zone"/>
+ <arg name="hole_obs_cloud_out" value="/$(arg ns_nav3d)/far_hole_obs"/>
+ <arg name="output" value="$(arg output)"/>
+ <arg name="launch_prefix" value="$(arg launch_prefix)"/>
</include>
</launch>
diff --git a/launch/include/lidar_nodelets.launch b/launch/include/lidar_nodelets.launch
index a7de794d62709b4fd05856394dd1d88312d4f859..f6abd2514bad8b80d432c340a9c01fadf1e68189 100644
--- a/launch/include/lidar_nodelets.launch
+++ b/launch/include/lidar_nodelets.launch
@@ -2,21 +2,23 @@
<!-- -->
<launch>
- <arg name="ns" default="ana" />
- <arg name="output" default="log"/>
- <arg name="launch_prefix" default=""/>
+ <arg name="ns" default="ana" />
+ <arg name="lidar_name" default="lidar"/>
+ <arg name="ns2" default="nav3d/$(arg lidar_name)"/>
+ <arg name="ns_nav3d" default="$(arg ns)/$(arg ns2)"/>
+
+ <arg name="output" default="log"/>
+ <arg name="launch_prefix" default=""/>
- <arg name="lidar_nodelet_manager" default="lidar_nodelet_manager"/>
+ <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="/pointcloud_in" />
+ <arg name="lidar_filter_config_file" default="$(find iri_point_cloud_angle_filter)/config/default_config.yaml"/>
+ <arg name="lidar_filter_cloud_in" default="/pointcloud_in" />
- <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"/>
<!-- Obstacle nodelets manager -->
- <group ns="$(arg ns)">
+ <group ns="$(arg ns_nav3d)">
<node name="$(arg lidar_nodelet_manager)"
pkg="nodelet"
type="nodelet"
@@ -25,24 +27,28 @@
</group>
<include file="$(find iri_point_cloud_angle_filter)/launch/nodelet.launch">
- <arg name="ns" value="$(arg ns)"/>
- <arg name="node_name" value="$(arg lidar_filter_node_name)"/>
+ <arg name="ns" value="$(arg ns_nav3d)"/>
+ <arg name="node_name" value="$(arg lidar_name)_angle_filter"/>
<arg name="lidar_nodelet_manager" value="$(arg lidar_nodelet_manager)"/>
- <arg name="config_file" value="$(arg lidar_filter_config_file)"/>
- <arg name="output" value="$(arg output)"/>
- <arg name="launch_prefix" value="$(arg launch_prefix)"/>
- <arg name="cloud_in" value="$(arg lidar_filter_cloud_in)" />
+ <arg name="config_file" value="$(arg lidar_filter_config_file)"/>
+ <arg name="output" value="$(arg output)"/>
+ <arg name="launch_prefix" value="$(arg launch_prefix)"/>
+ <arg name="cloud_in" value="/$(arg lidar_filter_cloud_in)" />
+ <arg name="cloud_out" value="/$(arg ns_nav3d)/angle_filtered" />
</include>
<include file="$(find iri_lidar_obstacle_detector)/launch/nodelet.launch">
- <arg name="ns" value="$(arg ns)"/>
- <arg name="node_name" value="$(arg lidar_detector_node_name)"/>
+ <arg name="ns" value="$(arg ns_nav3d)"/>
+ <arg name="node_name" value="$(arg lidar_name)_obstacles"/>
<arg name="lidar_nodelet_manager" value="$(arg lidar_nodelet_manager)"/>
- <arg name="config_file" value="$(arg lidar_detector_config_file)"/>
- <arg name="ranges_file" value="$(arg lidar_filter_config_file)"/>
- <arg name="output" value="$(arg output)"/>
- <arg name="launch_prefix" value="$(arg launch_prefix)"/>
- <arg name="cloud_in" value="/$(arg ns)/$(arg lidar_filter_node_name)/cloud_out"/>
+ <arg name="config_file" value="$(arg lidar_detector_config_file)"/>
+ <arg name="ranges_file" value="$(arg lidar_filter_config_file)"/>
+ <arg name="output" value="$(arg output)"/>
+ <arg name="launch_prefix" value="$(arg launch_prefix)"/>
+ <arg name="cloud_in" value="/$(arg ns_nav3d)/angle_filtered"/>
+ <arg name="debug_image" value="/$(arg ns_nav3d)/debug_image"/>
+ <arg name="obstacles_cloud_out" value="/$(arg ns_nav3d)/obstacles"/>
+ <arg name="free_space_cloud_out" value="/$(arg ns_nav3d)/free_space"/>
</include>
</launch>
diff --git a/launch/include/lidar_nodes.launch b/launch/include/lidar_nodes.launch
index e555dc6047f4140a40c4efc52d0c6e72d89644b8..1c6503bc1d55cb6e6d9bb808c0677e7e2f229329 100644
--- a/launch/include/lidar_nodes.launch
+++ b/launch/include/lidar_nodes.launch
@@ -2,34 +2,40 @@
<!-- -->
<launch>
- <arg name="ns" default="ana" />
- <arg name="output" default="log"/>
- <arg name="launch_prefix" default=""/>
+ <arg name="ns" default="ana" />
+ <arg name="lidar_name" default="lidar"/>
+ <arg name="ns2" default="nav3d/$(arg lidar_name)"/>
+ <arg name="ns_nav3d" default="$(arg ns)/$(arg ns2)"/>
- <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="/pointcloud_in" />
+ <arg name="output" default="log"/>
+ <arg name="launch_prefix" default=""/>
+
+ <arg name="lidar_filter_config_file" default="$(find iri_point_cloud_angle_filter)/config/default_config.yaml"/>
+ <arg name="lidar_filter_cloud_in" default="/pointcloud_in" />
- <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"/>
<include file="$(find iri_point_cloud_angle_filter)/launch/node.launch">
- <arg name="ns" value="$(arg ns)"/>
- <arg name="node_name" value="$(arg lidar_filter_node_name)"/>
- <arg name="config_file" value="$(arg lidar_filter_config_file)"/>
- <arg name="output" value="$(arg output)"/>
+ <arg name="ns" value="$(arg ns_nav3d)"/>
+ <arg name="node_name" value="$(arg lidar_name)_angle_filter"/>
+ <arg name="config_file" value="$(arg lidar_filter_config_file)"/>
+ <arg name="output" value="$(arg output)"/>
<arg name="launch_prefix" value="$(arg launch_prefix)"/>
- <arg name="cloud_in" value="$(arg lidar_filter_cloud_in)" />
+ <arg name="cloud_in" value="/$(arg lidar_filter_cloud_in)" />
+ <arg name="cloud_out" value="/$(arg ns_nav3d)/angle_filtered" />
</include>
<include file="$(find iri_lidar_obstacle_detector)/launch/node.launch">
- <arg name="ns" value="$(arg ns)"/>
- <arg name="node_name" value="$(arg lidar_detector_node_name)"/>
- <arg name="config_file" value="$(arg lidar_detector_config_file)"/>
- <arg name="ranges_file" value="$(arg lidar_filter_config_file)"/>
- <arg name="output" value="$(arg output)"/>
- <arg name="launch_prefix" value="$(arg launch_prefix)"/>
- <arg name="cloud_in" value="/$(arg ns)/$(arg lidar_filter_node_name)/cloud_out"/>
+ <arg name="ns" value="$(arg ns_nav3d)"/>
+ <arg name="node_name" value="$(arg lidar_name)_obstacles"/>
+ <arg name="config_file" value="$(arg lidar_detector_config_file)"/>
+ <arg name="ranges_file" value="$(arg lidar_filter_config_file)"/>
+ <arg name="output" value="$(arg output)"/>
+ <arg name="launch_prefix" value="$(arg launch_prefix)"/>
+ <arg name="cloud_in" value="/$(arg ns_nav3d)/angle_filtered"/>
+ <arg name="debug_image" value="/$(arg ns_nav3d)/debug_image"/>
+ <arg name="obstacles_cloud_out" value="/$(arg ns_nav3d)/obstacles"/>
+ <arg name="free_space_cloud_out" value="/$(arg ns_nav3d)/free_space"/>
</include>
</launch>