odom_conversions.py 文件源码

def numpy_to_odom(arr, frame_id=None):
    """
    Takes in a numpy array [x,y,theta] and a frame ID and converts it to an
    Odometry message. 

    WARN: May require a timestamp.
    """
    odom = Odometry()
    if frame_id is not None: odom.header.frame_id = frame_id
    odom.pose.pose.position.x = arr[0]
    odom.pose.pose.position.y = arr[1]
    odom.pose.pose.orientation = Quaternion(*tr.quaternion_from_euler(arr[2], 0, 0, 'szyx'))
    return odom