def turn(self, angle):
"""Turn robot a specified angle."""
start = self.robot.adjusted.heading
self.move(numpy.sign(angle) * 100, True)
# Keep turning while not reached angle.
while util.angle_diff(start, self.robot.adjusted.heading) < angle:
time.sleep(0.02)
self.move(0, False)
def get_median_measurements(self):
"""Return the median of the readings in the 20 degree range about each angle, from 0 to 360."""
measurements = self.get_measurements()
result = []
for i in range(360):
# Get the measurements in a 20 degree range.
close_measurements = [x.distance for x in measurements if util.angle_diff(x.angle, i) < 10]
if len(close_measurements) > 0:
# Get the median, and append the measurement
result.append(Measurement(self.robot.adjusted, i, util.middle(close_measurements)))
return result
def move_robot(self, measurements):
"""Move the robot, turning if there is an object in front of it.
Args:
measurements (list): List of measurements.
"""
self.prev = copy.deepcopy(self.current)
self.allow_control = self.controlled
if not self.controlled:
obstacles = [
m for m in measurements
if util.angle_diff(m.angle, 0) < 20 and m.distance < 30
]
if len(obstacles) > 0:
self.comm.turn(90)
self.comm.drive(30)
def extract_landmarks(measurements):
"""Function which extracts landmarks from a set of points.
Args:
measurements (:obj:`list` of :obj:`Measurement`): A set of Measurement objects that detail the robots
observations.
Returns:
list: A list of discovered landmarks.
"""
landmarks = [] # Array to store detected landmarks.
assigned = []
trials = 0 # Number of trials attempted.
# Look for landmarks while haven't reached max trials, and still enough unassigned points left.
# noinspection PyTypeChecker
while trials < MAX_TRIALS and len(assigned) < ASSIGNED * len(measurements):
# Randomly select a clustered subsample of the points.
unassigned = [m for m in measurements if m not in assigned]
angle = numpy.random.choice(unassigned).angle
radius = [
p for p in unassigned
if util.angle_diff(p.angle, angle) < ANGLE_RANGE
]
sample = numpy.array(
[p for p in numpy.random.choice(radius, SAMPLE_SIZE)])
# Calculate the x and y standard deviation of the sample.
std_x = numpy.std(numpy.array([p.location for p in sample])[:, 0])
std_y = numpy.std(numpy.array([p.location for p in sample])[:, 1])
is_vertical = std_x == 0 or std_y / std_x > 1
consensus = find_consensus(unassigned, sample, is_vertical)
# If there are enough matching points, a landmark has been found.
if len(consensus) > RANSAC_CONSENSUS:
segment = recalculate_line(consensus, is_vertical)
if not segment:
trials += 1
continue
assigned.extend(consensus)
landmarks.append(Landmark(segment))
trials = 0 # Found a landmark, so reset trials.
else:
trials += 1
return landmarks
def turn(self, angle):
start = self.robot.adjusted.heading
self.move(np.sign(angle)*180, True)
while util.angle_diff(start, self.robot.adjusted.heading) < abs(angle):
time.sleep(0.02)
self.move(0, False)