def __init__(self, random_seed=None, name=None):
"""
Config is a static conception, which specified the parameters of one element.
There parameters doesn't change, such as length of straight road, max speed of one vehicle, etc.
"""
self.name = random_string() if name is None else name
assert isinstance(
self.PARAMETER_SPACE, PGSpace
) or random_seed is None, "Using PGSpace to define parameter spaces of " + self.class_name
self._config = PGConfig(
{k: None
for k in self.PARAMETER_SPACE.parameters})
self.random_seed = 0 if random_seed is None else random_seed
if self.PARAMETER_SPACE is not None:
self.PARAMETER_SPACE.seed(self.random_seed)
self.render = False if AssetLoader.loader is None else True
# each element has its node_path to render, physics node are child nodes of it
self.node_path = None
# Temporally store bullet nodes that have to place in bullet world (not NodePath)
self.dynamic_nodes = PhysicsNodeList()
# Nodes in this tuple didn't interact with other nodes! they only used to do rayTest or sweepTest
self.static_nodes = PhysicsNodeList()
if self.render:
self.loader = AssetLoader.get_loader()
if not hasattr(self.loader, "loader"):
# It is closed before!
self.loader.__init__()
def test_base_vehicle():
env = PGDriveEnv()
try:
env.reset()
pg_world = env.pg_world
map = env.current_map
# v_config = BaseVehicle.get_vehicle_config(dict())
v_config = PGConfig(BASE_DEFAULT_CONFIG["vehicle_config"]).update(
PGDriveEnvV1_DEFAULT_CONFIG["vehicle_config"])
v_config.update({"use_render": False, "use_image": False})
v = BaseVehicle(pg_world, vehicle_config=v_config)
v.add_lidar()
v.add_routing_localization(True)
v.add_routing_localization(False)
v.routing_localization.set_force_calculate_lane_index(True)
v.update_map_info(map)
for heading in [-1.0, 0.0, 1.0]:
for pos in [[0., 0.], [-100., -100.], [100., 100.]]:
v.reset(map, pos=pos, heading=heading)
np.testing.assert_almost_equal(_get_heading_deg(
v.heading_theta),
heading,
decimal=3)
v_pos = v.position
# v_pos[1] = -v_pos[1], this position is converted to pg_position in reset() now
np.testing.assert_almost_equal(v_pos, pos)
v.set_position(pos)
v_pos = v.position
np.testing.assert_almost_equal(v_pos, pos)
v.update_state(detector_mask=None)
v.reset(map, pos=np.array([10, 0]))
for a_x in [-1, 0, 0.5, 1]:
for a_y in [-1, 0, 0.5, 1]:
v.prepare_step([a_x, a_y])
v.set_act([a_x, a_y])
_assert_vehicle(v)
v.set_incremental_action([a_x, a_y])
_assert_vehicle(v)
state = v.get_state()
v.set_state(state)
assert _get_heading_deg(v.heading_theta) == _get_heading_deg(
state["heading"])
np.testing.assert_almost_equal(v.position, state["position"])
v.projection([a_x, a_y])
_nan_speed(env)
v.destroy()
del v
finally:
env.close()
def test_config_unchangeable():
c = PGConfig({"aaa": 100}, unchangeable=True)
try:
c['aaa'] = 1000
except ValueError as e:
print('Great! ', e)
assert c['aaa'] == 100
def _get_single_vehicle_config(self, extra_config: dict):
"""
Newly introduce method
"""
vehicle_config = merge_dicts(self.config["vehicle_config"],
extra_config,
allow_new_keys=False)
return PGConfig(vehicle_config)
def merge_config(old_dict, new_dict, new_keys_allowed=False):
from pgdrive.utils import PGConfig
if isinstance(old_dict, PGConfig):
old_dict = old_dict.get_dict()
if isinstance(new_dict, PGConfig):
new_dict = new_dict.get_dict()
merged = merge_dicts(old_dict, new_dict, allow_new_keys=new_keys_allowed)
return PGConfig(merged)
def _update_dict_item(self, k, v, allow_overwrite):
if not isinstance(v, (dict, PGConfig)):
if allow_overwrite:
return False
else:
raise TypeError(
"Type error! The item {} has original type {} and updating type {}.".format(
k, type(self[k]), type(v)
)
)
if not isinstance(self[k], PGConfig):
self._set_item(k, PGConfig(self[k]), allow_overwrite)
self[k].update(v, allow_overwrite=allow_overwrite)
return True
def __init__(self, config: dict = None):
self.default_config_copy = PGConfig(self.default_config(),
unchangeable=True)
merged_config = self._process_extra_config(config)
self.config = self._post_process_config(merged_config)
self.num_agents = self.config["num_agents"]
self.is_multi_agent = self.config["is_multi_agent"]
if not self.is_multi_agent:
assert self.num_agents == 1
assert isinstance(self.num_agents, int) and (self.num_agents > 0
or self.num_agents == -1)
# observation and action space
self.agent_manager = AgentManager(
init_observations=self._get_observations(),
never_allow_respawn=not self.config["allow_respawn"],
debug=self.config["debug"],
delay_done=self.config["delay_done"],
infinite_agents=self.num_agents == -1)
self.agent_manager.init_space(
init_observation_space=self._get_observation_space(),
init_action_space=self._get_action_space())
# map setting
self.start_seed = self.config["start_seed"]
self.env_num = self.config["environment_num"]
# lazy initialization, create the main vehicle in the lazy_init() func
self.pg_world: Optional[PGWorld] = None
self.scene_manager: Optional[SceneManager] = None
self.main_camera = None
self.controller = None
self.restored_maps = dict()
self.episode_steps = 0
self.maps = {
_seed: None
for _seed in range(self.start_seed, self.start_seed + self.env_num)
}
self.current_seed = self.start_seed
self.current_map = None
self.dones = None
self.episode_rewards = defaultdict(float)
# In MARL envs with respawn mechanism, varying episode lengths might happen.
self.episode_lengths = defaultdict(int)
self._pending_force_seed = None
def _update_spawn_roads_with_configs(self, spawn_roads=None):
assert self.num_agents <= len(self.target_vehicle_configs), (
"Too many agents! We only accept {} agents, which is specified by the number of configs in "
"target_vehicle_configs, but you have {} agents! "
"You should require less agent or not to specify the target_vehicle_configs!"
.format(len(self.target_vehicle_configs), self.num_agents))
target_vehicle_configs = []
safe_spawn_places = []
for v_id, v_config in self.target_vehicle_configs.items():
lane_tuple = v_config["spawn_lane_index"]
target_vehicle_configs.append(
PGConfig(dict(identifier="|".join(
(str(s) for s in lane_tuple)),
config=v_config,
force_agent_name=v_id),
unchangeable=True))
safe_spawn_places.append(target_vehicle_configs[-1].copy())
return target_vehicle_configs, safe_spawn_places
def _update_spawn_roads_randomly(self, spawn_roads):
assert not self.custom_target_vehicle_config, "This will overwrite your custom target vehicle config"
assert len(spawn_roads) > 0
interval = self.RESPAWN_REGION_LONGITUDE
num_slots = int(floor(self.exit_length / interval))
assert num_slots > 0, "The exist length {} should greater than minimal longitude interval {}.".format(
self.exit_length, interval)
interval = self.exit_length / num_slots
self._longitude_spawn_interval = interval
if self.num_agents is not None:
assert self.num_agents > 0 or self.num_agents == -1
assert self.num_agents <= self.lane_num * len(
spawn_roads
) * num_slots, (
"Too many agents! We only accepet {} agents, but you have {} agents!"
.format(self.lane_num * len(spawn_roads) * num_slots,
self.num_agents))
# We can spawn agents in the middle of road at the initial time, but when some vehicles need to be respawn,
# then we have to set it to the farthest places to ensure safety (otherwise the new vehicles may suddenly
# appear at the middle of the road!)
target_vehicle_configs = []
safe_spawn_places = []
for i, road in enumerate(spawn_roads):
for lane_idx in range(self.lane_num):
for j in range(num_slots):
long = 1 / 2 * self.RESPAWN_REGION_LONGITUDE + j * self.RESPAWN_REGION_LONGITUDE
lane_tuple = road.lane_index(
lane_idx) # like (>>>, 1C0_0_, 1) and so on.
target_vehicle_configs.append(
PGConfig(
dict(identifier="|".join(
(str(s) for s in lane_tuple + (j, ))),
config={
"spawn_lane_index": lane_tuple,
"spawn_longitude": long,
"spawn_lateral": 0
},
force_agent_name=None),
unchangeable=True)) # lock the spawn positions
if j == 0:
safe_spawn_places.append(
target_vehicle_configs[-1].copy())
return target_vehicle_configs, safe_spawn_places
def __init__(self, pg_world: PGWorld, map_config: dict = None):
"""
Map can be stored and recover to save time when we access the map encountered before
"""
self.config = PGConfig(map_config)
self.film_size = (self.config["draw_map_resolution"],
self.config["draw_map_resolution"])
self.random_seed = self.config[self.SEED]
self.road_network = RoadNetwork()
# A flatten representation of blocks, might cause chaos in city-level generation.
self.blocks = []
# Generate map and insert blocks
self._generate(pg_world)
assert self.blocks, "The generate methods does not fill blocks!"
# a trick to optimize performance
self.road_network.after_init()
def update(self, new_dict: Union[dict, "PGConfig"], allow_overwrite=True, stop_recursive_update=None):
"""
Update this dict with extra configs
:param new_dict: extra configs
:param allow_overwrite: whether allowing to add new keys to existing configs or not
:param stop_recursive_update: Deep update and recursive-check will NOT be applied to keys in stop_recursive_update
:return: None
"""
stop_recursive_update = stop_recursive_update or []
new_dict = new_dict or dict()
new_dict = copy.deepcopy(new_dict)
if not allow_overwrite:
old_keys = set(self._config)
new_keys = set(new_dict)
diff = new_keys.difference(old_keys)
if len(diff) > 0:
raise KeyError(
"'{}' does not exist in existing config. "
"Please use config.update(...) to update the config. Existing keys: {}.".format(
diff, self._config.keys()
)
)
for k, v in new_dict.items():
if k not in self:
if isinstance(v, dict):
v = PGConfig(v)
self._config[k] = v # Placeholder
success = False
if isinstance(self._config[k], (dict, PGConfig)):
if k not in stop_recursive_update:
success = self._update_dict_item(k, v, allow_overwrite)
else:
self._set_item(k, v, allow_overwrite)
success = True
if not success:
self._update_single_item(k, v, allow_overwrite)
if k in self._config and not hasattr(self, k):
self.__setattr__(k, self._config[k])
return self
class Element:
"""
Element class are base class of all static objects, whose properties are fixed after calling init().
They have no any desire to change its state, e.g. moving, bouncing, changing color.
Instead, only other Elements or DynamicElements can affect them and change their states.
"""
PARAMETER_SPACE = PGSpace({})
def __init__(self, random_seed=None, name=None):
"""
Config is a static conception, which specified the parameters of one element.
There parameters doesn't change, such as length of straight road, max speed of one vehicle, etc.
"""
self.name = random_string() if name is None else name
assert isinstance(
self.PARAMETER_SPACE, PGSpace
) or random_seed is None, "Using PGSpace to define parameter spaces of " + self.class_name
self._config = PGConfig(
{k: None
for k in self.PARAMETER_SPACE.parameters})
self.random_seed = 0 if random_seed is None else random_seed
if self.PARAMETER_SPACE is not None:
self.PARAMETER_SPACE.seed(self.random_seed)
self.render = False if AssetLoader.loader is None else True
# each element has its node_path to render, physics node are child nodes of it
self.node_path = None
# Temporally store bullet nodes that have to place in bullet world (not NodePath)
self.dynamic_nodes = PhysicsNodeList()
# Nodes in this tuple didn't interact with other nodes! they only used to do rayTest or sweepTest
self.static_nodes = PhysicsNodeList()
if self.render:
self.loader = AssetLoader.get_loader()
if not hasattr(self.loader, "loader"):
# It is closed before!
self.loader.__init__()
@property
def class_name(self):
return self.__class__.__name__
def get_config(self, copy=True):
if copy:
return self._config.copy()
return self._config
def set_config(self, config: dict):
# logging.debug("Read config to " + self.class_name)
self._config.update(config)
def attach_to_pg_world(self, parent_node_path: NodePath,
pg_physics_world: PGPhysicsWorld):
if self.render:
# double check :-)
assert isinstance(
self.node_path,
NodePath), "No render model on node_path in this Element"
self.node_path.reparentTo(parent_node_path)
self.dynamic_nodes.attach_to_physics_world(
pg_physics_world.dynamic_world)
self.static_nodes.attach_to_physics_world(
pg_physics_world.static_world)
def detach_from_pg_world(self, pg_physics_world: PGPhysicsWorld):
"""
It is not fully remove, if this element is useless in the future, call Func delete()
"""
self.node_path.detachNode()
self.dynamic_nodes.detach_from_physics_world(
pg_physics_world.dynamic_world)
self.static_nodes.detach_from_physics_world(
pg_physics_world.static_world)
def destroy(self, pg_world):
"""
Fully delete this element and release the memory
"""
self.detach_from_pg_world(pg_world.physics_world)
self.node_path.removeNode()
self.dynamic_nodes.clear()
self.static_nodes.clear()
self._config.clear()
def __del__(self):
logging.debug("{} is destroyed".format(self.class_name))
@property
def heading_theta(self):
return 0.0 # Not used!
def __init__(
self,
pg_world: PGWorld,
vehicle_config: Union[dict, PGConfig] = None,
physics_config: dict = None,
random_seed: int = 0,
name: str = None,
):
"""
This Vehicle Config is different from self.get_config(), and it is used to define which modules to use, and
module parameters. And self.physics_config defines the physics feature of vehicles, such as length/width
:param pg_world: PGWorld
:param vehicle_config: mostly, vehicle module config
:param physics_config: vehicle height/width/length, find more physics para in VehicleParameterSpace
:param random_seed: int
"""
self.vehicle_config = PGConfig(vehicle_config)
# self.vehicle_config = self.get_vehicle_config(vehicle_config) \
# if vehicle_config is not None else self._default_vehicle_config()
# observation, action
self.action_space = self.get_action_space_before_init(
extra_action_dim=self.vehicle_config["extra_action_dim"])
super(BaseVehicle, self).__init__(random_seed, name=name)
# config info
self.set_config(self.PARAMETER_SPACE.sample())
if physics_config is not None:
self.set_config(physics_config)
self.increment_steering = self.vehicle_config["increment_steering"]
self.enable_reverse = self.vehicle_config["enable_reverse"]
self.max_speed = self.vehicle_config["max_speed"]
self.max_steering = self.vehicle_config["max_steering"]
self.pg_world = pg_world
self.node_path = NodePath("vehicle")
# create
self.spawn_place = (0, 0)
self._add_chassis(pg_world.physics_world)
self.wheels = self._create_wheel()
# modules
self.image_sensors = {}
self.lidar: Optional[Lidar] = None
self.side_detector: Optional[SideDetector] = None
self.lane_line_detector: Optional[LaneLineDetector] = None
self.routing_localization: Optional[RoutingLocalizationModule] = None
self.lane: Optional[AbstractLane] = None
self.lane_index = None
self.vehicle_panel = VehiclePanel(self.pg_world) if (
self.pg_world.mode == RENDER_MODE_ONSCREEN) else None
# state info
self.throttle_brake = 0.0
self.steering = 0
self.last_current_action = deque([(0.0, 0.0), (0.0, 0.0)], maxlen=2)
self.last_position = self.spawn_place
self.last_heading_dir = self.heading
self.dist_to_left_side = None
self.dist_to_right_side = None
# collision info render
self.collision_info_np = self._init_collision_info_render(pg_world)
self.collision_banners = {} # to save time
self.current_banner = None
self.attach_to_pg_world(self.pg_world.pbr_render,
self.pg_world.physics_world)
# step info
self.out_of_route = None
self.on_lane = None
# self.step_info = None
self._init_step_info()
# others
self._add_modules_for_vehicle(self.vehicle_config["use_render"])
self.takeover = False
self._expert_takeover = False
self.energy_consumption = 0
# overtake_stat
self.front_vehicles = set()
self.back_vehicles = set()
def default_config(cls) -> "PGConfig":
return PGConfig(BASE_DEFAULT_CONFIG)
def copy(self, unchangeable=None):
"""If unchangeable is None, then just following the original config's setting."""
if unchangeable is None:
unchangeable = self._unchangeable
return PGConfig(self, unchangeable)