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 'baselink' 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 'baselink' 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.y
    trans_bo_z = self.transform_bl_odom.transform.translation.z
    trans_bo = [trans_bo_x,trans_bo_y,trans_bo_z]
    rot_bo_x = self.transform_bl_odom.transform.rotation.x
    rot_bo_y = self.transform_bl_odom.transform.rotation.y
    rot_bo_z = self.transform_bl_odom.transform.rotation.z
    rot_bo_w = self.transform_bl_odom.transform.rotation.w
    quat_bo = [rot_bo_x,rot_bo_y,rot_bo_z,rot_bo_w]
    q_bo = tf.transformations.euler_from_quaternion(quat_bo, axes='rxyz')
    tf_bo = tf.transformations.euler_matrix(q_bo[0],q_bo[1],q_bo[2], axes='rxyz')
    tf_bo[:,3] = [trans_bo_x,trans_bo_y,trans_bo_z, 1.0]
    #print(tf_bo)
    ##transform matrix from base_link to recentered frame
    self.tf_br = np.matmul(tf_or, tf_bo)
    #print(tf_br)
    tp = self.tf_br[:,3]
    q_br = tf.transformations.euler_from_matrix(self.tf_br,'rxyz')
    quat_br = tf.transformations.quaternion_from_euler(q_br[0], q_br[1],q_br[2], 'ryxz')
    #print(quat_br)
    self.trans_br_x = tp[0]
    self.trans_br_y = tp[1]
    self.trans_br_z = tp[2]
    trans_br = [self.trans_br_x,self.trans_br_y,self.trans_br_z]
    #print(trans_br)
    self.rot_br_x = quat_br[0]
    self.rot_br_y = quat_br[1]
    self.rot_br_z = quat_br[2]
    self.rot_br_w = quat_br[3]

    self.x_odom = self.trans_br_x
    self.x_odom = self.trans_br_y
    self.ang_odom = self.rot_br_z


    #transform = self.tf_buffer.lookup_transform('recentered','odom',rospy.Time(0),rospy.Duration(1.0))
    #pose_transformed = tf2_geometry_msgs.do_transform_pose(self.msg, transform)
    #publish pose con el msg(TransformStamped)

    rospy.signal_shutdown('Command finished')

def callback_odom(self,msg):
    self.msg = msg
    #splitting informations from received msgs
    self.frame_id = msg.header.frame_id
    self.x_odom = msg.pose.pose.position.x
    self.y_odom = msg.pose.pose.position.y
    self.ang_odom = msg.pose.pose.orientation.z
    self.time = msg.header.stamp.secs + msg.header.stamp.nsecs * 1e-9
    #appending odom msgs to the lists
    self.X_meas.append(self.x_odom)
    self.Y_meas.append(self.y_odom)
    self.T_meas.append(self.time)
    self.Ang_meas.append(self.ang_odom)
    #print(self.x_odom)

def ros_init(self):
    rospy.init_node('pepper_cmd_evaluator',anonymous = True)
    rospy.Subscriber('pepper_robot/odom',Odometry,self.callback_odom)