How to publish pose in new frame?

edit

Hi all,

Thanks a lot for reading my question. I am currently running a simulator in python where I have my robot move in a forward direction by 'x' and then back by the same amount. Due to several factors after this sequence, the robot doesn't return to the exact initial position (0,0) . What I want to do, is make it go back to its starting position after the sequence is ran. As of now I have written a code where I listen to the transform from the 'odom' frame to the 'base_link' frame, the latter being the frame I want to bring back to the origin. My approach was to publish a new frame called 'recentered' after the sequence is done and then calculate the pose of the 'base_link' frame in this new frame. Now I don't know how to publish this Pose (which is an identity 4x4 TF matrix) so that the robot starts at that position in the next run. I would be thankful for any inputs on how to do this. My code:

def __init__(self):
    self.cmd_vel_pub = rospy.Publisher('cmd_vel',Twist,queue_size = 0.7)
    self.tf_pub = rospy.Publisher('transform', TransformStamped, queue_size = 0.5)
    self.tf2 = tf.TransformerROS(True, rospy.Duration(10.0))
    self.X_sent = []
    self.Y_sent = []
    self.T_sent = []
    self.Ang_sent = []
    self.X_meas = []
    self.Y_meas = []
    self.T_meas = []
    self.Ang_meas = []
    self.X_trans = []
    self.Y_trans = []
    #self.rand = random.random()

def run_command(self):
    sequence = [[1,0,0.05],[0,0,0],[0,0,0.1292],[1,0,0.1293],[0,0,0.1292],[1,0,0],[0,0,0],[-1,0,0],
                [0,0,-0.1292],[-1,0,-0.1],[0,0,-0.1292],[-1,0,0],[0,0,0]]
    for cmd in sequence:
        #rospy.loginfo('Publishing sequence')
        rospy.Rate(0.5).sleep()
        msg = Twist()
        msg.linear.x = cmd[0]
        msg.linear.y = cmd[1]
        msg.angular.z = cmd[2]
        t = rospy.get_rostime()
        self.X_sent.append(cmd[0])
        self.Y_sent.append(cmd[1])
        self.Ang_sent.append(cmd[2])
        self.T_sent.append(t.secs + t.nsecs * 1e-9)
        self.cmd_vel_pub.publish(msg)

    self.tf_buffer = tf2_ros.Buffer()
    self.tf2_listener = tf2_ros.TransformListener(self.tf_buffer)
    self.transform_odom_rec = self.tf_buffer.lookup_transform('odom','recentered',rospy.Time(0),rospy.Duration(5)) #from rec. to odom
    self.transform_bl_odom = self.tf_buffer.lookup_transform('base_link','odom',rospy.Time(0),rospy.Duration(5)) #from odom to base_link
    self.tf2_listener.transformPose('recentered', self.msg)
    ##transform matrix from odometry to recentered frame.
    trans_or_x = self.transform_odom_rec.transform.translation.x
    trans_or_y = self.transform_odom_rec.transform.translation.y
    trans_or_z = self.transform_odom_rec.transform.translation.z
    trans_or = [trans_or_x,trans_or_y,trans_or_z]
    #print(trans_or)
    rot_or_x = self.transform_odom_rec.transform.rotation.x
    rot_or_y = self.transform_odom_rec.transform.rotation.y
    rot_or_z = self.transform_odom_rec.transform.rotation.z
    rot_or_w = self.transform_odom_rec.transform.rotation.w
    quat_or = [rot_or_x,rot_or_y,rot_or_z,rot_or_w]
    q_or = tf.transformations.euler_from_quaternion(quat_or, axes='sxyz')
    tf_or = tf.transformations.euler_matrix(q_or[0],q_or[1],q_or[2], axes='sxyz')
    tf_or[:,3] = [trans_or_x,trans_or_y,trans_or_z, 1.0]
    #print(tf_or)

    ##transform matrix from base_link to odom frame
    trans_bo_x = self.transform_bl_odom.transform.translation.x
    trans_bo_y = self.transform_bl_odom.transform.translation ...