def on_run(self, *args):
if self.last_run_time is None:
return
dt = self.this_run_time - self.last_run_time
self.revolve_angle += self.degrees_per_second * dt
if self.revolve_angle > 390:
Heading(self.initial_heading)()
Pitch(0)()
Roll(0)()
self.finish()
return
rot_2d = rotate((0, 1), self.revolve_angle)
rotation_axis = np.array((rot_2d[0], rot_2d[1], 0))
conic_quat = quat_from_axis_angle(rotation_axis, self.conic_angle_rad)
pointing_vector = conic_quat * np.array((0, 0, 1))
# TODO How can we achieve the same effect without using heading?
heading, pitch, roll = conic_quat.hpr()
Heading(heading)()
Pitch(pitch)()
Roll(roll)()
def on_run(self, *args):
if self.last_run_time is None:
return
dt = self.this_run_time - self.last_run_time
self.revolve_angle += self.degrees_per_second * dt
if self.revolve_angle > 390:
Heading(self.initial_heading)()
Pitch(0)()
Roll(0)()
self.finish()
return
rot_2d = rotate((0, 1), self.revolve_angle)
rotation_axis = np.array((rot_2d[0], rot_2d[1], 0))
conic_quat = quat_from_axis_angle(rotation_axis, self.conic_angle_rad)
# TODO How can we achieve the same effect without using heading?
heading, pitch, roll = conic_quat.hpr()
Heading(heading)()
Pitch(pitch)()
Roll(roll)()
def on_first_run(self, vector, deadband=0.01, *args, **kwargs):
self.pos_con_man = PositionalControlManager()
self.pos_con_man.set(1)
delta_north, delta_east = rotate(vector, kalman.heading.get())
n_position = RelativeToInitialPositionN(offset=delta_north, error=deadband)
e_position = RelativeToInitialPositionE(offset=delta_east, error=deadband)
self.motion = Concurrent(n_position, e_position, finite=False)
def on_first_run(self, vector, deadband=0.01, *args, **kwargs):
delta_north, delta_east = rotate(vector, kalman.heading.get())
n_position = RelativeToInitialPositionN(offset=delta_north,
error=deadband)
e_position = RelativeToInitialPositionE(offset=delta_east,
error=deadband)
self.use_task(
WithPositionalControl(
Concurrent(n_position, e_position, finite=False), ))
def on_first_run(self):
self.heading = shm.kalman.heading.get()
self.red_buoy = aslam.world.red_buoy.position()[:2]
self.green_buoy = aslam.world.green_buoy.position()[:2]
self.yellow_buoy = aslam.world.yellow_buoy.position()[:2]
self.all_buoys = [('red', self.red_buoy), ('green', self.green_buoy), ('yellow', self.yellow_buoy)]
self.sorted_buoys = sorted(self.all_buoys, key = lambda x: rotate(x[1], -self.heading)[1])
self.logi('Sorted buoys (left-to-right): {}'.format([x[0] for x in self.sorted_buoys]))
subtasks = []
subtasks.append(MasterConcurrent(HPRWiggle(), MoveXRough(-1.0)))
subtasks.append(Depth(PIPE_SEARCH_DEPTH))
if self.sorted_buoys[0][0] == 'yellow':
# yellow buoy far left, go right
subtasks.append(MoveYRough(1.0))
elif self.sorted_buoys[1][0] == 'yellow':
subtasks.append(MoveYRough(1.0))
else:
subtasks.append(MoveYRough(-1.0))
subtasks.append(MoveXRough(1.0))
center_buoy = n.array(self.sorted_buoys[1][1])
center_buoy += n.array(rotate((1, 0), self.heading)) # 1m beyond center buoy
subtasks.append(GoToPosition(center_buoy[0], center_buoy[1], depth=PIPE_SEARCH_DEPTH))
self.subtask = Sequential(*subtasks)
def on_first_run(self):
self.heading = shm.kalman.heading.get()
self.red_buoy = aslam.world.red_buoy.position()[:2]
self.green_buoy = aslam.world.green_buoy.position()[:2]
self.yellow_buoy = aslam.world.yellow_buoy.position()[:2]
self.all_buoys = [('red', self.red_buoy), ('green', self.green_buoy), ('yellow', self.yellow_buoy)]
self.sorted_buoys = sorted(self.all_buoys, key = lambda x: rotate(x[1], -self.heading)[1])
self.logi('Sorted buoys (left-to-right): {}'.format([x[0] for x in self.sorted_buoys]))
subtasks = []
subtasks.append(MasterConcurrent(HPRWiggle(), MoveXRough(-1.0)))
subtasks.append(Depth(PIPE_SEARCH_DEPTH))
if self.sorted_buoys[0][0] == 'yellow':
# yellow buoy far left, go right
subtasks.append(MoveYRough(1.0))
elif self.sorted_buoys[1][0] == 'yellow':
subtasks.append(MoveYRough(1.0))
else:
subtasks.append(MoveYRough(-1.0))
subtasks.append(MoveXRough(1.0))
center_buoy = n.array(self.sorted_buoys[1][1])
center_buoy += n.array(rotate((1, 0), self.heading)) # 1m beyond center buoy
subtasks.append(GoToPosition(center_buoy[0], center_buoy[1], depth=PIPE_SEARCH_DEPTH))
self.subtask = Sequential(*subtasks)
def follow_heading(self, new_ping):
distance_to_pinger = self.elevation_to_distance(new_ping.elevation)
speed = self.get_follow_speed(distance_to_pinger)
# Check if the ping suggests the pinger is in a heading deviating from
# the current follow heading
new_ping_heading = (new_ping.heading + shm.kalman.heading.get()) % 360
heading_deviation = abs(new_ping_heading - self.heading_to_pinger)
# When we are close to the pinger, headings will vary more, allow for
# deviation
# Note, this is fairly broken, because it assumes we get good pings
# while moving! Realistically, we should just move forward for a set time,
# and then get new heading. BUT, since the consistency check will really
# just wait for the requisite number of pings (since its a safe assumption
# that we're just getting garbage), we can essentially turn this into
# a makeshift timer by changing the window size on consistency check (:139)
if new_ping.elevation > MIN_DEVIATING_PING_ELEVATION:
deviating_ping = heading_deviation > MAX_FOLLOW_HEADING_DEVIATION
if deviating_ping:
self.logi(
"Ping heading deviated from follow heading by %0.3f, more "
"than maximum allowed" % heading_deviation)
else:
self.heading_to_pinger = new_ping_heading
if self.ping_deviating_checker.check(deviating_ping):
self.logi(
"Consistently getting deviating pings! Stopping to listen."
)
self.ping_deviating_checker.clear()
return "listen"
self.logi("Going straight for the pinger!")
self.follow_vel_x(x_dir * speed)
self.follow_vel_y(0.0)
self.follow_change_heading(self.heading_to_pinger + heading_offset)
else:
velocity = rotate([speed, 0], new_ping.heading)
self.logi("We are close! Translating to pinger: Velocity (%0.3f, "
"%0.3f)" % (velocity[0], velocity[1]))
self.follow_vel_x(velocity[0])
self.follow_vel_y(velocity[1])
return None
def follow_heading(self, new_ping):
distance_to_pinger = self.elevation_to_distance(new_ping.elevation)
speed = self.get_follow_speed(distance_to_pinger)
# Check if the ping suggests the pinger is in a heading deviating from
# the current follow heading
new_ping_heading = (new_ping.heading + shm.kalman.heading.get()) % 360
heading_deviation = abs(new_ping_heading - self.heading_to_pinger)
# When we are close to the pinger, headings will vary more, allow for
# deviation
if new_ping.elevation > MIN_DEVIATING_PING_ELEVATION:
deviating_ping = heading_deviation > MAX_FOLLOW_HEADING_DEVIATION
if deviating_ping:
self.logi(
"Ping heading deviated from follow heading by %0.3f, more "
"than maximum allowed" % heading_deviation)
else:
self.heading_to_pinger = new_ping_heading
if self.ping_deviating_checker.check(deviating_ping):
self.logi(
"Consistently getting deviating pings! Stopping to listen."
)
self.ping_deviating_checker.clear()
return "listen"
self.logi("Going straight for the pinger!")
self.follow_vel_x(speed)
self.follow_vel_y(0.0)
self.follow_change_heading(self.heading_to_pinger)
else:
velocity = rotate([speed, 0], new_ping.heading)
self.logi("We are close! Translating to pinger: Velocity (%0.3f, "
"%0.3f)" % (velocity[0], velocity[1]))
self.follow_vel_x(velocity[0])
self.follow_vel_y(velocity[1])
return None
def on_first_run(self, angle, distance, deadband=0.01, *args, **kwargs):
super().on_first_run(rotate((distance, 0), angle), deadband=deadband)
def on_first_run(self, angle, distance, deadband=0.01, *args, **kwargs):
super().on_first_run(rotate((distance, 0), angle), deadband=deadband)
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)
def calculate_bin_vector(bin1, bin2): body_frame = [(bin1.x, bin1.y), (bin2.x, bin2.y)] world_frame = [np.array(rotate(body, -shm.kalman.heading.get())) for body in body_frame] bin2bin = world_frame[1] - world_frame[0] return bin2bin / np.linalg.norm(bin2bin)