def on_first_run(self, slide_direction="right", *args, **kwargs): self.logi("Starting UncoverBin task") self.init_time = self.this_run_time if slide_direction == "right": factor = 1 elif slide_direction == "left": factor = -1 else: raise Exception("Invalid slide direction %s for UncoverBin" % slide_direction) self.initial_depth = shm.kalman.depth.get() self.set_depth = Depth(self.initial_depth + shm.dvl.savg_altitude.get() - BINS_PICKUP_ALTITUDE, deadband=0.01) self.initial_position = get_sub_position() self.pickup = MasterConcurrent(Sequential(Timer(2.0), Roll(-30 * factor, error=90)), VelocityY(1.0 * factor)) self.drop = Sequential(Roll(90 * factor), Timer(1.0), Roll(120 * factor, error=40), Timer(0.5), Roll(90 * factor), Timer(0.3), Roll(45 * factor), Timer(0.5), Roll(0)) #self.return_to_bin = GoToPosition(lambda: self.initial_position[0], lambda: self.initial_position[1], depth=self.initial_depth) self.return_to_bin = Sequential(Timed(VelocityY(-1.0 * factor), 2.0), VelocityY(0.0), MoveX(-BINS_CAM_TO_ARM_OFFSET)) self.tasks = Sequential( MoveY(-0.65 * factor), MoveX(BINS_CAM_TO_ARM_OFFSET), self.set_depth, Roll(60 * factor), self.pickup, VelocityY(0.0), RelativeToInitialDepth(-1.0), Timed(VelocityY(1.0 * factor), 1.0), VelocityY(0.0), self.drop, Timed(VelocityY(-1.0 * factor), 1.0), self.return_to_bin )
def func(watcher, quit_event): watcher.watch(shm.hydrophones_results_track) observations = [] localizer = Localizer(shm.hydrophones_settings.track_frequency_target.get()) while not quit_event.is_set(): #input() results = shm.hydrophones_results_track.get() kalman = shm.kalman.get() sub_pos = get_sub_position(kalman) sub_quat = get_sub_quaternion(kalman) print("------------------------------------------------------") localizer.add_observation((results.diff_phase_x, results.diff_phase_y), sub_pos, sub_quat) pinger_pos, pinger_pos_all = localizer.compute_position() print("%d observations. current position: %0.2f %0.2f %0.2f" % \ (len(localizer.observations), sub_pos[0], sub_pos[1], sub_pos[2])) print("Predicted (2 Trans): %0.2f %0.2f %0.2f" % \ (pinger_pos[0], pinger_pos[1], pinger_pos[2])) print("Predicted (3 Trans): %0.2f %0.2f %0.2f" % \ (pinger_pos_all[0], pinger_pos_all[1], pinger_pos_all[2])) watcher.wait(new_update=False)
def func(watcher, quit_event): watcher.watch(shm.hydrophones_results_track) observations = [] localizer = Localizer( shm.hydrophones_settings.track_frequency_target.get()) while not quit_event.is_set(): # input() results = shm.hydrophones_results_track.get() kalman = shm.kalman.get() sub_pos = get_sub_position(kalman) sub_quat = get_sub_quaternion(kalman) print("------------------------------------------------------") localizer.add_observation( (results.diff_phase_x, results.diff_phase_y), sub_pos, sub_quat) pinger_pos, pinger_pos_all = localizer.compute_position() print("%d observations. current position: %0.2f %0.2f %0.2f" % \ (len(localizer.observations), sub_pos[0], sub_pos[1], sub_pos[2])) print("Predicted (2 Trans): %0.2f %0.2f %0.2f" % \ (pinger_pos[0], pinger_pos[1], pinger_pos[2])) print("Predicted (3 Trans): %0.2f %0.2f %0.2f" % \ (pinger_pos_all[0], pinger_pos_all[1], pinger_pos_all[2])) watcher.wait(new_update=False)
def collision_validator(self): # TODO even more robust collision validator current = yellow_buoy_results.percent_frame.get() aligned = self.concurrent_align_task.finished max_ram_speed = self.MAX_RAM_SPEED if not aligned: max_ram_speed /= 3 self.ram_speed = scaled_speed(final_value=self.PERCENT_FRAME_THRESHOLD + 1, initial_value=self.PERCENT_FRAME_SLOWDOWN_THRESHOLD, final_speed=0.0, initial_speed=max_ram_speed, current_value=current) if self.ready_to_ram_checker.check(current >= self.PERCENT_FRAME_THRESHOLD and \ aligned): self.target_position = get_sub_position() self.target_depth = shm.kalman.depth.get() self.logi("Close enough to yellow buoy to scuttle!") return True #if yellow_buoy_results.center_y.get() > (shm.camera.forward_height.get() - 10) \ # and abs(self.last_percent_frame - current) <= self.PERCENT_FRAME_DELTA_THRESHOLD: # return True #self.last_percent_frame = current return False
def on_first_run(self): shm.hydrophones_settings.track_frequency_target.set(PINGER_FREQUENCY) shm.hydrophones_settings.track_magnitude_threshold.set(TRACK_MAG_THRESH) shm.hydrophones_settings.track_cooldown_samples.set(150000) shm.navigation_settings.position_controls.set(1) shm.navigation_settings.optimize.set(0) self.localizer = Localizer(PINGER_FREQUENCY) self.hydro_watcher = shm.watchers.watcher() self.hydro_watcher.watch(shm.hydrophones_results_track) self.time_since_last_ping = self.this_run_time self.pinger_positions = collections.deque(maxlen=7) self.PINGS_LISTEN = 10 self.CONSISTENT_PINGS = 3 self.listens = 0 self.queued_moves = [] self.pinger_found = False self.silencer = ThrusterSilencer() self.available_depths = set([HYDROPHONES_SEARCH_DEPTH, HYDROPHONES_SEARCH_DEPTH + 0.4, HYDROPHONES_SEARCH_DEPTH + 0.2]) self.last_depth = None self.elevation_checker = ConsistencyCheck(3, 3) self.observe_from(get_sub_position())
def update_pings(self): self.silencer() # TODO Better way to filter out bad pings when thrusters are running. # If the watcher has not changed, there is no new ping if not self.hydro_watcher.has_changed(): # TODO Do something here if too much time has passed since last ping. return None # There is a new ping! self.time_since_last_ping = self.this_run_time # TODO Will this be long after the watcher fired? # Need to ensure that there is little delay. results = shm.hydrophones_results_track.get() kalman = shm.kalman.get() phases = (results.diff_phase_x, results.diff_phase_y) if not self.localizer.is_valid(phases[0], phases[1]): self.logi("Warning: Measured phases (%0.3f %0.3f) are not possible given " "physical parameters" % \ (phases[0], phases[1])) head, elev = self.localizer.get_heading_elevation(*phases) abs_head = (head + shm.kalman.heading.get()) % 360 self.logi("Ping: Phases: (%0.3f %0.3f), Relative Heading: %0.5f, Absolute " "Heading: %0.3f, Elevation: %0.1f" % \ (phases[0], phases[1], head, abs_head, elev)) in_silence = self.silencer.in_silence() # TODO Better way to detect ping period. this_ping_time = \ results.daemon_start_time + results.tracked_ping_time self.silencer.schedule_silence(this_ping_time + PINGER_PERIOD - 0.2, 3.0) self.logi("This time: %0.3f, This ping time %0.3f, Diff: %0.3f" % (self.this_run_time, this_ping_time, self.this_run_time - this_ping_time)) # If the thrusters are not in silence, then we may have heard thruster # noise and thought it was a ping; ignore it if not in_silence: self.logi("Heard ping but thrusters are not silenced, ignoring") return None # Record ping data sub_pos = get_sub_position(kalman) sub_quat = get_sub_quaternion(kalman) ping_data = PingData(phases, head, elev, sub_pos, sub_quat) self.pings.append(ping_data) return ping_data
def on_first_run(self, slide_direction="right", *args, **kwargs): self.logi("Starting UncoverBin task") self.init_time = self.this_run_time if slide_direction == "right": factor = 1 elif slide_direction == "left": factor = -1 else: raise Exception("Invalid slide direction %s for UncoverBin" % slide_direction) self.initial_depth = shm.kalman.depth.get() pool_depth = get_pool_depth(self.logw, self.initial_depth) self.set_depth = Timeout(Depth(pool_depth - BINS_PICKUP_ALTITUDE, deadband=0.01), 15) self.initial_position = get_sub_position() SLIDE_SPEED = 0.35 self.pickup = MasterConcurrent(Sequential(Timer(0.7 * 6), Roll(0, error=90), Timer(0.3 * 2), Timed(Concurrent(RelativeToCurrentDepth(-2.0), Roll(-15 * factor, error=90)), 2.0 * 1), RelativeToCurrentDepth(0, error=2), Roll(-30 * factor, error=90)), VelocityY(SLIDE_SPEED * factor)) self.drop = Sequential(Roll(90 * factor, error=20), Timer(1.0), Roll(120 * factor, error=40), Timer(0.5), Roll(90 * factor, error=20), Timer(0.3), Roll(45 * factor), Timer(0.5), Roll(0)) #self.return_to_bin = GoToPosition(lambda: self.initial_position[0], lambda: self.initial_position[1], depth=self.initial_depth) self.return_to_bin = Sequential(Timed(VelocityY(-1.0 * factor), 1.8), VelocityY(0.0), Timeout(MoveX(-BINS_CAM_TO_ARM_OFFSET, deadband=0.03), 10)) self.tasks = Sequential( Timeout(MoveY(-0.85 * factor, deadband=0.025), 10), Timeout(MoveX(BINS_CAM_TO_ARM_OFFSET, deadband=0.015), 10), self.set_depth, Roll(60 * factor, error=20), self.pickup, VelocityY(0.0), RelativeToInitialDepth(-0.4, error=2), Timed(VelocityY(1.0 * factor), 1.0), VelocityY(0.0), self.drop, Timed(VelocityY(-1.0 * factor), 1.0), self.return_to_bin )
def downward_position(camera_position=(0, 0)): """ distance = (objsize) * (camsize) / (image size * tan(theta) very similar to distance_from_size """ # TODO: add pitch and roll compensation pos = get_sub_position()[:2] altitude = shm.dvl.savg_altitude.get() heading_rad = radians(shm.kalman.heading.get()) heading_v = numpy.array((cos(heading_rad), sin(heading_rad))) heading_yv = numpy.array((-heading_v[1], heading_v[0])) screen_width = shm.camera.downward_width.get() screen_height = shm.camera.downward_height.get() screen_area = screen_width * screen_height if screen_area == 0: return numpy.array((0, 0)) from locator.camera import compute_fov_from_lens_params, DOWNWARD_LENS_PARAMS h_fov, v_fov = compute_fov_from_lens_params(*DOWNWARD_LENS_PARAMS) max_angle_x = 2 * tan(h_fov / 2) max_angle_y = 2 * tan(v_fov / 2) camera_x, camera_y = camera_position camera_x -= .5 * screen_width camera_y -= .5 * screen_height x_scale = (camera_x / screen_width) * max_angle_x y_scale = (camera_y / screen_height) * max_angle_y x_distance = -1 * altitude * y_scale y_distance = altitude * x_scale return pos + (x_distance * heading_v) + (y_distance * heading_yv)
def downward_position(camera_position=(0, 0)): """ distance = (objsize) * (camsize) / (image size * tan(theta) very similar to distance_from_size """ # TODO: add pitch and roll compensation pos = get_sub_position()[:2] altitude = shm.dvl.savg_altitude.get() heading_rad = radians(shm.kalman.heading.get()) heading_v = numpy.array((cos(heading_rad), sin(heading_rad))) heading_yv = numpy.array((-heading_v[1], heading_v[0])) screen_width = shm.camera.downward_width.get() screen_height = shm.camera.downward_height.get() screen_area = screen_width * screen_height if screen_area == 0: return numpy.array((0, 0)) from locator.camera import compute_fov_from_lens_params, DOWNWARD_LENS_PARAMS h_fov, v_fov = compute_fov_from_lens_params(*DOWNWARD_LENS_PARAMS) max_angle_x = 2 * tan(h_fov / 2) max_angle_y = 2 * tan(v_fov / 2) camera_x, camera_y = camera_position camera_x -= .5 * screen_width camera_y -= .5 * screen_height x_scale = (camera_x / screen_width) * max_angle_x y_scale = (camera_y / screen_height) * max_angle_y x_distance = -1 * altitude * y_scale y_distance = altitude * x_scale return pos + (x_distance * heading_v) + (y_distance * heading_yv)
def on_run(self): self.motion_tasks() self.silencer() # TODO Better way to filter out bad pings when thrusters are running. if not self.hydro_watcher.has_changed() or \ (not LISTEN_WHILE_MOVING and not self.motion_tasks.finished): # TODO Do something here if too much time has passed since last ping. return if self.pinger_found: self.finish() return self.time_since_last_ping = self.this_run_time self.pings_here += 1 # TODO Will this be long after the watcher fired? # Need to ensure that there is little delay. results = shm.hydrophones_results_track.get() kalman = shm.kalman.get() phases = (results.diff_phase_x, results.diff_phase_y) self.logi("Got " + str(phases)) in_silence = self.silencer.in_silence() # TODO Better way to detect ping period. self.silencer.schedule_silence(self.this_run_time + PINGER_PERIOD - 0.1, 0.4) if not in_silence: return sub_pos = get_sub_position(kalman) sub_quat = get_sub_quaternion(kalman) ping_data = PingData(phases, sub_pos, sub_quat) self.pings.append(ping_data) if len(self.pings) < 2: return if not CLUSTER_HEADINGS: data = get_clusterable([ping.phases[0] for ping in self.pings]) clusters = fclusterdata(data, 0.3, criterion="distance") else: headings, elevations = zip(*[self.localizer.get_heading_elevation(*ping.phases) for ping in self.pings]) data = get_clusterable(headings) clusters = fclusterdata(data, 8, criterion="distance") counted = collections.Counter(clusters) best_cluster, n_best = max(counted.items(), key=lambda item: item[1]) if n_best >= self.CONSISTENT_PINGS: good_pings = [self.pings[i] for i, cluster_num in enumerate(clusters) \ if cluster_num == best_cluster] avg_phase_x = np.mean([ping.phases[0] for ping in good_pings]) avg_phase_y = np.mean([ping.phases[1] for ping in good_pings]) self.logi("Found nice phases (%f, %f) in %s" % \ (avg_phase_x, avg_phase_y, str(self.pings))) for ping in good_pings: self.localizer.add_observation(ping.phases, ping.sub_pos, ping.sub_quat) est_pinger_pos = self.localizer.compute_position()[USE_THREE] self.pinger_positions.append(est_pinger_pos) self.logi("We think the pinger is at %s" % str(est_pinger_pos)) self.logi("All estimated positions: %s" % str(self.pinger_positions)) if len(self.pinger_positions) > 5: data = np.array(self.pinger_positions) clusters = fclusterdata(data, 0.6, criterion="distance") counted = collections.Counter(clusters) best_cluster, n_best = max(counted.items(), key=lambda item: item[1]) if n_best >= 3: avg_position = sum([np.array(self.pinger_positions[i]) for i, cluster_num in enumerate(clusters) if cluster_num == best_cluster]) / n_best self.logi("Found pinger at %0.2f %0.2f using localization!" % \ (avg_position[0], avg_position[1])) self.observe_from(avg_position) self.pinger_found = True return if FOLLOW_HEADING: heading, elevation = self.localizer.get_heading_elevation(avg_phase_x, avg_phase_y) depth_distance = HYDROPHONES_PINGER_DEPTH - kalman.depth distance = math.tan(math.radians(elevation)) * depth_distance # Elevations seem to be wrong; nerf them. distance = min(0.5 * distance, 4.2) direction = rotate((1, 0), kalman.heading + heading) observe_position = sub_pos + np.array((direction[0], direction[1], sub_pos[2])) * distance self.logi("Heading is %f, Elevation is %f" % (heading, elevation)) self.observe_from(observe_position, kalman.heading + heading) if self.elevation_checker(elevation < 25): self.logi("Found pinger using elevations!") self.pinger_found = True elif not FOLLOW_HEADING: if self.listens <= 3: # TODO Change heading in an optimal fashion. self.observe_from(sub_pos, heading=(kalman.heading + 30) % 360) else: if not self.queued_moves: to_pinger = est_pinger_pos[:2] - sub_pos[:2] distance = np.linalg.norm(to_pinger) direction = to_pinger / distance move_dist = 3 if self.listens > 10 and distance < 3: move_dist = distance new_pos = sub_pos[:2] + direction * move_dist heading = math.atan2(direction[1], direction[0]) if DO_PARALLAX: perp = np.array(rotate(direction, 90)) self.queued_moves.extend([(new_pos, heading), (new_pos + 2 * perp, heading), (new_pos - 2 * perp, heading)]) else: if PERP_TO_PINGER: heading += 90 self.queued_moves.extend([(new_pos, heading+30), (new_pos, heading-30), (new_pos, heading)]) next_pos, heading = self.queued_moves.pop(0) self.observe_from(next_pos, heading)
def on_run(self): self.motion_tasks() self.silencer() # TODO Better way to filter out bad pings when thrusters are running. if not self.hydro_watcher.has_changed() or \ not self.motion_tasks.finished: # TODO Do something here if too much time has passed since last ping. return if self.pinger_found: self.finish() return self.time_since_last_ping = self.this_run_time self.pings_here += 1 # TODO Will this be long after the watcher fired? # Need to ensure that there is little delay. results = shm.hydrophones_results_track.get() kalman = shm.kalman.get() phases = (results.diff_phase_x, results.diff_phase_y) self.logi("Got " + str(phases)) in_silence = self.silencer.in_silence() self.silencer.schedule_silence(self.this_run_time + 0.9, 0.4) if not in_silence: return self.pings.append(results.diff_phase_x) if len(self.pings) < 2: return data = get_clusterable(self.pings) clusters = fclusterdata(data, 0.1, criterion="distance") counted = collections.Counter(clusters) best_cluster, n_best = max(counted.items(), key=lambda item: item[1]) if n_best >= self.CONSISTENT_PINGS: avg_phase = sum([self.pings[i] for i, cluster_num in enumerate(clusters) if cluster_num == best_cluster]) / n_best self.logi("Found nice phase %f in %s" % (avg_phase, str(self.pings))) if FOLLOW_HEADING: # TODO Use average x and y phases here. heading, elevation = self.localizer.get_heading_elevation(*phases) if elevation > 45: move_distance = 4 elif elevation > 30: move_distance = 2 else: move_distance = 1 sub_pos = get_sub_position(kalman) direction = rotate((1, 0), kalman.heading + heading) observe_position = sub_pos + np.array((direction[0], direction[1], sub_pos[2])) * move_distance self.logi("Heading is %f, Elevation is %f" % (heading, elevation)) self.observe_from(observe_position, kalman.heading + heading) elif not FOLLOW_HEADING: sub_pos = get_sub_position(kalman) sub_quat = get_sub_quaternion(kalman) self.localizer.add_observation(phases, sub_pos, sub_quat) # For now, use the estimate from only 2 transducers. est_pinger_pos = self.localizer.compute_position()[USE_THREE] self.pinger_positions.append(est_pinger_pos) self.logi("We think the pinger is at %s" % str(est_pinger_pos)) self.logi("All estimated positions: %s" % str(self.pinger_positions)) if len(self.pinger_positions) > 5: data = np.array(self.pinger_positions) clusters = fclusterdata(data, 0.5, criterion="distance") counted = collections.Counter(clusters) best_cluster, n_best = max(counted.items(), key=lambda item: item[1]) if n_best > 3: avg_position = sum([np.array(self.pinger_positions[i]) for i, cluster_num in enumerate(clusters) if cluster_num == best_cluster]) / n_best self.logi("Found pinger at %0.2f %0.2f!" % (avg_position[0], avg_position[1])) self.observe_from(avg_position) self.pinger_found = True return if self.listens <= 3: # TODO Change heading in an optimal fashion. self.observe_from(sub_pos, heading=(kalman.heading + 30) % 360) else: if not self.queued_moves: to_pinger = est_pinger_pos[:2] - sub_pos[:2] distance = np.linalg.norm(to_pinger) direction = to_pinger / distance move_dist = 3 if self.listens > 10 and distance < 3: move_dist = distance new_pos = sub_pos[:2] + direction * move_dist heading = math.atan2(direction[1], direction[0]) if DO_PARALLAX: perp = np.array(rotate(direction, 90)) self.queued_moves.extend([(new_pos, heading), (new_pos + 2 * perp, heading), (new_pos - 2 * perp, heading)]) else: if PERP_TO_PINGER: heading += 90 self.queued_moves.extend([(new_pos, heading+30), (new_pos, heading-30), (new_pos, heading)]) next_pos, heading = self.queued_moves.pop(0) self.observe_from(next_pos, heading)
PINGER_INTERVAL = 2.0 SOUND_SPEED = 1481.0 NIPPLE_DISTANCE = 0.013 PHASE2RATIO = SOUND_SPEED / (2 * math.pi * PINGER_FREQUENCY * NIPPLE_DISTANCE) hydro_shm = shm.hydrophones_results_track # Vector in body frame between first and second transducers. trans_vec = np.array((1, 0, 0)) trans_vec2 = np.array((0, -1, 0)) while 1: kalman = shm.kalman.get() sub_pos = get_sub_position(kalman) sub_quat = get_sub_quaternion(kalman) trans_deltas = (sub_quat * trans_vec, sub_quat * trans_vec2) ratios = misc.hydro2trans.h(PINGER_POSITION, sub_pos, trans_deltas) phases = [ratio / PHASE2RATIO for ratio in ratios] hydro_group = hydro_shm.get() hydro_group.diff_phase_x = phases[0] hydro_group.diff_phase_y = phases[1] hydro_shm.set(hydro_group) time.sleep(PINGER_INTERVAL)
# Vector in body frame between first and second transducers. trans_vec = np.array((1, 0, 0)) trans_vec2 = np.array((0, -1, 0)) start_time = time.time() daemon_start_time = int(start_time) # Turn off the default Python ^C handler that raises a KeyboardInterrupt. # Now SIGINT should behave identically to SIGTERM: kill the program. import signal signal.signal(signal.SIGINT, signal.SIG_DFL) while True: kalman = shm.kalman.get() sub_pos = get_sub_position(kalman) sub_quat = get_sub_quaternion(kalman) trans_deltas = (sub_quat * trans_vec, sub_quat * trans_vec2) ratios = misc.hydro2trans.h(PINGER_POSITION, sub_pos, trans_deltas) phases = [ratio / PHASE2RATIO for ratio in ratios] hydro_group = hydro_shm.get() hydro_group.diff_phase_x = phases[0] hydro_group.diff_phase_y = phases[1] hydro_group.ping_clock = 0 if hydro_group.ping_clock else 1 hydro_group.daemon_start_time = daemon_start_time hydro_group.tracked_ping_time = time.time() - daemon_start_time hydro_shm.set(hydro_group) time.sleep(PINGER_INTERVAL -