Ask Your Question

Revision history [back]

You need a tf frame that the correcting node has control over (nothing else is trying to broadcast it), and insert it in your tf tree between the incorrect frame and the robot or other frame you are correcting. That's how odom is set up. If there is no correction available the node ought to regularly broadcast a translation of (0,0,0) and tf.transformations.quaternion_from_matrix(numpy.identity(4)) or similar using 0,0,0 for roll-pitch-yaw for the quaternion, or the last good correction if there is one.

    br = tf.TransformBroadcaster()
    br.sendTransform(
        (pose.position.x, pose.position.y, pose.position.z),
        (pose.orientation.x, pose.orientation.y, pose.orientation.z, pose.orientation.w)
        rospy.Time.now(),  
        child_frame,    # maybe base_link
        parent_frame    # equivalent of odom
        )

You don't need to store the correction as a Pose unless you are using that as a convenient container or want to publish it as a topic also. geometry_msgs/Transform is more technically correct then even though the contents are the same.

You need a tf frame that the correcting node has control over (nothing else is trying to broadcast it), and insert it in your tf tree between the incorrect frame and the robot or other frame you are correcting. That's how odom is set up. If there is no correction available the node ought to regularly broadcast a translation of (0,0,0) and tf.transformations.quaternion_from_matrix(numpy.identity(4)) or similar using 0,0,0 for roll-pitch-yaw for the quaternion, or the last good correction if there is one.

    br = tf.TransformBroadcaster()
    br.sendTransform(
        (pose.position.x, pose.position.y, pose.position.z),
        (pose.orientation.x, pose.orientation.y, pose.orientation.z, pose.orientation.w)
        rospy.Time.now(),  
        child_frame,    # maybe base_link
        parent_frame    # equivalent of odom
        )

You don't need to store the correction as a Pose unless you are using that as a convenient container or want to publish it as a topic also. geometry_msgs/Transform is more technically correct then even though the contents are the same.

You need a tf frame that the correcting node has control over (nothing else is trying to broadcast it), and insert it in your tf tree between the incorrect frame and the robot or other frame you are correcting. That's how odom is set up. If there is no correction available the node ought to regularly broadcast a translation of (0,0,0) and tf.transformations.quaternion_from_matrix(numpy.identity(4)) or similar using 0,0,0 for roll-pitch-yaw for the quaternion, or the last good correction if there is one.

    br = tf.TransformBroadcaster()
    br.sendTransform(
        (pose.position.x, pose.position.y, pose.position.z),
        (pose.orientation.x, pose.orientation.y, pose.orientation.z, pose.orientation.w)
        rospy.Time.now(),  
        child_frame,    # maybe base_link
        parent_frame    # equivalent of odom
        )

You don't need to store the correction as a Pose unless you are using that as a convenient container or want to publish it as a topic also. geometry_msgs/Transform is more technically correct even though the contents are the same.

If your Pose actually is relative to the higher level in the tf tree, and not a correction, then you have to pre-multiply it by the inverse of incorrect frame to turn it into a correction, and convert it to a numpy 4x4 matrix and then get it back into a translation + quaternion tuple format using tf.transformations for use by sendTransform.

You need a tf frame that the correcting node has control over (nothing else is trying to broadcast it), and insert it in your tf tree between the incorrect frame and the robot or other frame you are correcting. That's how odom is set up. If there is no correction available the node ought to regularly broadcast a translation of (0,0,0) and tf.transformations.quaternion_from_matrix(numpy.identity(4)) or similar using 0,0,0 for roll-pitch-yaw for the quaternion, or the last good correction if there is one.

    br = tf.TransformBroadcaster()
    br.sendTransform(
        (pose.position.x, pose.position.y, pose.position.z),
        (pose.orientation.x, pose.orientation.y, pose.orientation.z, pose.orientation.w)
        rospy.Time.now(),  
        child_frame,    # maybe base_link
        parent_frame    # equivalent of odom
        )

You don't need to store the correction as a Pose unless you are using that as a convenient container or want to publish it as a topic also. geometry_msgs/Transform is more technically correct even though the contents are the same.

If your Pose actually is relative to the higher level in the tf tree, and not a correction, then you have to pre-multiply it by the inverse of incorrect frame to turn it into a correction, and convert it to a numpy 4x4 matrix and then get it back into a translation + quaternion tuple format using tf.transformations for use by sendTransform.

    # numpy arrays to 4x4 transform matrix 
    trans_mat = tf.transformations.translation_matrix(trans)
    rot_mat = tf.transformations.quaternion_matrix(rot)
    mat = numpy.dot(trans_mat, rot_mat)




    rot2 = tf.transformations.quaternion_from_matrix(mat)
    trans2 = tf.transformations.translation_from_matrix(mat)

You need a tf frame that the correcting node has control over (nothing else is trying to broadcast it), and insert it in your tf tree between the incorrect frame and the robot or other frame you are correcting. That's how odom is set up. If there is no correction available the node ought to regularly broadcast a translation of (0,0,0) and tf.transformations.quaternion_from_matrix(numpy.identity(4)) or similar using 0,0,0 for roll-pitch-yaw for the quaternion, or the last good correction if there is one.

    br = tf.TransformBroadcaster()
    br.sendTransform(
        (pose.position.x, pose.position.y, pose.position.z),
        (pose.orientation.x, pose.orientation.y, pose.orientation.z, pose.orientation.w)
        rospy.Time.now(),  
        child_frame,    # maybe base_link
        parent_frame    # equivalent of odom
        )

You don't need to store the correction as a Pose unless you are using that as a convenient container or want to publish it as a topic also. geometry_msgs/Transform is more technically correct even though the contents are the same.

