From 821f7dded982bf29e3b8f2997cf572bb10d11ff1 Mon Sep 17 00:00:00 2001 From: Quanyi Li <785878978@qq.com> Date: Fri, 21 May 2021 19:45:06 +0800 Subject: [PATCH] Introduce Tollgate MARL environment (#447) * add flexible bottle part length * toll block * fix object bug * runable * fix memory leak * add building type * test script * speed limit * finish * 10 agent * format * test reward * add crash building flag & add test script * format * fix test and format * update config * update camera setting * update configs * update reward scheme * rename to toll gate * update side detector * remove navi info * yellow line * min pass time * finish * format * rename tollgate env * move config to top * format * optimize config * format * optimize * finish this branch Co-authored-by: pengzhenghao --- pgdrive/constants.py | 16 +- .../envs/generation_envs/safe_pgdrive_env.py | 7 +- pgdrive/envs/marl_envs/marl_intersection.py | 11 +- pgdrive/envs/marl_envs/marl_tollgate.py | 536 +++++++++++++ pgdrive/envs/pgdrive_env_v2.py | 14 +- pgdrive/scene_creator/blocks/block.py | 76 +- pgdrive/scene_creator/blocks/bottleneck.py | 4 +- pgdrive/scene_creator/blocks/first_block.py | 1 + pgdrive/scene_creator/blocks/tollgate.py | 98 +++ pgdrive/scene_creator/buildings/__init__.py | 0 .../scene_creator/buildings/base_building.py | 16 + .../scene_creator/highway_vehicle/behavior.py | 6 +- .../highway_vehicle/kinematics.py | 8 +- pgdrive/scene_creator/lane/abs_lane.py | 6 + pgdrive/scene_creator/lane/circular_lane.py | 3 +- pgdrive/scene_creator/lane/straight_lane.py | 6 +- pgdrive/scene_creator/object/__init__.py | 2 +- pgdrive/scene_creator/object/base_object.py | 24 + .../scene_creator/object/traffic_object.py | 10 +- pgdrive/scene_creator/pg_blocks.py | 10 +- pgdrive/scene_creator/road/road.py | 5 +- pgdrive/scene_creator/vehicle/base_vehicle.py | 8 + .../vehicle/base_vehicle_node.py | 2 + .../scene_creator/vehicle/traffic_vehicle.py | 6 + pgdrive/scene_creator/vehicle_module/lidar.py | 4 +- pgdrive/scene_manager/object_manager.py | 19 +- pgdrive/scene_manager/traffic_manager.py | 10 +- pgdrive/tests/test_env/test_ma_tollgate.py | 717 ++++++++++++++++++ pgdrive/utils/pg_space.py | 4 + pgdrive/world/collision_callback.py | 3 + 30 files changed, 1572 insertions(+), 60 deletions(-) create mode 100644 pgdrive/envs/marl_envs/marl_tollgate.py create mode 100644 pgdrive/scene_creator/blocks/tollgate.py create mode 100644 pgdrive/scene_creator/buildings/__init__.py create mode 100644 pgdrive/scene_creator/buildings/base_building.py create mode 100644 pgdrive/scene_creator/object/base_object.py create mode 100644 pgdrive/tests/test_env/test_ma_tollgate.py diff --git a/pgdrive/constants.py b/pgdrive/constants.py index 17df0b55e..013b4c847 100644 --- a/pgdrive/constants.py +++ b/pgdrive/constants.py @@ -17,6 +17,7 @@ class TerminationState: CRASH = "crash" CRASH_VEHICLE = "crash_vehicle" CRASH_OBJECT = "crash_object" + CRASH_BUILDING = "crash_building" HELP_MESSAGE = "Keyboard Shortcuts:\n" \ @@ -53,12 +54,14 @@ class BodyName: Broken_line = "Broken Line" Sidewalk = "Sidewalk" Ground = "Ground" + InvisibleWall = "InvisibleWall" Base_vehicle = "Target Vehicle" Base_vehicle_beneath = "Target Vehicle Beneath" Traffic_vehicle = "Traffic Vehicle" Lane = "Lane" Traffic_cone = "Traffic Cone" Traffic_triangle = "Traffic Triangle" + TollGate = "Toll Gate" COLOR = { @@ -70,7 +73,9 @@ class BodyName: BodyName.Traffic_cone: "orange", BodyName.Traffic_triangle: "orange", BodyName.Base_vehicle: "red", - BodyName.Base_vehicle_beneath: "red" + BodyName.Base_vehicle_beneath: "red", + BodyName.InvisibleWall: "red", + BodyName.TollGate: "red", } @@ -110,6 +115,7 @@ class CollisionGroup: EgoVehicleBeneath = 6 BrokenLaneLine = 3 ContinuousLaneLine = 7 + InvisibleWall = 8 TrafficVehicle = 4 LaneSurface = 5 # useless now, since it is in another physics world @@ -124,6 +130,7 @@ def collision_rules(cls): (cls.Terrain, cls.EgoVehicleBeneath, False), (cls.Terrain, cls.TrafficVehicle, False), (cls.Terrain, cls.ContinuousLaneLine, False), + (cls.Terrain, cls.InvisibleWall, False), # block collision (cls.BrokenLaneLine, cls.BrokenLaneLine, False), @@ -133,6 +140,7 @@ def collision_rules(cls): # change it after we design a new traffic system ! (cls.BrokenLaneLine, cls.TrafficVehicle, False), (cls.BrokenLaneLine, cls.ContinuousLaneLine, False), + (cls.BrokenLaneLine, cls.InvisibleWall, False), # traffic vehicles collision (cls.TrafficVehicle, cls.TrafficVehicle, False), @@ -140,24 +148,30 @@ def collision_rules(cls): (cls.TrafficVehicle, cls.EgoVehicle, True), (cls.TrafficVehicle, cls.EgoVehicleBeneath, True), (cls.TrafficVehicle, cls.ContinuousLaneLine, False), + # FIXME maybe set to True in the future + (cls.TrafficVehicle, cls.InvisibleWall, False), # ego vehicle collision (cls.EgoVehicle, cls.EgoVehicle, True), (cls.EgoVehicle, cls.EgoVehicleBeneath, False), (cls.EgoVehicle, cls.LaneSurface, False), (cls.EgoVehicle, cls.ContinuousLaneLine, False), + (cls.EgoVehicle, cls.InvisibleWall, True), # lane surface (cls.LaneSurface, cls.LaneSurface, False), (cls.LaneSurface, cls.EgoVehicleBeneath, False), (cls.LaneSurface, cls.ContinuousLaneLine, False), + (cls.LaneSurface, cls.InvisibleWall, False), # vehicle beneath (cls.EgoVehicleBeneath, cls.EgoVehicleBeneath, True), (cls.EgoVehicleBeneath, cls.ContinuousLaneLine, True), + (cls.EgoVehicleBeneath, cls.InvisibleWall, True), # continuous lane line (cls.ContinuousLaneLine, cls.ContinuousLaneLine, False), + (cls.ContinuousLaneLine, cls.InvisibleWall, False), ] @classmethod diff --git a/pgdrive/envs/generation_envs/safe_pgdrive_env.py b/pgdrive/envs/generation_envs/safe_pgdrive_env.py index 5dc317c09..648843ac8 100644 --- a/pgdrive/envs/generation_envs/safe_pgdrive_env.py +++ b/pgdrive/envs/generation_envs/safe_pgdrive_env.py @@ -106,10 +106,10 @@ def cost_function(self, vehicle_id: str): "accident_prob": 1.0, "manual_control": True, "use_render": True, - "environment_num": 100, - "start_seed": 139, + "environment_num": 1, + "start_seed": 187, "out_of_road_cost": 1, - # "debug": True, + "debug": True, "cull_scene": True, "pg_world_config": { "pstats": True @@ -130,6 +130,7 @@ def cost_function(self, vehicle_id: str): o, r, d, info = env.step([0, 1]) total_cost += info["cost"] env.render(text={"cost": total_cost, "seed": env.current_map.random_seed, "reward": r}) + print(len(env.scene_manager.traffic_manager.traffic_vehicles)) if d: total_cost = 0 print("done_cost:{}".format(info["cost"])) diff --git a/pgdrive/envs/marl_envs/marl_intersection.py b/pgdrive/envs/marl_envs/marl_intersection.py index 6792566ac..61b6e4b5f 100644 --- a/pgdrive/envs/marl_envs/marl_intersection.py +++ b/pgdrive/envs/marl_envs/marl_intersection.py @@ -41,16 +41,7 @@ def _generate(self, pg_world): InterSection.EXIT_PART_LENGTH = length last_block = InterSection(1, last_block.get_socket(index=0), self.road_network, random_seed=1) last_block.add_u_turn(True) - last_block.construct_block( - parent_node_path, - pg_physics_world, - extra_config={ - "exit_radius": 10, - "inner_radius": 30, - "angle": 70, - # Note: lane_num is set in config.map_config.lane_num - } - ) + last_block.construct_block(parent_node_path, pg_physics_world) self.blocks.append(last_block) diff --git a/pgdrive/envs/marl_envs/marl_tollgate.py b/pgdrive/envs/marl_envs/marl_tollgate.py new file mode 100644 index 000000000..b4d48e89d --- /dev/null +++ b/pgdrive/envs/marl_envs/marl_tollgate.py @@ -0,0 +1,536 @@ +import gym +import numpy as np +from pgdrive.constants import TerminationState +from pgdrive.envs.multi_agent_pgdrive import MultiAgentPGDrive +from pgdrive.obs.state_obs import LidarStateObservation, StateObservation +from pgdrive.scene_creator.blocks.bottleneck import Merge, Split +from pgdrive.scene_creator.blocks.first_block import FirstBlock +from pgdrive.scene_creator.blocks.tollgate import TollGate +from pgdrive.scene_creator.map import PGMap +from pgdrive.scene_creator.road.road import Road +from pgdrive.utils import PGConfig, clip + +MATollConfig = dict( + map_config=dict(exit_length=70, lane_num=3, toll_lane_num=8, toll_length=10), + top_down_camera_initial_x=125, + top_down_camera_initial_y=0, + top_down_camera_initial_z=120, + cross_yellow_line_done=True, + # ===== Reward Scheme ===== + speed_reward=0.0, + overspeed_penalty=0.5, + vehicle_config=dict( + min_pass_steps=30, # We ask the agents to stop at tollgate for at least 6s (30 steps). + show_lidar=False, + # "show_side_detector": True, + # "show_lane_line_detector": True, + side_detector=dict(num_lasers=72, distance=20), # laser num, distance + lane_line_detector=dict(num_lasers=4, distance=20), + lidar=dict(num_lasers=72, distance=20), + ) +) + + +class StayTimeManager: + def __init__(self): + self.entry_time = {} + self.exit_time = {} + self.last_block = {} + + def reset(self): + self.entry_time = {} + self.exit_time = {} + self.last_block = {} + + def record(self, agents, time_step): + for v_id, v in agents.items(): + cur_block_id = v.current_road.block_ID() + if v_id in self.last_block: + last_block_id = self.last_block[v_id] + self.last_block[v_id] = cur_block_id + if last_block_id != cur_block_id: + if cur_block_id == TollGate.ID: + # entry + self.entry_time[v_id] = time_step + elif (cur_block_id == Merge.ID or cur_block_id == Split.ID) and last_block_id == TollGate.ID: + self.exit_time[v_id] = time_step + else: + self.last_block[v_id] = cur_block_id + + +class TollGateStateObservation(StateObservation): + # no intersection exclude navi info now + @property + def observation_space(self): + # Navi info + Other states + shape = self.ego_state_obs_dim + self.get_side_detector_dim() + return gym.spaces.Box(-0.0, 1.0, shape=(shape, ), dtype=np.float32) + + def observe(self, vehicle): + ego_state = self.vehicle_state(vehicle) + return ego_state + + +class TollGateObservation(LidarStateObservation): + def __init__(self, vehicle_config): + super(LidarStateObservation, self).__init__(vehicle_config) + self.state_obs = TollGateStateObservation(vehicle_config) + self.in_toll_time = 0 + + @property + def observation_space(self): + shape = list(self.state_obs.observation_space.shape) + if self.config["lidar"]["num_lasers"] > 0 and self.config["lidar"]["distance"] > 0: + # Number of lidar rays and distance should be positive! + shape[0] += self.config["lidar"]["num_lasers"] + self.config["lidar"]["num_others"] * 4 + 2 + return gym.spaces.Box(-0.0, 1.0, shape=tuple(shape), dtype=np.float32) + + def reset(self, env, vehicle=None): + self.in_toll_time = 0 + + def observe(self, vehicle): + cur_block_is_toll = vehicle.current_road.block_ID() == TollGate.ID + self.in_toll_time += 1 if cur_block_is_toll else 0 + if not cur_block_is_toll: + toll_obs = [0.0, 0.0] + else: + toll_obs = [ + 1.0 if cur_block_is_toll else 0.0, + 1.0 if self.in_toll_time > vehicle.vehicle_config["min_pass_steps"] else 0.0 + ] + # print(toll_obs) + state = self.state_observe(vehicle) + other_v_info = self.lidar_observe(vehicle) + return np.concatenate((state, np.asarray(other_v_info), np.asarray(toll_obs))) + + +class MATollGateMap(PGMap): + BOTTLE_LENGTH = 35 + + def _generate(self, pg_world): + length = self.config["exit_length"] + + parent_node_path, pg_physics_world = pg_world.worldNP, pg_world.physics_world + assert len(self.road_network.graph) == 0, "These Map is not empty, please create a new map to read config" + + # Build a first-block + last_block = FirstBlock( + self.road_network, + self.config[self.LANE_WIDTH], + self.config["lane_num"], + parent_node_path, + pg_physics_world, + 1, + length=length + ) + self.blocks.append(last_block) + + split = Split(1, last_block.get_socket(index=0), self.road_network, random_seed=1) + split.construct_block( + parent_node_path, pg_physics_world, { + "length": 2, + "lane_num": self.config["toll_lane_num"] - self.config["lane_num"], + "bottle_len": self.BOTTLE_LENGTH, + } + ) + self.blocks.append(split) + toll = TollGate(2, split.get_socket(index=0), self.road_network, random_seed=1) + toll.construct_block(parent_node_path, pg_physics_world, { + "length": self.config["toll_length"], + }) + + self.blocks.append(toll) + + # Build Toll + merge = Merge(3, toll.get_socket(index=0), self.road_network, random_seed=1) + merge.construct_from_config( + dict( + lane_num=self.config["toll_lane_num"] - self.config["lane_num"], + length=self.config["exit_length"], + bottle_len=self.BOTTLE_LENGTH, + ), parent_node_path, pg_physics_world + ) + self.blocks.append(merge) + + +class MultiAgentTollgateEnv(MultiAgentPGDrive): + spawn_roads = [Road(FirstBlock.NODE_2, FirstBlock.NODE_3), -Road(Merge.node(3, 0, 0), Merge.node(3, 0, 1))] + + def __init__(self, config): + super(MultiAgentTollgateEnv, self).__init__(config) + self.stay_time_manager = StayTimeManager() + + def reset(self, *args, **kwargs): + self.stay_time_manager.reset() + return super(MultiAgentTollgateEnv, self).reset(*args, **kwargs) + + @staticmethod + def default_config() -> PGConfig: + assert MATollConfig["vehicle_config"]["side_detector"]["num_lasers"] > 2 + assert MATollConfig["vehicle_config"]["lane_line_detector"]["num_lasers"] > 2 + return MultiAgentPGDrive.default_config().update(MATollConfig, allow_overwrite=True) + + def _update_map(self, episode_data: dict = None, force_seed=None): + if episode_data is not None: + raise ValueError() + map_config = self.config["map_config"] + map_config.update({"seed": self.current_seed}) + + if self.current_map is None: + self.current_seed = 0 + new_map = MATollGateMap(self.pg_world, map_config) + self.maps[self.current_seed] = new_map + self.current_map = self.maps[self.current_seed] + + def reward_function(self, vehicle_id: str): + """ + Override this func to get a new reward function + :param vehicle_id: id of BaseVehicle + :return: reward + """ + vehicle = self.vehicles[vehicle_id] + step_info = dict() + + # Reward for moving forward in current lane + if vehicle.lane in vehicle.routing_localization.current_ref_lanes: + current_lane = vehicle.lane + else: + current_lane = vehicle.routing_localization.current_ref_lanes[0] + current_road = vehicle.current_road + long_last, _ = current_lane.local_coordinates(vehicle.last_position) + long_now, lateral_now = current_lane.local_coordinates(vehicle.position) + + # reward for lane keeping, without it vehicle can learn to overtake but fail to keep in lane + if self.config["use_lateral"]: + lateral_factor = clip( + 1 - 2 * abs(lateral_now) / vehicle.routing_localization.get_current_lane_width(), 0.0, 1.0 + ) + else: + lateral_factor = 1.0 + + reward = 0.0 + reward += self.config["driving_reward"] * (long_now - long_last) * lateral_factor + + if vehicle.current_road.block_ID() == TollGate.ID: + if vehicle.overspeed: # Too fast! + reward = -self.config["overspeed_penalty"] * vehicle.speed / vehicle.max_speed + else: + # Good! At very low speed + pass + else: + reward += self.config["speed_reward"] * (vehicle.speed / vehicle.max_speed) + + step_info["step_reward"] = reward + + if vehicle.arrive_destination: + reward = +self.config["success_reward"] + elif self._is_out_of_road(vehicle): + reward = -self.config["out_of_road_penalty"] + elif vehicle.crash_vehicle: + reward = -self.config["crash_vehicle_penalty"] + elif vehicle.crash_object: + reward = -self.config["crash_object_penalty"] + return reward, step_info + + def _is_out_of_road(self, vehicle): + # A specified function to determine whether this vehicle should be done. + # return vehicle.on_yellow_continuous_line or (not vehicle.on_lane) or vehicle.crash_sidewalk + ret = vehicle.crash_sidewalk + if self.config["cross_yellow_line_done"]: + ret = ret or vehicle.on_yellow_continuous_line + return ret + + def done_function(self, vehicle_id): + done, done_info = super(MultiAgentPGDrive, self).done_function(vehicle_id) + if done_info[TerminationState.CRASH_VEHICLE] and (not self.config["crash_done"]): + assert done_info[TerminationState.CRASH_VEHICLE] or done_info[TerminationState.CRASH_BUILDING] or \ + done_info[TerminationState.SUCCESS] or done_info[TerminationState.OUT_OF_ROAD] + if not (done_info[TerminationState.SUCCESS] or done_info[TerminationState.OUT_OF_ROAD]): + # Does not revert done if high-priority termination happens! + done = False + + if done_info[TerminationState.OUT_OF_ROAD] and (not self.config["out_of_road_done"]): + assert done_info[TerminationState.CRASH_VEHICLE] or done_info[TerminationState.CRASH_BUILDING] or \ + done_info[TerminationState.SUCCESS] or done_info[TerminationState.OUT_OF_ROAD] + if not done_info[TerminationState.SUCCESS]: + done = False + + if vehicle_id in self.stay_time_manager.entry_time and vehicle_id in self.stay_time_manager.exit_time: + entry = self.stay_time_manager.entry_time[vehicle_id] + exit = self.stay_time_manager.exit_time[vehicle_id] + if (exit - entry) < self.config["vehicle_config"]["min_pass_steps"]: + done = True + done_info["out_of_road"] = True + + return done, done_info + + def get_single_observation(self, vehicle_config): + o = TollGateObservation(vehicle_config) + return o + + def step(self, actions): + o, r, d, i = super(MultiAgentTollgateEnv, self).step(actions) + self.stay_time_manager.record(self.agent_manager.active_agents, self.episode_steps) + return o, r, d, i + + +def _draw(): + env = MultiAgentTollgateEnv() + o = env.reset() + from pgdrive.utils import draw_top_down_map + import matplotlib.pyplot as plt + + plt.imshow(draw_top_down_map(env.current_map)) + plt.show() + env.close() + + +def _expert(): + env = MultiAgentTollgateEnv( + { + "vehicle_config": { + "lidar": { + "num_lasers": 240, + "num_others": 4, + "distance": 50 + }, + "use_saver": True, + "save_level": 1. + }, + "pg_world_config": { + "debug_physics_world": True + }, + "fast": True, + # "use_render": True, + "debug": True, + "manual_control": True, + "num_agents": 4, + } + ) + o = env.reset() + total_r = 0 + ep_s = 0 + for i in range(1, 100000): + o, r, d, info = env.step(env.action_space.sample()) + for r_ in r.values(): + total_r += r_ + ep_s += 1 + d.update({"total_r": total_r, "episode length": ep_s}) + # env.render(text=d) + if d["__all__"]: + print( + "Finish! Current step {}. Group Reward: {}. Average reward: {}".format( + i, total_r, total_r / env.agent_manager.next_agent_count + ) + ) + break + if len(env.vehicles) == 0: + total_r = 0 + print("Reset") + env.reset() + env.close() + + +def _vis_debug_respawn(): + env = MultiAgentTollgateEnv( + { + "horizon": 100000, + "vehicle_config": { + "lidar": { + "num_lasers": 72, + "num_others": 0, + "distance": 40 + }, + "show_lidar": False, + }, + "pg_world_config": { + "debug_physics_world": True + }, + "fast": True, + "use_render": True, + "debug": False, + "manual_control": True, + "num_agents": 20, + } + ) + o = env.reset() + total_r = 0 + ep_s = 0 + for i in range(1, 100000): + action = {k: [.0, 1.0] for k in env.vehicles.keys()} + o, r, d, info = env.step(action) + for r_ in r.values(): + total_r += r_ + ep_s += 1 + # d.update({"total_r": total_r, "episode length": ep_s}) + render_text = { + "total_r": total_r, + "episode length": ep_s, + "cam_x": env.main_camera.camera_x, + "cam_y": env.main_camera.camera_y, + "cam_z": env.main_camera.top_down_camera_height + } + env.render(text=render_text) + if d["__all__"]: + print( + "Finish! Current step {}. Group Reward: {}. Average reward: {}".format( + i, total_r, total_r / env.agent_manager.next_agent_count + ) + ) + # break + if len(env.vehicles) == 0: + total_r = 0 + print("Reset") + env.reset() + env.close() + + +def _vis(): + env = MultiAgentTollgateEnv( + { + "horizon": 100000, + "vehicle_config": { + "lidar": { + "num_lasers": 72, + "num_others": 0, + "distance": 40 + }, + # "show_lidar": True, + # "show_side_detector":True, + # "show_lane_line_detector":True, + }, + "traffic_density": 0., + "traffic_mode": "hybrid", + "debug": True, + "fast": True, + "use_render": True, + # "debug": True, + "manual_control": True, + "num_agents": 1, + } + ) + o = env.reset() + total_r = 0 + ep_s = 0 + for i in range(1, 100000): + o, r, d, info = env.step({k: [0, 1] for k in env.vehicles.keys()}) + for r_ in r.values(): + total_r += r_ + ep_s += 1 + # d.update({"total_r": total_r, "episode length": ep_s}) + render_text = { + "total_r": total_r, + "episode length": ep_s, + "cam_x": env.main_camera.camera_x, + "cam_y": env.main_camera.camera_y, + "cam_z": env.main_camera.top_down_camera_height + } + track_v = env.agent_manager.object_to_agent(env.current_track_vehicle.name) + if track_v in r: + render_text["tack_v_reward"] = r[track_v] + render_text["dist_to_right"] = env.current_track_vehicle.dist_to_right_side + render_text["dist_to_left"] = env.current_track_vehicle.dist_to_left_side + render_text["overspeed"] = env.current_track_vehicle.overspeed + render_text["lane"] = env.current_track_vehicle.lane_index + render_text["block"] = env.current_track_vehicle.current_road.block_ID() + env.render(text=render_text) + if d["__all__"]: + print(info) + print( + "Finish! Current step {}. Group Reward: {}. Average reward: {}".format( + i, total_r, total_r / env.agent_manager.next_agent_count + ) + ) + # break + if len(env.vehicles) == 0: + total_r = 0 + print("Reset") + env.reset() + env.close() + + +def _profile(): + import time + env = MultiAgentTollgateEnv({"num_agents": 8}) + obs = env.reset() + start = time.time() + for s in range(10000): + o, r, d, i = env.step(env.action_space.sample()) + + # mask_ratio = env.scene_manager.detector_mask.get_mask_ratio() + # print("Mask ratio: ", mask_ratio) + + if all(d.values()): + env.reset() + if (s + 1) % 100 == 0: + print( + "Finish {}/10000 simulation steps. Time elapse: {:.4f}. Average FPS: {:.4f}".format( + s + 1, + time.time() - start, (s + 1) / (time.time() - start) + ) + ) + print(f"(PGDriveEnvV2) Total Time Elapse: {time.time() - start}") + + +def _long_run(): + # Please refer to test_ma_Toll_reward_done_alignment() + _out_of_road_penalty = 3 + env = MultiAgentTollgateEnv( + { + "num_agents": 8, + "vehicle_config": { + "lidar": { + "num_others": 8 + } + }, + **dict( + out_of_road_penalty=_out_of_road_penalty, + crash_vehicle_penalty=1.333, + crash_object_penalty=11, + crash_vehicle_cost=13, + crash_object_cost=17, + out_of_road_cost=19, + ) + } + ) + try: + obs = env.reset() + assert env.observation_space.contains(obs) + for step in range(10000): + act = env.action_space.sample() + o, r, d, i = env.step(act) + if step == 0: + assert not any(d.values()) + + if any(d.values()): + print("Current Done: {}\nReward: {}".format(d, r)) + for kkk, ddd in d.items(): + if ddd and kkk != "__all__": + print("Info {}: {}\n".format(kkk, i[kkk])) + print("\n") + + for kkk, rrr in r.items(): + if rrr == -_out_of_road_penalty: + assert d[kkk] + + if (step + 1) % 200 == 0: + print( + "{}/{} Agents: {} {}\nO: {}\nR: {}\nD: {}\nI: {}\n\n".format( + step + 1, 10000, len(env.vehicles), list(env.vehicles.keys()), + {k: (oo.shape, oo.mean(), oo.min(), oo.max()) + for k, oo in o.items()}, r, d, i + ) + ) + if d["__all__"]: + print('Current step: ', step) + break + finally: + env.close() + + +if __name__ == "__main__": + # _draw() + _vis() + # _vis_debug_respawn() + # _profile() + # _long_run() diff --git a/pgdrive/envs/pgdrive_env_v2.py b/pgdrive/envs/pgdrive_env_v2.py index 3bd46afcd..89f6db6c8 100644 --- a/pgdrive/envs/pgdrive_env_v2.py +++ b/pgdrive/envs/pgdrive_env_v2.py @@ -94,7 +94,9 @@ def _is_out_of_road(self, vehicle): def done_function(self, vehicle_id: str): vehicle = self.vehicles[vehicle_id] done = False - done_info = dict(crash_vehicle=False, crash_object=False, out_of_road=False, arrive_dest=False) + done_info = dict( + crash_vehicle=False, crash_object=False, crash_building=False, out_of_road=False, arrive_dest=False + ) if vehicle.arrive_destination: done = True logging.info("Episode ended! Reason: arrive_dest.") @@ -105,17 +107,23 @@ def done_function(self, vehicle_id: str): done_info[TerminationState.OUT_OF_ROAD] = True elif vehicle.crash_vehicle: done = True - logging.info("Episode ended! Reason: crash. ") + logging.info("Episode ended! Reason: crash vehicle ") done_info[TerminationState.CRASH_VEHICLE] = True # elif vehicle.out_of_route or not vehicle.on_lane or vehicle.crash_sidewalk: elif vehicle.crash_object: done = True done_info[TerminationState.CRASH_OBJECT] = True + logging.info("Episode ended! Reason: crash object ") + elif vehicle.crash_building: + done = True + done_info[TerminationState.CRASH_BUILDING] = True + logging.info("Episode ended! Reason: crash building ") # for compatibility # crash almost equals to crashing with vehicles done_info[TerminationState.CRASH - ] = done_info[TerminationState.CRASH_VEHICLE] or done_info[TerminationState.CRASH_OBJECT] + ] = done_info[TerminationState.CRASH_VEHICLE] or done_info[TerminationState.CRASH_OBJECT] or \ + done_info[TerminationState.CRASH_BUILDING] return done, done_info def cost_function(self, vehicle_id: str): diff --git a/pgdrive/scene_creator/blocks/block.py b/pgdrive/scene_creator/blocks/block.py index e249a50aa..30f9f063c 100644 --- a/pgdrive/scene_creator/blocks/block.py +++ b/pgdrive/scene_creator/blocks/block.py @@ -1,13 +1,13 @@ import logging from collections import OrderedDict -from typing import Dict, Union, List +from typing import Dict, Union, List, Tuple import copy import numpy from panda3d.bullet import BulletBoxShape, BulletRigidBodyNode, BulletGhostNode from panda3d.core import Vec3, LQuaternionf, BitMask32, Vec4, CardMaker, TextureStage, RigidBodyCombiner, \ TransparencyAttrib, SamplerState, NodePath -from pgdrive.constants import Decoration, BodyName, CamMask +from pgdrive.constants import Decoration, BodyName, CamMask, CollisionGroup from pgdrive.scene_creator.blocks.constants import BlockDefault from pgdrive.scene_creator.lane.abs_lane import AbstractLane, LineType, LaneNode, LineColor from pgdrive.scene_creator.lane.circular_lane import CircularLane @@ -15,7 +15,7 @@ from pgdrive.scene_creator.road.road import Road from pgdrive.scene_creator.road.road_network import RoadNetwork from pgdrive.utils.asset_loader import AssetLoader -from pgdrive.utils.coordinates_shift import panda_position +from pgdrive.utils.coordinates_shift import panda_position, panda_heading from pgdrive.utils.element import Element from pgdrive.utils.math_utils import norm, PGVector from pgdrive.world.pg_physics_world import PGPhysicsWorld @@ -91,6 +91,7 @@ def __init__(self, block_index: int, pre_block_socket: BlockSocket, global_netwo # each block contains its own road network and a global network self._global_network = global_network self.block_network = RoadNetwork() + self._block_objects = None # used to spawn npc self._respawn_roads = [] @@ -136,8 +137,14 @@ def construct_block( Randomly Construct a block, if overlap return False """ self.set_config(self.PARAMETER_SPACE.sample()) + self.node_path = NodePath(self._block_name) + self._block_objects = [] if extra_config: - self.set_config(extra_config) + assert set(extra_config.keys()).issubset(self.PARAMETER_SPACE.parameters), \ + "Make sure the parameters' name are as same as what defined in pg_space.py" + raw_config = self.get_config() + raw_config.update(extra_config) + self.set_config(raw_config) success = self._sample_topology() self._create_in_world() self.attach_to_pg_world(root_render_np, pg_physics_world) @@ -149,6 +156,9 @@ def destruct_block(self, pg_physics_world: PGPhysicsWorld): self.node_path.removeNode() self.dynamic_nodes.clear() self.static_nodes.clear() + for obj in self._block_objects: + obj.destroy(pg_physics_world) + self._block_objects = None def _sample_topology(self) -> bool: """ @@ -162,12 +172,7 @@ def _sample_topology(self) -> bool: return no_cross def construct_from_config(self, config: Dict, root_render_np: NodePath, pg_physics_world: PGPhysicsWorld): - assert set(config.keys()) == self.PARAMETER_SPACE.parameters, \ - "Make sure the parameters' name are as same as what defined in pg_space.py" - self.set_config(config) - success = self._sample_topology() - self._create_in_world() - self.attach_to_pg_world(root_render_np, pg_physics_world) + success = self.construct_block(root_render_np, pg_physics_world, config) return success def get_socket(self, index: Union[str, int]) -> BlockSocket: @@ -303,7 +308,6 @@ def _create_in_world(self): self.lane_vis_node_path.node().collect() self.lane_vis_node_path.hide(CamMask.DepthCam | CamMask.ScreenshotCam) - self.node_path = NodePath(self._block_name) self.node_path.hide(CamMask.Shadow) self.sidewalk_node_path.reparentTo(self.node_path) @@ -589,3 +593,53 @@ def get_socket_indices(self): def get_socket_list(self): return list(self._sockets.values()) + + def _generate_invisible_static_wall( + self, + position: Tuple, + heading: float, + heading_length: float, + side_width: float, + height=10, + name=BodyName.InvisibleWall, + collision_group=CollisionGroup.InvisibleWall + ): + """ + Add an invisible physics wall to physics world + You can add some building models to the same location, add make it be detected by lidar + ---------------------------------- + | * | --->>> + ---------------------------------- + * position + --->>> heading direction + ------ longitude length + | lateral width + + **CAUTION**: position is the middle point of longitude edge + :param position: position in PGDrive + :param heading: heading in PGDrive [degree] + :param heading_length: rect length in heading direction + :param side_width: rect width in side direction + :param height: the detect will be executed from this height to 0 + :param name: name of this invisible wall + :param collision_group: control the collision of this static wall and other elements + :return node_path + """ + shape = BulletBoxShape(Vec3(heading_length / 2, side_width / 2, height)) + body_node = BulletRigidBodyNode(name) + body_node.setActive(False) + body_node.setKinematic(False) + body_node.setStatic(True) + wall_np = self.node_path.attachNewNode(body_node) + body_node.addShape(shape) + body_node.setIntoCollideMask(BitMask32.bit(collision_group)) + self.dynamic_nodes.append(body_node) + wall_np.setPos(panda_position(position)) + wall_np.setH(panda_heading(heading)) + return wall_np + + def construct_block_buildings(self, object_manager): + """ + Buildings will be added to object_manager as static object automatically + """ + pass diff --git a/pgdrive/scene_creator/blocks/bottleneck.py b/pgdrive/scene_creator/blocks/bottleneck.py index 7e08bd30a..248bd8df4 100644 --- a/pgdrive/scene_creator/blocks/bottleneck.py +++ b/pgdrive/scene_creator/blocks/bottleneck.py @@ -15,7 +15,7 @@ class Bottleneck(Block): PARAMETER_SPACE = PGSpace(BlockParameterSpace.BOTTLENECK_PARAMETER) # property of bottleneck - BOTTLENECK_LEN = 20 # Add to parameter sapce in the future + BOTTLENECK_LEN = None class Merge(Bottleneck): @@ -33,6 +33,7 @@ class Merge(Bottleneck): def _try_plug_into_previous_block(self) -> bool: no_cross = True parameters = self.get_config() + self.BOTTLENECK_LEN = parameters["bottle_len"] lane_num_changed = parameters[Parameter.lane_num] start_ndoe = self.pre_block_socket.positive_road.end_node @@ -167,6 +168,7 @@ class Split(Bottleneck): def _try_plug_into_previous_block(self) -> bool: no_cross = True parameters = self.get_config() + self.BOTTLENECK_LEN = parameters["bottle_len"] lane_num_changed = parameters[Parameter.lane_num] start_ndoe = self.pre_block_socket.positive_road.end_node diff --git a/pgdrive/scene_creator/blocks/first_block.py b/pgdrive/scene_creator/blocks/first_block.py index e636a85fb..ac95be632 100644 --- a/pgdrive/scene_creator/blocks/first_block.py +++ b/pgdrive/scene_creator/blocks/first_block.py @@ -50,6 +50,7 @@ def __init__( CreateRoadFrom(next_lane, lane_num, other_v_spawn_road, self.block_network, self._global_network) CreateAdverseRoad(other_v_spawn_road, self.block_network, self._global_network) + self.node_path = NodePath(self._block_name) self._create_in_world() # global_network += self.block_network diff --git a/pgdrive/scene_creator/blocks/tollgate.py b/pgdrive/scene_creator/blocks/tollgate.py new file mode 100644 index 000000000..e56f8401b --- /dev/null +++ b/pgdrive/scene_creator/blocks/tollgate.py @@ -0,0 +1,98 @@ +import numpy as np + +from pgdrive.constants import BodyName +from pgdrive.scene_creator.blocks.block import BlockSocket +from pgdrive.scene_creator.blocks.bottleneck import Block +from pgdrive.scene_creator.blocks.create_block_utils import CreateAdverseRoad, CreateRoadFrom, ExtendStraightLane +from pgdrive.scene_creator.buildings.base_building import BaseBuilding +from pgdrive.scene_creator.lane.abs_lane import LineType, LineColor +from pgdrive.scene_creator.road.road import Road +from pgdrive.utils.asset_loader import AssetLoader +from pgdrive.utils.pg_space import PGSpace, Parameter, BlockParameterSpace + +TollGateBuilding = BaseBuilding + + +class TollGate(Block): + """ + Toll, like Straight, but has speed limit + """ + SOCKET_NUM = 1 + PARAMETER_SPACE = PGSpace(BlockParameterSpace.BOTTLENECK_PARAMETER) + ID = "$" + + SPEED_LIMIT = 3 # m/s ~= 5 miles per hour https://bestpass.com/feed/61-speeding-through-tolls + BUILDING_LENGTH = 10 + BUILDING_HEIGHT = 5 + + def _try_plug_into_previous_block(self) -> bool: + self.set_part_idx(0) # only one part in simple block like straight, and curve + para = self.get_config() + length = para[Parameter.length] + self.BUILDING_LENGTH = length + basic_lane = self.positive_basic_lane + new_lane = ExtendStraightLane(basic_lane, length, [LineType.CONTINUOUS, LineType.SIDE]) + start = self.pre_block_socket.positive_road.end_node + end = self.add_road_node() + socket = Road(start, end) + _socket = -socket + + # create positive road + no_cross = CreateRoadFrom( + new_lane, + self.positive_lane_num, + socket, + self.block_network, + self._global_network, + center_line_color=LineColor.YELLOW, + center_line_type=LineType.CONTINUOUS, + inner_lane_line_type=LineType.CONTINUOUS, + side_lane_line_type=LineType.SIDE + ) + + # create negative road + no_cross = CreateAdverseRoad( + socket, + self.block_network, + self._global_network, + center_line_color=LineColor.YELLOW, + center_line_type=LineType.CONTINUOUS, + inner_lane_line_type=LineType.CONTINUOUS, + side_lane_line_type=LineType.SIDE + ) and no_cross + + self.add_sockets(BlockSocket(socket, _socket)) + self._add_building_and_speed_limit(socket) + self._add_building_and_speed_limit(_socket) + return no_cross + + def _add_building_and_speed_limit(self, road): + # add house + lanes = road.get_lanes(self.block_network) + for idx, lane in enumerate(lanes): + lane.set_speed_limit(self.SPEED_LIMIT) + if idx % 2 == 1: + # add toll + position = lane.position(lane.length / 2, 0) + node_path = self._generate_invisible_static_wall( + position, + np.rad2deg(lane.heading_at(0)), + self.BUILDING_LENGTH, + self.lane_width, + self.BUILDING_HEIGHT / 2, + name=BodyName.TollGate + ) + if self.render: + building_model = self.loader.loadModel(AssetLoader.file_path("models", "box.bam")) + building_model.setScale(self.BUILDING_LENGTH, self.lane_width, self.BUILDING_HEIGHT) + building_model.setColor(0.2, 0.2, 0.2) + building_model.reparentTo(node_path) + + building = TollGateBuilding( + lane, (road.start_node, road.end_node, idx), position, lane.heading_at(0), node_path + ) + self._block_objects.append(building) + + def construct_block_buildings(self, object_manager): + for building in self._block_objects: + object_manager.add_block_buildings(building) diff --git a/pgdrive/scene_creator/buildings/__init__.py b/pgdrive/scene_creator/buildings/__init__.py new file mode 100644 index 000000000..e69de29bb diff --git a/pgdrive/scene_creator/buildings/base_building.py b/pgdrive/scene_creator/buildings/base_building.py new file mode 100644 index 000000000..0db6b8be9 --- /dev/null +++ b/pgdrive/scene_creator/buildings/base_building.py @@ -0,0 +1,16 @@ +from pgdrive.scene_creator.object.base_object import BaseObject + + +class BaseBuilding(BaseObject): + def __init__(self, lane, lane_index, position, heading: float = 0., node_path=None): + super(BaseBuilding, self).__init__(lane, lane_index, position, heading) + assert node_path is not None + self.node_path = node_path + + def destroy(self, pg_physics_world): + self.detach_from_pg_world(pg_physics_world) + self.node_path.removeNode() + self.dynamic_nodes.clear() + self.static_nodes.clear() + self._config.clear() + self.node_path.removeNode() diff --git a/pgdrive/scene_creator/highway_vehicle/behavior.py b/pgdrive/scene_creator/highway_vehicle/behavior.py index 78dd5c6ea..5ebd38b26 100644 --- a/pgdrive/scene_creator/highway_vehicle/behavior.py +++ b/pgdrive/scene_creator/highway_vehicle/behavior.py @@ -6,7 +6,8 @@ from pgdrive.constants import Route, LaneIndex from pgdrive.scene_creator.highway_vehicle.controller import ControlledVehicle from pgdrive.scene_creator.highway_vehicle.kinematics import Vehicle -from pgdrive.scene_creator.object.traffic_object import Object +from pgdrive.scene_creator.object.traffic_object import TrafficObject +from pgdrive.scene_creator.object.base_object import BaseObject from pgdrive.scene_manager.traffic_manager import TrafficManager from pgdrive.utils.math_utils import clip @@ -148,12 +149,13 @@ def acceleration( :param rear_vehicle: the vehicle following the ego-vehicle :return: the acceleration command for the ego-vehicle [m/s2] """ - if not ego_vehicle or isinstance(ego_vehicle, Object): + if not ego_vehicle or isinstance(ego_vehicle, BaseObject): return 0 ego_target_speed = utils.not_zero(getattr(ego_vehicle, "target_speed", 0)) acceleration = self.COMFORT_ACC_MAX * (1 - np.power(max(ego_vehicle.speed, 0) / ego_target_speed, self.DELTA)) if front_vehicle: + from pgdrive.scene_creator.buildings.base_building import BaseBuilding d = ego_vehicle.lane_distance_to(front_vehicle) acceleration -= self.COMFORT_ACC_MAX * \ np.power(self.desired_gap(ego_vehicle, front_vehicle) / utils.not_zero(d), 2) diff --git a/pgdrive/scene_creator/highway_vehicle/kinematics.py b/pgdrive/scene_creator/highway_vehicle/kinematics.py index cf7d9bf87..57615463d 100644 --- a/pgdrive/scene_creator/highway_vehicle/kinematics.py +++ b/pgdrive/scene_creator/highway_vehicle/kinematics.py @@ -7,7 +7,7 @@ import pgdrive.utils.math_utils as utils from pgdrive.constants import LaneIndex from pgdrive.scene_creator.lane.abs_lane import AbstractLane -from pgdrive.scene_creator.object.traffic_object import Object +from pgdrive.scene_creator.object.traffic_object import TrafficObject from pgdrive.scene_manager.traffic_manager import TrafficManager from pgdrive.utils import get_np_random, random_string, distance_greater, norm @@ -181,7 +181,7 @@ def lane_distance_to(self, vehicle: "Vehicle", lane: AbstractLane = None) -> flo lane = self.lane return lane.local_coordinates(vehicle.position)[0] - lane.local_coordinates(self.position)[0] - def check_collision(self, other: Union['Vehicle', 'Object']) -> None: + def check_collision(self, other: Union['Vehicle', 'TrafficObject']) -> None: """ Check for collision with another vehicle. @@ -197,14 +197,14 @@ def check_collision(self, other: Union['Vehicle', 'Object']) -> None: if self._is_colliding(other): self.speed = other.speed = min([self.speed, other.speed], key=abs) self.crashed = other.crashed = True - elif isinstance(other, Object): + elif isinstance(other, TrafficObject): if not self.COLLISIONS_ENABLED: return if self._is_colliding(other): self.speed = min([self.speed, 0], key=abs) self.crashed = other.hit = True - elif isinstance(other, Object): + elif isinstance(other, TrafficObject): if self._is_colliding(other): other.hit = True diff --git a/pgdrive/scene_creator/lane/abs_lane.py b/pgdrive/scene_creator/lane/abs_lane.py index d550b284b..0769fb444 100644 --- a/pgdrive/scene_creator/lane/abs_lane.py +++ b/pgdrive/scene_creator/lane/abs_lane.py @@ -42,6 +42,12 @@ class AbstractLane(object): forbidden = None line_color = [LineColor.GREY, LineColor.GREY] + def __init__(self): + self.speed_limit = 1000 # should be set manually + + def set_speed_limit(self, speed_limit): + self.speed_limit = speed_limit + @abstractmethod def position(self, longitudinal: float, lateral: float) -> np.ndarray: """ diff --git a/pgdrive/scene_creator/lane/circular_lane.py b/pgdrive/scene_creator/lane/circular_lane.py index 271166830..661a32d4c 100644 --- a/pgdrive/scene_creator/lane/circular_lane.py +++ b/pgdrive/scene_creator/lane/circular_lane.py @@ -18,7 +18,7 @@ def __init__( width: float = AbstractLane.DEFAULT_WIDTH, line_types: Tuple[LineType, LineType] = (LineType.BROKEN, LineType.BROKEN), forbidden: bool = False, - speed_limit: float = 20, + speed_limit: float = 1000, priority: int = 0 ) -> None: super().__init__() @@ -31,7 +31,6 @@ def __init__( self.line_types = line_types self.forbidden = forbidden self.priority = priority - self.speed_limit = speed_limit self.length = self.radius * (self.end_phase - self.start_phase) * self.direction def update_properties(self): diff --git a/pgdrive/scene_creator/lane/straight_lane.py b/pgdrive/scene_creator/lane/straight_lane.py index c8bbcffa8..6c07b0469 100644 --- a/pgdrive/scene_creator/lane/straight_lane.py +++ b/pgdrive/scene_creator/lane/straight_lane.py @@ -15,7 +15,7 @@ def __init__( width: float = AbstractLane.DEFAULT_WIDTH, line_types: Tuple[LineType, LineType] = (LineType.BROKEN, LineType.BROKEN), forbidden: bool = False, - speed_limit: float = 20, + speed_limit: float = 1000, priority: int = 0 ) -> None: """ @@ -28,19 +28,20 @@ def __init__( :param forbidden: is changing to this lane forbidden :param priority: priority level of the lane, for determining who has right of way """ + super(StraightLane, self).__init__() self.start = np.array(start) self.end = np.array(end) self.width = width self.line_types = line_types or [LineType.BROKEN, LineType.BROKEN] self.forbidden = forbidden self.priority = priority - self.speed_limit = speed_limit self.length = norm((self.end - self.start)[0], (self.end - self.start)[1]) self.heading = math.atan2(self.end[1] - self.start[1], self.end[0] - self.start[0]) self.direction = (self.end - self.start) / self.length self.direction_lateral = np.array([-self.direction[1], self.direction[0]]) def update_properties(self): + super(StraightLane, self).__init__() self.length = norm((self.end - self.start)[0], (self.end - self.start)[1]) self.heading = math.atan2(self.end[1] - self.start[1], self.end[0] - self.start[0]) self.direction = (self.end - self.start) / self.length @@ -63,6 +64,7 @@ def local_coordinates(self, position: Tuple[float, float]) -> Tuple[float, float return float(longitudinal), float(lateral) def reset_start_end(self, start: Vector, end: Vector): + super(StraightLane, self).__init__() self.start = start self.end = end self.update_properties() diff --git a/pgdrive/scene_creator/object/__init__.py b/pgdrive/scene_creator/object/__init__.py index 8b1378917..f70aa6647 100644 --- a/pgdrive/scene_creator/object/__init__.py +++ b/pgdrive/scene_creator/object/__init__.py @@ -1 +1 @@ - +from pgdrive.scene_creator.object.traffic_object import TrafficObject diff --git a/pgdrive/scene_creator/object/base_object.py b/pgdrive/scene_creator/object/base_object.py new file mode 100644 index 000000000..e961f34d4 --- /dev/null +++ b/pgdrive/scene_creator/object/base_object.py @@ -0,0 +1,24 @@ +from typing import Sequence, Tuple + +import numpy as np + +from pgdrive.utils.element import Element + +LaneIndex = Tuple[str, str, int] + + +class BaseObject(Element): + def __init__(self, lane, lane_index: LaneIndex, position: Sequence[float], heading: float = 0.): + """ + :param lane: the lane to spawn object + :param lane_index: the lane_index of the spawn point + :param position: cartesian position of object in the surface + :param heading: the angle from positive direction of horizontal axis + """ + super(BaseObject, self).__init__() + self.position = position + self.speed = 0 + self.heading = heading / np.pi * 180 + self.lane_index = lane_index + self.lane = lane + self.body_node = None diff --git a/pgdrive/scene_creator/object/traffic_object.py b/pgdrive/scene_creator/object/traffic_object.py index 9959d6b7c..68cb0856d 100644 --- a/pgdrive/scene_creator/object/traffic_object.py +++ b/pgdrive/scene_creator/object/traffic_object.py @@ -6,7 +6,7 @@ from pgdrive.constants import BodyName from pgdrive.utils.asset_loader import AssetLoader from pgdrive.utils.coordinates_shift import panda_position, panda_heading -from pgdrive.utils.element import Element +from pgdrive.scene_creator.object.base_object import BaseObject from pgdrive.utils import get_np_random, random_string, distance_greater, norm LaneIndex = Tuple[str, str, int] @@ -24,7 +24,7 @@ def __init__(self, object_body_name: str): self.crashed = False -class Object(Element): +class TrafficObject(BaseObject): """ Common interface for objects that appear on the road, beside vehicles. """ @@ -41,7 +41,7 @@ def __init__(self, lane, lane_index: LaneIndex, position: Sequence[float], headi :param heading: the angle from positive direction of horizontal axis """ assert self.NAME is not None, "Assign a name for this class for finding it easily" - super(Object, self).__init__() + super(TrafficObject, self).__init__(lane, lane_index, position, heading) self.position = position self.speed = 0 self.heading = heading / np.pi * 180 @@ -71,7 +71,7 @@ def type(cls): return cls.__subclasses__() -class TrafficCone(Object): +class TrafficCone(TrafficObject): """Placed near the construction section to indicate that traffic is prohibited""" NAME = BodyName.Traffic_cone @@ -91,7 +91,7 @@ def __init__(self, lane, lane_index: LaneIndex, position: Sequence[float], headi model.reparentTo(self.node_path) -class TrafficTriangle(Object): +class TrafficTriangle(TrafficObject): """Placed behind the vehicle when it breaks down""" NAME = BodyName.Traffic_triangle diff --git a/pgdrive/scene_creator/pg_blocks.py b/pgdrive/scene_creator/pg_blocks.py index f523fda56..3ce4b66fc 100644 --- a/pgdrive/scene_creator/pg_blocks.py +++ b/pgdrive/scene_creator/pg_blocks.py @@ -6,6 +6,8 @@ from pgdrive.scene_creator.blocks.std_intersection import StdInterSection from pgdrive.scene_creator.blocks.std_t_intersection import StdTInterSection from pgdrive.scene_creator.blocks.straight import Straight +from pgdrive.scene_creator.blocks.parking_lot import ParkingLot +from pgdrive.scene_creator.blocks.tollgate import TollGate class PGBlock: @@ -21,7 +23,9 @@ class PGBlock: InFork: 0.00, OutFork: 0.00, Merge: 0.00, - Split: 0.00 + Split: 0.00, + ParkingLot: 0.00, + TollGate: 0.00 } BLOCK_TYPE_DISTRIBUTION_V2 = { @@ -39,7 +43,9 @@ class PGBlock: InFork: 0.00, OutFork: 0.00, Merge: 0.00, - Split: 0.00 + Split: 0.00, + ParkingLot: 0.00, + TollGate: 0.00 } @classmethod diff --git a/pgdrive/scene_creator/road/road.py b/pgdrive/scene_creator/road/road.py index e022e4d12..3df4a5f20 100644 --- a/pgdrive/scene_creator/road/road.py +++ b/pgdrive/scene_creator/road/road.py @@ -41,9 +41,10 @@ def lane_num(self, road_network): return len(self.get_lanes(road_network)) def block_ID(self): - if re.search(">", self.end_node) is not None: + search_node = self.end_node if not self.is_negative_road() else self.start_node + if re.search(">", search_node) is not None: return ">" - block_id = re.search("[a-zA-Z]", self.end_node).group(0) + block_id = re.search("[a-zA-Z$]", search_node).group(0) return block_id def __eq__(self, other): diff --git a/pgdrive/scene_creator/vehicle/base_vehicle.py b/pgdrive/scene_creator/vehicle/base_vehicle.py index d64d188f9..f7290df36 100644 --- a/pgdrive/scene_creator/vehicle/base_vehicle.py +++ b/pgdrive/scene_creator/vehicle/base_vehicle.py @@ -824,6 +824,10 @@ def on_broken_line(self): def set_static(self, flag): self.chassis_np.node().setStatic(flag) + @property + def crash_building(self): + return self.chassis_np.node().getPythonTag(BodyName.Base_vehicle).crash_building + @property def reference_lanes(self): return self.routing_localization.current_ref_lanes @@ -832,3 +836,7 @@ def set_wheel_friction(self, new_friction): raise DeprecationWarning("Bug exists here") for wheel in self.wheels: wheel.setFrictionSlip(new_friction) + + @property + def overspeed(self): + return True if self.lane.speed_limit < self.speed else False diff --git a/pgdrive/scene_creator/vehicle/base_vehicle_node.py b/pgdrive/scene_creator/vehicle/base_vehicle_node.py index 276719a2f..4a2efa336 100644 --- a/pgdrive/scene_creator/vehicle/base_vehicle_node.py +++ b/pgdrive/scene_creator/vehicle/base_vehicle_node.py @@ -14,6 +14,7 @@ def __init__(self, body_name: str, base_vehicle): self.crash_vehicle = False self.crash_object = False self.crash_sidewalk = False + self.crash_building = False # lane line detection self.on_yellow_continuous_line = False @@ -24,6 +25,7 @@ def init_collision_info(self): self.crash_vehicle = False self.crash_object = False self.crash_sidewalk = False + self.crash_building = False self.on_yellow_continuous_line = False self.on_white_continuous_line = False self.on_broken_line = False diff --git a/pgdrive/scene_creator/vehicle/traffic_vehicle.py b/pgdrive/scene_creator/vehicle/traffic_vehicle.py index c2836b170..29b7bd1cf 100644 --- a/pgdrive/scene_creator/vehicle/traffic_vehicle.py +++ b/pgdrive/scene_creator/vehicle/traffic_vehicle.py @@ -35,6 +35,7 @@ class PGTrafficVehicle(DynamicElement): LENGTH = 4 WIDTH = 2 path = None + break_down = False model_collection = {} # save memory, load model once def __init__(self, index: int, kinematic_model: IDMVehicle, enable_respawn: bool = False, np_random=None): @@ -87,6 +88,8 @@ def prepare_step(self, scene_manager) -> None: self.vehicle_node.kinematic_model.act(scene_manager=scene_manager) def step(self, dt): + if self.break_down: + return self.vehicle_node.kinematic_model.step(dt) position = panda_position(self.vehicle_node.kinematic_model.position, 0) self.node_path.setPos(position) @@ -173,6 +176,9 @@ def create_traffic_vehicle_from_config(cls, traffic_mgr: TrafficManager, config: v = IDMVehicle(traffic_mgr, config["position"], config["heading"], np_random=None) return cls(config["index"], v, config["enable_respawn"]) + def set_break_down(self, break_down=True): + self.break_down = break_down + def __del__(self): self.vehicle_node.clearTag(BodyName.Traffic_vehicle) super(PGTrafficVehicle, self).__del__() diff --git a/pgdrive/scene_creator/vehicle_module/lidar.py b/pgdrive/scene_creator/vehicle_module/lidar.py index fcea498d1..f290eadce 100644 --- a/pgdrive/scene_creator/vehicle_module/lidar.py +++ b/pgdrive/scene_creator/vehicle_module/lidar.py @@ -14,7 +14,9 @@ class Lidar(DistanceDetector): def __init__(self, parent_node_np: NodePath, num_lasers: int = 240, distance: float = 50, enable_show=False): super(Lidar, self).__init__(parent_node_np, num_lasers, distance, enable_show) self.node_path.hide(CamMask.RgbCam | CamMask.Shadow | CamMask.Shadow | CamMask.DepthCam) - self.mask = BitMask32.bit(PGTrafficVehicle.COLLISION_MASK) | BitMask32.bit(CollisionGroup.EgoVehicle) + self.mask = BitMask32.bit(PGTrafficVehicle.COLLISION_MASK) | BitMask32.bit( + CollisionGroup.EgoVehicle + ) | BitMask32.bit(CollisionGroup.InvisibleWall) def get_surrounding_vehicles(self) -> Set: vehicles = set() diff --git a/pgdrive/scene_manager/object_manager.py b/pgdrive/scene_manager/object_manager.py index 197d85c89..1c2620ccb 100644 --- a/pgdrive/scene_manager/object_manager.py +++ b/pgdrive/scene_manager/object_manager.py @@ -3,7 +3,8 @@ from pgdrive.scene_creator.blocks.straight import Straight from pgdrive.scene_creator.lane.abs_lane import AbstractLane from pgdrive.scene_creator.map import Map -from pgdrive.scene_creator.object.traffic_object import Object +from pgdrive.scene_creator.object.traffic_object import TrafficObject +from pgdrive.scene_creator.object.base_object import BaseObject from pgdrive.scene_creator.road.road import Road from pgdrive.scene_creator.road.road_network import LaneIndex from pgdrive.utils import RandomEngine @@ -29,6 +30,7 @@ class ObjectManager(RandomEngine): def __init__(self): self._spawned_objects = [] + self._block_objects = [] self.accident_prob = 0. # init random engine @@ -41,11 +43,18 @@ def reset(self, pg_world: PGWorld, map: Map, accident_prob: float = 0): self._clear_objects(pg_world) self.update_random_seed(map.random_seed) self.accident_prob = accident_prob + for block in map.blocks: + block.construct_block_buildings(self) def _clear_objects(self, pg_world: PGWorld): + # only destroy self-generated objects for obj in self._spawned_objects: obj.destroy(pg_world=pg_world) self._spawned_objects = [] + self._block_objects = [] + + def add_block_buildings(self, building: BaseObject): + self._block_objects.append(building) def spawn_one_object( self, @@ -55,7 +64,7 @@ def spawn_one_object( longitude: float, lateral: float, static: bool = False - ) -> Object: + ) -> TrafficObject: """ Spawn an object by assigning its type and position on the lane :param object_type: object name or the class name of the object @@ -66,7 +75,7 @@ def spawn_one_object( :param static: static object can not be moved by any force :return: None """ - for t in Object.type(): + for t in TrafficObject.type(): if t.__name__ == object_type or t.NAME == object_type: obj = t.make_on_lane(lane, lane_index, longitude, lateral) obj.set_static(static) @@ -136,6 +145,7 @@ def break_down_scene( scene_manager.traffic_manager.random_vehicle_type(), lane, longitude, False ) breakdown_vehicle.attach_to_pg_world(pg_world.pbr_worldNP, pg_world.physics_world) + breakdown_vehicle.set_break_down() alert = self.spawn_one_object("Traffic Triangle", lane, lane_index, longitude - self.ALERT_DIST, 0) alert.attach_to_pg_world(pg_world.pbr_worldNP, pg_world.physics_world) @@ -182,7 +192,8 @@ def prohibit_scene( def destroy(self, pg_world: PGWorld): self._clear_objects(pg_world) self._spawned_objects = None + self._block_objects = None @property def objects(self): - return self._spawned_objects + return self._spawned_objects + self._block_objects diff --git a/pgdrive/scene_manager/traffic_manager.py b/pgdrive/scene_manager/traffic_manager.py index 4ee44f284..2ed6fddc8 100644 --- a/pgdrive/scene_manager/traffic_manager.py +++ b/pgdrive/scene_manager/traffic_manager.py @@ -74,8 +74,6 @@ def generate(self, pg_world: PGWorld, map: Map, traffic_density: float): for v in self.vehicles: self.is_target_vehicle_dict[v.name] = True - self._traffic_vehicles = deque() # it is used to step all vehicles on scene - traffic_density = self.density if abs(traffic_density - 0.0) < 1e-2: return @@ -148,14 +146,14 @@ def update_state(self): # create a new one lane = self.np_random.choice(self.respawn_lanes) vehicle_type = self.random_vehicle_type() - random_v = self.spawn_one_vehicle(vehicle_type, lane, self.np_random.rand() * lane.length / 2, True) - self._traffic_vehicles.append(random_v) + self.spawn_one_vehicle(vehicle_type, lane, self.np_random.rand() * lane.length / 2, True) def _clear_traffic(self, pg_world: PGWorld): if self._spawned_vehicles is not None: for v in self._spawned_vehicles: v.destroy(pg_world) self._spawned_vehicles = [] + self._traffic_vehicles = deque() # it is used to step all vehicles on scene def reset(self, pg_world: PGWorld, map: Map, traffic_density: float) -> None: """ @@ -247,6 +245,7 @@ def spawn_one_vehicle(self, vehicle_type, lane: AbstractLane, long: float, enabl len(self._spawned_vehicles), self, lane, long, seed=self.random_seed, enable_respawn=enable_respawn ) self._spawned_vehicles.append(random_v) + self._traffic_vehicles.append(random_v) return random_v def _create_vehicles_on_lane(self, traffic_density: float, lane: AbstractLane, is_respawn_lane): @@ -268,8 +267,7 @@ def _create_vehicles_on_lane(self, traffic_density: float, lane: AbstractLane, i # Do special handling for ramp, and there must be vehicles created there continue vehicle_type = self.random_vehicle_type() - random_v = self.spawn_one_vehicle(vehicle_type, lane, long, is_respawn_lane) - _traffic_vehicles.append(random_v) + self.spawn_one_vehicle(vehicle_type, lane, long, is_respawn_lane) return _traffic_vehicles def _create_respawn_vehicles(self, pg_world: PGWorld, map: Map, traffic_density: float): diff --git a/pgdrive/tests/test_env/test_ma_tollgate.py b/pgdrive/tests/test_env/test_ma_tollgate.py new file mode 100644 index 000000000..8b52aba5e --- /dev/null +++ b/pgdrive/tests/test_env/test_ma_tollgate.py @@ -0,0 +1,717 @@ +import time + +import numpy as np +from gym.spaces import Box, Dict +from pgdrive.envs.marl_envs.marl_tollgate import MultiAgentTollgateEnv +from pgdrive.utils import distance_greater, norm + + +def _check_spaces_before_reset(env): + a = set(env.config["target_vehicle_configs"].keys()) + b = set(env.observation_space.spaces.keys()) + c = set(env.action_space.spaces.keys()) + assert a == b == c + _check_space(env) + + +def _check_spaces_after_reset(env, obs=None): + a = set(env.config["target_vehicle_configs"].keys()) + b = set(env.observation_space.spaces.keys()) + assert a == b + _check_shape(env) + + if obs: + assert isinstance(obs, dict) + assert set(obs.keys()) == a + + +def _check_shape(env): + b = set(env.observation_space.spaces.keys()) + c = set(env.action_space.spaces.keys()) + d = set(env.vehicles.keys()) + e = set(env.scene_manager.target_vehicles.keys()) + f = set([k for k in env.observation_space.spaces.keys() if not env.dones[k]]) + assert d == e == f, (b, c, d, e, f) + assert c.issuperset(d) + _check_space(env) + + +def _check_space(env): + assert isinstance(env.action_space, Dict) + assert isinstance(env.observation_space, Dict) + o_shape = None + for k, s in env.observation_space.spaces.items(): + assert isinstance(s, Box) + if o_shape is None: + o_shape = s.shape + assert s.shape == o_shape + a_shape = None + for k, s in env.action_space.spaces.items(): + assert isinstance(s, Box) + if a_shape is None: + a_shape = s.shape + assert s.shape == a_shape + + +def _act(env, action): + assert env.action_space.contains(action) + obs, reward, done, info = env.step(action) + _check_shape(env) + if not done["__all__"]: + assert len(env.vehicles) > 0 + if not (set(obs.keys()) == set(reward.keys()) == set(env.observation_space.spaces.keys())): + raise ValueError + assert env.observation_space.contains(obs) + assert isinstance(reward, dict) + assert isinstance(info, dict) + assert isinstance(done, dict) + return obs, reward, done, info + + +def test_ma_toll_env(): + for env in [MultiAgentTollgateEnv({"delay_done": 0, "num_agents": 1, "vehicle_config": {"lidar": {"num_others": 8}}} + ), MultiAgentTollgateEnv({"num_agents": 1, "delay_done": 0, + "vehicle_config": {"lidar": {"num_others": 0}}}), + MultiAgentTollgateEnv({"num_agents": 4, "delay_done": 0, + "vehicle_config": {"lidar": {"num_others": 8}}}), + MultiAgentTollgateEnv({"num_agents": 4, "delay_done": 0, + "vehicle_config": {"lidar": {"num_others": 0}}}), + MultiAgentTollgateEnv({"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_toll_horizon(): + # test horizon + for _ in range(3): # This function is really easy to break, repeat multiple times! + env = MultiAgentTollgateEnv( + { + "horizon": 100, + "num_agents": 4, + "vehicle_config": { + "lidar": { + "num_others": 2 + } + }, + "out_of_road_penalty": 777, + "out_of_road_cost": 778, + "crash_done": False + } + ) + try: + _check_spaces_before_reset(env) + obs = env.reset() + _check_spaces_after_reset(env, obs) + assert env.observation_space.contains(obs) + last_keys = set(env.vehicles.keys()) + for step in range(1, 1000): + act = {k: [1, 1] for k in env.vehicles.keys()} + o, r, d, i = _act(env, act) + new_keys = set(env.vehicles.keys()) + if step == 0: + assert not any(d.values()) + if any(d.values()): + assert len(last_keys) <= 4 # num of agents + assert len(new_keys) <= 4 # num of agents + for k in new_keys.difference(last_keys): + assert k in o + assert k in d + print("Step {}, Done: {}".format(step, d)) + + for kkk, rrr in r.items(): + if rrr == -777: + assert d[kkk] + assert i[kkk]["cost"] == 778 + assert i[kkk]["out_of_road"] + + for kkk, iii in i.items(): + if iii and (iii["out_of_road"] or iii["cost"] == 778): + assert d[kkk] + assert i[kkk]["cost"] == 778 + assert i[kkk]["out_of_road"] + assert r[kkk] == -777 + + if d["__all__"]: + break + last_keys = new_keys + finally: + env.close() + + +def test_ma_toll_reset(): + env = MultiAgentTollgateEnv({"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 = MultiAgentTollgateEnv({"horizon": 100, "num_agents": 32, "success_reward": 777}) + 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.routing_localization.final_lane.end + v.set_position(loc) + pos = v.position + np.testing.assert_almost_equal(pos, loc, decimal=3) + new_loc = v.routing_localization.final_lane.end + long, lat = v.routing_localization.final_lane.local_coordinates(v.position) + flag1 = ( + v.routing_localization.final_lane.length - 5 < long < + v.routing_localization.final_lane.length + 5 + ) + flag2 = ( + v.routing_localization.get_current_lane_width() / 2 >= lat >= + (0.5 - v.routing_localization.get_current_lane_num()) * + v.routing_localization.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.routing_localization.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() + + +def test_ma_toll_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) + + MultiAgentTollgateEnv._DEBUG_RANDOM_SEED = 1 + env = MultiAgentTollgateEnv( + { + # "use_render": True, "fast": True, + "horizon": 50, + "num_agents": 12, + "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() + MultiAgentTollgateEnv._DEBUG_RANDOM_SEED = None + + +def test_ma_toll_reward_done_alignment(): + # out of road + env = MultiAgentTollgateEnv({"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__" and not d["__all__"] and not i[kkk]["max_step"]: + if r[kkk] != -777: + raise ValueError + 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 = MultiAgentTollgateEnv( + { + "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 = MultiAgentTollgateEnv( + { + "map_config": { + "exit_length": 110, + "lane_num": 1 + }, + # "use_render": True, + # "fast": True, + "horizon": 200, + "num_agents": 24, + "crash_vehicle_penalty": 1.7777, + } + ) + 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__" and not d["__all__"]: + assert i[kkk]["out_of_road"] or i[kkk]["arrive_dest"] + # 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 = MultiAgentTollgateEnv( + { + "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"].routing_localization.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_toll_reward_sign(): + """ + If agent is simply moving forward without any steering, it will at least gain ~100 rewards, since we have a long + straight road before coming into toll. + However, some bugs cause the vehicles receive negative reward by doing this behavior! + """ + class TestEnv(MultiAgentTollgateEnv): + _respawn_count = 0 + + def _update_agent_pos_configs(self, config): + config = super(TestEnv, self)._update_agent_pos_configs(config) + safe_places = [] + for c, bid in enumerate(self._spawn_manager.safe_spawn_places.keys()): + safe_places.append((bid, self._spawn_manager.safe_spawn_places[bid])) + self._safe_places = safe_places + return config + + env = TestEnv({"num_agents": 1}) + try: + _check_spaces_before_reset(env) + obs = env.reset() + _check_spaces_after_reset(env) + ep_reward = 0.0 + for step in range(1000): + act = {k: [0, 1] for k in env.vehicles.keys()} + o, r, d, i = env.step(act) + ep_reward += next(iter(r.values())) + if any(d.values()): + print("Finish respawn count: {}, reward {}".format(env._respawn_count, ep_reward)) + env._respawn_count += 1 + assert ep_reward > 10, ep_reward + ep_reward = 0 + if env._respawn_count >= len(env._safe_places): + break + if d["__all__"]: + break + finally: + env.close() + + +def test_ma_toll_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 = MultiAgentTollgateEnv(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_toll_no_short_episode(): + env = MultiAgentTollgateEnv({ + "horizon": 300, + "num_agents": 36, + }) + 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_toll_horizon_termination(): + # test horizon + env = MultiAgentTollgateEnv({ + "horizon": 100, + "num_agents": 8, + }) + 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_ma_toll_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 = MultiAgentTollgateEnv({"horizon": 50, "num_agents": 36}) + 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_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 = MultiAgentTollgateEnv({"horizon": 300, "num_agents": 36, "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_randomize_spawn_place(): + last_pos = {} + env = MultiAgentTollgateEnv({"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() + + +if __name__ == '__main__': + # test_ma_toll_env() + # test_ma_toll_horizon() + # test_ma_toll_reset() + # test_ma_toll_reward_done_alignment() + test_ma_toll_close_spawn() + # test_ma_toll_reward_sign() + # test_ma_toll_init_space() + # test_ma_toll_no_short_episode() + # test_ma_toll_horizon_termination() + test_ma_toll_40_agent_reset_after_respawn() + # test_ma_no_reset_error() + # test_randomize_spawn_place() diff --git a/pgdrive/utils/pg_space.py b/pgdrive/utils/pg_space.py index f2b01bcc1..b700cf0cc 100644 --- a/pgdrive/utils/pg_space.py +++ b/pgdrive/utils/pg_space.py @@ -280,6 +280,10 @@ class BlockParameterSpace: BOTTLENECK_PARAMETER = { Parameter.length: PGBoxSpace(min=20, max=50), # the length of straigh part Parameter.lane_num: PGDiscreteSpace(min=1, max=2), # the lane num increased or descreased now 1-2 + "bottle_len": PGConstantSpace(20) + } + TOLLGATE_PARAMETER = { + Parameter.length: PGConstantSpace(20), # the length of straigh part } PARKING_LOT_PARAMETER = { Parameter.one_side_vehicle_num: PGDiscreteSpace(min=2, max=10), diff --git a/pgdrive/world/collision_callback.py b/pgdrive/world/collision_callback.py index 84ffd4494..a9b6a9c3d 100644 --- a/pgdrive/world/collision_callback.py +++ b/pgdrive/world/collision_callback.py @@ -27,4 +27,7 @@ def pg_collision_callback(contact): nodes[i].getPythonTag(BodyName.Base_vehicle).crash_object = True if another_nodes[i].getPythonTag(another_node_name).COST_ONCE: another_nodes[i].getPythonTag(another_node_name).crashed = True + # crash invisible wall or building + elif another_node_name in [BodyName.InvisibleWall, BodyName.TollGate]: + nodes[i].getPythonTag(BodyName.Base_vehicle).crash_building = True logging.debug("{} crash with {}".format(nodes[i].getName(), another_nodes[i].getName()))