tf2 lookuptransform failed to get target_frame in rosbag play

[rslim97]

I have a multi camera setup and is attempting to get the extrinsic transformations for each camera. For this I used a class object in python to retrieve the static transform of each camera during initialization as follows:

import rospy
import tf2_ros
import numpy as np

class SynchronisedCamera():
   def __init__(self, camera_param, reference_frame):
       self.trans = None
       self.rot = None        
       self.reference_frame = reference_frame
       self.name = camera_param["name"]
       self.tf_buffer = tf2_ros.Buffer()
       self.tf2_listener = tf2_ros.TransformListener(self.tf_buffer)
       try:
           self.camera_frame = camera_param["camera_frame"]
       except KeyError:
           self.camera_frame = None
       rospy.sleep(0.1)
       self.get_transform_with_retry(camera_param)

   def get_transform_with_retry(self, camera_param):
       max_attempts = 3
       for attempt in range(max_attempts):
           self.tf_buffer.can_transform(self.camera_frame, self.reference_frame, rospy.Duration(2))
           try:
               # Intrinsic
               camera_info = rospy.wait_for_message(camera_param["camera_info"], CameraInfo, timeout=10)
               self.K = camera_info.K
               if self.orientation % 2 != 0:
                   fu = camera_info.K[0]
                   fv = camera_info.K[4]
                   u0 = camera_info.K[2]
                   v0 = camera_info.K[5]
                   self.K = [fv, 0, v0, 0, fu, u0, 0, 0, 1.0]

               # Intrinsic
               self.intrinsic_matrix = np.asarray(self.K).reshape(3, 3)

               # Extrinsic
               # Look up the transform from 'source_frame' to 'target_frame'
               transform = self.tf_buffer.lookup_transform(self.camera_frame, self.reference_frame, rospy.Time(0),
                                                           rospy.Duration(1))
               trans = [transform.transform.translation.x, transform.transform.translation.y,                         transform.transform.translation.z]
               rot = [transform.transform.rotation.x, transform.transform.rotation.y, transform.transform.rotation.z,
                      transform.transform.rotation.w]
               self.extrinsic_matrix = self.fromTranslationRotation(trans, rot)
           except rospy.ROSException as e:
                rospy.logwarn(e)

    def translation_matrix(self, direction):
        M = np.eye(4)
        M[:3, 3] = direction[:3]
        return M

    def quaternion_matrix(self, quaternion):
        """Return homogeneous rotation matrix from quaternion.

        # >>> R = quaternion_matrix([0.06146124, 0, 0, 0.99810947])
        # >>> numpy.allclose(R, rotation_matrix(0.123, (1, 0, 0)))
        True

        """
        q = np.array(quaternion[:4], dtype=np.float64, copy=True)
        nq = np.dot(q, q)
        if nq < _EPS:
            return np.eye(4)
        q *= np.sqrt(2.0 / nq)
        q = np.outer(q, q)
        return np.array((
            (1.0 - q[1, 1] - q[2, 2], q[0, 1] - q[2, 3], q[0, 2] + q[1, 3], 0.0),
            (q[0, 1] + q[2, 3], 1.0 - q[0, 0] - q[2, 2], q[1, 2] - q[0, 3], 0.0),
            (q[0, 2] - q[1, 3], q[1, 2] + q[0, 3], 1.0 - q[0, 0] - q[1, 1], 0.0),
            (0.0, 0.0, 0.0, 1.0)
        ), dtype=np.float64)

    def fromTranslationRotation(self, trans, rot):
        return np.dot(self.translation_matrix(trans), self.quaternion_matrix(rot))

The code can run in a realtime setting with the camera ros driver running and getting realtime camera feedbacks. However when I attempted to analyse the rosbags using robag play tf2 was unable to lookuptransform for the second camera (I am using two cameras). It is peculiar that the code is able to get camera transform from one camera only using rosbag play, but failed when getting transforms from both cameras.

Following is the error snippet when attempting to get transforms from the two cameras (left and right):

File src/tracker/SynchronisedCamera.py", line 231, in get_transform_with_retry transform = self.tf_buffer.lookup_transform(self.camera_frame, self.reference_frame, rospy.Time(0), File "/opt/ros/noetic/lib/python3/dist-packages/tf2_ros/buffer.py", line 90, in lookup_transform return self.lookup_transform_core(target_frame, source_frame, time) tf2.LookupException: "camera_right_rgb_optical_frame" passed to lookupTransform argument target_frame does not exist.