add camera_type args in launch to switch between d435 and l515 as rea…

…lsense_torso & changed extrinsic parameters both of them

jsk_baxter_robot/jsk_baxter_startup/baxter.launch

@@ -14,6 +14,7 @@
   <arg name="launch_spherical_kodak" default="false" />
   <arg name="launch_spherical_stereo" default="false" />
   <arg name="start_openni" default="false"/>
+  <arg name="camera_type" default="l515"/>
   <arg name="REALSENSE_CAMERA_NS" value="realsense_torso" />
   <arg name="SPHERICAL_CAMERA_NS" value="kodak_head" />
   <arg name="SPHERICAL_LEFT_CAMERA_NS" value="elp_head_left" />
@@ -144,10 +145,11 @@
     <arg name="launch_openni" value="$(arg start_openni)" />
   </include>
 
-  <!-- Use realsense L515 on torso -->
+  <!-- Use realsense L515 or d435 on torso -->
   <include if="$(arg launch_realsense_torso)"
            file="$(find jsk_baxter_startup)/jsk_baxter_sensors/baxter_realsense_torso.launch">
     <arg name="realsense_camera_ns" value="$(arg REALSENSE_CAMERA_NS)" />
+    <arg name="camera_type" value="$(arg camera_type)"/>
   </include>
 
   <!-- Use spherical kodak on head -->

jsk_baxter_robot/jsk_baxter_startup/jsk_baxter_sensors/baxter_realsense_torso.launch

@@ -1,6 +1,10 @@
 <launch>
   <arg name="launch_transformable_server" default="true" />
   <arg name="realsense_camera_ns" default="realsense_torso" />
+  <arg name="camera_type" default="l515" />
+  <arg name="serial_no" default="f0232933" if="$(eval arg('camera_type') == 'l515' )"/>
+  <arg name="serial_no" default="827112071768" if="$(eval arg('camera_type') == 'd435' )" />
+
   <arg name="publish_tf" default="false" />
   <!-- in order to get higher resolution, we need USB3.2 -->
   <arg name="color_width" default="960" />
@@ -26,7 +30,8 @@
   <!-- tf for realsense at baxter's torso -->
   <node name="marker_6dof_tf_base_to_color"
         pkg="jsk_interactive_marker" type="marker_6dof">
-    <rosparam command="load" file="$(find jsk_baxter_startup)/jsk_baxter_sensors/l515_torso_pose.yaml" />
+    <!-- <rosparam command="load" file="$(find jsk_baxter_startup)/jsk_baxter_sensors/l515_torso_pose.yaml" /> -->
+    <rosparam command="load" file="$(find jsk_baxter_startup)/jsk_baxter_sensors/$(arg camera_type)_torso_pose.yaml" />
     <rosparam subst_value="true" >
       object_type: cube
       publish_tf: true
@@ -40,6 +45,7 @@
   <include file="$(find jsk_baxter_startup)/jsk_baxter_sensors/realsense.launch">
     <arg name="realsense_camera_ns" value="$(arg realsense_camera_ns)" />
     <arg name="manager" value="$(arg manager)" />
+    <arg name="serial_no" value="$(arg serial_no)" />
     <arg name="publish_tf" value="$(arg publish_tf)" />
     <arg name="color_width"  value="$(arg color_width)" />
     <arg name="color_height" value="$(arg color_height)" />

jsk_baxter_robot/jsk_baxter_startup/jsk_baxter_sensors/d435_torso_pose.yaml

@@ -0,0 +1,10 @@
+frame_id: base
+initial_x: 0.2235
+initial_y: 0.0316
+initial_z: 0.3311
+initial_orientation: [-0.5, 0.5, -0.5, 0.5]
+
+# initial_x: 0.20
+# initial_y: 0.04
+# initial_z: 0.35
+# initial_orientation: [-0.5, 0.5, -0.5, 0.5]

jsk_baxter_robot/jsk_baxter_startup/jsk_baxter_sensors/l515_torso_pose.yaml

@@ -2,7 +2,7 @@ frame_id: base
 initial_x: 0.253
 initial_y: -0.004
 initial_z: 0.563
-initial_orientation: [-0.679, 0.685, -0.153, 0.213]
+initial_orientation: [-0.683, 0.685, -0.183, 0.183]
 # - Translation: [0.253, -0.004, 0.563]
 # - Rotation: in Quaternion [-0.679, 0.685, -0.153, 0.213]
 #             in RPY (radian) [-2.616, 0.085, -1.557]

jsk_baxter_robot/jsk_baxter_startup/jsk_baxter_sensors/realsense.launch

@@ -1,6 +1,7 @@
 <launch>
   <arg name="realsense_camera_ns" />
   <arg name="manager" />
+  <arg name="serial_no" />
   <arg name="publish_tf" />
   <arg name="color_width" />
   <arg name="color_height" />
@@ -15,6 +16,7 @@
   <include file="$(find realsense2_camera)/launch/rs_rgbd.launch">
     <arg name="camera" value="$(arg realsense_camera_ns)" />
     <arg name="manager" value="$(arg manager)" />
+    <arg name="serial_no" value="$(arg serial_no)"/>
     <arg name="publish_tf" value="$(arg publish_tf)" />
     <arg name="color_width"  value="$(arg color_width)" />
     <arg name="color_height" value="$(arg color_height)" />