draw_utils.py 文件源码

def geom_pose_from_rt(rt): 
    msg = geom_msg.Pose()
    msg.position = geom_msg.Point(x=rt.tvec[0], y=rt.tvec[1], z=rt.tvec[2])
    xyzw = rt.quat.to_xyzw()
    msg.orientation = geom_msg.Quaternion(x=xyzw[0], y=xyzw[1], z=xyzw[2], w=xyzw[3])
    return msg