class IdentifyBins(Task):
"""Identifies which bin to drop markers into, centers over it
Start: Both bins visible in downward cam
Finish: Centered over chosen bin
"""
def on_first_run(self, bin_group, heading=None, *args, **kwargs):
self.logi("Centering over bins...")
self.logv("Starting IdentifyBins task")
self.task = DownwardTarget(px=0.0025, py=0.0025)
self.align_checker = ConsistencyCheck(6, 6)
# TODO start alignment task.
self.init_time = self.this_run_time
self.bin_group = bin_group
if bin1.covered == FAST_RUN:
self.target_bin = bin1
else:
self.target_bin = bin2
def on_run(self, bin_group, heading=None):
self.bin1_results = self.bin_group.get()
target = get_camera_center(self.bin1_results)
# TODO Increase deadband as time increases.
self.task((self.bin1_results.x, self.bin1_results.y), target=target, deadband=(25, 25), valid=lambda: self.bin1_results.probability > 0.0)
if self.task.finished:
if heading is None:
target_heading = shm.kalman.heading.get() + self.bin1_results.angle
else:
target_heading = heading()
align_task = Heading(target_heading, deadband=0.5)
align_task()
if self.align_checker.check(align_task.finished):
VelocityX(0)()
VelocityY(0)()
self.finish()
else:
self.align_checker.clear()
def on_finish(self):
self.logi("Centered!")
self.logv('IdentifyBins task finished in {} seconds!'.format(
self.this_run_time - self.init_time))
class ConsistentObject:
"""
Consistency-check an object's existence
Inputted objects must have an x and y field
"""
def __init__(self, seen_cons_check=(2, 3), unseen_cons_check=(5, 6)):
self.last_obj = None
self.tracking = False
self.seen_cons_check = ConsistencyCheck(*seen_cons_check)
self.unseen_cons_check = ConsistencyCheck(*unseen_cons_check)
def map(self, obj):
"""
Call with a valid object to count as "seeing the object", or with None
to count as "not seeing the object"
"""
if obj is not None:
self.last_obj = obj
if self.tracking:
self.unseen_cons_check.add(False)
else:
self.seen_cons_check.add(True)
if self.seen_cons_check.check():
self.tracking = True
self.seen_cons_check.clear()
self.unseen_cons_check.clear()
else:
if self.tracking:
self.unseen_cons_check.add(True)
if self.unseen_cons_check.check():
self.tracking = False
self.seen_cons_check.clear()
self.unseen_cons_check.clear()
self.last_obj = None
else:
self.seen_cons_check.add(False)
self.last_obj = None
if self.tracking:
return self.last_obj
else:
return None
class ConsistentObject:
"""
Consistency-check an object's existence
"""
def __init__(self, seen_cons_check=(2, 3), unseen_cons_check=(5, 6)):
self.last_obj = None
self.tracking = False
self.seen_cons_check = ConsistencyCheck(*seen_cons_check)
self.unseen_cons_check = ConsistencyCheck(*unseen_cons_check)
def map(self, obj):
"""
Call with a valid object to count as "seeing the object", or with None
to count as "not seeing the object"
"""
if obj is not None:
self.last_obj = obj
if self.tracking:
self.unseen_cons_check.add(False)
else:
self.seen_cons_check.add(True)
if self.seen_cons_check.check():
self.tracking = True
self.seen_cons_check.clear()
self.unseen_cons_check.clear()
else:
if self.tracking:
self.unseen_cons_check.add(True)
if self.unseen_cons_check.check():
self.tracking = False
self.seen_cons_check.clear()
self.unseen_cons_check.clear()
self.last_obj = None
else:
self.seen_cons_check.add(False)
self.last_obj = None
if self.tracking:
return self.last_obj
else:
return None
class FindPinger(StatefulTask):
def __init__(self,
method,
ontop_of_pinger=ontop_of_pinger_elevation,
*args,
**kwargs):
super().__init__(*args, **kwargs)
self.track_method = method
self.ontop_of_pinger = ontop_of_pinger
def on_first_run(self):
assert isinstance(self.track_method, TrackMethod)
self.follow_task = None
self.heading_to_pinger = None
set_gain()
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(
TRACK_COOLDOWN_SAMPLES)
shm.navigation_settings.position_controls.set(1)
shm.navigation_settings.optimize.set(0)
self.localizer = Localizer(PINGER_FREQUENCY)
self.has_made_progress = False
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.silencer = ThrusterSilencer()
self.pinger = None
self.ping_deviating_checker = ConsistencyCheck(7, 10)
ontop_success = None
if STOP_OVER_PINGER:
ontop_success = 2
else:
ontop_success = 6
self.ontop_checker = ConsistencyCheck(ontop_success, 5)
self.pings = []
def generate_states(self):
return "listen", {
"listen": {
"enter": self.enter_listen,
"exit": self.exit_listen,
"tick": self.listen
},
"follow": {
"enter": self.enter_follow,
"tick": self.follow
}
}
# Check for new pings, if there is one return its heading and elevation and add it to the
# pings list
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 enter_listen(self):
self.logi("Stopping to listen for pings")
kalman = shm.kalman.get()
shm.navigation_settings.position_controls.set(1)
desires = shm.navigation_desires.get()
desires.north = kalman.north
desires.east = kalman.east
shm.navigation_desires.set(desires)
# Clear previous pings
self.pings = []
def exit_listen(self):
self.logi("Exiting listen state")
shm.navigation_settings.position_controls.set(0)
def listen(self):
self.update_pings()
# If we haven't even gotten MIN_CONSISTENT_PINGS in total, there won't be
# enough consistent ones, no point in continuing
if len(self.pings) < MIN_CONSISTENT_PINGS:
return
# We want to make sure pings are consistent before following them. To do
# this, first cluster the pings by heading
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")
# The best cluster is the one with the most pings in it
counted = collections.Counter(clusters)
best_cluster, n_best = max(counted.items(), key=lambda item: item[1])
# To follow a heading, we require that at least three pings are in the
# cluster which contains it
if n_best >= MIN_CONSISTENT_PINGS:
# Compute average phase for best cluster
good_pings = [self.pings[i] for i, cluster_num in enumerate(clusters) \
if cluster_num == best_cluster]
x_phases = [ping.phases[0] for ping in good_pings]
y_phases = [ping.phases[1] for ping in good_pings]
avg_phase_x = np.mean(x_phases)
avg_phase_y = np.mean(y_phases)
self.logi("Average phases (%f, %f) for best cluster %s" % \
(avg_phase_x, avg_phase_y, str(self.pings)))
if self.track_method == TrackMethod.position:
# Localize pinger to a position
for ping in good_pings:
self.localizer.add_observation(ping.phases, ping.sub_pos,
ping.sub_quat)
est_pinger_pos = self.localizer.compute_position()
self.pinger_positions.append(est_pinger_pos)
self.logi("Localized pinger to: %s" % str(est_pinger_pos))
self.logi("All estimated positions: %s" %
str(self.pinger_positions))
self.follow_position = est_pinger_pos
elif self.track_method == TrackMethod.heading:
# Update the heading and elevation of the pinger
pinger_head, elev = self.localizer.get_heading_elevation(
avg_phase_x, avg_phase_y)
sub_head = shm.kalman.heading.get()
self.heading_to_pinger = heading = (pinger_head +
sub_head) % 360
self.follow_elevation = elev
return "follow"
def elevation_to_distance(self, elevation):
return HYDROPHONES_PINGER_DEPTH * math.tan(math.radians(elevation))
def enter_follow(self):
self.logi("Following a heading of %0.3f" % self.heading_to_pinger)
self.follow_change_heading = Heading()
self.follow_inital_heading = Heading(self.heading_to_pinger +
heading_offset)
self.follow_vel_x = VelocityX()
self.follow_vel_y = VelocityY()
distance_to_pinger = self.elevation_to_distance(self.follow_elevation)
self.follow_vel_x(x_dir * self.get_follow_speed(distance_to_pinger))
self.follow_vel_y(0.0)
# Proportional control to slow down near the pinger
def get_follow_speed(self, distance):
# Start slowing down at SLOW_DOWN_DISTANCE
speed = distance / SLOW_DOWN_DISTANCE
# Don't do nothing dumb
if speed > MAX_FOLLOW_SPEED:
speed = MAX_FOLLOW_SPEED
if speed < MIN_FOLLOW_SPEED:
speed = MIN_FOLLOW_SPEED
return speed
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_position(self):
return None
def follow(self):
# Turn to face the pinger
if not self.follow_inital_heading.finished:
self.follow_inital_heading()
return
new_ping = self.update_pings()
if new_ping is not None:
# Check whether we are on top of the pinger
ontop_of_pinger = self.ontop_of_pinger(new_ping)
if (ontop_of_pinger):
self.logi(
"Ontop of pinger validator returned true! Waiting for "
"consitent positives to terminate following.")
if self.ontop_checker.check(ontop_of_pinger):
self.finish()
if self.track_method == TrackMethod.heading:
return self.follow_heading(new_ping)
elif self.track_method == TrackMethod.position:
return self.follow_position(new_ping)
class IdentifyBins(Task):
""" Identifies which bin to drop markers into, centers over it """
def on_first_run(self, run_type, heading=None, uncovered_bin_vector=None):
self.logi("Centering over bins...")
self.logv("Starting IdentifyBins task")
self.center_valid = False
self.center_coords = (0, 0)
self.task = DownwardTarget(px=0.0025, py=0.0025,
point=lambda: self.center_coords,
valid=lambda: self.center_valid)
self.center_checker = ConsistencyCheck(15, 17)
self.align_checker = ConsistencyCheck(6, 6)
# TODO start alignment task.
self.init_time = self.this_run_time
self.uncovered_bin_vector = None
self.seen_two = False
cam = get_downward_camera()
self.cover_tracker = Tracker(cam['width'] * 0.15)
self.yellow_tracker = Tracker(cam['width'] * 0.15)
def on_run(self, run_type, heading=None, uncovered_bin_vector=None):
yellows = [TrackedBin(b.center_x, b.center_y) if b.probability > 0.0 else None for b in [yellow1.get(), yellow2.get()] ]
cover_g = cover.get()
covert = TrackedBin(cover_g.center_x, cover_g.center_y) if cover_g.probability > 0.0 else None
self.consistent_bins = self.yellow_tracker.track(*yellows)
self.consistent_cover = self.cover_tracker.track(covert, None)
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)
if any(self.consistent_cover) and any(self.consistent_bins):
if run_type == "cover":
good_cover = self.consistent_cover[0]
if good_cover is None:
good_cover = self.consistent_cover[1]
good_yellow = self.consistent_bins[0]
if good_yellow is None:
good_yellow = self.consistent_bins[1]
bin2cover_hat = calculate_bin_vector(good_yellow, good_cover)
if self.uncovered_bin_vector is None:
# TODO Take average here.
self.uncovered_bin_vector = bin2cover_hat
self.logi("Discovered cover to bin world vector %0.2f %0.2f" % \
(self.uncovered_bin_vector[0], self.uncovered_bin_vector[1]))
if run_type == "cover":
cands = self.consistent_cover + self.consistent_bins
else:
if all(self.consistent_bins) and uncovered_bin_vector is not None:
bin2bin = calculate_bin_vector(self.consistent_bins[0], self.consistent_bins[1])
if bin2bin.dot(uncovered_bin_vector()) > 0:
index = 1
else:
index = 0
if not self.seen_two:
self.seen_two = True
self.uncovered_ind = index
self.logi("Chose bin with index %d: current coords %d %d" % \
(self.uncovered_ind, self.consistent_bins[self.uncovered_ind].x, self.consistent_bins[self.uncovered_ind].y))
else:
if self.uncovered_ind == index:
self.logv("Confirmed uncovered bin has index %d" % index)
else:
self.logi("WARNING: Detected new uncovered bin index %d!" % index)
if not self.seen_two:
self.logv("Did not find two yellow!")
cands = self.consistent_bins + self.consistent_cover
else:
cands = [self.consistent_bins[self.uncovered_ind], self.consistent_bins[1 - self.uncovered_ind]] + self.consistent_cover
for i, cand in enumerate(cands):
if cand is not None:
self.logv("Found good bin of index %d" % i)
self.center_valid = True
self.center_coords = cand.x, cand.y
break
else:
self.logv("No good bins found to center on!")
self.center_valid = False
# Assumes cover and contours from same camera
target = get_downward_camera_center()
# TODO Increase deadband as time increases.
self.task(target=target, deadband=(25, 25))
if self.center_checker.check(self.task.finished):
if run_type == "cover" or heading is None:
if heading is None:
target_heading = shm.kalman.heading.get() + cover_g.angle
else:
target_heading = heading()
align_task = Heading(target_heading, deadband=0.5)
align_task()
if self.align_checker.check(align_task.finished):
VelocityX(0)()
VelocityY(0)()
self.finish()
else:
self.finish()
else:
self.align_checker.clear()
def on_finish(self):
self.logi("Centered!")
self.logv('IdentifyBins task finished in {} seconds!'.format(
self.this_run_time - self.init_time))