def getBotTransAsDrcPosition3d(pose_in): pose = position_3d_t() translation = vector_3d_t() translation.x =pose_in.trans_vec[0] translation.y =pose_in.trans_vec[1] translation.z =pose_in.trans_vec[2] rotation = quaternion_t() rotation.w = pose_in.rot_quat[0] rotation.x = pose_in.rot_quat[1] rotation.y = pose_in.rot_quat[2] rotation.z = pose_in.rot_quat[3] pose.translation = translation pose.rotation = rotation return pose
def getPoseMsg(): pose = position_3d_t() translation = vector_3d_t() translation.x =0 translation.y =0 translation.z =0 rotation = quaternion_t() rotation.w = 1 rotation.x = 0 rotation.y = 0 rotation.z = 0 pose.translation = translation pose.rotation = rotation return pose
