add args to switch d435 and l515 as realsense_torso camera & change the extrinsic parameters 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_lifelog/estimated_endeffector.l

@@ -5,27 +5,31 @@
 
 (if (not (boundp '*baxter*))
     (setq *baxter* (instance baxter-robot :init)))
+(setq frames (make-hash-table))
+(dolist (arm (list :rarm :larm))
+  (sethash arm frames (send (send (send *baxter* arm :end-coords) :parent) :name))
+  )
 
-(ros::rate 20)
+(ros::rate 100)
 
 (defun right-state-cb(msg)
-  (state-cb "/robot/end_effector/right/wrench" :rarm (send msg :wrench))
+  (state-cb "/robot/end_effector/right/wrench" :rarm (send msg :wrench) (send msg :header :stamp))
   )
 
 (defun left-state-cb(msg)
-  (state-cb "/robot/end_effector/left/wrench" :larm (send msg :wrench))
+  (state-cb "/robot/end_effector/left/wrench" :larm (send msg :wrench) (send msg :header :stamp))
   )
 
-(defun state-cb(advertise-topic arm wrench-msg)
+(defun state-cb(advertise-topic arm wrench-msg stamp)
   (let ((msg (instance geometry_msgs::WrenchStamped :init)))
     (send msg :wrench wrench-msg)
-    (send msg :header :stamp (ros::time-now))
-    (send msg :header :frame_id (send (send (send *baxter* arm :end-coords) :parent) :name))
+    (send msg :header :stamp stamp)
+    (send msg :header :frame_id (gethash arm frames))
     (ros::publish advertise-topic msg))
   )
 
 
 (ros::subscribe "/robot/limb/left/endpoint_state" baxter_core_msgs::EndpointState #'left-state-cb)
 (ros::subscribe "/robot/limb/right/endpoint_state" baxter_core_msgs::EndpointState #'right-state-cb)
 
-(ros::spin)
+(ros::spin)

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.196
+initial_y: 0.027
+initial_z: 0.356
+initial_orientation: [-0.498, 0.496, -0.501, 0.503]
+
+# 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

@@ -1,8 +1,9 @@
 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_x: 0.252
+initial_y: 0.000
+initial_z: 0.561
+initial_orientation: [-0.665, 0.694, -0.191, 0.198]
+
 # - 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)" />