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): sub = yield sub_scripting.get_sub(nh) sub.move.go(linear=[0.25, 0, 0]) yield sub.visual_approach('forward', 'shooter', size_estimate=7*.0254, desired_distance=1.5, selector=select_by_body_direction([0,1,0])) yield util.sleep(5) yield sub.move.forward(.5).go() yield sub.move.up(5*.0254).go() yield sub.move.right(3.5*.0254).go() yield util.sleep(5) yield sub.move.backward(2).go() yield sub.fire_left_torpedo() yield sub.visual_approach('forward', 'shooter', size_estimate=7*.0254, desired_distance=1.5, selector=select_by_body_direction([0,-1,0])) yield util.sleep(5) yield sub.move.forward(.5).go() yield sub.move.up(5*.0254).go() yield sub.move.left(3.5*.0254).go() yield util.sleep(5) yield sub.move.backward(2).go() yield sub.fire_right_torpedo() """yield sub.move.right(.6).go()
def get_distance_from_object(self, radius): temp_distance = 0 avg_distance = 0 shortest_distance = 100 farthest_distance = 0 return_array = [] hold = [] while len(hold) <= 0: # get pointcloud pointcloud = yield self.get_pointcloud() yield util.sleep(.2) # sleep to avoid tooPast errors pointcloud_base = yield self.to_baselink(pointcloud) yield util.sleep(.2) # sleep to avoid tooPast errors # Filter lidar data to only data right in front of the boat hold = filter(lambda x: abs(x[1]) < radius, pointcloud_base) # Calculate several distances between target and boat for x in range(len(hold)): dist = hold[x] temp_distance += dist[0] # Check and assign the closest object to the boat if dist[0] < shortest_distance: shortest_distance = dist[0] if dist[0] > farthest_distance: farthest_distance = dist[0] avg_distance = temp_distance / len(hold) shortest_distance = shortest_distance farthest_distance = farthest_distance return_array.append(avg_distance) return_array.append(shortest_distance) return_array.append(farthest_distance) defer.returnValue(return_array)
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 "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 get_distance_from_object(self, radius): temp_distance = 0 avg_distance = 0 shortest_distance = 100 farthest_distance = 0 return_array = [] hold = [] while len(hold) <= 0: # get pointcloud pointcloud = yield self.get_pointcloud() yield util.sleep(.2) # sleep to avoid tooPast errors pointcloud_base = yield self.to_baselink(pointcloud) yield util.sleep(.2) # sleep to avoid tooPast errors # Filter lidar data to only data right in front of the boat hold = filter(lambda x: abs(x[1]) < radius, pointcloud_base) # Calculate several distances between target and boat for x in range(len(hold)): dist = hold[x] temp_distance += dist[0] # Check and assign the closest object to the boat if dist[0] < shortest_distance: shortest_distance = dist[0] if dist[0] > farthest_distance: farthest_distance = dist[0] avg_distance = temp_distance/len(hold) shortest_distance = shortest_distance farthest_distance = farthest_distance return_array.append(avg_distance) return_array.append(shortest_distance) return_array.append(farthest_distance) defer.returnValue(return_array)
def main(nh): sub = yield sub_scripting.get_sub(nh) #yield sub.lower_down_grabber() while True: yield util.sleep(2) yield sub.lower_down_grabber() yield sub.open_down_grabber() yield util.sleep(2)
def main(): nh = yield txros.NodeHandle.from_argv('thruster_calibrator') pub = nh.advertise('/thrusters/command/test', ThrusterCommand) port = yield nh.get_param('~port') lc = LoadCell(nh, port) power_holder = [0] @util.cancellableInlineCallbacks def set_thruster_thread(): while True: pub.publish(ThrusterCommand(force=power_holder[0])) yield util.sleep(.1) set_thruster_thread() for i in xrange(1, -20, -1): power = i / 20 power_holder[0] = power yield util.sleep(1) res = [] for i in xrange(-20, 20 + 1): power = i / 20 power_holder[0] = power yield util.sleep(2) s = 0 for j in xrange(400): s += (yield lc.measurement_event.get_deferred())[0] s /= 400 if i == 0: zero_s = s print power, s res.append((s, power)) print[(-(x - zero_s), y) for x, y in res] power_holder[0] = 0 yield util.sleep(1)
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 fail_list(nh, boat): try: print 'fail start' yield util.sleep(1) print 'fail end' finally: print 'fail finally'
def main(nh, target = None, orient_target = None): sub = yield sub_scripting.get_sub(nh) picked_up_target = False align_target = target if align_target == None: align_target = 'delorean' orientation_align_target = orient_target if orientation_align_target == None: orientation_align_target = 'delorean' x_location = 0 y_location = 0 while not rospy.is_shutdown(): yield sub.orient_and_align(align_target, orientation_align_target, MOVE_SCALE, ANGULAR_TOLERANCE, ORIENTATION_TOLERANCE) # Move down until distance from obbject is 1 meter yield move_down(sub) while picked_up_target == False: yield sub.close_gripper() yield sub.move.up(1).go() # Scan ten times to verify that we have picked up object for x in xrange(1,10): target_scan = yield sub.get_target_location(align_target) yield util.sleep(.1) if target_scan.x == 0 and target_scan.y == 0: picked_up_target = True
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 get_weight(sub): yield util.sleep(10) start = time.time() res = [] while time.time() < start + 5: res.append((yield sub.get_z_force())) defer.returnValue(sum(res) / len(res))
def main(nh): s = yield json_server_proxy.get_server(nh) try: #url_was_set = s.interact('http://ec2-52-7-253-202.compute-1.amazonaws.com:80','openTest') url = raw_input('url: ') course = raw_input('Course:') url_was_set = yield s.interact('http://' + url, course) if url_was_set.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" yield s.end_run() print 'Ended last run' run_started = (yield s.start_run()).success if run_started: print "Run started succesfully" images = yield s.get_server_images() print images sent_image = yield s.send_image_info('0.png', 'ZERO') print sent_image dock = yield s.get_dock_info() print dock gates = yield s.get_gate_info() print gates color = 'blue' for c in ['blue', 'green', 'red', 'yellow', 'black']: check = yield s.send_buoy_info(c) if check.is_right_buoy: print 'Got the right buoy: ' + c color = c break else: print 'Got the wrong buoy: ' + c print test print '____Beat?____' yield util.sleep(60) finally: yield s.end_run() print 'Ended run'
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)
def main(nh): sub = yield sub_scripting.get_sub(nh) print "firing left torpedo" yield sub.fire_left_torpedo() print "fired, waiting 3 seconds" yield util.sleep(3) print "firing right torpedo" yield sub.fire_right_torpedo() print "fired right torpedo"
def main(nh): sub = yield sub_scripting.get_sub(nh) print "lowering impaler" yield sub.lower_impaler() print "lowered, waiting 3 seconds" yield util.sleep(3) print "raising impaler" yield sub.raise_impaler() print "raised impaler"
def float(self): while True: self._send_constant_wrench_service(SendConstantWrenchRequest( wrench=Wrench( force=Vector3(0, 0, 0), torque=Vector3(0, 0, 0), ), lifetime=genpy.Duration(1), )) yield util.sleep(.5)
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)
def float(self): while True: self._send_constant_wrench_service( SendConstantWrenchRequest( wrench=Wrench( force=Vector3(0, 0, 0), torque=Vector3(0, 0, 0), ), lifetime=genpy.Duration(1), )) yield util.sleep(.5)
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 _send_result(ci, course, lat, lon): colors = set(['yellow', 'blue', 'black', 'green', 'red']) while colors: color = random.choice(list(colors)) try: res = yield ci.send_pinger_answer(course, color, lat, lon) print res if res: return if not res: colors.remove(color) except: traceback.print_exc() yield util.sleep(5)
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 deploy_hydrophone(self): #INITIALLY WHEN THE BOAT STARTS THE CABLE SHOULD BE FEED OVER THE TOP OF THE SERVO deploy_msg=DynamixelFullConfig() deploy_msg.id=4 #id 4 is the stern servo for the hydrophones deploy_msg.led=0 deploy_msg.goal_position=-2*math.pi deploy_msg.moving_speed=1.4 # 1.4 rad/s~22 rpm deploy_msg.torque_limit=173 # 173/1023 is about 17% torque deploy_msg.goal_acceleration=20 deploy_msg.control_mode=DynamixelFullConfig.CONTINUOUS_ANGLE deploy_msg.goal_velocity=1.4 for i in xrange(100): self.servo_full_config_pub.publish(deploy_msg) yield util.sleep(8.5/100)
def deploy_hydrophone(self): #INITIALLY WHEN THE BOAT STARTS THE CABLE SHOULD BE FEED OVER THE TOP OF THE SERVO deploy_msg = DynamixelFullConfig() deploy_msg.id = 4 #id 4 is the stern servo for the hydrophones deploy_msg.led = 0 deploy_msg.goal_position = -2 * math.pi deploy_msg.moving_speed = 1.4 # 1.4 rad/s~22 rpm deploy_msg.torque_limit = 173 # 173/1023 is about 17% torque deploy_msg.goal_acceleration = 20 deploy_msg.control_mode = DynamixelFullConfig.CONTINUOUS_ANGLE deploy_msg.goal_velocity = 1.4 for i in xrange(100): self.servo_full_config_pub.publish(deploy_msg) yield util.sleep(8.5 / 100)
def retract_hydrophone(self): #WHEN THE HYDROPHONES RETRACT CABLE SHOULD FEED BACK OVER THE TOP OF THE SERVO deploy_msg = DynamixelFullConfig() deploy_msg.id = 4 #id 4 is the stern servo for the hydrophones deploy_msg.led = 0 deploy_msg.goal_position = 4.3 # 2.4 rad/s~22 rpm NOTE: we explicitly retract to pi to try and avoid being at the 0/2*PI boundary on a powerup deploy_msg.moving_speed = 1.4 # 1.4 rad/s~22 rpm deploy_msg.torque_limit = 143 # 143/1023 is about 14% torque (so we don't break the rope if someone didn't feed them correctly to start) deploy_msg.goal_acceleration = 20 deploy_msg.control_mode = DynamixelFullConfig.CONTINUOUS_ANGLE deploy_msg.goal_velocity = 1.4 self.servo_full_config_pub.publish(deploy_msg) for i in xrange(100): self.servo_full_config_pub.publish(deploy_msg) yield util.sleep(20 / 100)
def retract_hydrophone(self): #WHEN THE HYDROPHONES RETRACT CABLE SHOULD FEED BACK OVER THE TOP OF THE SERVO deploy_msg=DynamixelFullConfig() deploy_msg.id=4 #id 4 is the stern servo for the hydrophones deploy_msg.led=0 deploy_msg.goal_position=4.3 # 2.4 rad/s~22 rpm NOTE: we explicitly retract to pi to try and avoid being at the 0/2*PI boundary on a powerup deploy_msg.moving_speed=1.4 # 1.4 rad/s~22 rpm deploy_msg.torque_limit=143 # 143/1023 is about 14% torque (so we don't break the rope if someone didn't feed them correctly to start) deploy_msg.goal_acceleration=20 deploy_msg.control_mode=DynamixelFullConfig.CONTINUOUS_ANGLE deploy_msg.goal_velocity=1.4 self.servo_full_config_pub.publish(deploy_msg) for i in xrange(100): self.servo_full_config_pub.publish(deploy_msg) yield util.sleep(20/100)
def hydrophone_align(self, frequency): good = 0 while True: x = self.float() try: yield util.sleep(.5) ping_return = yield self.get_processed_ping(frequency) finally: x.cancel() print ping_return if ping_return.declination > 1.2: good += 1 if good > 4: return elif abs(ping_return.heading) > math.radians(30): good = 0 yield self.move.yaw_left(ping_return.heading).go() else: good = 0 yield self.move.forward(2).go()
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, 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): sub = yield sub_scripting.get_sub(nh) orig_depth = -sub.pose.position[2] #yield sub.move.forward(1).go() print "aligning down" dist = yield sub.get_dvl_range() fwd_move = sub.move.go(linear=[0.25, 0, 0]) try: yield sub.visual_align('down', 'bins/all', distance_estimate=dist-.3, turn=True, angle=math.radians(45)) finally: fwd_move.cancel() dist = yield sub.get_dvl_range() yield sub.visual_align('down', 'bins/all', distance_estimate=dist-.3, turn=False) #yield sub.move.down(dist-.3 - 2.5).go() print "aligned down" yield sub.move.down(dist - 3.2).go() dist = yield sub.get_dvl_range() centered = sub.move for x in ["1", "4"]: print 'going to', x yield centered.go() print "aligning single", x dist = yield sub.get_dvl_range() yield sub.visual_align('down', 'bins/single', distance_estimate=dist-.3, selector=select(x), turn=False) print 'done' yield sub.move.down(dist - 2).go() yield sub.visual_align('down', 'bins/single', distance_estimate=2, selector=select_centered, turn=True) yield sub.move.backward(.25).go() print 'dropping' yield sub.drop_ball() yield util.sleep(3) print 'done dropping' yield sub.move.depth(orig_depth).go()
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, 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, 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): 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): # #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 orient(boat): angle_to_move = 0 final_cloud = [] x_mean = 0 y_mean = 0 numerator = 0 denom = 0 pointcloud_base = [] # loops until lidar gets a good filtered lidar reading while len(final_cloud) <= 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 = [] # Append pointcloud data to 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 final_cloud = [] # Filter lidar data to only data right in front of the boat # Since both pointcloud have the sane index for the same points, # we then use the baselink pointcloud data to fiter enu points 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: final_cloud.append(pointcloud_enu[i]) # Use final point cloud to get x and y mean to be used in line generation for i in final_cloud: x_mean += i[0] y_mean += i[1] # Find x-mean and y-mean y_mean = y_mean / len(final_cloud) x_mean = x_mean / len(final_cloud) for i in final_cloud: 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 # Vector we want to be parallel to parallel_vector = m * 1 + b # Gets heading of the boat as unit vector position = boat.odom.position[0:2] yaw = quat_to_rotvec(boat.odom.orientation)[2] heading = numpy.array([numpy.cos(yaw), numpy.sin(yaw)]) # Vector to be parrallel to vec1 = numpy.array([x1, parallel_vector, 0]) # Vector of the boat vec2 = numpy.array([(heading[0]), (heading[1]), 0]) angle_to_move = 2 move = 0 angle_to_move = 1 # Finds angle between boat unit vector and dock 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.acos(cosine) # Scan lidar to get the distances from the shore to the boat for use in creating triangle distances = yield boat.get_distance_from_object(.05) # Caluculate distance to move to be directly in front of point theda_one = .9 - angle_to_move L1 = distances[0] distance_to_move = math.sin(theda_one) * L1 if vec2[0] < 0: #print "Yawing left ", angle_to_move print "Moving right ", distance_to_move #boat.move.turn_left(angle_to_move).go() left = boat.move.left(-distance_to_move - 2).go() forward = boat.move.forward(distance_to_move - 3).go() yield left, forward if vec2[0] > 0: #print "Yawing right ", angle_to_move print "Moving left ", distance_to_move #boat.move.turn_left(angle_to_move).go() left = boat.move.left(distance_to_move - 2).go() forward = boat.move.forward(distance_to_move - 3).go() yield left, forward
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 hold_at_current_pos(self): move = self.move.set_position(self.odom.position).go() yield util.sleep(3) move.cancel()
def raise_impaler(self): yield self._set_valve_service(1, False) yield util.sleep(0.1) yield self._set_valve_service(0, True)
def lower_impaler(self): yield self._set_valve_service(0, False) yield util.sleep(0.1) yield self._set_valve_service(1, True)
def close_gripper(self): yield self._set_valve_service(SetValveRequest(valve=1, opened=False)) yield util.sleep(.3) yield self._set_valve_service(SetValveRequest(valve=3, opened=True))
def orient(boat): angle_to_move = 0 final_cloud = [] x_mean = 0 y_mean = 0 numerator = 0 denom = 0 pointcloud_base = [] # loops until lidar gets a good filtered lidar reading while len(final_cloud) <= 0: pointcloud = yield boat.get_pointcloud() yield util.sleep(0.1) # sleep to avoid tooPast errors pointcloud_base = yield boat.to_baselink(pointcloud) yield util.sleep(0.1) # sleep to avoid tooPast errors pointcloud_enu = [] # Append pointcloud data to 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(0.1) # sleep to avoid tooPast errors final_cloud = [] # Filter lidar data to only data right in front of the boat # Since both pointcloud have the sane index for the same points, # we then use the baselink pointcloud data to fiter enu points 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]) < 0.3: final_cloud.append(pointcloud_enu[i]) # Use final point cloud to get x and y mean to be used in line generation for i in final_cloud: x_mean += i[0] y_mean += i[1] # Find x-mean and y-mean y_mean = y_mean / len(final_cloud) x_mean = x_mean / len(final_cloud) for i in final_cloud: 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 # Vector we want to be parallel to parallel_vector = m * 1 + b # Gets heading of the boat as unit vector position = boat.odom.position[0:2] yaw = quat_to_rotvec(boat.odom.orientation)[2] heading = numpy.array([numpy.cos(yaw), numpy.sin(yaw)]) # Vector to be parrallel to vec1 = numpy.array([x1, parallel_vector, 0]) # Vector of the boat vec2 = numpy.array([(heading[0]), (heading[1]), 0]) angle_to_move = 2 move = 0 angle_to_move = 1 # Finds angle between boat unit vector and dock 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.acos(cosine) # Scan lidar to get the distances from the shore to the boat for use in creating triangle distances = yield boat.get_distance_from_object(0.05) # Caluculate distance to move to be directly in front of point theda_one = 0.9 - angle_to_move L1 = distances[0] distance_to_move = math.sin(theda_one) * L1 if vec2[0] < 0: # print "Yawing left ", angle_to_move print "Moving right ", distance_to_move # boat.move.turn_left(angle_to_move).go() left = boat.move.left(-distance_to_move - 2).go() forward = boat.move.forward(distance_to_move - 3).go() yield left, forward if vec2[0] > 0: # print "Yawing right ", angle_to_move print "Moving left ", distance_to_move # boat.move.turn_left(angle_to_move).go() left = boat.move.left(distance_to_move - 2).go() forward = boat.move.forward(distance_to_move - 3).go() yield left, forward
def set_thruster_thread(): while True: pub.publish(ThrusterCommand(force=power_holder[0])) yield util.sleep(.1)