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 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, 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 __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()