conv_tools.py 文件源码

def dict_to_list_euler(pose_dict):

    """
    Convert a pose dictionary to a list in the order x, y, z,
    orientation x, orientation y, orientation z.
    """

    qtn = Quaternion()
    qtn.x = pose_dict['orientation'].x
    qtn.y = pose_dict['orientation'].y
    qtn.z = pose_dict['orientation'].z
    qtn.w = pose_dict['orientation'].w
    elr_x, elr_y, elr_z, = tf.transformations.euler_from_quaternion(
        [qtn.x,qtn.y,qtn.z,qtn.w])

    lst = []
    lst.append(pose_dict['position'].x)
    lst.append(pose_dict['position'].y)
    lst.append(pose_dict['position'].z)
    lst.append(elr_x)
    lst.append(elr_y)
    lst.append(elr_z)
    return lst