def main():
parser = argparse.ArgumentParser()
parser.add_argument("--player", help="player 1-6", type=int, default=1)
args = parser.parse_args()
config = Config(CONFIG_FILE, args.player)
comms_config = {
'rx_ip': config.client_ip,
'rx_port': config.client_port,
'tx_ip': config.sim_ip,
'tx_port': config.sim_port
}
print("Rx at {}:{}".format(comms_config["rx_ip"], comms_config["rx_port"]))
print("Tx to {}:{}".format(comms_config["tx_ip"], comms_config["tx_port"]))
commands_mutex = Lock()
# Launch comms in background thread
comms = CommsThread(comms_config, False, commands_mutex)
comms.daemon = True
comms.start()
# Launch perception, motion planning, and controls in main thread
sweep_builder = SweepBuilder()
perception = Perception(config)
planning = Planning(config)
controls = Controls(config)
visualize = Visualize(config)
try:
while True:
vehicle_state = sweep_builder.run(comms.get_vehicle_states())
if vehicle_state is not None:
t1 = time.time()
world_state = perception.run(vehicle_state)
plan = planning.run(world_state)
vehicle_commands = controls.run(plan)
t2 = time.time()
freq = 1 / (t2 - t1)
print(f"Running at {freq} Hz")
vehicle_commands['draw'] = visualize.run(world_state, plan)
with commands_mutex:
# hold the lock to prevent the Comms thread from
# sending the commands dict while we're modifying it
comms.vehicle_commands.update(vehicle_commands)
except KeyboardInterrupt:
pass
class TestRun(unittest.TestCase):
def setUp(self):
self.config = Mock()
self.config.field_elements = [Polygon(make_square_vertices(side_length=2, center=(-5, -5)))]
self.config.ball_radius = BALL_RADIUS
self.perception = Perception(self.config)
def test_run(self):
obstacle = make_linear_vertices(start=(5,5), end=(6,6), num_pts=20)
ball = make_circular_vertices(radius=BALL_RADIUS, center=(1,1), num_pts=8)
lidar_sweep = list()
lidar_sweep.extend(obstacle)
lidar_sweep.extend(ball)
x = 0
y = 0
theta = 0
ingested_balls = 0
vehicle_state = {
'x': x,
'y': y,
'theta': theta,
'lidarSweep': np.array(lidar_sweep),
'numIngestedBalls': ingested_balls
}
world_state = self.perception.run(vehicle_state)
actual_balls = world_state['balls']
actual_first_ball = actual_balls[0]
actual_others = world_state['obstacles']
expected_balls = [(1, 1)]
expected_others = [((5.0, 5.0), (5.95, 5.95))]
expected_first_ball = expected_balls[0]
self.assertEqual(len(expected_balls), len(actual_balls))
self.assertAlmostEqual(expected_first_ball[0], actual_first_ball[0])
self.assertAlmostEqual(expected_first_ball[1], actual_first_ball[1])
self.assertEqual(expected_others, actual_others)