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