def main(nh, entrance='x', exit='1'):
boat = yield boat_scripting.get_boat(nh)
try:
boat.switch_path_planner('a_star_rpp')
while True:
gates = yield boat.get_gates()
if len(gates) == 0:
print 'No gates detected'
continue
current_boat_pos = boat.odom.position
yaw = quat_to_rotvec(boat.odom.orientation)[2] % np.pi
# Zip up numpy positions in enu - boat_pos
gate_pos = [
xyz_array(g.position) - current_boat_pos for g in gates
]
gates = zip(gate_pos, gates)
# Filter out yaw
gate = None
if len(gates) > 0:
gate = min(gates, key=lambda x: np.linalg.norm(x[0]))
else:
gate = gates[0]
#print gate
# Translate back to enu
gate_pos = gate[0] + current_boat_pos
gate_orientation = gate[1].yaw + np.pi / 2
if abs(gate_orientation - yaw) < np.pi / 2:
yield move_on_line.main(nh, gate_pos,
np.array([0, 0, gate_orientation]))
else:
yield move_on_line.main(nh,
gate_pos,
np.array([0, 0, gate_orientation]),
flip=True)
finally:
boat.default_state()
def main(nh, entrance='x', exit='1'):
boat = yield boat_scripting.get_boat(nh)
try:
boat.switch_path_planner('a_star_rpp')
while True:
gates = yield boat.get_gates()
if len(gates) == 0:
print 'No gates detected'
continue
current_boat_pos = boat.odom.position
yaw = quat_to_rotvec(boat.odom.orientation)[2] % np.pi
# Zip up numpy positions in enu - boat_pos
gate_pos = [xyz_array(g.position) - current_boat_pos for g in gates]
gates = zip(gate_pos, gates)
# Filter out yaw
gate = None
if len(gates) > 0:
gate = min(gates, key = lambda x: np.linalg.norm(x[0]))
else:
gate = gates[0]
#print gate
# Translate back to enu
gate_pos = gate[0] + current_boat_pos
gate_orientation = gate[1].yaw + np.pi/2
if abs(gate_orientation - yaw) < np.pi / 2:
yield move_on_line.main(nh, gate_pos, np.array([0, 0, gate_orientation]))
else:
yield move_on_line.main(nh, gate_pos, np.array([0, 0, gate_orientation]), flip=True)
finally:
boat.default_state()
def main(nh):
# #Print mission start msg
#print 'Finding start gate with laser'
boat = yield boat_scripting.get_boat(nh)
try:
yaw = quat_to_rotvec(boat.odom.orientation)[2]
#print 'Pan the lidar between the maximum angle and slightly above horizontal'
boat.pan_lidar(max_angle=3.264, min_angle=3.15, freq=0.5)
# How many gates we've gone through
gates_passed = 0
have_gate = False
last_gate_pos = None
move = None
while gates_passed < 2:
#print 'Get gates'
gates = yield boat.get_gates()
(gate_pos, gate_orientation) = (None, None)
if gates_passed == 0:
(gate_pos,
gate_orientation) = filter_gates(boat, gates, yaw, False)
else:
(gate_pos,
gate_orientation) = filter_gates(boat, gates, yaw, True)
# Check if valid gate found
if gate_pos is not None:
have_gate = True
# Check if we previously found a gate
if last_gate_pos is not None:
# Check if the gate center has drifted
# Don't go to a different gate (dis < 5)
dis = numpy.linalg.norm(last_gate_pos - gate_pos)
#print 'Distance: ', dis
if dis >= 0.1 and dis < 3:
print 'Gate drifted re-placing goal point'
#move = boat.move.set_position(gate_pos).set_orientation(gate_orientation).go()
if gates_passed == 0:
move = move_on_line.main(nh, gate_pos,
gate_orientation,
-GATE_DISTANCE)
else:
move = move_on_line.main(nh, gate_pos,
gate_orientation,
GATE_DISTANCE)
else:
#print 'Still happy with my current goal'
pass
else:
# Just found a gate
print 'Moving to gate ' + str(gates_passed + 1)
#move = boat.move.set_position(gate_pos).set_orientation(gate_orientation).go()
if gates_passed == 0:
#yield boat.move.yaw_left_deg(30).go()
move = move_on_line.main(nh, gate_pos,
gate_orientation,
-GATE_DISTANCE)
print 'zzz'
yield util.sleep(1.0)
else:
move = move_on_line.main(nh, gate_pos,
gate_orientation,
GATE_DISTANCE)
# Set last gate pos
last_gate_pos = gate_pos
else:
if have_gate is False:
print 'No gate found moving forward 1m'
yield boat.move.forward(1).go()
else:
print 'Lost sight of gate; Continuing to last known position'
# Check if task complete
if have_gate and move.called:
print 'Move complete'
#yield boat.move.forward(3).go()
yaw = quat_to_rotvec(boat.odom.orientation)[2]
have_gate = False
last_gate_pos = None
gates_passed = gates_passed + 1
finally:
boat.default_state()
def main(nh):
# #Print mission start msg
#print 'Finding start gate with laser'
boat = yield boat_scripting.get_boat(nh)
try:
yaw = quat_to_rotvec(boat.odom.orientation)[2]
#print 'Pan the lidar between the maximum angle and slightly above horizontal'
boat.pan_lidar(max_angle=3.264, min_angle=3.15, freq=0.5)
# How many gates we've gone through
gates_passed = 0
have_gate = False
last_gate_pos = None
move = None
while gates_passed < 2:
#print 'Get gates'
gates = yield boat.get_gates()
(gate_pos, gate_orientation) = (None, None)
if gates_passed == 0:
(gate_pos, gate_orientation) = filter_gates(boat, gates, yaw, False)
else:
(gate_pos, gate_orientation) = filter_gates(boat, gates, yaw, True)
# Check if valid gate found
if gate_pos is not None:
have_gate = True
# Check if we previously found a gate
if last_gate_pos is not None:
# Check if the gate center has drifted
# Don't go to a different gate (dis < 5)
dis = numpy.linalg.norm(last_gate_pos - gate_pos)
#print 'Distance: ', dis
if dis >= 0.1 and dis < 3:
print 'Gate drifted re-placing goal point'
#move = boat.move.set_position(gate_pos).set_orientation(gate_orientation).go()
if gates_passed == 0:
move = move_on_line.main(nh, gate_pos, gate_orientation, -GATE_DISTANCE)
else:
move = move_on_line.main(nh, gate_pos, gate_orientation, GATE_DISTANCE)
else:
#print 'Still happy with my current goal'
pass
else:
# Just found a gate
print 'Moving to gate ' + str(gates_passed + 1)
#move = boat.move.set_position(gate_pos).set_orientation(gate_orientation).go()
if gates_passed == 0:
#yield boat.move.yaw_left_deg(30).go()
move = move_on_line.main(nh, gate_pos, gate_orientation, -GATE_DISTANCE)
print 'zzz'
yield util.sleep(1.0)
else:
move = move_on_line.main(nh, gate_pos, gate_orientation, GATE_DISTANCE)
# Set last gate pos
last_gate_pos = gate_pos
else:
if have_gate is False:
print 'No gate found moving forward 1m'
yield boat.move.forward(1).go()
else:
print 'Lost sight of gate; Continuing to last known position'
# Check if task complete
if have_gate and move.called:
print 'Move complete'
#yield boat.move.forward(3).go()
yaw = quat_to_rotvec(boat.odom.orientation)[2]
have_gate = False
last_gate_pos = None
gates_passed = gates_passed + 1
finally:
boat.default_state()