If your Pose actually is relative to the higher level in the tf tree, and not a correction, then you have to pre-multiply it by the inverse of incorrect frame to turn it into a correction, and convert it to a numpy 4x4 matrix and then get it back into a translation + quaternion tuple format using tf.transformations for use by sendTransform.

    # numpy arrays to 4x4 transform matrix 
    trans_mat = tf.transformations.translation_matrix(trans)
    rot_mat = tf.transformations.quaternion_matrix(rot)
    mat = numpy.dot(trans_mat, rot_mat)



     # gi back to quaternion and 3x1 arrays
    rot2 = tf.transformations.quaternion_from_matrix(mat)
    trans2 = tf.transformations.translation_from_matrix(mat)

You need a tf frame that the correcting node has control over (nothing else is trying to broadcast it), and insert it in your tf tree between the incorrect frame and the robot or other frame you are correcting. That's how odom is set up. If there is no correction available the node ought to regularly broadcast a translation of (0,0,0) and tf.transformations.quaternion_from_matrix(numpy.identity(4)) or similar using 0,0,0 for roll-pitch-yaw for the quaternion, or the last good correction if there is one.

    br = tf.TransformBroadcaster()
    br.sendTransform(
        (pose.position.x, pose.position.y, pose.position.z),
        (pose.orientation.x, pose.orientation.y, pose.orientation.z, pose.orientation.w)
        rospy.Time.now(),  
        child_frame,    # maybe base_link
        parent_frame    # equivalent of odom
        )

You don't need to store the correction as a Pose unless you are using that as a convenient container or want to publish it as a topic also. geometry_msgs/Transform is more technically correct even though the contents are the same.

If your Pose actually is relative to the higher level in the tf tree, and not a correction, then you have to pre-multiply it by the inverse of incorrect frame to turn it into a correction, and convert it to a numpy 4x4 matrix and then get it back into a translation + quaternion tuple format using tf.transformations for use by sendTransform.

    # numpy arrays to 4x4 transform matrix 
    trans_mat = tf.transformations.translation_matrix(trans)
    rot_mat = tf.transformations.quaternion_matrix(rot)
    mat = numpy.dot(trans_mat, rot_mat)

     ... do something with numpy.linalg.pinv() ...

    # gi go back to quaternion and 3x1 arrays
    rot2 = tf.transformations.quaternion_from_matrix(mat)
    trans2 = tf.transformations.translation_from_matrix(mat)

One route you can take to get the correction transform is to broadcast the correct transform in parallel, then ask tf for the transform from odometry to correct and broadcast that as the tf from odometry to base_link:

                               parent
                             /       \
 correct (but low frequency?)      odom (low quality but hi Hz)
                                       \
                                   base_link

I think there is one oddity here where the correct frame is only getting updated infrequently, and if odom -> to base_link is also low frequency then no one else will be able to get parent -> base_link at the desired higher rate of parent -> odom. So the publisher of odom -> base_link has to know when odom -> correct is fresh and only look it up then but rebroadcast that same tf at a higher rate until a new correct arrives.

You need a tf frame that the correcting node has control over (nothing else is trying to broadcast it), and insert it in your tf tree between the incorrect frame and the robot or other frame you are correcting. That's how odom is set up. If there is no correction available the node ought to regularly broadcast a translation of (0,0,0) and tf.transformations.quaternion_from_matrix(numpy.identity(4)) or similar using 0,0,0 for roll-pitch-yaw for the quaternion, or the last good correction if there is one.

    br = tf.TransformBroadcaster()
    br.sendTransform(
        (pose.position.x, pose.position.y, pose.position.z),
        (pose.orientation.x, pose.orientation.y, pose.orientation.z, pose.orientation.w)
        rospy.Time.now(),  
        child_frame,    # maybe base_link
        parent_frame    # equivalent of odom
        )

You don't need to store the correction as a Pose unless you are using that as a convenient container or want to publish it as a topic also. geometry_msgs/Transform is more technically correct even though the contents are the same.

If your Pose actually is relative to the higher level in the tf tree, and not a correction, then you have to pre-multiply it by the inverse of incorrect frame to turn it into a correction, and convert it to a numpy 4x4 matrix and then get it back into a translation + quaternion tuple format using tf.transformations for use by sendTransform.

    # numpy arrays to 4x4 transform matrix 
    trans_mat = tf.transformations.translation_matrix(trans)
    rot_mat = tf.transformations.quaternion_matrix(rot)
    # create a 4x4 matrix
    mat = numpy.dot(trans_mat, rot_mat)

    ... 
    # do something with numpy.linalg.pinv() ...
numpy.linalg.pinv(mat) (can't do a simple transpose with a full 4x4 to get the inverse as can be done with a 3x3 rotation matrix)

    # go back to quaternion and 3x1 arrays
    rot2 = tf.transformations.quaternion_from_matrix(mat)
    trans2 = tf.transformations.translation_from_matrix(mat)

One route you can take to get the correction transform is to broadcast the correct transform in parallel, then ask tf for the transform from odometry to correct and broadcast that as the tf from odometry to base_link:

                               parent
                             /       \
 correct (but low frequency?)      odom (low quality but hi Hz)
                                       \
                                   base_link

I think there is one oddity here where the correct frame is only getting updated infrequently, and if odom -> to base_link is also low frequency then no one else will be able to get parent -> base_link at the desired higher rate of parent -> odom. So the publisher of odom -> base_link has to know when odom -> correct is fresh and only look it up then but rebroadcast that same tf at a higher rate until a new correct arrives.