t265_fixed_transforms.py

#!/usr/bin/env python  
import rospy
import tf2_ros
import tf2_msgs.msg
import geometry_msgs.msg
from math import pi
from tf.transformations import quaternion_from_euler

class FixedTFBroadcaster:

    def __init__(self):
        self.pub_tf = rospy.Publisher("/tf", tf2_msgs.msg.TFMessage, queue_size=1)

        while not rospy.is_shutdown():
            rospy.sleep(0.1)
            # world -> t265 pose frame
            t = geometry_msgs.msg.TransformStamped()
            t.header.frame_id = "world"
            t.header.stamp = rospy.Time.now()
            t.child_frame_id = "pose_frame"
            t.transform.translation.x = 0.40
            q = quaternion_from_euler(pi/2, 0, -pi/2, 'sxyz')
            t.transform.rotation.x = q[0]
            t.transform.rotation.y = q[1]
            t.transform.rotation.z = q[2]
            t.transform.rotation.w = q[3]

            tfm = tf2_msgs.msg.TFMessage([t])
            self.pub_tf.publish(tfm)

            # t265_pose_frame -> ros_frame
            t = geometry_msgs.msg.TransformStamped()
            t.header.frame_id = "pose_frame"
            t.header.stamp = rospy.Time.now()
            t.child_frame_id = "ros_frame"
            t.transform.translation.z = -0.40
            q = quaternion_from_euler(pi/2, 0, -pi/2, 'sxyz')
            # q = quaternion_from_euler(-pi/2, pi/2, 0, 'sxyz') # presumably correct transform
            t.transform.rotation.x = q[0]
            t.transform.rotation.y = q[1]
            t.transform.rotation.z = q[2]
            t.transform.rotation.w = q[3]
            tfm = tf2_msgs.msg.TFMessage([t])
            self.pub_tf.publish(tfm)

if __name__ == '__main__':
    rospy.init_node('fixed_tf2_broadcaster')
    tfb = FixedTFBroadcaster()

    rospy.spin()