def test_find_wall():
"""quick sanity check to make sure that things run"""
f = FieldModel()
r = Robot()
rotation = FakeRotation(f, r)
cart_data = rotation.cartesian_data()
(wall_magnitude, point) = find_wall(cart_data)
(x, y) = point
#
# These values should be verified, but they look *close*.
# Should add this function with some nicely colored output
# into the field.py simulation.
#
assert round(wall_magnitude, 1) == 153.7
assert round(x, 1) == 140.5
assert round(y, 1) == 22.3
def test_r_squared():
# simple 45 degree line
data = [(1,1), (2,2), (3,3), (4,4)]
r2 = r_squared(data)
assert round(1000*r2) == 1000
# simple -45 degree line
data = [(-1,1), (-2,2), (-3,3), (-4,4)]
r2 = r_squared(data)
assert round(1000*r2) == 1000
# horizontal line
data = [(1,5), (2,5), (3,5), (4,5)]
r2 = r_squared(data)
#print r2
#assert round(1000*r2) == 1000
# vertical line
data = [(5,1), (5,2), (5,3), (5,4)]
r2 = r_squared(data)
#print r2
#assert r2 == 0
# more interesting line
data = [(1,1), (2,2), (3,4), (4,1), (7,8), (12,14)]
r2 = r_squared(data)
#print r2
# now let's try from the field model (what is the r_squared on the whole thing?)
f = FieldModel()
rotation = FakeRotation(f)
cart_data = rotation.cartesian_data()
for i in range(0, len(cart_data)-4):
slice = cart_data[i:i+4]
r2 = r_squared(slice)
print("{:.2f}".format(r2),end=" => ")
print(slice)
def test_analzyer_on_field_model():
"""
Create a model of the field, then ask the laser to compute
the range for sweep. Then move the robot and ask again.
"""
robot = Robot()
field = FieldModel()
tower_range = field.tower_range_from_origin()
rotation = FakeRotation(field, robot)
heading, sweep_range = Analyzer.range_at_heading(rotation.polar_data(), (-10,11))
# closest point should be dead ahead
assert heading == 0
assert sweep_range == tower_range
movement = 100
robot.move(movement)
rotation = FakeRotation(field, robot)
heading, sweep_range = Analyzer.range_at_heading(rotation.polar_data(), (-10,11))
# closest point should be dead ahead
assert heading == 0
assert sweep_range == (tower_range - movement)
def update_all_results (lidar_viewer, field_model, robot, factor):
"""generate synthetic lidar data based on model and analyze/plot"""
rotation = FakeRotation(field_model, robot)
# do simple sweep analysis and report
sweep_heading, sweep_range = Analyzer.range_at_heading(rotation.polar_data(), (-5, 6))
print("Closest point in (-5,6) sweep is ({:d},{:.2f})".format(sweep_heading, sweep_range))
# do some analysis and gather up the results so they can be plotted
# run a 4-point r-squared and collect those less than threshold value
cart_data = rotation.cartesian_data()
# try out the analyzer find_wall
(heading, distance, orientation) = find_wall_midpoint(cart_data)
print("Analyzer.find_wall() heading {:.1f}, range {:.1f}, orientation {:.1f})".format(heading, distance, orientation))
# plot the polar data for reference
lidar_viewer.plot_polar(rotation.polar_data())
# plot the rectangular data
lidar_viewer.plot_cartesian(cart_data)
# polar markers (heading in degrees)
find_wall_markers = [ (-heading, distance) ]
#
# display the average distance of data points
# might be used to dynamically choose the r2 thresholds
# or dynamically select the r-factor for analysis?
avg_dist = avg_distance(cart_data)
print("Average distance: {:.1f}\n".format(avg_dist))
# plot any calculated markers in their specified color
#lidar_viewer.plot_markers(r2_markers, 'g')
lidar_viewer.plot_polar_markers(find_wall_markers, 'r')