Skip to content
GitLab
Commit bda460f5 authored by Sergi Hernandez's avatar Sergi Hernandez
Browse files

Merge branch 'namespace' into 'master' 

Removed node names as arguments to the launch file.

See merge request !6
parents 465f2cdd 9ec69e26
Hide whitespace changes
Inline Side-by-side
config/default_outlier_removal_config.yaml 0 → 100644
View file @ bda460f5
mean_k: 20
stddev: 2.0
negative: False
config/default_passthrough_config.yaml 0 → 100644
View file @ bda460f5
filter_field_name: x
filter_limit_min: 1.1
filter_limit_max: 2.2
filter_limit_negative: False
config/default_radius_outlier_removal_config.yaml 0 → 100644
View file @ bda460f5
radius_search: 0.1
min_neighbors: 15
config/default_voxels_config.yaml 0 → 100644
View file @ bda460f5
leaf_size: 0.03
filter_field_name: z
filter_limit_min: -0.5
filter_limit_max: 3.0
filter_limit_negative: False
launch/include/3d_nav_camera.launch
View file @ bda460f5
......@@ -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>
......
launch/include/3d_nav_lidar.launch
View file @ bda460f5
......@@ -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>
......
launch/include/camera_nodelets.launch
View file @ bda460f5
......@@ -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>
......
launch/include/camera_nodes.launch
View file @ bda460f5
......@@ -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>