Commit fdb49a37 authored by Sergi Hernandez's avatar Sergi Hernandez
Browse files

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.
parent 4f625dc7
......@@ -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>
......
......@@ -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>
......
......@@ -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>
......
......@@ -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>
......