Add nav3d namespace to camera launch files

launch/include/3d_nav_camera.launch

@@ -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>

launch/include/camera_nodelets.launch

@@ -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)"/>

launch/include/camera_nodes.launch

@@ -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)"/>

launch/include/lidar_nodelets.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=""/>
 
@@ -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>

launch/include/lidar_nodes.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>