def main(nh):
boat = yield boat_scripting.get_boat(nh)
print 'Next'
yield boat.move.forward(2).go()
print 'Next'
yield boat.move.forward(2).go()
print 'Next0'
yield boat.move.forward(2).go()
print 'Next'
yield boat.move.forward(2).go()
print 'Next'
yield boat.move.forward(2).go()
print 'Next'
yield boat.move.forward(-2).go()
print 'Next'
yield boat.move.forward(-2).go()
print 'Next0'
yield boat.move.forward(-2).go()
print 'Next'
yield boat.move.forward(-2).go()
print 'Next'
yield boat.move.forward(-2).go()
def main(nh):
boat = yield boat_scripting.get_boat(nh)
for i in xrange(4):
print 'Side', i
yield boat.move.forward(10).go()
yield boat.move.turn_left_deg(90).go()
def main(nh, course=course_alice, min_freq=27E3, max_freq=40E3):
# nh = Node Handler from txros
# course = Set of GPS points
# freq = 27440 Remnant from previous mission file. I don't really know why this is here.
boat = yield boat_scripting.get_boat(nh)
try:
print "Beginning ping mission"
print "Deploying hydrophone..."
yield boat.deploy_hydrophone()
print "Hydrophone has been deployed"
try:
yield util.wrap_timeout(hydrophone_align.main(nh, min_freq, max_freq), 60*2)
except Exception:
traceback.print_exc()
print "Ping mission has finished"
#temp = gps.latlongheight_from_ecef(boat.odom.position)
#print "latitude: ", temp[0], " longitude: ", temp[1]
# In the main mission state machine remember to rerun the retract hydrophone mission
# in case this mission timesout.
print "Retracting hydrophone..."
yield boat.retract_hydrophone()
print "Hydrophone retracted"
print "Done"
finally:
boat.default_state()
def main(nh):
boat = yield boat_scripting.get_boat(nh)
for i in xrange(4):
print 'Side', i
yield boat.move.forward(2).go()
yield boat.move.turn_left_deg(90).go()
def main(nh):
boat = yield boat_scripting.get_boat(nh)
for TARGET in ['triangle', 'circle', 'cruciform'][::-1]:
yield boat.visual_approach('forward', 'boat_dock/' + TARGET, 24*.0254, 3)
yield boat.move.forward(1.4).go(speed=.15)
yield boat.move.backward(4).go()
def main(nh, ci, course, freq):
freq=27440
boat = yield boat_scripting.get_boat(nh)
#float_df = boat.float()
#yield boat.retract_hydrophone() # why was this here? it shouldn't need this, the hydrophones should be up on start
print "Starting ping mission"
print "Deploying Hydrophone"
yield boat.deploy_hydrophone()
print "Hydrophone Deployed"
try:
yield util.wrap_timeout(boat.hydrophone_align(freq), 60*2)
except Exception:
traceback.print_exc()
print "Finished ping mission"
# Note: /absodom is Earth-Centered,Earth-Fixed (ECEF), so This means that ECEF rotates with the earth and a point fixed on the surface of the earth do not change.
# See: http://en.wikipedia.org/wiki/ECEF
msg = yield boat.get_gps_odom()
temp = gps.latlongheight_from_ecef([msg.pose.pose.position.x,msg.pose.pose.position.y,msg.pose.pose.position.z])
print "latitude: ", temp[0]," longitude: ", temp[1]
_send_result(ci, course, math.degrees(temp[0]), math.degrees(temp[1])) # async XXX
#float_df.cancel()
print "Retracting Hydrophone"
yield boat.retract_hydrophone()
print "Hydrophone Retracted"
print "Done"
def main(nh):
boat = yield boat_scripting.get_boat(nh)
while True:
yield util.sleep(.1)
time_left_str = yield util.nonblocking_raw_input('Enter time left: (e.g. 5:40) ')
try:
m, s = time_left_str.split(':')
time_left = 60 * int(m) + int(s)
except Exception:
traceback.print_exc()
else:
end_time = time.time() + time_left
del time_left
break
print
print 'WAIT 10 SECONDS'
print
while True:
yield util.sleep(.1)
course = yield util.nonblocking_raw_input('Course: (A or B) ')
if course in ['A', 'B', 'pool']:
break
try:
yield util.wrap_timeout(main_list(nh, boat, course), max(0, end_time - time.time()))
except Exception:
traceback.print_exc()
yield fail_list(nh, boat)
def main(nh, x):
print 'aa'
boat = yield boat_scripting.get_boat(nh)
print 'bb'
start_pose = boat.move
targetdesc.mesh = from_obj(roslib.packages.resource_file('boat_sim', 'models', x + '.obj'))
targetdesc.prior_distribution.pose.orientation = Quaternion(*boat.pose.orientation)
print 'a'
fwd_move = boat.move.go(linear=[0.3, 0, 0])
try:
yield boat.visual_approach_3d('forward', 4, targetdesc)
finally:
yield fwd_move.cancel()
yield boat.move.forward(1.5).go()
fwd_task = boat.move.forward(100).go(speed=.2)
try:
yield boat.wait_for_bump()
finally:
fwd_task.cancel()
print 'b'
yield start_pose.backward(0).go()
def main(nh):
boat = yield boat_scripting.get_boat(nh)
while True:
yield util.sleep(.1)
time_left_str = yield util.nonblocking_raw_input(
'Enter time left: (e.g. 5:40) ')
try:
m, s = time_left_str.split(':')
time_left = 60 * int(m) + int(s)
except Exception:
traceback.print_exc()
else:
end_time = time.time() + time_left
del time_left
break
print
print 'WAIT 10 SECONDS'
print
while True:
yield util.sleep(.1)
course = yield util.nonblocking_raw_input('Course: (A or B) ')
if course in ['A', 'B', 'pool']:
break
try:
yield util.wrap_timeout(main_list(nh, boat, course),
max(0, end_time - time.time()))
except Exception:
traceback.print_exc()
yield fail_list(nh, boat)
def main(nh):
print 'aa'
boat = yield boat_scripting.get_boat(nh)
buoy_targetdesc.prior_distribution.pose.orientation = Quaternion(*boat.pose.orientation)
print 'a'
yield boat.visual_approach_3d('forward', 1.5, buoy_targetdesc)
print 'b'
def main(nh):
boat = yield boat_scripting.get_boat(nh)
while True:
print 'Forward 10'
yield boat.move.forward(10).go()
print 'Turn around'
yield boat.move.turn_left_deg(180).go()
def main(nh):
boat = yield boat_scripting.get_boat(nh)
while True:
print 'Forward 10'
yield boat.move.forward(10).go()
print 'Turn around'
yield boat.move.turn_left_deg(180).go()
def main(nh):
boat = yield boat_scripting.get_boat(nh)
for i in xrange(4):
print 'Step Number #', i
yield boat.move.forward(2).go()
yield boat.move.turn_right_deg(90).go()
yield boat.move.forward(2).go()
yield boat.move.turn_left_deg(90).go()
def main(nh):
print 'aa'
boat = yield boat_scripting.get_boat(nh)
buoy_targetdesc.prior_distribution.pose.orientation = Quaternion(
*boat.pose.orientation)
print 'a'
yield boat.visual_approach_3d('forward', 1.5, buoy_targetdesc)
print 'b'
def main(nh):
boat = yield boat_scripting.get_boat(nh)
yield boat.go_to_ecef_pos(ll(29.641090, -82.361292))
yield boat.go_to_ecef_pos(ll(29.641260, -82.361197))
yield boat.go_to_ecef_pos(ll(29.641394, -82.361293))
yield boat.go_to_ecef_pos(ll(29.641651, -82.361444))
yield boat.go_to_ecef_pos(ll(29.641245, -82.361014))
yield boat.go_to_ecef_pos(ll(29.641360, -82.361104))
yield boat.go_to_ecef_pos(ll(29.641104, -82.361470))
yield boat.go_to_ecef_pos(ll(29.641274, -82.361403))
def main(nh):
boat = yield boat_scripting.get_boat(nh)
yield boat.go_to_ecef_pos(ll(29.641090, -82.361292))
yield boat.go_to_ecef_pos(ll(29.641260, -82.361197))
yield boat.go_to_ecef_pos(ll(29.641394, -82.361293))
yield boat.go_to_ecef_pos(ll(29.641651, -82.361444))
yield boat.go_to_ecef_pos(ll(29.641245, -82.361014))
yield boat.go_to_ecef_pos(ll(29.641360, -82.361104))
yield boat.go_to_ecef_pos(ll(29.641104, -82.361470))
yield boat.go_to_ecef_pos(ll(29.641274, -82.361403))
def main(nh, pos, speed=0, turn=True):
print 'go_to_ecef_pos: ', pos, "\n\tAt: ", speed, 'm/s\n\t', 'Turn first: ', turn
boat = yield boat_scripting.get_boat(nh)
success = False
try:
first = True
last_enu_pos = numpy.zeros(3)
while True:
odom_df = boat.get_gps_odom_rel()
abs_msg = yield boat.get_gps_odom()
msg = yield odom_df
error_ecef = pos - orientation_helpers.xyz_array(abs_msg.pose.pose.position)
error_enu = gps.enu_from_ecef(ecef_v=error_ecef, ecef_pos=orientation_helpers.xyz_array(abs_msg.pose.pose.position))
error_enu[2] = 0
#print error_enu, '=>', numpy.linalg.norm(error_enu), '->', 1
enu_pos = orientation_helpers.xyz_array(msg.pose.pose.position) + error_enu
enu_pos[2] = boat.pose.position[2]
if numpy.linalg.norm(error_enu) < 1:
yield boat.move.set_position(enu_pos).go()
print 'go_to_ecef_pos', 'succeeded'
success = True
return
# Turn and send initial move incase enu_pos is within half a metter of [0,0,0]
if first and turn:
if turn:
yield boat.move.look_at_without_pitching(enu_pos).go(speed=speed)
boat._moveto_action_client.send_goal(
boat.pose.set_position(enu_pos).as_MoveToGoal(speed=speed)).forget()
first = False
# Check if the position drifted more than half a meter radius
# if so send a new goal
if numpy.linalg.norm(last_enu_pos - enu_pos) > 0.5:
print 'Set new position'
boat._moveto_action_client.send_goal(
boat.pose.set_position(enu_pos).as_MoveToGoal(speed=speed)).forget()
last_enu_pos = enu_pos
success = True
finally:
if not success:
print 'go_to_ecef_pos failed to attain desired position, holding current position'
yield boat.hold_at_current_pos()
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 main(nh):
boat = yield boat_scripting.get_boat(nh)
print "Starting ping mission"
boat.deploy_hydrophone()
print "Deploying Hydrophone"
boat.hydrophone_align(boat.get_hydrophone_freq())
print "Finished ping mission"
# Note: /absodom is Earth-Centered,Earth-Fixed (ECEF), so This means that ECEF rotates with the earth and a point fixed on the surface of the earth do not change.
# See: http://en.wikipedia.org/wiki/ECEF
msg=boat.get_gps_odom()
temp=latlongheight_from_ecef(msg.pose.pose.position.x,msg.pose.pose.position.y,msg.pose.pose.position.z)
print "latitude: ", temp[0]," longitude: ", temp[1]
boat.retract_hydrophone()
print "Retracting Hydrophone"
def main(nh):
boat = yield boat_scripting.get_boat(nh)
print "Starting ping mission"
boat.deploy_hydrophone()
print "Deploying Hydrophone"
boat.hydrophone_align(boat.get_hydrophone_freq())
print "Finished ping mission"
# Note: /absodom is Earth-Centered,Earth-Fixed (ECEF), so This means that ECEF rotates with the earth and a point fixed on the surface of the earth do not change.
# See: http://en.wikipedia.org/wiki/ECEF
msg = boat.get_gps_odom()
temp = latlongheight_from_ecef(msg.pose.pose.position.x,
msg.pose.pose.position.y,
msg.pose.pose.position.z)
print "latitude: ", temp[0], " longitude: ", temp[1]
boat.retract_hydrophone()
print "Retracting Hydrophone"
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, location='lake_alice'):
boat = yield boat_scripting.get_boat(nh)
boat.set_current_challenge('test')
try:
while True:
print 'Float 1 sec'
boat.float_on()
yield util.sleep(1)
print 'Spin'
boat.float_off()
print 'O pose: ', boat.odom.position
print 'O orien: ', boat.odom.orientation
print 'P pose: ', boat.pose.position
print 'P orien: ', boat.pose.orientation
#yield boat.move.set_position(boat.odom.position).set_orientation(boat.odom.orientation).go()
#yield boat.move.turn_left_deg(90).go()
finally:
boat.default_state()
def main(nh, boat=None):
print "Q"
if boat is None:
boat = yield boat_scripting.get_boat(nh)
while True:
print 'find gates'
angle = yield boat.get_start_gate_vision()
print "raw angle", angle
distance = 0
#yield boat.move.as_MoveToGoal([3,0,0],angle).go()
# ADDED BBY ZACH -- Measures the distance the boat has gone forward to tell when done
# Breaks and returns true so that we know we actually got done.
# Not a great implimentation but works for now
# Change depending on how far apart the bouys are
if distance > 30:
print "Exiting Gates"
break
if abs(angle) < (numpy.pi/12):
print "forward"
yield boat.move.forward(6).go()
yield util.sleep(0.5)
print "forward command sent"
distance += 6
elif angle < 0:
print "turn_left: ", angle/5
yield boat.move.turn_left(abs(angle/5)).go()
yield util.sleep(0.5)
elif angle > 0:
print "turn_right: ", angle/5
yield boat.move.turn_right(angle/5).go()
yield util.sleep(0.5)
'''
def main(nh, min_freq, max_freq):
boat = yield boat_scripting.get_boat(nh)
while True:
ping_return = None
while ping_return is None:
try:
print 'Float'
boat.float_on()
yield util.sleep(.5)
print 'Listen'
ping_return = yield util.wrap_timeout(
boat.get_processed_ping((min_freq, max_freq)), 20)
except Exception:
# Timeout rotate 30 deg
print 'Timeout no ping rotating 30 degrees left'
boat.float_off()
#yield boat.move.set_position(boat.odom.position).set_orientation(boat.odom.orientation)
yield boat.move.yaw_left_deg(30).go()
print 'Rotated 30 degrees'
boat.float_off()
# Changed message type to handle this. Conversion is carried out in the hydrophones script in
# software_common/hydrophones/scripts
print 'Ping is: ', ping_return
print 'I say heading is: ', ping_return.heading
print 'I say declination is: ', ping_return.declination
if ping_return.declination > 1.2:
good += 1
if good > 4:
print 'Success!'
return
elif abs(ping_return.heading) > math.radians(30):
good = 0
print 'Turn to ping'
yield boat.move.yaw_left(ping_return.heading).go()
else:
good = 0
print 'Move towards ping'
yield boat.move.forward(2).go()
def main(nh):
boat = yield boat_scripting.get_boat(nh)
@util.cancellableInlineCallbacks
def go(x):
while True:
print 'a'
scan = yield boat.get_scan()
best = None
angles = numpy.linspace(scan.angle_min, scan.angle_max,
len(scan.ranges))
pos = lambda i: numpy.array(
[math.cos(angles[i]),
math.sin(angles[i]), 0]) * scan.ranges[i]
for i in xrange(len(scan.ranges) // 4, len(scan.ranges) * 3 // 4):
if not (scan.range_min <= scan.ranges[i] <= scan.range_max):
continue
for j in xrange(i + 1, len(scan.ranges) * 3 // 4):
if not (scan.range_min <= scan.ranges[j] <=
scan.range_max):
continue
a = (pos(i) + pos(j)) / 2
vec = pos(j) - pos(i)
score = 1e6 - a[0]
if abs(vec[1]) / numpy.linalg.norm(vec) < .5: score = 0
if numpy.linalg.norm(vec) < 1.5: score = 0
if numpy.linalg.norm(vec) > 7: score = 0
if a[0] < abs(a[1]) * 1.5: score = 0
if best is None or score > best_score:
best = a
best_score = score
print best
df = boat.move.relative(best).forward(x).go()
if numpy.linalg.norm(x) < 3.5:
yield df
return
yield go(0)
yield go(+3)
def main(nh, min_freq, max_freq):
boat = yield boat_scripting.get_boat(nh)
while True:
ping_return = None
while ping_return is None:
try:
print 'Float'
boat.float_on()
yield util.sleep(.5)
print 'Listen'
ping_return = yield util.wrap_timeout(boat.get_processed_ping((min_freq, max_freq)), 20)
except Exception:
# Timeout rotate 30 deg
print 'Timeout no ping rotating 30 degrees left'
boat.float_off()
#yield boat.move.set_position(boat.odom.position).set_orientation(boat.odom.orientation)
yield boat.move.yaw_left_deg(30).go()
print 'Rotated 30 degrees'
boat.float_off()
# Changed message type to handle this. Conversion is carried out in the hydrophones script in
# software_common/hydrophones/scripts
print 'Ping is: ', ping_return
print 'I say heading is: ', ping_return.heading
print 'I say declination is: ', ping_return.declination
if ping_return.declination > 1.2:
good += 1
if good > 4:
print 'Success!'
return
elif abs(ping_return.heading) > math.radians(30):
good = 0
print 'Turn to ping'
yield boat.move.yaw_left(ping_return.heading).go()
else:
good = 0
print 'Move towards ping'
yield boat.move.forward(2).go()
def do_dock(nh, shape): boat = yield boat_scripting.get_boat(nh) if shape == 'circle': try: print "Docking --- circle" yield util.wrap_timeout(find_shape.main(nh, 'circle'), CIRCLE_TIME) except Exception: print "Could not find circle, moving to next shape" if shape == 'triangle': try: print "Docking --- triangle" yield util.wrap_timeout(find_shape.main(nh, 'triangle'), TRIANGLE_TIME) except Exception: print "Could not find triangle, moving to next shape" if shape == 'cross': try: print "Docking --- cross" yield util.wrap_timeout(find_shape.main(nh, 'cross'), CROSS_TIME) except Exception: print "Could not find cross, moving to next shape"
def main(nh, boat=None):
print "Q"
if boat is None:
boat = yield boat_scripting.get_boat(nh)
while True:
print 'find gates'
angle = yield boat.get_start_gate_vision()
print "raw angle", angle
distance = 0
#yield boat.move.as_MoveToGoal([3,0,0],angle).go()
# ADDED BBY ZACH -- Measures the distance the boat has gone forward to tell when done
# Breaks and returns true so that we know we actually got done.
# Not a great implimentation but works for now
# Change depending on how far apart the bouys are
if distance > 30:
print "Exiting Gates"
break
if abs(angle) < (numpy.pi / 12):
print "forward"
yield boat.move.forward(6).go()
yield util.sleep(0.5)
print "forward command sent"
distance += 6
elif angle < 0:
print "turn_left: ", angle / 5
yield boat.move.turn_left(abs(angle / 5)).go()
yield util.sleep(0.5)
elif angle > 0:
print "turn_right: ", angle / 5
yield boat.move.turn_right(angle / 5).go()
yield util.sleep(0.5)
'''
def do_dock(nh, shape):
boat = yield boat_scripting.get_boat(nh)
if shape == 'circle':
try:
print "Docking --- circle"
yield util.wrap_timeout(find_shape.main(nh, 'circle'), CIRCLE_TIME)
except Exception:
print "Could not find circle, moving to next shape"
if shape == 'triangle':
try:
print "Docking --- triangle"
yield util.wrap_timeout(find_shape.main(nh, 'triangle'),
TRIANGLE_TIME)
except Exception:
print "Could not find triangle, moving to next shape"
if shape == 'cross':
try:
print "Docking --- cross"
yield util.wrap_timeout(find_shape.main(nh, 'cross'), CROSS_TIME)
except Exception:
print "Could not find cross, moving to next shape"
def main(nh):
boat = yield boat_scripting.get_boat(nh)
@util.cancellableInlineCallbacks
def go(x):
while True:
print 'a'
scan = yield boat.get_scan()
best = None
angles = numpy.linspace(scan.angle_min, scan.angle_max, len(scan.ranges))
pos = lambda i: numpy.array([math.cos(angles[i]), math.sin(angles[i]), 0]) * scan.ranges[i]
for i in xrange(len(scan.ranges)//4, len(scan.ranges)*3//4):
if not (scan.range_min <= scan.ranges[i] <= scan.range_max):
continue
for j in xrange(i+1, len(scan.ranges)*3//4):
if not (scan.range_min <= scan.ranges[j] <= scan.range_max):
continue
a = (pos(i) + pos(j))/2
vec = pos(j) - pos(i)
score = 1e6-a[0]
if abs(vec[1])/numpy.linalg.norm(vec) < .5: score = 0
if numpy.linalg.norm(vec) < 1.5: score = 0
if numpy.linalg.norm(vec) > 7: score = 0
if a[0] < abs(a[1])*1.5: score = 0
if best is None or score > best_score:
best = a
best_score = score
print best
df = boat.move.relative(best).forward(x).go()
if numpy.linalg.norm(x) < 3.5:
yield df
return
yield go(0)
yield go(+3)
def main(nh):
boat = yield boat_scripting.get_boat(nh)
print 'Next'
yield boat.move.forward(2).go()
print 'Next'
yield boat.move.forward(2).go()
print 'Next0'
yield boat.move.forward(2).go()
print 'Next'
yield boat.move.forward(2).go()
print 'Next'
yield boat.move.forward(2).go()
print 'Next'
yield boat.move.forward(-2).go()
print 'Next'
yield boat.move.forward(-2).go()
print 'Next0'
yield boat.move.forward(-2).go()
print 'Next'
yield boat.move.forward(-2).go()
print 'Next'
yield boat.move.forward(-2).go()
def main(nh):
boat = yield boat_scripting.get_boat(nh)
yield boat.hold_at_current_pos()
def main(nh):
global course
global obstical_info
global docking_info
global images_info
global sent_image
# Grab interfaces for boat and JSON server
boat = yield boat_scripting.get_boat(nh)
s = yield json_server_proxy.get_server(nh)
# Grab mission input such as JSON server ip, and course
ip_port = raw_input('Enter ip:port (ex. 10.0.2.1:8080): ')
course = raw_input('Enter course with corect spelling! (courseA, courseB, ...): ')
# Check that the course is in the dictionaries
assert course in DOCK.keys(), '%s is not in %s' % (course, DOCK.keys())
shape1 = None
shape2 = None
color1 = None
color2 = None
boat.default_state()
# Giant try finally to make sure boat ends run and returns to its default state
try:
# Main mission code
# JSON initilization
# TODO: if any failures start over
# IP - http://10.0.2.1:8080
url_was_set = (yield s.interact('http://'+ip_port, course)).was_set
assert url_was_set, 'Failed to set URL to ' + 'http://'+ip_port + ' on course ' + course
# Set the current challange
set_challenge = (yield s.set_current_challenge('gates')).was_set
print "Url and course were set succesfully"
# end run before start just in case
end_run = yield s.end_run()
run_started = (yield s.start_run()).success
assert run_started, 'Run failed to start'
print "Run started succesfully"
##------------------------------- Grab all JSON data --------------------------------------------------
print 'Starting mass JSON download'
obstical_info = s.get_gate_info()
docking_info = s.get_dock_info()
images_info = s.get_server_images()
##-------------------------------- GATES ---------------------------------------------------------------
try:
yield util.wrap_timeout(start_gates(nh, boat, s), START_GATE_TIME)
print 'succesfully'
except Exception as e:
print 'Could not complete start gates: ' + str(e)
finally:
print 'Finally start gate'
#boat.default_state()
##-------------------------------- OBS COURSE ------------------------------------------------------------
print 'obstical'
try:
yield util.wrap_timeout(obstical_course(nh, boat, s), OBS_COURSE_TIME)
print 'util'
except:
print 'Could not complete obstacle course'
finally: pass
#boat.default_state()
##-------------------------------- DOCKING ---------------------------------------------------------------
try:
yield util.wrap_timeout(docking(nh, boat, s), DOCK_TIME)
except:
print 'Could not complete docking'
finally: pass
#boat.default_state()
##-------------------------------- QUAD ---------------------------------------------------------------
try:
yield util.wrap_timeout(interoperability(nh, boat, s), NTROP_TIME)
except:
print 'Could not complete interoperability'
finally: pass
#boat.default_state()
##-------------------------------- PINGER ---------------------------------------------------------------
try:
yield util.wrap_timeout(pinger(nh, boat, s), HYDRO_TIME)
except:
print 'Could not complete pinger'
finally: pass
#boat.default_state()
##-------------------------------- RETURN ---------------------------------------------------------------
print "Run complete, coming back to the dock"
s.set_current_challenge('return')
"""
if course is 'courseA':
print "Moving to safe point to avoid fountain"
yield util.wrap_timeout(go_to_ecef_pos.main(nh, SAFE_POINT_1[course]), ECEF_TIME)
"""
print "Moving to zero point to get home"
try:
yield util.wrap_timeout(go_to_ecef_pos.main(nh, HOME_0[course]), ECEF_TIME)
except:
print 'ECEF timeout'
print "Moving to first point to get home"
try:
yield util.wrap_timeout(go_to_ecef_pos.main(nh, HOME_1[course]), ECEF_TIME)
except:
print 'ECEF timeout'
print "Moving to second point to get home"
try:
yield util.wrap_timeout(go_to_ecef_pos.main(nh, HOME_2[course]), ECEF_TIME)
except:
print 'ECEF timeout'
print "Moving to third point to get home"
try:
yield util.wrap_timeout(go_to_ecef_pos.main(nh, HOME_3[course]), ECEF_TIME)
except:
print 'ECEF timeout'
print "Adjusting heading"
yield boat.move.heading(HOME_3_HEADING[course]).go()
##------------------------------ CLEAN UP -----------------------------------------------------
print 'Returned to dock'
# Make sure quad copter image sent before we end the mission
yield sent_image
print 'Eneded run succesfully! Go Gators!'
s.end_run()
finally:
# We do not yield here because if something hangs we still want everything else to complete
print 'Finally: Ending run and returning to default state'
s.end_run()
boat.default_state()
def main(nh):
boat = yield boat_scripting.get_boat(nh)
boat_x = boat.odom.position[0]
boat_y = boat.odom.position[1]
closest = None
summation = 100
closest_buoy = 0
buoys = yield boat.get_bouys()
while closest is None:
while len(buoys.buoys) <= 0:
buoys = yield boat.get_bouys()
for i in buoys.buoys:
x_dif = boat_x - i.position.x
y_dif = boat_y - i.position.y
temp_sum = math.sqrt(x_dif ** 2 + y_dif ** 2)
if temp_sum > 10:
# Don't go more than 10 meters
continue
#print temp_sum
if temp_sum < summation:
summation = temp_sum
closest = i
x_move = boat_x - closest.position.x
y_move = boat_y - closest.position.y
point = numpy.array([closest.position.x, closest.position.y, 0])
position = boat.odom.position[0:2]
yaw = quat_to_rotvec(boat.odom.orientation)[2]
heading = numpy.array([numpy.cos(yaw), numpy.sin(yaw)])
vec2 = numpy.array([heading[0], heading[1]])
vec1 = numpy.array([closest.position.x, closest.position.y])
numerator = numpy.dot(vec1, vec2)
one = numpy.linalg.norm(vec1)
two = numpy.linalg.norm(vec2)
cosine = numerator / (one * two)
# Angle betwen the boat and the shore
angle_to_move = math.atan(cosine)
print angle_to_move
boat.move.yaw_left(-angle_to_move).go()
yield boat.move.set_position(point).go()
yield boat.move.turn_left_deg(-20).go()
for i in xrange(4):
yield boat.move.forward(3).go()
yield boat.move.turn_left_deg(90).go()
def main(nh):
boat = yield boat_scripting.get_boat(nh, False, False)
yield boat.deploy_hydrophone()
def main(nh):
boat = yield boat_scripting.get_boat(nh, False)
yield boat.retract_hydrophone()
def main(nh, camera, object_name, distance_estimate, selector=lambda items, body_tf: items[0], turn=True):
boat = yield boat_scripting.get_boat(nh)
goal_mgr = self._camera_2d_action_clients[camera].send_goal(legacy_vision_msg.FindGoal(
object_names=[object_name],
))
start_pose = self.pose
start_map_transform = tf.Transform(
start_pose.position, start_pose.orientation)
try:
while True:
feedback = yield goal_mgr.get_feedback()
res = map(json.loads, feedback.targetreses[0].object_results)
try:
transform = yield self._tf_listener.get_transform('/base_link',
feedback.header.frame_id, feedback.header.stamp)
map_transform = yield self._tf_listener.get_transform('/enu',
'/base_link', feedback.header.stamp)
except Exception:
traceback.print_exc()
continue
if not res: continue
obj = selector(res, transform)
if obj is None: continue
ray_start_camera = numpy.array([0, 0, 0])
ray_dir_camera = numpy.array(map(float, obj['center']))
obj_dir_camera = numpy.array(map(float, obj['direction']))
ray_start_world = map_transform.transform_point(
transform.transform_point(ray_start_camera))
ray_dir_world = map_transform.transform_vector(
transform.transform_vector(ray_dir_camera))
obj_dir_world = map_transform.transform_vector(
transform.transform_vector(obj_dir_camera))
axis_camera = [0, 0, 1]
axis_body = transform.transform_vector(axis_camera)
axis_world = start_map_transform.transform_vector(axis_body)
# project ray onto plane defined by distance_estimate
# from start_pose.position along axis_world
plane_point = distance_estimate * axis_world + start_pose.position
plane_vector = axis_world
x = plane_vector.dot(ray_start_world - plane_point) / plane_vector.dot(ray_dir_world)
object_pos = ray_start_world - ray_dir_world * x
desired_pos = object_pos - start_map_transform.transform_vector(transform.transform_point(ray_start_camera))
desired_pos = desired_pos - axis_world * axis_world.dot(desired_pos - start_pose.position)
error_pos = desired_pos - map_transform.transform_point([0, 0, 0])
error_pos = error_pos - axis_world * axis_world.dot(error_pos)
error_pos[2] = 0
print desired_pos, numpy.linalg.norm(error_pos)/3e-2
if numpy.linalg.norm(error_pos) < 3e-2: # 3 cm
if turn:
direction_symmetry = int(obj['direction_symmetry'])
dangle = 2*math.pi/direction_symmetry
def rotate(x, angle):
return transformations.rotation_matrix(angle, axis_world)[:3, :3].dot(x)
for sign in [-1, +1]:
while rotate(obj_dir_world, sign*dangle).dot(start_pose.forward_vector) > obj_dir_world.dot(start_pose.forward_vector):
obj_dir_world = rotate(obj_dir_world, sign*dangle)
yield (self.move
.set_position(desired_pos)
.look_at_rel_without_pitching(obj_dir_world)
.go())
else:
yield (self.move
.set_position(desired_pos)
.go())
return
# go towards desired position
self._moveto_action_client.send_goal(
start_pose.set_position(desired_pos).as_MoveToGoal(speed=0.1)).forget()
finally:
goal_mgr.cancel()
yield self.move.go() # stop moving
def main(nh):
assert TOTAL_TIME > CIRCLE_TIME + CROSS_TIME + TRIANGLE_TIME + (ECEF_TIME * 4) + START_GATE_TIME
'''
Test Main Mission:
1. Go to point A and turn North, then begin speed gate challenge - One minute timeout
2. Do speedgates, then go to point F and turn South - One minute timeout
3. Go to position E to begin docking challenges - One minute each
4. When done with docking move back to point A and turn north
SPEED ONLY SET TO ONE TO START
'''
boat = yield boat_scripting.get_boat(nh)
################## Start server like this ########################
s = yield json_server_proxy.get_server(nh)
url_was_set = (yield s.interact('http://ec2-52-7-253-202.compute-1.amazonaws.com:80','openTest')).was_set
if url_was_set:
print "Url and course were set succesfully"
challenge_was_set = (yield s.set_current_challenge('gates')).was_set
if challenge_was_set:
print "Challenge was set succesfully"
run_started = (yield s.start_run()).success
if run_started:
print "Run started succesfully"
##################################################################
############## Get gate info #####################################
#request that the gate info be retrieved
gate_info = s.get_gate_info()
############### Get the docking bay info like so ##################
#request that the dock info be retrieved
dock_info = s.get_dock_info()
#################################################################
################# Get server images #############################
print "Downloading images..."
#requests that the images be dowloaded
images_info = s.get_server_images()
################################################################
print "Starting main mission"
print "Moving to first position: ", str(WAYPOINT_A)
try:
yield util.wrap_timeout(go_to_ecef_pos.main(nh, WAYPOINT_A, SPEED), ECEF_TIME)
yield boat.move.heading(NORTH).go()
print "Arrived at first position"
except Exception:
print "Could not make it to first position in " + str(ECEF_TIME) + " seconds"
finally:
boat.default_state()
print "Starting speed gates now"
try:
# Set the current challenge to gates
s.set_current_challenge('gates')
##################### Recover gate info #######################
gate_info = yield gate_info
entrance_gate = gate_info.entrance
exit_gate = gate_info.exit
print "Gate info: " + entrance_gate +" "+ exit_gate
##############################################################
start_gate = yield util.wrap_timeout(start_gate_laser.main(nh), ONE_MINUTE)
print "Startgates completed succesfully!"
except Exception:
print "Could not complete stargates in" + str(START_GATE_TIME) + " seconds"
finally:
boat.default_state()
print "Moving to safe position now: ", str(WAYPOINT_F)
try:
yield util.wrap_timeout(go_to_ecef_pos.main(nh, WAYPOINT_F, SPEED), ONE_MINUTE)
yield boat.move.heading(EAST).go()
print "Moved to " + str(WAYPOINT_F) + ", now moving to " + str(WAYPOINT_E) + " to begin docking"
except Exception:
print "Could not make it to second position in " + str(ONE_MINUTE) + " seconds"
finally:
boat.default_state()
try:
yield util.wrap_timeout(go_to_ecef_pos.main(nh, WAYPOINT_E, SPEED), ONE_MINUTE)
yield boat.move.heading(EAST).go()
print "Moved to " + str(WAYPOINT_E) + ", starting docking challenge"
except Exception:
print "Could not make it to third position in " + str(ONE_MINUTE) + " seconds"
finally:
boat.default_state()
try:
# Set the current challenge to docking
s.set_current_challenge('docking')
######################## Recover docking info ###################
dock_info = yield dock_info
dock_one_symbol = dock_info.first_dock_symbol
dock_one_color = dock_info.first_dock_color
dock_two_symbol = dock_info.second_dock_symbol
dock_two_color = dock_info.second_dock_color
print "Dock info: " + dock_one_symbol +" "+ dock_one_color +" "+ dock_two_symbol +" "+ dock_two_color
#################################################################
yield do_dock(nh, 'circle')
except Exception:
pass
finally:
boat.default_state()
try:
s.set_current_challenge('docking')
yield do_dock(nh, 'triangle')
except Exception:
pass
finally:
boat.default_state()
try:
yield s.set_current_challenge('docking')
yield do_dock(nh, 'cross')
except Exception:
pass
finally:
boat.default_state()
print "Docking complete"
print "Moving back to startate begining position", str(WAYPOINT_A)
# Set current challenge to interop
s.set_current_challenge('interop')
################ Recover image path and image count #############
images_info = yield images_info
images_path = images_info.file_path
images_count = images_info.image_count
print "Images info: " + images_path +" "+ str(images_count)
################################################################
############### Report an image to te server ####################
print "Sending image to server..."
#The paramenters would be the information regarding the identified image
send_image = yield s.send_image_info('1.jpg','ONE')
#Only yield the server response right away if you need to work with the respose
#right away. This might take some time if the server is slow and the mission will
#halt at this point until you get a server response
print "Is it the right image?: " + str(send_image.is_right_image)
#################################################################
############## Report the pinger buoy to the server #############
#Set challenge to pinger
s.set_current_challenge('pinger')
print "Sending buoy color to server..."
#The paramenter is the identified color
send_buoy = yield s.send_buoy_info('yellow')
#Only yield the server response right away if you need to work with the respose
#right away. This might take some time if the server is slow and the mission will
#halt at this point until you get a server response
print "Is this the right buoy?: " + str(send_buoy.is_right_buoy)
#################################################################
try:
s.set_current_challenge('return')
yield util.wrap_timeout(go_to_ecef_pos.main(nh, WAYPOINT_A, SPEED), ONE_MINUTE)
yield boat.move.heading(NORTH).go()
print "Arrived at ", str(WAYPOINT_A)
except Exception:
print "Could not make it to third position in " + str(ONE_MINUTE) + " seconds"
finally:
######### Make sure the run is ended with the server ###############
print "Ending run"
yield s.end_run()
####################################################################
boat.default_state()
def main(nh):
assert TOTAL_TIME > CIRCLE_TIME + CROSS_TIME + TRIANGLE_TIME + (
ECEF_TIME * 4) + START_GATE_TIME
'''
Test Main Mission:
1. Go to point A and turn North, then begin speed gate challenge - One minute timeout
2. Do speedgates, then go to point F and turn South - One minute timeout
3. Go to position E to begin docking challenges - One minute each
4. When done with docking move back to point A and turn north
SPEED ONLY SET TO ONE TO START
'''
boat = yield boat_scripting.get_boat(nh)
################## Start server like this ########################
s = yield json_server_proxy.get_server(nh)
url_was_set = (yield s.interact(
'http://ec2-52-7-253-202.compute-1.amazonaws.com:80',
'openTest')).was_set
if url_was_set:
print "Url and course were set succesfully"
challenge_was_set = (yield s.set_current_challenge('gates')).was_set
if challenge_was_set:
print "Challenge was set succesfully"
run_started = (yield s.start_run()).success
if run_started:
print "Run started succesfully"
##################################################################
############## Get gate info #####################################
#request that the gate info be retrieved
gate_info = s.get_gate_info()
############### Get the docking bay info like so ##################
#request that the dock info be retrieved
dock_info = s.get_dock_info()
#################################################################
################# Get server images #############################
print "Downloading images..."
#requests that the images be dowloaded
images_info = s.get_server_images()
################################################################
print "Starting main mission"
print "Moving to first position: ", str(WAYPOINT_A)
try:
yield util.wrap_timeout(go_to_ecef_pos.main(nh, WAYPOINT_A, SPEED),
ECEF_TIME)
yield boat.move.heading(NORTH).go()
print "Arrived at first position"
except Exception:
print "Could not make it to first position in " + str(
ECEF_TIME) + " seconds"
finally:
boat.default_state()
print "Starting speed gates now"
try:
# Set the current challenge to gates
s.set_current_challenge('gates')
##################### Recover gate info #######################
gate_info = yield gate_info
entrance_gate = gate_info.entrance
exit_gate = gate_info.exit
print "Gate info: " + entrance_gate + " " + exit_gate
##############################################################
start_gate = yield util.wrap_timeout(start_gate_laser.main(nh),
ONE_MINUTE)
print "Startgates completed succesfully!"
except Exception:
print "Could not complete stargates in" + str(
START_GATE_TIME) + " seconds"
finally:
boat.default_state()
print "Moving to safe position now: ", str(WAYPOINT_F)
try:
yield util.wrap_timeout(go_to_ecef_pos.main(nh, WAYPOINT_F, SPEED),
ONE_MINUTE)
yield boat.move.heading(EAST).go()
print "Moved to " + str(WAYPOINT_F) + ", now moving to " + str(
WAYPOINT_E) + " to begin docking"
except Exception:
print "Could not make it to second position in " + str(
ONE_MINUTE) + " seconds"
finally:
boat.default_state()
try:
yield util.wrap_timeout(go_to_ecef_pos.main(nh, WAYPOINT_E, SPEED),
ONE_MINUTE)
yield boat.move.heading(EAST).go()
print "Moved to " + str(WAYPOINT_E) + ", starting docking challenge"
except Exception:
print "Could not make it to third position in " + str(
ONE_MINUTE) + " seconds"
finally:
boat.default_state()
try:
# Set the current challenge to docking
s.set_current_challenge('docking')
######################## Recover docking info ###################
dock_info = yield dock_info
dock_one_symbol = dock_info.first_dock_symbol
dock_one_color = dock_info.first_dock_color
dock_two_symbol = dock_info.second_dock_symbol
dock_two_color = dock_info.second_dock_color
print "Dock info: " + dock_one_symbol + " " + dock_one_color + " " + dock_two_symbol + " " + dock_two_color
#################################################################
yield do_dock(nh, 'circle')
except Exception:
pass
finally:
boat.default_state()
try:
s.set_current_challenge('docking')
yield do_dock(nh, 'triangle')
except Exception:
pass
finally:
boat.default_state()
try:
yield s.set_current_challenge('docking')
yield do_dock(nh, 'cross')
except Exception:
pass
finally:
boat.default_state()
print "Docking complete"
print "Moving back to startate begining position", str(WAYPOINT_A)
# Set current challenge to interop
s.set_current_challenge('interop')
################ Recover image path and image count #############
images_info = yield images_info
images_path = images_info.file_path
images_count = images_info.image_count
print "Images info: " + images_path + " " + str(images_count)
################################################################
############### Report an image to te server ####################
print "Sending image to server..."
#The paramenters would be the information regarding the identified image
send_image = yield s.send_image_info('1.jpg', 'ONE')
#Only yield the server response right away if you need to work with the respose
#right away. This might take some time if the server is slow and the mission will
#halt at this point until you get a server response
print "Is it the right image?: " + str(send_image.is_right_image)
#################################################################
############## Report the pinger buoy to the server #############
#Set challenge to pinger
s.set_current_challenge('pinger')
print "Sending buoy color to server..."
#The paramenter is the identified color
send_buoy = yield s.send_buoy_info('yellow')
#Only yield the server response right away if you need to work with the respose
#right away. This might take some time if the server is slow and the mission will
#halt at this point until you get a server response
print "Is this the right buoy?: " + str(send_buoy.is_right_buoy)
#################################################################
try:
s.set_current_challenge('return')
yield util.wrap_timeout(go_to_ecef_pos.main(nh, WAYPOINT_A, SPEED),
ONE_MINUTE)
yield boat.move.heading(NORTH).go()
print "Arrived at ", str(WAYPOINT_A)
except Exception:
print "Could not make it to third position in " + str(
ONE_MINUTE) + " seconds"
finally:
######### Make sure the run is ended with the server ###############
print "Ending run"
yield s.end_run()
####################################################################
boat.default_state()
def main(nh):
boat = yield boat_scripting.get_boat(nh)
yield boat.hold_at_current_pos()
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):
global course
global obstical_info
global docking_info
global images_info
global sent_image
# Grab interfaces for boat and JSON server
boat = yield boat_scripting.get_boat(nh)
s = yield json_server_proxy.get_server(nh)
# Grab mission input such as JSON server ip, and course
ip_port = raw_input('Enter ip:port (ex. 10.0.2.1:8080): ')
course = raw_input(
'Enter course with corect spelling! (courseA, courseB, ...): ')
# Check that the course is in the dictionaries
assert course in DOCK.keys(), '%s is not in %s' % (course, DOCK.keys())
shape1 = None
shape2 = None
color1 = None
color2 = None
boat.default_state()
# Giant try finally to make sure boat ends run and returns to its default state
try:
# Main mission code
# JSON initilization
# TODO: if any failures start over
# IP - http://10.0.2.1:8080
url_was_set = (yield s.interact('http://' + ip_port, course)).was_set
assert url_was_set, 'Failed to set URL to ' + 'http://' + ip_port + ' on course ' + course
# Set the current challange
set_challenge = (yield s.set_current_challenge('gates')).was_set
print "Url and course were set succesfully"
# end run before start just in case
end_run = yield s.end_run()
run_started = (yield s.start_run()).success
assert run_started, 'Run failed to start'
print "Run started succesfully"
##------------------------------- Grab all JSON data --------------------------------------------------
print 'Starting mass JSON download'
obstical_info = s.get_gate_info()
docking_info = s.get_dock_info()
images_info = s.get_server_images()
##-------------------------------- GATES ---------------------------------------------------------------
try:
yield util.wrap_timeout(start_gates(nh, boat, s), START_GATE_TIME)
print 'succesfully'
except Exception as e:
print 'Could not complete start gates: ' + str(e)
finally:
print 'Finally start gate'
#boat.default_state()
##-------------------------------- OBS COURSE ------------------------------------------------------------
print 'obstical'
try:
yield util.wrap_timeout(obstical_course(nh, boat, s),
OBS_COURSE_TIME)
print 'util'
except:
print 'Could not complete obstacle course'
finally:
pass
#boat.default_state()
##-------------------------------- DOCKING ---------------------------------------------------------------
try:
yield util.wrap_timeout(docking(nh, boat, s), DOCK_TIME)
except:
print 'Could not complete docking'
finally:
pass
#boat.default_state()
##-------------------------------- QUAD ---------------------------------------------------------------
try:
yield util.wrap_timeout(interoperability(nh, boat, s), NTROP_TIME)
except:
print 'Could not complete interoperability'
finally:
pass
#boat.default_state()
##-------------------------------- PINGER ---------------------------------------------------------------
try:
yield util.wrap_timeout(pinger(nh, boat, s), HYDRO_TIME)
except:
print 'Could not complete pinger'
finally:
pass
#boat.default_state()
##-------------------------------- RETURN ---------------------------------------------------------------
print "Run complete, coming back to the dock"
s.set_current_challenge('return')
"""
if course is 'courseA':
print "Moving to safe point to avoid fountain"
yield util.wrap_timeout(go_to_ecef_pos.main(nh, SAFE_POINT_1[course]), ECEF_TIME)
"""
print "Moving to zero point to get home"
try:
yield util.wrap_timeout(go_to_ecef_pos.main(nh, HOME_0[course]),
ECEF_TIME)
except:
print 'ECEF timeout'
print "Moving to first point to get home"
try:
yield util.wrap_timeout(go_to_ecef_pos.main(nh, HOME_1[course]),
ECEF_TIME)
except:
print 'ECEF timeout'
print "Moving to second point to get home"
try:
yield util.wrap_timeout(go_to_ecef_pos.main(nh, HOME_2[course]),
ECEF_TIME)
except:
print 'ECEF timeout'
print "Moving to third point to get home"
try:
yield util.wrap_timeout(go_to_ecef_pos.main(nh, HOME_3[course]),
ECEF_TIME)
except:
print 'ECEF timeout'
print "Adjusting heading"
yield boat.move.heading(HOME_3_HEADING[course]).go()
##------------------------------ CLEAN UP -----------------------------------------------------
print 'Returned to dock'
# Make sure quad copter image sent before we end the mission
yield sent_image
print 'Eneded run succesfully! Go Gators!'
s.end_run()
finally:
# We do not yield here because if something hangs we still want everything else to complete
print 'Finally: Ending run and returning to default state'
s.end_run()
boat.default_state()
def main(nh):
boat = yield boat_scripting.get_boat(nh)
for i in xrange(1):
print 'Side', i
yield boat.move.turn_left_deg(45).go()
def main(nh, shape=None, color=None):
if shape == None:
shape = DEFAULT_SHAPE
if color == None:
color = DEFAULT_COLOR
boat = yield boat_scripting.get_boat(nh, False, False)
reorient = True
# While the boat is still distant from the target
while True and not rospy.is_shutdown():
temp_distance = 0
avg_distance = 0
shortest_distance = 100
farthest_distance = 0
x_mean = 0
y_mean = 0
numerator = 0
denom = 0
try:
yield util.wrap_timeout(center_sign(boat, shape, color), 20)
print "Locked onto shape"
except Exception:
print "Could not dock find shape, moving on to dock"
finally:
pass
# Only reorients one time
'''
if reorient == True and REORIENT == True:
print "reorienting"
orientation = yield orient(boat)
sign_centered = yield center_sign(boat, shape, color)
reorient = False
'''
print "holding at current position"
print "Scanning lidar"
distances = yield boat.get_distance_from_object(.05)
avg_distance = distances[0] - BOAT_LENGTH_OFFSET
shortest_distance = distances[1] - BOAT_LENGTH_OFFSET
farthest_distance = distances[2] - BOAT_LENGTH_OFFSET
print "Average distance from target:", avg_distance
print "Shortest distance between boat and object:", shortest_distance
print "Farther distance between boat and object:", farthest_distance
final_move = shortest_distance / MOVE_OFFSET
if farthest_distance > 3.5:
#raw_input("Press enter to move forward" + str(final_move) + " meters")
# Print only if a move is commanded
print "Moving forward " + str(final_move) + " meters"
yield boat.move.forward(final_move).go()
if farthest_distance <= 1.5:
#raw_input("Press enter to move forward one meter")
# Print only if a move is commanded
print "Moving forward one meter"
boat.move.forward(1.5).go()
print "Moving back 5 meters"
yield util.sleep(5)
yield boat.move.forward(-5).go()
print 'Find shape success'
break
# delete variables to avoid any threading problems
del avg_distance, temp_distance, farthest_distance, shortest_distance
def main(nh):
boat = yield boat_scripting.get_boat(nh, False, False)
yield boat.deploy_hydrophone()
def main(nh, direction):
print 1
boat = yield boat_scripting.get_boat(nh)
print 2
assert direction in ['left', 'right']
@util.cancellableInlineCallbacks
def go():
print 'a'
last_best = None
while True:
scan = yield boat.get_scan()
pose = boat.odom
best = None
best2 = None
angles = numpy.linspace(scan.angle_min, scan.angle_max, len(scan.ranges))
pos = lambda i: scan.ranges[i]*numpy.array([math.cos(angles[i]), math.sin(angles[i]), 0])
for i in xrange(len(scan.ranges)//6, len(scan.ranges)*5//6):
if not (scan.range_min <= scan.ranges[i] <= scan.range_max):
continue
for j in xrange(i, len(scan.ranges)*5//6):
if not (scan.range_min <= scan.ranges[j] <= scan.range_max):
continue
center = (pos(i) + pos(j))/2
vec = pos(j) - pos(i)
if numpy.linalg.norm(vec) < 1 or numpy.linalg.norm(vec) > 5:
continue
bad = False
for k in xrange(int(i*.75+j*.25), int(i*.25+j*.75)):
if not (scan.range_min <= scan.ranges[k] <= scan.range_max):
continue
if numpy.linalg.norm(pos(k) - center) < 5:
bad = True
break
if bad: continue
score = center[1] if direction == 'left' else -center[1]
if best is None or score > best_score:
best = center
best_score = score
best2 = i, j, center, vec
print best, best2
print scan.ranges[best2[0]:best2[1]+3]
if best is not None:
last_best = pose.relative(best).position
if last_best is None:
df = boat.move.forward(5).go()
else:
df = boat.move.set_position(last_best).go()
if numpy.linalg.norm(last_best - boat.pose.position) < 5:
print 'waiting'
yield df
break
yield util.sleep(.1)
yield go()
yield boat.move.forward(2).go()
yield go()
yield boat.move.forward(2).go()
def main(nh):
while not rospy.is_shutdown():
boat = yield boat_scripting.get_boat(nh)
#boat.pan_lidar(min_angle=2.7, max_angle=3.14, freq=.75)
#boat.float_on()
#gate_viz_pub = rospy.Publisher('gates_viz', Marker, queue_size = 10)
#rospy.init_node("sdfhjhsdfjhgsdf")
angle_to_move = 0
hold = []
x_mean = 0
y_mean = 0
numerator = 0
denom = 0
pointcloud_base = []
# loops until lidar gets a good filtered lidar reading
while len(hold) <= 0:
pointcloud = yield boat.get_pointcloud()
yield util.sleep(.1) # sleep to avoid tooPast errors
pointcloud_base = yield boat.to_baselink(pointcloud)
yield util.sleep(.1) # sleep to avoid tooPast errors
pointcloud_enu = []
for p in pc2.read_points(pointcloud, field_names=("x", "y", "z"), skip_nans=False, uvs=[]):
pointcloud_enu.append((p[0], p[1], p[2]))
yield util.sleep(.1) # sleep to avoid tooPast errors
point_in_base = []
hold = []
# Filter lidar data to only data right in front of the boat
print "Filtering lidar data"
for i in range(0, len(pointcloud_base)):
temp = pointcloud_base[i]
if abs(temp[1]) < .3:
hold.append(pointcloud_enu[i])
for i in hold:
x_mean += i[0]
y_mean += i[1]
y_mean = y_mean/len(hold)
x_mean = x_mean/len(hold)
for i in hold:
numerator += (i[0] - x_mean)*(i[1] - y_mean)
denom += (i[0] - x_mean)*(i[0] - x_mean)
if denom == 0: denom = 1
m = numerator/denom
b = y_mean - (m * x_mean)
x1 = 1
x2 = 100
par_vect = m*1 + b
point2 = m*x2 + b
position = boat.odom.position[0:2]
yaw = quat_to_rotvec(boat.odom.orientation)[2]
heading = numpy.array([numpy.cos(yaw), numpy.sin(yaw)])
# Line we want to be orthagonal to
position1 = to_vector(x1, par_vect)
position3 = to_vector((heading[0]) + boat.odom.position[0], (heading[1]) + boat.odom.position[1])
position4 = to_vector((heading[0]+ .01) + boat.odom.position[0], (heading[1]+.1) + boat.odom.position[1])
#Vector to be parrallel to
vec1 = numpy.array([x1, par_vect, 0])
vec2 = ([0,0,0])
angle_to_move = 2
move = 0
angle_to_move = 1
#def find_angle():
# Calculte angle_to_move and position to move to get in front of object
position = boat.odom.position[0:2]
yaw = quat_to_rotvec(boat.odom.orientation)[2]
heading = numpy.array([numpy.cos(yaw), numpy.sin(yaw)])
vec2 = numpy.array([(heading[0]), (heading[1]), 0])
numerator = numpy.dot(vec1, vec2)
one = numpy.linalg.norm(vec1)
two = numpy.linalg.norm(vec2)
cosine = numerator / (one * two)
angle_to_move = math.acos(cosine)
#find_angle()
distances = yield boat.get_distance_from_object(.05)
theda_one = .9 - angle_to_move
L1 = distances[0]
distance_to_move = math.sin(theda_one) * L1
veccc = to_vector(x1,par_vect)
vecc = to_vector(x2, point2)
'''
m = Marker()
m.header = Header(frame_id = '/enu', stamp = rospy.Time.now())
m.ns = 'gates'
m.id = 1
m.type = Marker.LINE_STRIP
m.action = Marker.ADD
m.scale.x = 0.1
m.color.r = 0.5
m.color.b = 0.75
m.color.g = 0.1
m.color.a = 1.0
m.points.append(veccc)
m.points.append(vecc)
gate_viz_pub.publish(m)
'''
if vec2[0] < 0:
print "Yawing left ", angle_to_move
print "Moving right ", distance_to_move
raw_input("Press enter to continue...")
boat.move.turn_left(angle_to_move).go()
yield boat.move.left(-distance_to_move-2).go()
if vec2[0] > 0:
print "Yawing right ", angle_to_move
print "Moving left ", distance_to_move
raw_input("Press enter to continue...")
boat.move.turn_left(angle_to_move).go()
yield boat.move.left(distance_to_move-2).go()
def main(nh, camera, object_name, size_estimate, desired_distance, selector=lambda items, body_tf: items[0]):
boat = yield boat_scripting.get_boat(nh)
goal_mgr = boat._camera_2d_action_clients[camera].send_goal(legacy_vision_msg.FindGoal(
object_names=[object_name],
))
start_pose = boat.pose
start_map_transform = tf.Transform(
start_pose.position, start_pose.orientation)
try:
while True:
feedback = yield goal_mgr.get_feedback()
res = map(json.loads, feedback.targetreses[0].object_results)
try:
transform = yield boat._tf_listener.get_transform('/base_link',
feedback.header.frame_id, feedback.header.stamp)
map_transform = yield boat._tf_listener.get_transform('/enu',
'/base_link', feedback.header.stamp)
except Exception:
traceback.print_exc()
continue
if not res: continue
obj = selector(res, transform)
if obj is None: continue
ray_start_camera = numpy.array([0, 0, 0])
ray_dir_camera = numpy.array(map(float, obj['center']))
obj_dir_camera = numpy.array(map(float, obj['direction']))
ray_start_world = map_transform.transform_point(
transform.transform_point(ray_start_camera))
ray_dir_world = map_transform.transform_vector(
transform.transform_vector(ray_dir_camera))
obj_dir_world = map_transform.transform_vector(
transform.transform_vector(obj_dir_camera))
axis_camera = [0, 0, 1]
axis_body = transform.transform_vector(axis_camera)
axis_world = start_map_transform.transform_vector(axis_body)
distance_estimate = size_estimate / (2 * numpy.linalg.norm(obj_dir_camera))
print distance_estimate
object_pos = ray_start_world + ray_dir_world * distance_estimate
print object_pos
desired_pos = object_pos - axis_world * desired_distance
error_pos = desired_pos - map_transform.transform_point([0, 0, 0])
error_pos[2] = 0
print desired_pos, numpy.linalg.norm(error_pos)/6e-2
if numpy.linalg.norm(error_pos) < 6e-2: # 6 cm
yield (boat.move
.set_position(desired_pos)
.go())
return
# go towards desired position
boat._moveto_action_client.send_goal(
start_pose.set_position(desired_pos).as_MoveToGoal(speed=0.1)).forget()
finally:
goal_mgr.util.cancel()
yield boat.move.go() # stop moving
def main(nh,
camera,
object_name,
distance_estimate,
selector=lambda items, body_tf: items[0],
turn=True):
boat = yield boat_scripting.get_boat(nh)
goal_mgr = self._camera_2d_action_clients[camera].send_goal(
legacy_vision_msg.FindGoal(object_names=[object_name], ))
start_pose = self.pose
start_map_transform = tf.Transform(start_pose.position,
start_pose.orientation)
try:
while True:
feedback = yield goal_mgr.get_feedback()
res = map(json.loads, feedback.targetreses[0].object_results)
try:
transform = yield self._tf_listener.get_transform(
'/base_link', feedback.header.frame_id,
feedback.header.stamp)
map_transform = yield self._tf_listener.get_transform(
'/enu', '/base_link', feedback.header.stamp)
except Exception:
traceback.print_exc()
continue
if not res: continue
obj = selector(res, transform)
if obj is None: continue
ray_start_camera = numpy.array([0, 0, 0])
ray_dir_camera = numpy.array(map(float, obj['center']))
obj_dir_camera = numpy.array(map(float, obj['direction']))
ray_start_world = map_transform.transform_point(
transform.transform_point(ray_start_camera))
ray_dir_world = map_transform.transform_vector(
transform.transform_vector(ray_dir_camera))
obj_dir_world = map_transform.transform_vector(
transform.transform_vector(obj_dir_camera))
axis_camera = [0, 0, 1]
axis_body = transform.transform_vector(axis_camera)
axis_world = start_map_transform.transform_vector(axis_body)
# project ray onto plane defined by distance_estimate
# from start_pose.position along axis_world
plane_point = distance_estimate * axis_world + start_pose.position
plane_vector = axis_world
x = plane_vector.dot(ray_start_world -
plane_point) / plane_vector.dot(ray_dir_world)
object_pos = ray_start_world - ray_dir_world * x
desired_pos = object_pos - start_map_transform.transform_vector(
transform.transform_point(ray_start_camera))
desired_pos = desired_pos - axis_world * axis_world.dot(
desired_pos - start_pose.position)
error_pos = desired_pos - map_transform.transform_point([0, 0, 0])
error_pos = error_pos - axis_world * axis_world.dot(error_pos)
error_pos[2] = 0
print desired_pos, numpy.linalg.norm(error_pos) / 3e-2
if numpy.linalg.norm(error_pos) < 3e-2: # 3 cm
if turn:
direction_symmetry = int(obj['direction_symmetry'])
dangle = 2 * math.pi / direction_symmetry
def rotate(x, angle):
return transformations.rotation_matrix(
angle, axis_world)[:3, :3].dot(x)
for sign in [-1, +1]:
while rotate(obj_dir_world, sign * dangle).dot(
start_pose.forward_vector) > obj_dir_world.dot(
start_pose.forward_vector):
obj_dir_world = rotate(obj_dir_world,
sign * dangle)
yield (self.move.set_position(desired_pos).
look_at_rel_without_pitching(obj_dir_world).go())
else:
yield (self.move.set_position(desired_pos).go())
return
# go towards desired position
self._moveto_action_client.send_goal(
start_pose.set_position(desired_pos).as_MoveToGoal(
speed=0.1)).forget()
finally:
goal_mgr.cancel()
yield self.move.go() # stop moving
def main(nh, shape=None, color=None):
if shape == None:
shape = DEFAULT_SHAPE
if color == None:
color = DEFAULT_COLOR
boat = yield boat_scripting.get_boat(nh, False, False)
reorient = True
# While the boat is still distant from the target
while True and not rospy.is_shutdown():
temp_distance = 0
avg_distance = 0
shortest_distance = 100
farthest_distance = 0
x_mean = 0
y_mean = 0
numerator = 0
denom = 0
try:
yield util.wrap_timeout(center_sign(boat, shape, color), 20)
print "Locked onto shape"
except Exception:
print "Could not dock find shape, moving on to dock"
finally:
pass
# Only reorients one time
"""
if reorient == True and REORIENT == True:
print "reorienting"
orientation = yield orient(boat)
sign_centered = yield center_sign(boat, shape, color)
reorient = False
"""
print "holding at current position"
print "Scanning lidar"
distances = yield boat.get_distance_from_object(0.05)
avg_distance = distances[0] - BOAT_LENGTH_OFFSET
shortest_distance = distances[1] - BOAT_LENGTH_OFFSET
farthest_distance = distances[2] - BOAT_LENGTH_OFFSET
print "Average distance from target:", avg_distance
print "Shortest distance between boat and object:", shortest_distance
print "Farther distance between boat and object:", farthest_distance
final_move = shortest_distance / MOVE_OFFSET
if farthest_distance > 3.5:
# raw_input("Press enter to move forward" + str(final_move) + " meters")
# Print only if a move is commanded
print "Moving forward " + str(final_move) + " meters"
yield boat.move.forward(final_move).go()
if farthest_distance <= 1.5:
# raw_input("Press enter to move forward one meter")
# Print only if a move is commanded
print "Moving forward one meter"
boat.move.forward(1.5).go()
print "Moving back 5 meters"
yield util.sleep(5)
yield boat.move.forward(-5).go()
print "Find shape success"
break
# delete variables to avoid any threading problems
del avg_distance, temp_distance, farthest_distance, shortest_distance
def main(nh):
boat = yield boat_scripting.get_boat(nh, False)
yield boat.retract_hydrophone()
