From fdb49a37abc2b7f780f8f93939dc6774cf3ee47b Mon Sep 17 00:00:00 2001
From: Sergi Hernandez Juan <shernand@iri.upc.edu>
Date: Thu, 3 Mar 2022 13:49:04 +0100
Subject: [PATCH] Removed node names as arguments to the launch file. Removed
intermediate pointcloud topics as arguments to the launch files. Removed the
hardcoded configuration of the voxel, passthrough, outlier removal and radius
outlier removal. Using external configuration files now. Added the
camera/lidar name to the launch file to generate the namespaces. Added the
output topic of the IRI launch files.
---
launch/include/3d_nav_camera.launch | 100 ++++++++++---------
launch/include/3d_nav_lidar.launch | 21 ++--
launch/include/camera_nodelets.launch | 131 ++++++++++++------------
launch/include/camera_nodes.launch | 138 +++++++++++++-------------
launch/include/lidar_nodelets.launch | 49 ++++-----
launch/include/lidar_nodes.launch | 45 +++++----
6 files changed, 243 insertions(+), 241 deletions(-)
diff --git a/launch/include/3d_nav_camera.launch b/launch/include/3d_nav_camera.launch
index d22f408..b117dd5 100644
--- a/launch/include/3d_nav_camera.launch
+++ b/launch/include/3d_nav_camera.launch
@@ -2,67 +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"/>
+ <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="base_frame_id" value="$(arg base_frame_id)"/>
- <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_nodes.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="base_frame_id" value="$(arg base_frame_id)"/>
- <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="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 4813621..40fcabb 100644
--- a/launch/include/3d_nav_lidar.launch
+++ b/launch/include/3d_nav_lidar.launch
@@ -3,42 +3,39 @@
<launch>
<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_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="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_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_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="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_node_name" value="$(arg lidar_detector_node_name)"/>
<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 5971e20..e441984 100644
--- a/launch/include/camera_nodelets.launch
+++ b/launch/include/camera_nodelets.launch
@@ -2,31 +2,35 @@
<launch>
- <arg name="ns" default="robot"/>
- <arg name="ns2" default="nav3d/camera"/>
- <arg name="ns_nav3d" default="$(arg ns)/$(arg ns2)"/>
+ <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="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_nav3d)">
@@ -40,93 +44,82 @@
<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_nav3d)"/>
- <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="cloud_in" value="$(arg camera_cloud_in)_throttle"/>
- <arg name="output" value="$(arg output)"/>
- <arg name="launch_prefix" value="$(arg launch_prefix)"/>
+ <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_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 base_frame_id)"/>
+ <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>
- leaf_size: 0.03
- filter_field_name: z
- filter_limit_min: -0.5
- filter_limit_max: 5.0
- filter_limit_negative: False
- </rosparam>
+ <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_nav3d)"/>
- <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 ******************************* -->
@@ -141,20 +134,24 @@
<include file="$(find iri_point_cloud_hole_detection)/launch/nodelet.launch">
<arg name="ns" value="$(arg ns_nav3d)"/>
- <arg name="node_name" value="$(arg near_hole_detection_node_name)"/>
+ <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="cloud_in" value="$(arg hole_detection_cloud_in)"/>
+ <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_nav3d)"/>
- <arg name="node_name" value="$(arg far_hole_detection_node_name)"/>
+ <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="cloud_in" value="$(arg hole_detection_cloud_in)"/>
+ <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>
diff --git a/launch/include/camera_nodes.launch b/launch/include/camera_nodes.launch
index 3676b10..09c20e4 100644
--- a/launch/include/camera_nodes.launch
+++ b/launch/include/camera_nodes.launch
@@ -2,47 +2,51 @@
<launch>
- <arg name="ns" default="ana"/>
- <arg name="ns2" default="nav3d/camera"/>
- <arg name="ns_nav3d" default="$(arg ns)/$(arg ns2)"/>
+ <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="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="average_node_name" default="average_point_cloud"/>
- <arg name="average_config_file" default="$(find iri_average_point_cloud)/config/default_config.yaml"/>
+ <arg name="radius_outlier_removal_config_file" default="$(find iri_rosnav)/config/default_radius_outlier_removal_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="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="near_hole_detection_config_file" default="$(find iri_point_cloud_hole_detection)/config/near_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="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="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_nav3d)">
- <node name="pc_throttler"
+ <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_nav3d)"/>
- <arg name="node_name" value="$(arg average_node_name)"/>
+ <arg name="node_name" value="$(arg camera_name)_average"/>
<arg name="config_file" value="$(arg average_config_file)"/>
- <arg name="cloud_in" value="$(arg camera_cloud_in)_throttle"/>
+ <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>
@@ -56,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" />
- <param name="input_frame" value="$(arg base_frame_id)"/>
+ <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>
- leaf_size: 0.03
- filter_field_name: z
- filter_limit_min: -0.5
- filter_limit_max: 5.0
- filter_limit_negative: False
- </rosparam>
+ <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_nav3d)"/>
- <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" value="$(arg launch_prefix)"/>
- <arg name="cloud_in" value="$(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_nav3d)"/>
- <arg name="node_name" value="$(arg near_hole_detection_node_name)"/>
- <arg name="config_file" value="$(arg near_hole_detection_config_file)"/>
- <arg name="cloud_in" value="$(arg hole_detection_cloud_in)"/>
- <arg name="output" value="$(arg output)"/>
- <arg name="launch_prefix" value="$(arg launch_prefix)"/>
+ <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_nav3d)"/>
- <arg name="node_name" value="$(arg far_hole_detection_node_name)"/>
- <arg name="config_file" value="$(arg far_hole_detection_config_file)"/>
- <arg name="cloud_in" value="$(arg hole_detection_cloud_in)"/>
- <arg name="output" value="$(arg output)"/>
- <arg name="launch_prefix" value="$(arg launch_prefix)"/>
+ <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 0971d11..f6abd25 100644
--- a/launch/include/lidar_nodelets.launch
+++ b/launch/include/lidar_nodelets.launch
@@ -2,20 +2,19 @@
<!-- -->
<launch>
- <arg name="ns" default="ana" />
- <arg name="ns2" default="nav3d/lidar"/>
- <arg name="ns_nav3d" default="$(arg ns)/$(arg ns2)"/>
+ <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="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 -->
@@ -28,24 +27,28 @@
</group>
<include file="$(find iri_point_cloud_angle_filter)/launch/nodelet.launch">
- <arg name="ns" value="$(arg ns_nav3d)"/>
- <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_nav3d)"/>
- <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_nav3d)/$(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 9e3a6b3..1c6503b 100644
--- a/launch/include/lidar_nodes.launch
+++ b/launch/include/lidar_nodes.launch
@@ -2,37 +2,40 @@
<!-- -->
<launch>
- <arg name="ns" default="ana" />
- <arg name="ns2" default="nav3d/lidar"/>
- <arg name="ns_nav3d" default="$(arg ns)/$(arg ns2)"/>
+ <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="output" default="log"/>
+ <arg name="launch_prefix" default=""/>
- <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"/>
<include file="$(find iri_point_cloud_angle_filter)/launch/node.launch">
- <arg name="ns" value="$(arg ns_nav3d)"/>
- <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_nav3d)"/>
- <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_nav3d)/$(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>
--
GitLab