def do_dock(nh, boat, course, dock_item):
print "starting dock2"
yield boat.go_to_ecef_pos(dict(
pool=[1220427.17101, -4965353.32773, 3799835.96511],
#A=[1220408.5926, -4965366.56212, 3799819.98198],
#A=ll(36.80220, -76.19130),
A=ll(36.801740, -76.191918),
B=ll(36.801754, -76.191911),
)[course])
yield boat.move.heading_deg(dict(
pool=60,
A=90-146,
B=90-146,
)[course]).go()
#yield boat.move.backward(3).go()
s = boat.move
try:
yield util.wrap_timeout(dock2.main(nh, dock_item), 40)
except Exception:
traceback.print_exc()
yield s.left(2).go()
try:
yield util.wrap_timeout(dock2.main(nh, dock_item), 40)
except Exception:
traceback.print_exc()
yield s.right(2).go()
try:
yield util.wrap_timeout(dock2.main(nh, dock_item), 40)
except Exception:
traceback.print_exc()
yield s.right(4).go()
try:
yield util.wrap_timeout(dock2.main(nh, dock_item), 40)
except Exception:
traceback.print_exc()
def main_list(nh,sub):
'''
Wrap individual tasks in their own timeouts so we can assign time to
the task and any support moves needed that are not included in the tasks
mission main function
'''
yield sub.move.depth(1).go()
yield sub.move.forward(10).go()
try:
yield util.wrap_timeout(buoy(nh), BUOY_TIME)
except Exception:
traceback.print_exc()
finally: pass
#sub.change_current_vision(False,False,False,False,False,False)
try:
#sub.change_current_vision(False,False,False,False,True,False)
yield util.wrap_timeout(path.main(nh), PATH_TIME)
except Exception:
traceback.print_exc()
finally: pass
yield sub.move.forward(3).go()
try:
#sub.change_current_vision(False,False,False,False,True,False)
yield util.wrap_timeout(portal.main(nh), PORTAL_TIME)
except Exception:
traceback.print_exc()
finally: pass
#sub.change_current_vision(False,False,False,False,False,False)
try:
#sub.change_current_vision(False,False,False,False,True,False)
yield util.wrap_timeout(path.main(nh), PATH_TIME)
except Exception:
traceback.print_exc()
finally: pass
#sub.change_current_vision(False,False,False,False,False,False)
yield sub.move.forward(3).go()
try:
yield util.wrap_timeout(torpedos(nh), TORPEDO_TIME)
except Exception:
traceback.print_exc()
finally: pass
def interoperability(nh, boat, s):
global docking_info
global images_info
global sent_image
print "Moving to position to begin interoperability"
s.set_current_challenge('interop')
# Get the images
images_info = yield images_info
print 'JSON says: \n\n' + str(images_info) + '\n\n'
images_path = images_info.file_path
images_count = images_info.image_count
try:
yield util.wrap_timeout(go_to_ecef_pos.main(nh, QUAD[course]),
ECEF_TIME)
except:
print 'ECEF timeout'
to_get_send = ['a', 'a']
to_get_send = ssocr_simple.main(images_path)
# Send the image but don't yield that way we can move while it sends
print "Sending image", to_get_send
sent_image = yield s.send_image_info(
to_get_send[1], to_get_send[0]) # FOUR the sake of testing
print "Removing temporary work path"
shutil.rmtree(images_path)
# Wait here for show
print 'Chilling at interoperability challange'
yield util.sleep(5)
def main(nh):
sub = yield sub_scripting.get_sub(nh)
begin = False
while True:
begin = yield sub.get_begin()
if begin == True:
break
print "Starting mission"
'''
Vision order:
recovery_vision
torpedo_vision
torpedo_area_vision
train_vision
path_vision
buoys_vision
'''
#sub.change_current_vision(False,False,False,False,False,False)
# WRAP ENTIRE MISSION IN TIMEOUT
try:
yield util.wrap_timeout(main_list(nh, sub), TOTAL_TIME)
except Exception:
traceback.print_exc()
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):
print "starting shooter"
sub = yield sub_scripting.get_sub(nh)
fwd_move = sub.move.go(linear=[0.25, 0, 0])
try:
obj = yield util.wrap_timeout(sub.visual_approach('forward', 'shooter/hole', size_estimate=5*.0254, desired_distance=1.5, selector=select_by_body_direction([0,1,0])), 30)
finally:
yield fwd_move.cancel()
start = sub.move
#try:
# yield util.wrap_timeout(align(sub), 30)
#except Exception:
# yield start.go() # go back because alignment failed
yield sub.visual_approach('forward', 'shooter/hole', size_estimate=5*.0254, desired_distance=1.0, selector=select_by_body_direction([0,1,0]))
yield util.sleep(5)
yield sub.move.forward(.3).go()
yield sub.move.up(5*.0254).go()
yield sub.move.right(3.5*.0254).go()
yield sub.fire_left_torpedo()
yield sub.move.backward(2.0).go()
print 'going to second hole'
yield sub.visual_approach('forward', 'shooter/hole', size_estimate=5*.0254, desired_distance=1.0, selector=select_by_body_direction([0,-1,0]))
yield util.sleep(5)
yield sub.move.forward(0.3).go()
yield sub.move.up(5*.0254).go()
yield sub.move.left(3.5*.0254).go()
yield sub.fire_right_torpedo()
yield sub.move.backward(1.0).go()
"""yield sub.move.right(.6).go()
def interoperability(nh, boat, s):
global docking_info
global images_info
global sent_image
print "Moving to position to begin interoperability"
s.set_current_challenge('interop')
# Get the images
images_info = yield images_info
print 'JSON says: \n\n' + str(images_info) + '\n\n'
images_path = images_info.file_path
images_count = images_info.image_count
try:
yield util.wrap_timeout(go_to_ecef_pos.main(nh, QUAD[course]), ECEF_TIME)
except:
print 'ECEF timeout'
to_get_send = ['a','a']
to_get_send = ssocr_simple.main(images_path)
# Send the image but don't yield that way we can move while it sends
print "Sending image", to_get_send
sent_image = yield s.send_image_info(to_get_send[1], to_get_send[0]) # FOUR the sake of testing
print "Removing temporary work path"
shutil.rmtree(images_path)
# Wait here for show
print 'Chilling at interoperability challange'
yield util.sleep(5)
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):
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(param_prefix, args):
nh = yield NodeHandle.from_argv("on_the_fly_mission_runner",
anonymous=True)
image_sub = yield nh.subscribe(args.topic_name, Image)
img = yield util.wrap_timeout(image_sub.get_next_message(),
5).addErrback(errback)
np_img = image_helpers.get_image_msg(img, "bgr8")
cv2.imshow(args.topic_name, np_img)
t = Thresholder(np_img, 'hsv' if args.hsv else 'bgr')
k = 0
while k != ord('q'): # q to quit without saving
t.update_mask()
k = cv2.waitKey(50) & 0xFF
if k == ord('s'): # s to save parameters
print "Saving params:"
temp = t.to_dict()
print temp
nh.set_param(param_prefix, temp)
break
cv2.destroyAllWindows()
reactor.stop()
def torpedos(nh):
#sub.change_current_vision(False,True,True,False,False,False)
try:
yield util.wrap_timeout(torpedo.main(nh), TORPEDO_TIME / 2)
except Exception:
traceback.print_exc()
def buoy(nh):
#sub.change_current_vision(False,False,False,False,False,True)
try:
yield util.wrap_timeout(buoys.main(nh), BUOY_TIME)
except Exception:
traceback.print_exc()
def docking(nh, boat, s):
global docking_info
global course
print "Moving to position to begin docking"
s.set_current_challenge('docking')
try:
yield util.wrap_timeout(go_to_ecef_pos.main(nh, DOCK[course]),
ECEF_TIME)
except:
print 'ECEF timeout'
print "Turning to face dock"
yield boat.move.heading(DOCK_HEADING[course]).go()
# Get dock info
docking_info = yield docking_info
print 'JSON says: \n\n' + str(docking_info) + '\n\n'
shape1 = docking_info.first_dock_symbol
color1 = docking_info.first_dock_color
try:
print "Beginning Dock 1"
yield util.wrap_timeout(find_shape.main(nh, shape1, color1),
DOCK_TIME / 2.0)
print "Completed Dock 1"
except Exception:
print "Could not dock first shape, moving to next shape"
finally:
pass
#boat.default_state()
shape2 = docking_info.second_dock_symbol
color2 = docking_info.second_dock_color
try:
print "Beginning Dock 2"
yield util.wrap_timeout(find_shape.main(nh, shape2, color2),
DOCK_TIME / 2.0)
print "Completed Dock 2"
except Exception:
print "Could not dock second shape, moving on"
finally:
pass
def test_wrap_timeout2(self):
try:
@util.cancellableInlineCallbacks
def f():
yield util.wall_sleep(3)
defer.returnValue('retval')
res = yield util.wrap_timeout(f(), 1)
except util.TimeoutError:
pass
else:
assert False
def pinger(nh, boat, s):
print "Moving to position to begin pinger challenge"
s.set_current_challenge('pinger')
try:
yield util.wrap_timeout(go_to_ecef_pos.main(nh, HYDRO[course]), ECEF_TIME)
except:
print 'ECEF timeout'
yield boat.move.heading(HYDRO_HEADING[course]).go()
print "Beginning Pinger challenge"
try:
print "Beginning Pinger"
yield util.wrap_timeout(circle_buoy.main(nh), HYDRO_TIME)
print "Completed Pinger"
except Exception:
print "Could not dock second shape, moving on"
finally: pass
#boat.default_state()
'''
def test_wrap_timeout1(self):
try:
@util.cancellableInlineCallbacks
def f():
yield util.wall_sleep(1)
defer.returnValue('retval')
yield util.wrap_timeout(f(), 3)
except util.TimeoutError:
assert False
else:
assert True
def pinger(nh, boat, s):
print "Moving to position to begin pinger challenge"
s.set_current_challenge('pinger')
try:
yield util.wrap_timeout(go_to_ecef_pos.main(nh, HYDRO[course]),
ECEF_TIME)
except:
print 'ECEF timeout'
yield boat.move.heading(HYDRO_HEADING[course]).go()
print "Beginning Pinger challenge"
try:
print "Beginning Pinger"
yield util.wrap_timeout(circle_buoy.main(nh), HYDRO_TIME)
print "Completed Pinger"
except Exception:
print "Could not dock second shape, moving on"
finally:
pass
#boat.default_state()
'''
def docking(nh, boat, s):
global docking_info
global course
print "Moving to position to begin docking"
s.set_current_challenge('docking')
try:
yield util.wrap_timeout(go_to_ecef_pos.main(nh, DOCK[course]), ECEF_TIME)
except:
print 'ECEF timeout'
print "Turning to face dock"
yield boat.move.heading(DOCK_HEADING[course]).go()
# Get dock info
docking_info = yield docking_info
print 'JSON says: \n\n' + str(docking_info) + '\n\n'
shape1 = docking_info.first_dock_symbol
color1 = docking_info.first_dock_color
try:
print "Beginning Dock 1"
yield util.wrap_timeout(find_shape.main(nh, shape1, color1), DOCK_TIME / 2.0)
print "Completed Dock 1"
except Exception:
print "Could not dock first shape, moving to next shape"
finally: pass
#boat.default_state()
shape2 = docking_info.second_dock_symbol
color2 = docking_info.second_dock_color
try:
print "Beginning Dock 2"
yield util.wrap_timeout(find_shape.main(nh, shape2, color2), DOCK_TIME / 2.0)
print "Completed Dock 2"
except Exception:
print "Could not dock second shape, moving on"
finally: pass
def test_wrap_timeout1(self):
try:
@util.cancellableInlineCallbacks
def f():
yield util.wall_sleep(1)
defer.returnValue('retval')
res = yield util.wrap_timeout(f(), 3)
except util.TimeoutError:
assert False
else:
assert res == 'retval'
def test_wrap_timeout2(self):
try:
@util.cancellableInlineCallbacks
def f():
yield util.wall_sleep(3)
defer.returnValue('retval')
yield util.wrap_timeout(f(), 1)
except util.TimeoutError:
pass
else:
assert False
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 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 do_dock(nh, boat, course, dock_item):
print "starting dock2"
yield boat.go_to_ecef_pos(
dict(
pool=[1220427.17101, -4965353.32773, 3799835.96511],
#A=[1220408.5926, -4965366.56212, 3799819.98198],
#A=ll(36.80220, -76.19130),
A=ll(36.801740, -76.191918),
B=ll(36.801754, -76.191911),
)[course])
yield boat.move.heading_deg(
dict(
pool=60,
A=90 - 146,
B=90 - 146,
)[course]).go()
#yield boat.move.backward(3).go()
s = boat.move
try:
yield util.wrap_timeout(dock2.main(nh, dock_item), 40)
except Exception:
traceback.print_exc()
yield s.left(2).go()
try:
yield util.wrap_timeout(dock2.main(nh, dock_item), 40)
except Exception:
traceback.print_exc()
yield s.right(2).go()
try:
yield util.wrap_timeout(dock2.main(nh, dock_item), 40)
except Exception:
traceback.print_exc()
yield s.right(4).go()
try:
yield util.wrap_timeout(dock2.main(nh, dock_item), 40)
except Exception:
traceback.print_exc()
def main(nh):
#while True:
# 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:
# break
sub = yield sub_scripting.get_sub(nh)
try:
yield util.wrap_timeout(main_list(nh), 11*60)
except Exception:
traceback.print_exc()
yield fail_list(nh)
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, 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, freq=25e3):
sub = yield sub_scripting.get_sub(nh)
yield sub.raise_down_grabber()
if 1:
yield sub.move.depth(1).go()
fwd_move = sub.move.go(linear=[.3, 0, 0])
try:
yield sub.hydrophone_align(freq)
finally:
fwd_move.cancel()
yield sub.move.relative(RELATIVE_PINGER_MOVE).go()
#print 'surfacing'
#yield sub.move.depth(0).go()
yield sub.move.depth(0.4).go()
bin_pose = sub.move
try:
yield util.wrap_timeout(sub.visual_align('down', 'wreath/bin/high', 2, selector=select_centered, turn=False), 10)
yield sub.move.relative(RELATIVE_VISION_MOVE).go()
except util.TimeoutError:
print 'bin alignment timed out'
yield bin_pose.go()
except:
print 'bin alignment???'
yield bin_pose.go()
yield path.main(nh, orient_away_from=True, forward=False, depth=0.4)
yield sub.move.forward(2).go()
yield retry_to_grab(sub, 'moonrock', freq, surface=True)
yield retry_to_grab(sub, 'moonrock', freq, bubbles=True, surface=True)
yield retry_to_grab(sub, 'cheese', freq)
yield retry_to_grab(sub, 'cheese', freq)
yield retry_to_grab(sub, 'cheese', freq, bubbles=True)
yield retry_to_grab(sub, 'moonrock', freq)
while True:
yield retry_to_grab(sub, 'moonrock', freq)
def run(self, args):
# Parse Arguments
self.stc = yield self.get_sorted_objects(name='stc_platform', n=1)
self.stc = self.stc[1][0]
yield self.move.look_at(self.stc).go()
yield self.move.set_position(self.stc).backward(5).yaw_left(1.57).go()
print 'SELECTING'
# Get scan the code stuff
try:
result = yield util.wrap_timeout(self.stcsub.get_next_message(),
self.TIMEOUT_SECONDS, "test")
stc_result = result.color_pattern
except util.TimeoutError:
stc_result = "RGB"
self.stcpub.publish(ScanTheCode(color_pattern=stc_result))
self.net_stc_results = stc_result
if stc_result[0] == 'R':
yield self.move.look_at(self.stc).go()
yield self.move.left(10).forward(15).go()
elif stc_result[0] == 'G':
yield self.move.look_at(self.stc).go()
yield self.move.right(10).forward(15).go()
elif stc_result[0] == 'B':
yield self.move.left(5).go()
yield self.move.circle_point(self.stc,
direction='cw',
revolutions=1.25).go()
yield self.move.forward(10).go()
self.send_feedback('Done!')
def main(param_prefix, args):
nh = yield NodeHandle.from_argv("on_the_fly_mission_runner", anonymous=True)
image_sub = yield nh.subscribe(args.topic_name, Image)
img = yield util.wrap_timeout(image_sub.get_next_message(), 5).addErrback(errback)
np_img = image_helpers.get_image_msg(img, "bgr8")
cv2.imshow(args.topic_name, np_img)
t = Thresholder(np_img, 'hsv' if args.hsv else 'bgr')
k = 0
while k != ord('q'): # q to quit without saving
t.update_mask()
k = cv2.waitKey(50) & 0xFF
if k == ord('s'): # s to save parameters
print "Saving params:"
temp = t.to_dict()
print temp
nh.set_param(param_prefix, temp)
break
cv2.destroyAllWindows()
reactor.stop()
def __new__(cls, ns, name, addr, master_uri, remappings):
# constraints: anything blocking here should print something if it's
# taking a long time in order to avoid confusion
self = object.__new__(cls)
if ns:
assert ns[0] == '/'
assert not ns.endswith('/')
self._ns = ns # valid values: '', '/a', '/a/b'
assert '/' not in name
self._name = self._ns + '/' + name
self._shutdown_callbacks = set()
reactor.addSystemEventTrigger('before', 'shutdown', self.shutdown)
self._addr = addr
self._master_uri = master_uri
self._remappings = remappings
self._master_proxy = rosxmlrpc.Proxy(xmlrpc.Proxy(self._master_uri),
self._name)
self._is_running = True
self._xmlrpc_handlers = {}
self._xmlrpc_server = reactor.listenTCP(
0, server.Site(_XMLRPCSlave(self)))
self._shutdown_callbacks.add(self._xmlrpc_server.loseConnection)
self._xmlrpc_server_uri = 'http://%s:%i/' % (
self._addr, self._xmlrpc_server.getHost().port)
self._tcpros_handlers = {}
@util.cancellableInlineCallbacks
def _handle_tcpros_conn(conn):
try:
header = tcpros.deserialize_dict((yield conn.receiveString()))
def default(header, conn):
conn.sendString(
tcpros.serialize_dict(
dict(error='unhandled connection')))
conn.transport.loseConnection()
if 'service' in header:
self._tcpros_handlers.get(('service', header['service']),
default)(header, conn)
elif 'topic' in header:
self._tcpros_handlers.get(('topic', header['topic']),
default)(header, conn)
else:
conn.sendString(
tcpros.serialize_dict(
dict(error='no topic or service name detected')))
conn.transport.loseConnection()
except:
conn.transport.loseConnection()
raise
def _make_tcpros_protocol(addr):
conn = tcpros.Protocol()
_handle_tcpros_conn(conn)
return conn
self._tcpros_server = reactor.listenTCP(
0, util.AutoServerFactory(_make_tcpros_protocol))
self._shutdown_callbacks.add(self._tcpros_server.loseConnection)
self._tcpros_server_uri = 'rosrpc://%s:%i' % (
self._addr, self._tcpros_server.getHost().port)
self._tcpros_server_addr = self._addr, self._tcpros_server.getHost(
).port
while True:
try:
other_node_uri = yield self._master_proxy.lookupNode(
self._name)
except rosxmlrpc.Error:
break # assume that error means unknown node
except Exception:
traceback.print_exc()
yield util.wall_sleep(1) # pause so we don't retry immediately
else:
other_node_proxy = rosxmlrpc.Proxy(
xmlrpc.Proxy(other_node_uri), self._name)
try:
yield util.wrap_timeout(
other_node_proxy.shutdown(
'new node registered with same name'), 3)
except error.ConnectionRefusedError:
pass
except Exception:
traceback.print_exc()
break
try:
self._use_sim_time = yield self.get_param('/use_sim_time')
except rosxmlrpc.Error: # assume that error means not found
self._use_sim_time = False
if self._use_sim_time:
def got_clock(msg):
self._sim_time = msg.clock
self._clock_sub = self.subscribe('/clock', Clock, got_clock)
# make sure self._sim_time gets set before we continue
yield util.wrap_time_notice(self._clock_sub.get_next_message(), 1,
'getting simulated time from /clock')
for k, v in self._remappings.iteritems():
if k.startswith('_') and not k.startswith('__'):
yield self.set_param(self.resolve_name('~' + k[1:]),
yaml.load(v))
self.advertise_service('~get_loggers', GetLoggers,
lambda req: GetLoggersResponse())
self.advertise_service('~set_logger_level', SetLoggerLevel,
lambda req: SetLoggerLevelResponse())
defer.returnValue(self)
def main_list(nh, boat, course):
try:
print 'main start'
#yield boat.move.forward(3).go()
try:
dock_item = yield ci.start_automated_docking(course)
except Exception:
traceback.print_exc()
dock_item = 'triangle'
print 'Defaulting to dock_item:', dock_item
print 'dock_item:', dock_item
try:
gates = yield ci.start_obstacle_avoidance(course)
except Exception:
traceback.print_exc()
gates = random.choice(['1','2','3']), random.choice(['X', 'Y', 'Z'])
print 'Defaulting to gates:', gates
print 'gates:', gates
print 'Activating light sequence'
try:
res = yield ci.activate_light_sequence(course)
except Exception:
traceback.print_exc()
print 'LIGHT SEQUENCE FAILED! WARNING!'
#yield util.sleep(5)
else:
print 'Result:', res
yield util.sleep(2.0)
colors = ["red", "green", "blue", "yellow"]
@util.cancellableInlineCallbacks
def _work():
perms = set(itertools.product(colors, repeat=3))
while perms:
perm = random.choice(list(perms))
try:
res = yield ci.report_light_sequence(course, perm)
if res: break
if not res: perms.remove(perm)
except: traceback.print_exc()
yield util.sleep(1)
_work()
print 'Running gate2'
yield boat.move.forward(75).go()
#try:
# yield util.wrap_timeout(gate2.main(nh, 'left' if course == 'B' else 'right'), 60*2)
#except Exception:
# traceback.print_exc()
try:
yield util.wrap_timeout(do_obstacle_course(nh, boat, course, gates), 2*60)
except Exception:
traceback.print_exc()
# safe point
print 'Going to safe point 1'
yield boat.go_to_ecef_pos(dict(
pool=[1220416.51743, -4965356.4575, 3799838.03177],
A=ll(36.802040, -76.191835), # from google earth
B=ll(36.801972, -76.191849), # from google earth
)[course])
try:
yield util.wrap_timeout(do_dock(nh, boat, course, dock_item), 60*4)
except Exception:
traceback.print_exc()
# safe point
print 'Going to safe point 1'
yield boat.go_to_ecef_pos(dict(
pool=[1220416.51743, -4965356.4575, 3799838.03177],
A=ll(36.802040, -76.191835), # from google earth
B=ll(36.801972, -76.191849), # from google earth
)[course])
# safe point
print 'Going to safe point 3'
yield boat.go_to_ecef_pos(dict(
pool=[1220416.51743, -4965356.4575, 3799838.03177],
A=ll( 36.802185, -76.191492), # from google earth
B=ll(36.801972, -76.191849), # from google earth
)[course])
# safe point
print 'Going to safe point 4'
yield boat.go_to_ecef_pos(dict(
pool=[1220416.51743, -4965356.4575, 3799838.03177],
A=ll( 36.802365, -76.191650), # from google earth
B=ll(36.801972, -76.191849), # from google earth
)[course])
print 'going to pinger'
yield boat.go_to_ecef_pos(dict(
A=ll( 36.802720, -76.191470),
B=ll(36.80175, -76.19230),
)[course])
freq = dict(
A=27e3,
B=32e3,
)[course]
print 'acoustic_beacon'
yield acoustic_beacon.main(nh, ci, course, freq)
yield boat.go_to_ecef_pos(dict(
A=ll( 36.802720, -76.191470),
B=ll(36.80175, -76.19230),
)[course])
print 'Going to safe point 4'
yield boat.go_to_ecef_pos(dict(
pool=[1220416.51743, -4965356.4575, 3799838.03177],
A=ll( 36.802365, -76.191650), # from google earth
B=ll(36.801972, -76.191849), # from google earth
)[course])
print 'Going to safe point 3'
yield boat.go_to_ecef_pos(dict(
pool=[1220416.51743, -4965356.4575, 3799838.03177],
A=ll( 36.802185, -76.191492), # from google earth
B=ll(36.801972, -76.191849), # from google earth
)[course])
print 'Going to safe point a'
yield boat.go_to_ecef_pos(dict(
pool=[1220416.51743, -4965356.4575, 3799838.03177],
A=ll( 36.801897, -76.191622), # from google earth
B=ll(36.801972, -76.191849), # from google earth
)[course])
# center far
#yield boat.go_to_ecef_pos(ll(36.802094, -76.191680))
yield boat.go_to_ecef_pos(ll( 36.801694, -76.191034))
# center near
#yield boat.go_to_ecef_pos([1220440.29354, -4965392.18483, 3799791.58982])
yield boat.go_to_ecef_pos([1220451.80321, -4965388.40181, 3799791.9771])
yield boat.move.heading_deg(dict(
A=60+90+180,
)[course]).go()
fwd_task = boat.move.forward(100).go(speed=.2)
try:
yield boat.wait_for_bump()
finally:
fwd_task.cancel()
try:
x = boat.float()
yield util.sleep(1e6)
finally:
x.cancel()
print 'main end'
finally:
print 'main finally start'
#yield util.sleep(3)
print 'main finally end'
def main_list(nh, boat, course):
try:
print 'main start'
#yield boat.move.forward(3).go()
try:
dock_item = yield ci.start_automated_docking(course)
except Exception:
traceback.print_exc()
dock_item = 'triangle'
print 'Defaulting to dock_item:', dock_item
print 'dock_item:', dock_item
try:
gates = yield ci.start_obstacle_avoidance(course)
except Exception:
traceback.print_exc()
gates = random.choice(['1', '2',
'3']), random.choice(['X', 'Y', 'Z'])
print 'Defaulting to gates:', gates
print 'gates:', gates
print 'Activating light sequence'
try:
res = yield ci.activate_light_sequence(course)
except Exception:
traceback.print_exc()
print 'LIGHT SEQUENCE FAILED! WARNING!'
#yield util.sleep(5)
else:
print 'Result:', res
yield util.sleep(2.0)
colors = ["red", "green", "blue", "yellow"]
@util.cancellableInlineCallbacks
def _work():
perms = set(itertools.product(colors, repeat=3))
while perms:
perm = random.choice(list(perms))
try:
res = yield ci.report_light_sequence(course, perm)
if res: break
if not res: perms.remove(perm)
except:
traceback.print_exc()
yield util.sleep(1)
_work()
print 'Running gate2'
yield boat.move.forward(75).go()
#try:
# yield util.wrap_timeout(gate2.main(nh, 'left' if course == 'B' else 'right'), 60*2)
#except Exception:
# traceback.print_exc()
try:
yield util.wrap_timeout(
do_obstacle_course(nh, boat, course, gates), 2 * 60)
except Exception:
traceback.print_exc()
# safe point
print 'Going to safe point 1'
yield boat.go_to_ecef_pos(
dict(
pool=[1220416.51743, -4965356.4575, 3799838.03177],
A=ll(36.802040, -76.191835), # from google earth
B=ll(36.801972, -76.191849), # from google earth
)[course])
try:
yield util.wrap_timeout(do_dock(nh, boat, course, dock_item),
60 * 4)
except Exception:
traceback.print_exc()
# safe point
print 'Going to safe point 1'
yield boat.go_to_ecef_pos(
dict(
pool=[1220416.51743, -4965356.4575, 3799838.03177],
A=ll(36.802040, -76.191835), # from google earth
B=ll(36.801972, -76.191849), # from google earth
)[course])
# safe point
print 'Going to safe point 3'
yield boat.go_to_ecef_pos(
dict(
pool=[1220416.51743, -4965356.4575, 3799838.03177],
A=ll(36.802185, -76.191492), # from google earth
B=ll(36.801972, -76.191849), # from google earth
)[course])
# safe point
print 'Going to safe point 4'
yield boat.go_to_ecef_pos(
dict(
pool=[1220416.51743, -4965356.4575, 3799838.03177],
A=ll(36.802365, -76.191650), # from google earth
B=ll(36.801972, -76.191849), # from google earth
)[course])
print 'going to pinger'
yield boat.go_to_ecef_pos(
dict(
A=ll(36.802720, -76.191470),
B=ll(36.80175, -76.19230),
)[course])
freq = dict(
A=27e3,
B=32e3,
)[course]
print 'acoustic_beacon'
yield acoustic_beacon.main(nh, ci, course, freq)
yield boat.go_to_ecef_pos(
dict(
A=ll(36.802720, -76.191470),
B=ll(36.80175, -76.19230),
)[course])
print 'Going to safe point 4'
yield boat.go_to_ecef_pos(
dict(
pool=[1220416.51743, -4965356.4575, 3799838.03177],
A=ll(36.802365, -76.191650), # from google earth
B=ll(36.801972, -76.191849), # from google earth
)[course])
print 'Going to safe point 3'
yield boat.go_to_ecef_pos(
dict(
pool=[1220416.51743, -4965356.4575, 3799838.03177],
A=ll(36.802185, -76.191492), # from google earth
B=ll(36.801972, -76.191849), # from google earth
)[course])
print 'Going to safe point a'
yield boat.go_to_ecef_pos(
dict(
pool=[1220416.51743, -4965356.4575, 3799838.03177],
A=ll(36.801897, -76.191622), # from google earth
B=ll(36.801972, -76.191849), # from google earth
)[course])
# center far
#yield boat.go_to_ecef_pos(ll(36.802094, -76.191680))
yield boat.go_to_ecef_pos(ll(36.801694, -76.191034))
# center near
#yield boat.go_to_ecef_pos([1220440.29354, -4965392.18483, 3799791.58982])
yield boat.go_to_ecef_pos(
[1220451.80321, -4965388.40181, 3799791.9771])
yield boat.move.heading_deg(dict(A=60 + 90 + 180, )[course]).go()
fwd_task = boat.move.forward(100).go(speed=.2)
try:
yield boat.wait_for_bump()
finally:
fwd_task.cancel()
try:
x = boat.float()
yield util.sleep(1e6)
finally:
x.cancel()
print 'main end'
finally:
print 'main finally start'
#yield util.sleep(3)
print 'main finally end'
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):
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, 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 __new__(cls, ns, name, addr, master_uri, remappings):
# constraints: anything blocking here should print something if it's
# taking a long time in order to avoid confusion
self = object.__new__(cls)
if ns:
assert ns[0] == '/'
assert not ns.endswith('/')
self._ns = ns # valid values: '', '/a', '/a/b'
assert '/' not in name
self._name = self._ns + '/' + name
self._shutdown_callbacks = set()
reactor.addSystemEventTrigger('before', 'shutdown', self.shutdown)
self._addr = addr
self._master_uri = master_uri
self._remappings = remappings
self._master_proxy = rosxmlrpc.Proxy(xmlrpc.Proxy(self._master_uri), self._name)
self._is_running = True
self._xmlrpc_handlers = {}
self._xmlrpc_server = reactor.listenTCP(0, server.Site(_XMLRPCSlave(self)))
self._shutdown_callbacks.add(self._xmlrpc_server.loseConnection)
self._xmlrpc_server_uri = 'http://%s:%i/' % (self._addr, self._xmlrpc_server.getHost().port)
self._tcpros_handlers = {}
@util.cancellableInlineCallbacks
def _handle_tcpros_conn(conn):
try:
header = tcpros.deserialize_dict((yield conn.receiveString()))
def default(header, conn):
conn.sendString(tcpros.serialize_dict(dict(error='unhandled connection')))
conn.transport.loseConnection()
if 'service' in header:
self._tcpros_handlers.get(('service', header['service']), default)(header, conn)
elif 'topic' in header:
self._tcpros_handlers.get(('topic', header['topic']), default)(header, conn)
else:
conn.sendString(tcpros.serialize_dict(dict(error='no topic or service name detected')))
conn.transport.loseConnection()
except:
conn.transport.loseConnection()
raise
def _make_tcpros_protocol(addr):
conn = tcpros.Protocol()
_handle_tcpros_conn(conn)
return conn
self._tcpros_server = reactor.listenTCP(0, util.AutoServerFactory(_make_tcpros_protocol))
self._shutdown_callbacks.add(self._tcpros_server.loseConnection)
self._tcpros_server_uri = 'rosrpc://%s:%i' % (self._addr, self._tcpros_server.getHost().port)
self._tcpros_server_addr = self._addr, self._tcpros_server.getHost().port
while True:
try:
other_node_uri = yield self._master_proxy.lookupNode(self._name)
except rosxmlrpc.Error:
break # assume that error means unknown node
except Exception:
traceback.print_exc()
yield util.wall_sleep(1) # pause so we don't retry immediately
else:
other_node_proxy = rosxmlrpc.Proxy(xmlrpc.Proxy(other_node_uri), self._name)
try:
yield util.wrap_timeout(other_node_proxy.shutdown('new node registered with same name'), 3)
except error.ConnectionRefusedError:
pass
except Exception:
traceback.print_exc()
break
try:
self._use_sim_time = yield self.get_param('/use_sim_time')
except rosxmlrpc.Error: # assume that error means not found
self._use_sim_time = False
if self._use_sim_time:
def got_clock(msg):
self._sim_time = msg.clock
self._clock_sub = self.subscribe('/clock', Clock, got_clock)
# make sure self._sim_time gets set before we continue
yield util.wrap_time_notice(self._clock_sub.get_next_message(), 1,
'getting simulated time from /clock')
for k, v in self._remappings.iteritems():
if k.startswith('_') and not k.startswith('__'):
yield self.set_param(self.resolve_name('~' + k[1:]), yaml.load(v))
self.advertise_service('~get_loggers', GetLoggers, lambda req: GetLoggersResponse())
self.advertise_service('~set_logger_level', SetLoggerLevel, lambda req: SetLoggerLevelResponse())
defer.returnValue(self)
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 try_to_grab(sub, obj_name, freq, surface=False, bubbles=False):
assert obj_name in ['moonrock', 'cheese']
try:
yield sub.move.depth(0.4).go()
fwd_move = sub.move.go(linear=[0.25, 0, 0])
try:
yield sub.visual_align('down', 'wreath/board/high', 2, selector=select_centered, turn=True, angle=math.radians(45))
finally:
yield fwd_move.cancel()
board_pose = sub.pose.depth(1).right(.6)
yield sub.move.yaw_left_deg(90*random.randrange(4)).go()
yield sub.move.depth(2).go()
try:
yield util.wrap_timeout(sub.visual_align('down', 'wreath/moonrock/high', 2, selector=selector(obj_name), one_shot=True, turn=False), 20)
except util.TimeoutError:
print 'timed out'
yield sub.move.yaw_left_deg(120).go()
return
try:
yield util.wrap_timeout(sub.visual_align('down', 'wreath/moonrock/high', 2, selector=select_centered), 20)
except util.TimeoutError:
print 'timed out 1'
yield sub.move.yaw_left_deg(120).go()
return
#print "weighing"
#w1 = yield get_weight(sub)
#print 'w1', w1
print "moving down"
yield sub.move.down(1).go(speed=.2)
try:
yield util.wrap_timeout(sub.visual_align('down', 'wreath/moonrock/low', 1, selector=select_centered, turn=False), 20)
except util.TimeoutError:
print 'timed out 2'
return
yield sub.lower_down_grabber()
yield sub.open_down_grabber()
yield sub.move.relative([-.10 if obj_name == 'moonrock' else -.115,-.15,0]).go()
if obj_name == 'moonrock':
yield sub.move.down(.73).go(speed=.2)
else:
yield sub.move.down(.81).go(speed=.2)
yield sub.close_down_grabber()
print "moving back to surface"
#yield sub.move.up(.5).go(speed=.2)
yield sub.move.depth(1).go()
#w2 = yield get_weight(sub)
#print 'w2', w2
#gain = w2 - w1
#print 'weight gained', gain
#weights = {0: 0, 1: 2.97, 2: 4.00}
#objects = min(weights, key=lambda w: abs(weights[w] - gain))
#print 'object count', objects
#if objects != 1:
# yield sub.move.set_position(board_pose.position + [random.uniform(-.5, .5), random.uniform(-.5, .5), 0]).go()
# yield sub.open_down_grabber()
# yield util.sleep(1)
# yield sub.close_down_grabber()
# yield sub.move.turn_left_deg(120).go()
# defer.returnValue(False)
print "going to hydrophone"
yield sub.hydrophone_align(freq)
yield sub.move.relative(RELATIVE_PINGER_MOVE).go()
yield sub.move.depth(.4).go()
yield sub.raise_down_grabber()
bin_pose = sub.move
try:
yield util.wrap_timeout(sub.visual_align('down', 'wreath/bin/high', 2, selector=select_centered, turn=False), 10)
yield sub.move.relative(RELATIVE_VISION_MOVE).go()
except util.TimeoutError:
print 'bin alignment timed out'
yield bin_pose.go()
except:
print 'bin alignment???'
yield bin_pose.go()
if surface:
yield sub.move.depth(0).go()
yield sub.move.depth(.4).go()
yield sub.lower_down_grabber()
#yield sub.move.relative([-.15,-.2,0]).go()
yield sub.move.depth(2).go()
print "going down"
yield sub.open_down_grabber()
yield util.sleep(3)
yield sub.close_down_grabber()
yield util.sleep(1)
yield sub.open_down_grabber()
yield util.sleep(3)
yield sub.raise_down_grabber()
if bubbles:
yield sub.fire_left_torpedo()
yield sub.fire_right_torpedo()
yield sub.fire_left_torpedo()
yield sub.fire_right_torpedo()
yield sub.fire_left_torpedo()
yield sub.fire_right_torpedo()
print "going to board"
yield sub.move.look_at_without_pitching(board_pose.position).go()
yield sub.move.set_position(board_pose.position).go()
finally:
print "finally"
yield sub.close_down_grabber()
yield sub.raise_down_grabber()
#yield sub.move.depth(2).go()
defer.returnValue(True)
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):
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()
