Added parameter passing

robofit · Jan 21, 2016 · c679de0 · c679de0
1 parent 44bb58b
commit c679de0
Show file tree

Hide file tree

Showing 4 changed files with 232 additions and 217 deletions.
diff --git a/pr2_description/robots/pr2.urdf.xacro b/pr2_description/robots/pr2.urdf.xacro
@@ -24,7 +24,6 @@
   <xacro:include filename="$(find pr2_description)/urdf/base_v0/base.urdf.xacro" />
   <!-- Head sensors -->
   <xacro:include filename="$(find pr2_description)/urdf/sensors/head_sensor_package.urdf.xacro" />
-  <xacro:include filename="$(find pr2_description)/urdf/sensors/kinect_prosilica_camera.urdf.xacro" />
   <!-- Camera sensors -->
   <xacro:include filename="$(find pr2_description)/urdf/sensors/wge100_camera.urdf.xacro" />
   <!-- Texture projector -->
@@ -70,23 +69,6 @@
     <origin xyz="0 0.110 0.0546" rpy="0 0 0" />
   </xacro:projector_wg6802418_v0>
 
-  <!-- Base of Kinect/Prosilica Mount -->
-  <xacro:kinect_prosilica_camera_swept_back_v0 name="head_mount" parent="head_plate_frame" >
-    <origin xyz="-0.137376 0 0.091746" rpy="0 0 0" />
-    <!--
-          Was -0.1 0 0.09225, -0.138 0 0.09225 looks good front back and sheet metal on bolt pattern,
-          -0.138 0 0.06225 is where Prosilica FOV just clears the head.
-          This is 30 mm (!) below the position in SolidWorks.
-          Lowering the Prosilica by this amount in SW makes the FOV hit around the aluminum / plastic junction.
-          Turns out that head_plate_frame is in some random position in space.
-          It is [0.0232 0 0.0645 from its parent, head_tilt_link.
-          The distance from head_plate_frame to the head_kinect_prosilica is 137.376 mm X and 91.746 mm Z.
-          So the origin should in fact be -0.137376 0 0.091746.
-          John is changing the Prosilica FOV based on my numbers.
-          Just need to roscd and svn up.
-    -->
-  </xacro:kinect_prosilica_camera_swept_back_v0>
-
   <xacro:pr2_tilting_laser_v0 name="laser_tilt" parent="torso_lift_link" laser_calib_ref="0.0">
     <origin xyz="0.09893 0 0.227" rpy="0 0 0" />
   </xacro:pr2_tilting_laser_v0>
@@ -153,4 +135,36 @@
     <origin xyz=".135 0 .044" rpy="${M_PI/2} ${-32.25*M_PI/180} 0" />
   </xacro:wge100_camera_v0>
 
+  <!-- Kinect models -->
+  <xacro:arg name="kinect2" default="false"/>
+
+  <!-- Kinect1 xacro -->
+  <xacro:unless value="$(arg kinect2)">
+    <xacro:include filename="$(find pr2_description)/urdf/sensors/kinect_prosilica_camera.urdf.xacro" />
+    <!-- Base of Kinect/Prosilica Mount -->
+    <xacro:kinect_prosilica_camera_swept_back_v0 name="head_mount" parent="head_plate_frame" >
+      <origin xyz="-0.137376 0 0.091746" rpy="0 0 0" />
+      <!--
+            Was -0.1 0 0.09225, -0.138 0 0.09225 looks good front back and sheet metal on bolt pattern,
+            -0.138 0 0.06225 is where Prosilica FOV just clears the head.
+            This is 30 mm (!) below the position in SolidWorks.
+            Lowering the Prosilica by this amount in SW makes the FOV hit around the aluminum / plastic junction.
+            Turns out that head_plate_frame is in some random position in space.
+            It is [0.0232 0 0.0645 from its parent, head_tilt_link.
+            The distance from head_plate_frame to the head_kinect_prosilica is 137.376 mm X and 91.746 mm Z.
+            So the origin should in fact be -0.137376 0 0.091746.
+            John is changing the Prosilica FOV based on my numbers.
+            Just need to roscd and svn up.
+      -->
+    </xacro:kinect_prosilica_camera_swept_back_v0>
+  </xacro:unless>
+
+  <!-- Kinect2 xacro -->
+  <xacro:if value="$(arg kinect2)">
+    <xacro:include filename="$(find pr2_description)/urdf/sensors/kinect2.urdf.xacro" />
+    <xacro:kinect2_v0 name="head_mount" parent="head_plate_frame" >
+      <origin xyz="-0.137376 0 0.091746" rpy="0 0 0" />
+    </xacro:kinect2_v0>
+  </xacro:if>
+
 </robot>
diff --git a/pr2_description/robots/upload_pr2.launch b/pr2_description/robots/upload_pr2.launch
@@ -1,10 +1,16 @@
 <launch>
-    <arg name="kinect" default="true"/>
-    <!-- send pr2 urdf to param server -->
-    <group if="$(arg kinect)">
-      <param name="robot_description" command="$(find xacro)/xacro.py '$(find pr2_description)/robots/pr2.urdf.xacro'" />
+  <arg name="kinect1" default="false"/>
+  <arg name="kinect2" default="false"/>
+  <!-- send pr2 urdf to param server -->
+  <group if="$(arg kinect2)">
+    <param name="robot_description" command="$(find xacro)/xacro.py '$(find pr2_description)/robots/pr2.urdf.xacro' kinect2:=true" />
+  </group>
+  <group unless="$(arg kinect2)">
+    <group if="$(arg kinect1)">
+      <param name="robot_description" command="$(find xacro)/xacro.py '$(find pr2_description)/robots/pr2.urdf.xacro' kinect2:=false" />
     </group>
-    <group unless="$(arg kinect)">
+    <group unless="$(arg kinect1)">
       <param name="robot_description" command="$(find xacro)/xacro.py '$(find pr2_description)/robots/pr2_no_kinect.urdf.xacro'" />
     </group>
+  </group>
 </launch>