diff --git a/launch/include/3d_nav_camera.launch b/launch/include/3d_nav_camera.launch index d22f4082673451f5252ceb61ca91a8f34ec7c73d..b117dd54a605e1435886fc92102337179bc996ad 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 4813621e0fc78bd846950a0d59d8bcfc3fbdc97e..40fcabb2b1eea182705522da5e5ba6ab7cca56a9 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 5971e20e182a8cb46558e68dcc5cfb69e9eecc65..e44198452b9fe6210e5e4308d681ffa7e40083f3 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 3676b102b3702d5da0aa84d4c003748758e1e628..09c20e41afcb733e35f411e055063ebf10c5187d 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 0971d11311b3e2c7324d8bf9502bd96baa00ff10..f6abd2514bad8b80d432c340a9c01fadf1e68189 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 9e3a6b355affbce140bcd355d22ad210f22b3150..1c6503bc1d55cb6e6d9bb808c0677e7e2f229329 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>