def test_ma_intersection_close_spawn(): def _no_close_spawn(vehicles): vehicles = list(vehicles.values()) for c1, v1 in enumerate(vehicles): for c2 in range(c1 + 1, len(vehicles)): v2 = vehicles[c2] dis = norm(v1.position[0] - v2.position[0], v1.position[1] - v2.position[1]) assert distance_greater(v1.position, v2.position, length=2.2) MultiAgentIntersectionEnv._DEBUG_RANDOM_SEED = 1 env = MultiAgentIntersectionEnv({ # "use_render": True, "fast": True, "horizon": 50, "num_agents": 16, "map_config": { "exit_length": 30 } }) env.seed(100) try: _check_spaces_before_reset(env) for num_r in range(10): obs = env.reset() _check_spaces_after_reset(env) for _ in range(10): o, r, d, i = env.step({k: [0, 0] for k in env.vehicles.keys()}) assert not any(d.values()) _no_close_spawn(env.vehicles) print('Finish {} resets.'.format(num_r)) finally: env.close() MultiAgentIntersectionEnv._DEBUG_RANDOM_SEED = None
def test_ma_intersection_init_space(): try: for start_seed in [5000, 6000, 7000]: for num_agents in [16, 32]: for num_others in [0, 2, 4, 8]: for crash_vehicle_penalty in [0, 5]: env_config = dict( start_seed=start_seed, num_agents=num_agents, vehicle_config=dict(lidar=dict( num_others=num_others)), crash_vehicle_penalty=crash_vehicle_penalty) env = MultiAgentIntersectionEnv(env_config) single_space = env.observation_space["agent0"] assert single_space.shape is not None, single_space assert np.prod( single_space.shape) is not None, single_space single_space = env.action_space["agent0"] assert single_space.shape is not None, single_space assert np.prod( single_space.shape) is not None, single_space _check_spaces_before_reset(env) env.reset() _check_spaces_after_reset(env) env.close() print('Finish: ', env_config) finally: if "env" in locals(): env.close()
def test_ma_no_reset_error(): # It is possible that many agents are populated in the same spawn place! def check_pos(vehicles): while vehicles: v_1 = vehicles[0] for v_2 in vehicles[1:]: v_1_pos = v_1.position v_2_pos = v_2.position assert norm( v_1_pos[0] - v_2_pos[0], v_1_pos[1] - v_2_pos[1] ) > v_1.WIDTH / 2 + v_2.WIDTH / 2, "Vehicles overlap after reset()" if v_1.crash_vehicle: x = 1 raise ValueError("Vehicles overlap after reset()") vehicles.remove(v_1) env = MultiAgentIntersectionEnv({"horizon": 300, "num_agents": 40, "delay_done": 0, "use_render": False}) try: _check_spaces_before_reset(env) obs = env.reset() _check_spaces_after_reset(env, obs) assert env.observation_space.contains(obs) for step in range(50): check_pos(list(env.vehicles.values())) o, r, d, i = env.step({k: [0, 1] for k in env.vehicles.keys()}) env.reset() if d["__all__"]: break finally: env.close()
def test_ma_intersection_env(): for env in [MultiAgentIntersectionEnv({"delay_done": 0, "num_agents": 1, "vehicle_config": {"lidar": {"num_others": 8}}}), MultiAgentIntersectionEnv({"num_agents": 1, "delay_done": 0, "vehicle_config": {"lidar": {"num_others": 0}}}), MultiAgentIntersectionEnv({"num_agents": 4, "delay_done": 0, "vehicle_config": {"lidar": {"num_others": 8}}}), MultiAgentIntersectionEnv({"num_agents": 4, "delay_done": 0, "vehicle_config": {"lidar": {"num_others": 0}}}), MultiAgentIntersectionEnv({"num_agents": 8, "delay_done": 0, "vehicle_config": {"lidar": {"num_others": 0}}})]: try: _check_spaces_before_reset(env) obs = env.reset() _check_spaces_after_reset(env, obs) assert env.observation_space.contains(obs) for step in range(100): act = {k: [1, 1] for k in env.vehicles.keys()} o, r, d, i = _act(env, act) if step == 0: assert not any(d.values()) finally: env.close()
def test_ma_intersection_horizon_termination(): # test horizon env = MultiAgentIntersectionEnv({ "horizon": 100, "num_agents": 8, "crash_done": False }) try: for _ in range( 3 ): # This function is really easy to break, repeat multiple times! _check_spaces_before_reset(env) obs = env.reset() _check_spaces_after_reset(env, obs) assert env.observation_space.contains(obs) should_respawn = set() special_agents = set(["agent0", "agent7"]) for step in range(1, 10000): act = {k: [0, 0] for k in env.vehicles.keys()} for v_id in act.keys(): if v_id in special_agents: act[v_id] = [1, 1] # Add some randomness else: if v_id in env.vehicles: env.vehicles[v_id].set_static(True) obs, r, d, i = _act(env, act) if step == 0 or step == 1: assert not any(d.values()) if should_respawn: for kkk in should_respawn: assert kkk not in obs, "It seems the max_step agents is not respawn!" assert kkk not in r assert kkk not in d assert kkk not in i should_respawn.clear() for kkk, ddd in d.items(): if ddd and kkk == "__all__": print("Current: ", step) continue if ddd and kkk not in special_agents: assert i[kkk]["max_step"] assert not i[kkk]["out_of_road"] assert not i[kkk]["crash"] assert not i[kkk]["crash_vehicle"] should_respawn.add(kkk) if d["__all__"]: obs = env.reset() should_respawn.clear() break finally: env.close()
def test_randomize_spawn_place(): last_pos = {} env = MultiAgentIntersectionEnv({"num_agents": 4, "use_render": False, "fast": True}) try: obs = env.reset() for step in range(1000): act = {k: [1, 1] for k in env.vehicles.keys()} last_pos = {kkk: v.position for kkk, v in env.vehicles.items()} o, r, d, i = env.step(act) obs = env.reset() new_pos = {kkk: v.position for kkk, v in env.vehicles.items()} for kkk, new_p in new_pos.items(): assert not np.all(new_p == last_pos[kkk]), (new_p, last_pos[kkk], kkk) finally: env.close()
def test_ma_intersection_no_short_episode(): env = MultiAgentIntersectionEnv({ "horizon": 300, "num_agents": 40, }) try: _check_spaces_before_reset(env) o = env.reset() _check_spaces_after_reset(env, o) actions = [[0, 1], [1, 1], [-1, 1]] start = time.time() d_count = 0 d = {"__all__": False} for step in range(2000): # act = {k: actions[np.random.choice(len(actions))] for k in o.keys()} act = { k: actions[np.random.choice(len(actions))] for k in env.vehicles.keys() } o_keys = set(o.keys()).union({"__all__"}) a_keys = set(env.action_space.spaces.keys()).union(set(d.keys())) assert o_keys == a_keys o, r, d, i = _act(env, act) for kkk, iii in i.items(): if d[kkk]: assert iii["episode_length"] >= 1 d_count += 1 if d["__all__"]: o = env.reset() d = {"__all__": False} if (step + 1) % 100 == 0: print( "Finish {}/2000 simulation steps. Time elapse: {:.4f}. Average FPS: {:.4f}" .format(step + 1, time.time() - start, (step + 1) / (time.time() - start))) if d_count > 200: break finally: env.close()
def test_ma_intersection_40_agent_reset_after_respawn(): def check_pos(vehicles): while vehicles: v_1 = vehicles[0] for v_2 in vehicles[1:]: v_1_pos = v_1.position v_2_pos = v_2.position assert norm( v_1_pos[0] - v_2_pos[0], v_1_pos[1] - v_2_pos[1] ) > v_1.WIDTH / 2 + v_2.WIDTH / 2, "Vehicles overlap after reset()" assert not v_1.crash_vehicle, "Vehicles overlap after reset()" vehicles.remove(v_1) env = MultiAgentIntersectionEnv({ "horizon": 50, "num_agents": 40, "use_render": False }) try: _check_spaces_before_reset(env) obs = env.reset() _check_spaces_after_reset(env, obs) assert env.observation_space.contains(obs) for step in range(50): env.reset() check_pos(list(env.vehicles.values())) for v_id in list(env.vehicles.keys())[:20]: env.agent_manager.finish(v_id) env.step({k: [1, 1] for k in env.vehicles.keys()}) env.step({k: [1, 1] for k in env.vehicles.keys()}) env.step({k: [1, 1] for k in env.vehicles.keys()}) finally: env.close()
def test_ma_intersection_reward_done_alignment(): # out of road env = MultiAgentIntersectionEnv({ "horizon": 200, "num_agents": 4, "out_of_road_penalty": 777, "crash_done": False }) try: _check_spaces_before_reset(env) obs = env.reset() _check_spaces_after_reset(env, obs) assert env.observation_space.contains(obs) for action in [-1, 1]: for step in range(5000): act = {k: [action, 1] for k in env.vehicles.keys()} o, r, d, i = _act(env, act) for kkk, ddd in d.items(): if ddd and kkk != "__all__": # #assert r[kkk] == -777 assert i[kkk]["out_of_road"] # print('{} done passed!'.format(kkk)) for kkk, rrr in r.items(): if rrr == -777: assert d[kkk] assert i[kkk]["out_of_road"] # print('{} reward passed!'.format(kkk)) if d["__all__"]: env.reset() break finally: env.close() # crash env = MultiAgentIntersectionEnv({ "horizon": 100, "num_agents": 2, "crash_vehicle_penalty": 1.7777, "crash_done": True, "delay_done": 0, # "use_render": True, # "fast": True, "top_down_camera_initial_z": 160 }) # Force the seed here so that the agent1 and agent2 are in same heading! Otherwise they might be in vertical # heading and cause one of the vehicle raise "out of road" error! env._DEBUG_RANDOM_SEED = 1 try: _check_spaces_before_reset(env) obs = env.reset() _check_spaces_after_reset(env, obs) for step in range(5): act = {k: [0, 0] for k in env.vehicles.keys()} o, r, d, i = _act(env, act) env.vehicles["agent0"].set_position(env.vehicles["agent1"].position, height=1.2) for step in range(5000): act = {k: [0, 0] for k in env.vehicles.keys()} o, r, d, i = _act(env, act) if not any(d.values()): continue assert sum(d.values()) == 2 for kkk in ['agent0', 'agent1']: iii = i[kkk] assert iii["crash_vehicle"] assert iii["crash"] # #assert r[kkk] == -1.7777 # for kkk, ddd in d.items(): ddd = d[kkk] if ddd and kkk != "__all__": #assert r[kkk] == -1.7777 assert i[kkk]["crash_vehicle"] assert i[kkk]["crash"] # print('{} done passed!'.format(kkk)) # for kkk, rrr in r.items(): rrr = r[kkk] if rrr == -1.7777: assert d[kkk] assert i[kkk]["crash_vehicle"] assert i[kkk]["crash"] # print('{} reward passed!'.format(kkk)) # assert d["__all__"] # if d["__all__"]: break finally: env._DEBUG_RANDOM_SEED = None env.close() # crash with real fixed vehicle # crash 2 env = MultiAgentIntersectionEnv({ "map_config": { "exit_length": 110, "lane_num": 1 }, # "use_render": True, # "fast": True, "horizon": 200, "num_agents": 40, "crash_vehicle_penalty": 1.7777, "crash_done": False }) try: _check_spaces_before_reset(env) obs = env.reset() _check_spaces_after_reset(env, obs) for step in range(1): act = {k: [0, 0] for k in env.vehicles.keys()} o, r, d, i = _act(env, act) for v_id, v in env.vehicles.items(): if v_id != "agent0": v.set_static(True) for step in range(5000): act = {k: [0, 1] for k in env.vehicles.keys()} o, r, d, i = _act(env, act) for kkk, iii in i.items(): if iii["crash"]: assert iii["crash_vehicle"] if iii["crash_vehicle"]: assert iii["crash"] # #assert r[kkk] == -1.7777 for kkk, ddd in d.items(): if ddd and kkk != "__all__": assert i[kkk]["out_of_road"] # print('{} done passed!'.format(kkk)) for kkk, rrr in r.items(): if rrr == -1.7777: # assert d[kkk] assert i[kkk]["crash_vehicle"] assert i[kkk]["crash"] # print('{} reward passed!'.format(kkk)) if d["agent0"]: break if d["__all__"]: break finally: env.close() # success env = MultiAgentIntersectionEnv({ "horizon": 100, "num_agents": 2, "success_reward": 999, "out_of_road_penalty": 555, "crash_done": True }) try: _check_spaces_before_reset(env) obs = env.reset() _check_spaces_after_reset(env) env.vehicles["agent0"].set_position( env.vehicles["agent0"].navigation.final_lane.end) assert env.observation_space.contains(obs) for step in range(5000): act = {k: [0, 0] for k in env.vehicles.keys()} o, r, d, i = _act(env, act) if d["__all__"]: break kkk = "agent0" #assert r[kkk] == 999 assert i[kkk]["arrive_dest"] assert d[kkk] kkk = "agent1" #assert r[kkk] != 999 assert not i[kkk]["arrive_dest"] assert not d[kkk] break finally: env.close()
def test_ma_intersection_reset(): env = MultiAgentIntersectionEnv({"horizon": 50, "num_agents": 4}) try: _check_spaces_before_reset(env) obs = env.reset() _check_spaces_after_reset(env, obs) assert env.observation_space.contains(obs) for step in range(1000): act = {k: [1, 1] for k in env.vehicles.keys()} o, r, d, i = _act(env, act) if step == 0: assert not any(d.values()) if d["__all__"]: obs = env.reset() assert env.observation_space.contains(obs) _check_spaces_after_reset(env, obs) assert set(env.observation_space.spaces.keys()) == set(env.action_space.spaces.keys()) == \ set(env.observations.keys()) == set(obs.keys()) == \ set(env.config["target_vehicle_configs"].keys()) break finally: env.close() # Put vehicles to destination and then reset. This might cause error if agent is assigned destination BEFORE reset. env = MultiAgentIntersectionEnv({ "horizon": 100, "num_agents": 32, "success_reward": 777, "use_render": False }) try: _check_spaces_before_reset(env) success_count = 0 agent_count = 0 obs = env.reset() _check_spaces_after_reset(env, obs) assert env.observation_space.contains(obs) for num_reset in range(5): for step in range(1000): for _ in range(2): act = {k: [1, 1] for k in env.vehicles.keys()} o, r, d, i = _act(env, act) # Force vehicle to success! for v_id, v in env.vehicles.items(): loc = v.navigation.final_lane.end v.set_position(loc) pos = v.position np.testing.assert_almost_equal(pos, loc, decimal=3) new_loc = v.navigation.final_lane.end long, lat = v.navigation.final_lane.local_coordinates( v.position) flag1 = (v.navigation.final_lane.length - 5 < long < v.navigation.final_lane.length + 5) flag2 = (v.navigation.get_current_lane_width() / 2 >= lat >= (0.5 - v.navigation.get_current_lane_num()) * v.navigation.get_current_lane_width()) if not v.arrive_destination: print('sss') assert v.arrive_destination act = {k: [0, 0] for k in env.vehicles.keys()} o, r, d, i = _act(env, act) for v in env.vehicles.values(): assert len(v.navigation.checkpoints) > 2 for kkk, iii in i.items(): if iii and iii["arrive_dest"]: # print("{} success!".format(kkk)) success_count += 1 for kkk, ddd in d.items(): if ddd and kkk != "__all__": assert i[kkk]["arrive_dest"] agent_count += 1 for kkk, rrr in r.items(): if d[kkk]: assert rrr == 777 if d["__all__"]: print("Finish {} agents. Success {} agents.".format( agent_count, success_count)) o = env.reset() assert env.observation_space.contains(o) _check_spaces_after_reset(env, o) break finally: env.close()