Commit 419a621d authored by Fernando Herrero's avatar Fernando Herrero
Browse files

Add nav3d namespace to camera launch files

parent 465f2cdd
......@@ -13,6 +13,7 @@
<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_cloud_in" default="sensors/nav_cam/depth_registered/points_throttle"/>
<arg name="normals_node_name" default="obstacle_detection_normals"/>
<arg name="normals_config_file" default="$(find iri_obstacle_detection_normals)/config/default_config.yaml"/>
......@@ -23,6 +24,8 @@
<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="base_frame_id" default="$(arg ns)/base_footprint"/>
<group if="$(arg use_nodelets)">
<include file="$(find iri_rosnav)/launch/include/camera_nodelets.launch">
......@@ -32,6 +35,7 @@
<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="average_cloud_in" value="$(arg average_cloud_in)"/>
<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)"/>
......@@ -39,17 +43,19 @@
<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)"/>
</include>
</group>
<group unless="$(arg use_nodelets)">
<include file="$(find iri_rosnav)/launch/include/camera_nodelets.launch">
<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="average_cloud_in" value="$(arg average_cloud_in)"/>
<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)"/>
......@@ -57,6 +63,7 @@
<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)"/>
</include>
......
......@@ -3,6 +3,8 @@
<launch>
<arg name="ns" default="ana"/>
<arg name="ns2" default="nav3d/camera"/>
<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" />
......@@ -20,12 +22,14 @@
<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="base_frame_id" default="$(arg ns)/base_footprint"/>
<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)"
......@@ -44,7 +48,7 @@
</group>
<include file="$(find iri_average_point_cloud)/launch/nodelet.launch">
<arg name="ns" value="$(arg ns)"/>
<arg name="ns" value="$(arg ns_nav3d)"/>
<arg name="node_name" value="$(arg average_node_name)"/>
<arg name="camera_nodelet_manager" value="$(arg obstacle_nodelet_manager)"/>
<arg name="config_file" value="$(arg average_config_file)"/>
......@@ -53,7 +57,7 @@
<arg name="cloud_in" value="$(arg camera_cloud_in)_throttle"/>
</include>
<group ns="$(arg ns)">
<group ns="$(arg ns_nav3d)">
<!-- Downsampling points -->
<node name="voxel_grid_obs"
pkg="nodelet"
......@@ -61,8 +65,8 @@
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"/>
<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
......@@ -116,7 +120,7 @@
</group>
<include file="$(find iri_obstacle_detection_normals)/launch/nodelet.launch">
<arg name="ns" value="$(arg ns)"/>
<arg name="ns" value="$(arg ns_nav3d)"/>
<arg name="node_name" value="$(arg normals_node_name)"/>
<arg name="camera_nodelet_manager" value="$(arg obstacle_nodelet_manager)"/>
<arg name="config_file" value="$(arg normals_config_file)"/>
......@@ -127,7 +131,7 @@
<!-- **************************** HOLE DETECTION ******************************* -->
<group ns="$(arg ns)">
<group ns="$(arg ns_nav3d)">
<node name="$(arg hole_nodelet_manager)"
pkg="nodelet"
type="nodelet"
......@@ -136,7 +140,7 @@
</group>
<include file="$(find iri_point_cloud_hole_detection)/launch/nodelet.launch">
<arg name="ns" value="$(arg ns)"/>
<arg name="ns" value="$(arg ns_nav3d)"/>
<arg name="node_name" value="$(arg near_hole_detection_node_name)"/>
<arg name="camera_nodelet_manager" value="$(arg hole_nodelet_manager)"/>
<arg name="config_file" value="$(arg near_hole_detection_config_file)"/>
......@@ -146,7 +150,7 @@
</include>
<include file="$(find iri_point_cloud_hole_detection)/launch/nodelet.launch">
<arg name="ns" value="$(arg ns)"/>
<arg name="ns" value="$(arg ns_nav3d)"/>
<arg name="node_name" value="$(arg far_hole_detection_node_name)"/>
<arg name="camera_nodelet_manager" value="$(arg hole_nodelet_manager)"/>
<arg name="config_file" value="$(arg far_hole_detection_config_file)"/>
......
......@@ -3,6 +3,8 @@
<launch>
<arg name="ns" default="ana"/>
<arg name="ns2" default="nav3d/camera"/>
<arg name="ns_nav3d" default="$(arg ns)/$(arg ns2)"/>
<arg name="obstacle_nodelet_manager" default="obstacle_nodelet_manager" />
......@@ -22,13 +24,15 @@
<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="base_frame_id" default="$(arg ns)/base_footprint"/>
<arg name="output" default="screen"/>
<arg name="launch_prefix" default=""/>
<!-- ******************* OBSTACLE DETECTION ********************************** -->
<group ns="$(arg ns)">
<group ns="$(arg ns_nav3d)">
<node name="pc_throttler"
type="throttle"
pkg="topic_tools"
......@@ -36,7 +40,7 @@
</group>
<include file="$(find iri_average_point_cloud)/launch/node.launch">
<arg name="ns" value="$(arg ns)"/>
<arg name="ns" value="$(arg ns_nav3d)"/>
<arg name="node_name" value="$(arg average_node_name)"/>
<arg name="config_file" value="$(arg average_config_file)"/>
<arg name="output" value="$(arg output)"/>
......@@ -44,7 +48,7 @@
<arg name="cloud_in" default="$(arg average_cloud_in)"/>
</include>
<group ns="$(arg ns)">
<group ns="$(arg ns_nav3d)">
<node name="$(arg obstacle_nodelet_manager)"
pkg="nodelet"
......@@ -59,9 +63,9 @@
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)"/>
<param name="output_frame" value="$(arg base_frame_id)"/>
<rosparam>
input_frame: 'ana/base_footprint'
output_frame: 'ana/base_footprint'
leaf_size: 0.03
filter_field_name: z
filter_limit_min: -0.5
......@@ -114,7 +118,7 @@
</group>
<include file="$(find iri_obstacle_detection_normals)/launch/node.launch">
<arg name="ns" value="$(arg ns)"/>
<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)"/>
......@@ -125,7 +129,7 @@
<!-- **************************** HOLE DETECTION ******************************* -->
<include file="$(find iri_point_cloud_hole_detection)/launch/node.launch">
<arg name="ns" value="$(arg ns)"/>
<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="output" value="$(arg output)"/>
......@@ -134,7 +138,7 @@
</include>
<include file="$(find iri_point_cloud_hole_detection)/launch/node.launch">
<arg name="ns" value="$(arg ns)"/>
<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="output" value="$(arg output)"/>
......
......@@ -3,6 +3,9 @@
<launch>
<arg name="ns" default="ana" />
<arg name="ns2" default="nav3d/lidar"/>
<arg name="ns_nav3d" default="$(arg ns)/$(arg ns2)"/>
<arg name="output" default="log"/>
<arg name="launch_prefix" default=""/>
......@@ -16,7 +19,7 @@
<arg name="lidar_detector_config_file" default="$(find iri_lidar_obstacle_detector)/config/default_config.yaml"/>
<!-- Obstacle nodelets manager -->
<group ns="$(arg ns)">
<group ns="$(arg ns_nav3d)">
<node name="$(arg lidar_nodelet_manager)"
pkg="nodelet"
type="nodelet"
......@@ -25,7 +28,7 @@
</group>
<include file="$(find iri_point_cloud_angle_filter)/launch/nodelet.launch">
<arg name="ns" value="$(arg ns)"/>
<arg name="ns" value="$(arg ns_nav3d)"/>
<arg name="node_name" value="$(arg lidar_filter_node_name)"/>
<arg name="lidar_nodelet_manager" value="$(arg lidar_nodelet_manager)"/>
<arg name="config_file" value="$(arg lidar_filter_config_file)"/>
......@@ -35,14 +38,14 @@
</include>
<include file="$(find iri_lidar_obstacle_detector)/launch/nodelet.launch">
<arg name="ns" value="$(arg ns)"/>
<arg name="ns" value="$(arg ns_nav3d)"/>
<arg name="node_name" value="$(arg lidar_detector_node_name)"/>
<arg name="lidar_nodelet_manager" value="$(arg lidar_nodelet_manager)"/>
<arg name="config_file" value="$(arg lidar_detector_config_file)"/>
<arg name="ranges_file" value="$(arg lidar_filter_config_file)"/>
<arg name="output" value="$(arg output)"/>
<arg name="launch_prefix" value="$(arg launch_prefix)"/>
<arg name="cloud_in" value="/$(arg ns)/$(arg lidar_filter_node_name)/cloud_out"/>
<arg name="cloud_in" value="/$(arg ns_nav3d)/$(arg lidar_filter_node_name)/cloud_out"/>
</include>
</launch>
......
......@@ -3,6 +3,9 @@
<launch>
<arg name="ns" default="ana" />
<arg name="ns2" default="nav3d/lidar"/>
<arg name="ns_nav3d" default="$(arg ns)/$(arg ns2)"/>
<arg name="output" default="log"/>
<arg name="launch_prefix" default=""/>
......@@ -14,7 +17,7 @@
<arg name="lidar_detector_config_file" default="$(find iri_lidar_obstacle_detector)/config/default_config.yaml"/>
<include file="$(find iri_point_cloud_angle_filter)/launch/node.launch">
<arg name="ns" value="$(arg ns)"/>
<arg name="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)"/>
......@@ -23,13 +26,13 @@
</include>
<include file="$(find iri_lidar_obstacle_detector)/launch/node.launch">
<arg name="ns" value="$(arg ns)"/>
<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)/$(arg lidar_filter_node_name)/cloud_out"/>
<arg name="cloud_in" value="/$(arg ns_nav3d)/$(arg lidar_filter_node_name)/cloud_out"/>
</include>
</launch>
......
Supports Markdown
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment