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 -