[jsk_robot_startup] add t265_mode to CameraToBaseOffset.py

jsk_robot_common/jsk_robot_startup/scripts/CameraToBaseOffset.py

@@ -31,13 +31,15 @@ def __init__(self):
 
         # other parameters
         self.twod_mode = rospy.get_param("~2d_mode", False)
+        self.t265_mode = rospy.get_param("~t265_mode", True)
         self.enable_covariance = rospy.get_param("~enable_covariance", False)
 
         # other members
         if self.publish_tf:
             self.broadcast = tf.TransformBroadcaster()
         self.listener = tf.TransformListener(True, rospy.Duration(10))
         self.htm_odom_base_to_odom_camera = None
+        self.htm_odom_camera_to_t265_frame = None
         self.lock = threading.Lock()
 
         self.source_odom_sub = rospy.Subscriber(
@@ -53,6 +55,7 @@ def init_signal_callback(self, msg):
         time.sleep(1)  # wait to update odom_init frame
         with self.lock:
             self.htm_odom_base_to_odom_camera = None
+            self.htm_odom_camera_to_t265_frame = None
 
     def source_odom_callback(self, msg):
         with self.lock:
@@ -63,15 +66,29 @@ def source_odom_callback(self, msg):
 
             # htm (Homogeneous Transformation Matrix)
             #   from odom frame of camera to current camera frame
-            htm_odom_camera_to_camera = make_homogeneous_matrix(
-                [msg.pose.pose.position.x,
-                 msg.pose.pose.position.y,
-                 msg.pose.pose.position.z],
-                [msg.pose.pose.orientation.x,
-                 msg.pose.pose.orientation.y,
-                 msg.pose.pose.orientation.z,
-                 msg.pose.pose.orientation.w]
-            )
+            if self.t265_mode:
+                htm_t265_odom_frame_to_camera = make_homogeneous_matrix(
+                    [msg.pose.pose.position.x,
+                     msg.pose.pose.position.y,
+                     msg.pose.pose.position.z],
+                    [msg.pose.pose.orientation.x,
+                     msg.pose.pose.orientation.y,
+                     msg.pose.pose.orientation.z,
+                     msg.pose.pose.orientation.w]
+                )
+                if self.htm_t265_odom_frame_to_odom_camera is None:
+                    self.htm_t265_odom_frame_to_odom_camera = htm_t265_odom_frame_to_camera
+                htm_odom_camera_to_camera = numpy.linalg.inv(self.htm_t265_odom_frame_to_odom_camera) * self.htm_t265_odom_frame_to_camera
+            else:
+                htm_odom_camera_to_camera = make_homogeneous_matrix(
+                    [msg.pose.pose.position.x,
+                     msg.pose.pose.position.y,
+                     msg.pose.pose.position.z],
+                    [msg.pose.pose.orientation.x,
+                     msg.pose.pose.orientation.y,
+                     msg.pose.pose.orientation.z,
+                     msg.pose.pose.orientation.w]
+                )
 
             if self.htm_odom_base_to_odom_camera is None:
                 self.htm_odom_base_to_odom_camera = numpy.linalg.inv(
@@ -176,41 +193,51 @@ def source_odom_callback(self, msg):
                 broadcast_transform(
                     self.broadcast, transformed_odom_msg, self.invert_tf)
 
+    def get_htm_odom_base_to_t265_frame(self, stamp, t265_odom_frame):
+        return self.get_homogeneous_matrix(
+                    self.camera_frame_id,
+
     def get_htm_camera_to_base(self, stamp):
+        return self.get_homogeneous_matrix(
+                    self.camera_frame_id,
+                    self.base_frame_id,
+                    stamp)
+
+    def get_homogeneous_matrix(self, source_frame, target_frame, stamp):
         try:
-            (trans, rot) = self.listener.lookupTransform(
-                self.camera_frame_id,
-                self.base_frame_id,
-                stamp)
+            (trans, rot)=self.listener.lookupTransform(
+                    source_frame,
+                    target_frame,
+                    stamp)
         except (tf.LookupException,
                 tf.ConnectivityException,
                 tf.ExtrapolationException):
             try:
                 rospy.logwarn(
-                    "[%s] failed to solve camera_to_base tf in %f. Use rospy.Time(0): %s to %s",
+                    "[%s] failed to solve tf in %f. Use rospy.Time(0): %s to %s",
                     rospy.get_name(),
                     stamp.to_sec(),
-                    self.camera_frame_id,
-                    self.base_frame_id)
-                (trans, rot) = self.listener.lookupTransform(
-                    self.camera_frame_id,
-                    self.base_frame_id,
-                    rospy.Time(0))
+                    source_frame,
+                    target_frame)
+                (trans, rot)=self.listener.lookupTransform(
+                        source_frame,
+                        target_frame
+                        rospy.Time(0))
             except (tf.LookupException,
                     tf.ConnectivityException,
                     tf.ExtrapolationException):
                 rospy.logwarn(
-                    "[%s] failed to solve camera_to_base tf: %s to %s",
+                    "[%s] failed to solve tf: %s to %s",
                     rospy.get_name(),
-                    self.camera_frame_id,
-                    self.base_frame_id)
+                    source_frame,
+                    target_frame)
                 return None
         return make_homogeneous_matrix(trans, rot)
 
 
 if __name__ == '__main__':
     try:
-        node = CameraToBaseOffset()
+        node=CameraToBaseOffset()
         node.execute()
     except rospy.ROSInterruptException:
         pass