example_simple_client.py 文件源码

def create_pose(location, rotation):
    if len(location) >= 3:
        x, y, z = location[0:3]
    else:
        x = y = z = 0
    if len(rotation) >= 4:
        qw, qx, qy, qz = rotation[0:4]
    else:
        qw = 1
        qx = qy = qz = 0

    return geom_msgs.Pose(
        position=geom_msgs.Point(x=x, y=y, z=z),
        orientation=geom_msgs.Quaternion(w=qw, x=qx, y=qy, z=qz)
    )