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_columns = []
self.config.field_trenches = []
self.config.alliance = "Blue"
self.config.blue_goal_region = Polygon(
make_square_vertices(side_length=0.5, center=(-2.5, -3.5)))
self.config.blue_chute_pos = np.array([10, 10])
self.config.occupancy_grid_num_cols = 6
self.config.occupancy_grid_num_rows = 6
self.config.occupancy_grid_cell_resolution = 1
self.config.occupancy_grid_origin = (0, 0)
self.config.occupancy_grid_dilation_kernel_size = 3
self.config.ball_probability_decay_factor = 0
self.config.ball_probability_growth_factor = 1
self.config.ball_probability_threshold = 1
self.config.obstacle_probability_decay_factor = 0
self.config.obstacle_probability_growth_factor = 1
self.config.obstacle_probability_threshold = 1
self.planning = Planning(self.config)
def test_run(self):
world_state = {
'pose': ((-2.5, -2.5), 0),
'balls': [(2.5, 2.5)],
'obstacles': [],
'numIngestedBalls': 0,
}
self.planning.run(world_state)
expected = 2
actual = len(world_state['trajectory'])
self.assertEqual(expected, actual)
