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)
current_time = 0
seconds_per_output = 1
SECONDS_PER_MINUTE = 60.0
rotations = []
while 1:
try:
rotation = Rotation(lasr.gather_full_rotation())
rotations = rotations[-4:]
rotations.append(rotation)
# rotation = OldRotation(lasr.gather_full_rotation())
#
# For now, we just output a lidar data snapshot every 10 seconds
# We will want this for debugging (maybe every second instead)
# change the "seconds_per_output" to tune that.
#
tgt_heading, tgt_range = Analyzer.range_at_heading(rotation.polar_data(), (Analyzer.start, Analyzer.stop))
logger.info(
"{:d} points yields {:.2f} inches at {:2d} degrees)".format(
len(rotation.polar_data()), tgt_range, tgt_heading
)
)
# push the newly calculated data into the message
range_at_heading_message.heading = tgt_heading
range_at_heading_message.range = tgt_range
channel.send_to(range_at_heading_message.encode_message())
# push periodic message to the bot
periodic_message.status = "ok"
periodic_message.rpm = rotation.rpm()
channel.send_to(periodic_message.encode_message())
