odom_conversions.py 文件源码

def odom_add_offset(odom, offset):
    """
    Takes in two odometry messages and returns a new odometry message that has
    the two added together.

    WARN: Both messages should be in the same frame!
    """
    new_odom = copy.deepcopy(odom)
    new_odom.pose.pose.position.x += offset.pose.pose.position.x
    new_odom.pose.pose.position.y += offset.pose.pose.position.y
    quat = new_odom.pose.pose.orientation
    quat_offset = offset.pose.pose.orientation
    quat_arr = np.array([quat.x, quat.y, quat.z, quat.w])
    offset_arr = np.array([quat_offset.x, quat_offset.y, quat_offset.z, quat_offset.w])
    theta = tr.euler_from_quaternion(quat_arr, 'sxyz')[2]
    theta_offset = tr.euler_from_quaternion(offset_arr, 'sxyz')[2]
    new_odom.pose.pose.orientation = Quaternion(*tr.quaternion_from_euler(theta+theta_offset, 0, 0, 'szyx'))
    return new_odom