def main(nh, point, direction, offset = 0.0, flip = False):
boat = yield boat_scripting.get_boat(nh)
print 'Direction:', direction * 180 / np.pi
yaw = direction[2]
print 'Yaw:', yaw * 180 / np.pi
print 'dx:', np.cos(yaw) * offset
print 'dy:', np.sin(yaw) * offset
print 'Point:', point
point = point + [np.cos(yaw) * offset, np.sin(yaw) * offset, 0]
if flip:
direction[2] = (direction[2] + np.pi) % (2*np.pi)
yield boat.move.set_position(point).set_orientation(rotvec_to_quat(direction)).go()
def quaternion_from_rotvec(rot):
return quaternion_from_xyzw_array(rotvec_to_quat(rot))
def quaternion_from_rotvec(rot): return quaternion_from_xyzw_array(rotvec_to_quat(rot))