def _randomize_position_in_slot(self, target_vehicle_config):
vehicle_config = copy.deepcopy(target_vehicle_config)
long = self.RESPAWN_REGION_LONGITUDE - self.vehicle_length
lat = self.RESPAWN_REGION_LATERAL - self.vehicle_width
vehicle_config["spawn_longitude"] += get_np_random(self._seed).uniform(
-long / 2, long / 2)
vehicle_config["spawn_lateral"] += get_np_random(self._seed).uniform(
-lat / 2, lat / 2)
return vehicle_config
def update(self,
map: Map,
current_lane_index,
final_road_node=None,
random_seed=False):
start_road_node = current_lane_index[0]
self.map = map
if start_road_node is None:
start_road_node = FirstBlock.NODE_1
if final_road_node is None:
current_road_negative = Road(
*current_lane_index[:-1]).is_negative_road()
random_seed = random_seed if random_seed is not False else map.random_seed
# choose first block when born on negative road
block = map.blocks[0] if current_road_negative else map.blocks[-1]
sockets = block.get_socket_list()
while True:
socket = get_np_random(random_seed).choice(sockets)
if not socket.is_socket_node(start_road_node):
break
else:
sockets.remove(socket)
if len(sockets) == 0:
raise ValueError("Can not set a destination!")
# choose negative road end node when current road is negative road
final_road_node = socket.negative_road.end_node if current_road_negative else socket.positive_road.end_node
self.set_route(start_road_node, final_road_node)
def _init_agent_pos_configs(self, config):
target_vehicle_configs = []
assert config["num_agents"] <= config["map_config"]["lane_num"] * len(self.target_nodes), (
"Too many agents! We only accepet {} agents, but you have {} agents!".format(
config["map_config"]["lane_num"] * len(self.target_nodes), config["num_agents"]
)
)
for i in range(-1, len(self.target_nodes) - 1):
for lane_idx in range(config["map_config"]["lane_num"]):
target_vehicle_configs.append(
(
"agent_{}_{}".format(i + 1, lane_idx),
(self.target_nodes[i], self.target_nodes[i + 1], lane_idx),
)
)
target_agents = get_np_random().choice(
[i for i in range(len(self.target_nodes) * (config["map_config"]["lane_num"]))],
config["num_agents"],
replace=False
)
ret = {}
for real_idx, idx in enumerate(target_agents):
agent_name, v_config = target_vehicle_configs[idx]
# for rllib compatibility
ret["agent{}".format(real_idx)] = dict(spawn_lane_index=v_config)
config["target_vehicle_configs"] = ret
return config
def _update_destination_for(self, vehicle_id):
vehicle = self.vehicles[vehicle_id]
# when agent re-joined to the game, call this to set the new route to destination
end_road = -get_np_random(self._DEBUG_RANDOM_SEED).choice(
self.spawn_roads) # Use negative road!
vehicle.routing_localization.set_route(vehicle.lane_index[0],
end_road.end_node)
def get_target_vehicle_configs(self, seed=None):
# don't overwrite
if self.custom_target_vehicle_config:
ret = {}
for bp in self.target_vehicle_configs:
v_config = bp["config"]
ret[bp["force_agent_name"]] = v_config
return copy.deepcopy(ret)
num_agents = self.num_agents if self.num_agents is not None else len(
self.target_vehicle_configs)
assert len(self.target_vehicle_configs) > 0
if num_agents == -1: # Infinite number of agents
target_agents = list(range(len(self.target_vehicle_configs)))
else:
target_agents = get_np_random(seed).choice(
[i for i in range(len(self.target_vehicle_configs))],
num_agents,
replace=False)
# for rllib compatibility
ret = {}
if len(target_agents) > 1:
for real_idx, idx in enumerate(target_agents):
v_config = self.target_vehicle_configs[idx]["config"]
v_config = self._randomize_position_in_slot(v_config)
ret["agent{}".format(real_idx)] = v_config
else:
ret["agent0"] = self._randomize_position_in_slot(
self.target_vehicle_configs[0]["config"])
return copy.deepcopy(ret)
def update(self,
map: BaseMap,
current_lane_index,
final_road_node=None,
random_seed=None):
# TODO(pzh): We should not determine the destination of a vehicle in the navigation module.
start_road_node = current_lane_index[0]
self.map = map
if start_road_node is None:
start_road_node = FirstPGBlock.NODE_1
if final_road_node is None:
# auto find if PGMap
current_road_negative = Road(
*current_lane_index[:-1]).is_negative_road()
# choose first block when born on negative road
block = map.blocks[0] if current_road_negative else map.blocks[-1]
sockets = block.get_socket_list()
socket = get_np_random(random_seed).choice(sockets)
while True:
if not socket.is_socket_node(start_road_node) or len(
sockets) == 1:
break
else:
sockets.remove(socket)
if len(sockets) == 0:
raise ValueError("Can not set a destination!")
# choose negative road end node when current road is negative road
final_road_node = socket.negative_road.end_node if current_road_negative else socket.positive_road.end_node
self.set_route(current_lane_index, final_road_node)
def __init__(
self,
traffic_mgr: TrafficManager,
position: List,
heading: float = 0,
speed: float = 0,
np_random: np.random.RandomState = None,
name: str = None
):
deprecation_warning("Vehicle", "Policy Class", error=False)
self.name = random_string() if name is None else name
self.traffic_mgr = traffic_mgr
self._position = np.array(position).astype('float')
self.heading = heading
self.speed = speed
# self.lane_index, _ = self.traffic_mgr.current_map.road_network.get_closest_lane_index(
# self.position
# ) if self.traffic_mgr else (np.nan, np.nan)
# self.lane = self.traffic_mgr.current_map.road_network.get_lane(self.lane_index) if self.traffic_mgr else None
self.action = {'steering': 0, 'acceleration': 0}
self.crashed = False
# self.log = []
# self.history = deque(maxlen=30)
self.np_random = np_random if np_random else get_np_random()
def create_random(cls,
traffic_mgr: TrafficManager,
lane: AbstractLane,
longitude: float,
speed: float = None,
random_seed=None):
"""
Create a random vehicle on the road.
The lane and /or speed are chosen randomly, while longitudinal position is chosen behind the last
vehicle in the road with density based on the number of lanes.
:param longitude: the longitude on lane
:param lane: the lane where the vehicle is born
:param traffic_mgr: the traffic_mgr where the vehicle is driving
:param speed: initial speed in [m/s]. If None, will be chosen randomly
:return: A vehicle with random position and/or speed
"""
if speed is None:
speed = traffic_mgr.np_random.uniform(Vehicle.DEFAULT_SPEEDS[0],
Vehicle.DEFAULT_SPEEDS[1])
v = cls(traffic_mgr,
list(lane.position(longitude, 0)),
lane.heading_at(longitude),
speed,
np_random=get_np_random(random_seed))
return v
def _respawn_single_vehicle(self, randomize_position=False):
"""
Arbitrary insert a new vehicle to a new spawn place if possible.
"""
safe_places_dict = self._spawn_manager.get_available_respawn_places(
self.pg_world, self.current_map, randomize=randomize_position)
if len(safe_places_dict) == 0 or not self.agent_manager.allow_respawn:
# No more run, just wait!
return None, None
assert len(safe_places_dict) > 0
bp_index = get_np_random(self._DEBUG_RANDOM_SEED).choice(
list(safe_places_dict.keys()), 1)[0]
new_spawn_place = safe_places_dict[bp_index]
if new_spawn_place[self._spawn_manager.FORCE_AGENT_NAME] is not None:
if new_spawn_place[
self._spawn_manager.
FORCE_AGENT_NAME] != self.agent_manager.next_agent_id():
return None, None
new_agent_id, vehicle = self.agent_manager.propose_new_vehicle()
new_spawn_place_config = new_spawn_place["config"]
vehicle.vehicle_config.update(new_spawn_place_config)
vehicle.reset(self.current_map)
self._update_destination_for(new_agent_id)
vehicle.update_state(detector_mask=None)
self.dones[
new_agent_id] = False # Put it in the internal dead-tracking dict.
new_obs = self.observations[new_agent_id].observe(vehicle)
return new_agent_id, new_obs
def __init__(
self,
lane_num: int,
lane_width: float,
global_network: RoadNetwork,
render_node_path: NodePath,
physics_world: PhysicsWorld,
# block_type_version: str,
exit_length=50,
random_seed=None):
super(BIG, self).__init__()
self._block_sequence = None
self.random_seed = random_seed
self.np_random = get_np_random(random_seed)
# Don't change this right now, since we need to make maps identical to old one
self._lane_num = lane_num
self._lane_width = lane_width
self.block_num = None
self._render_node_path = render_node_path
self._physics_world = physics_world
self._global_network = global_network
self.blocks = []
self._exit_length = exit_length
first_block = FirstPGBlock(self._global_network,
self._lane_width,
self._lane_num,
self._render_node_path,
self._physics_world,
length=self._exit_length)
self.blocks.append(first_block)
self.next_step = NextStep.forward
def __init__(self, config):
super(ChangeDensityEnv, self).__init__(config)
self.density_dict = dict()
self._random_state = get_np_random(0) # Use a fixed seed
for seed in self.maps.keys():
self.density_dict[seed] = self._random_state.uniform(
self.config["density_min"], self.config["density_max"])
def update_random_seed(self, random_seed: int):
"""
Update the random seed and random engine of traffic
:param random_seed: int, random seed
:return: None
"""
self.random_seed = random_seed if not self.random_traffic else None
self.np_random = get_np_random(self.random_seed)
def _reset_global_seed(self, force_seed):
# create map
if force_seed is not None:
current_seed = force_seed
else:
current_seed = get_np_random(self._DEBUG_RANDOM_SEED
).randint(self.start_seed, self.start_seed + self.env_num)
self.seed(current_seed)
def __init__(self, config=None):
super(ChangeFrictionEnv, self).__init__(config)
self.parameter_list = dict()
self._random_state = get_np_random(0) # Use a fixed seed.
for k in self.maps:
self.parameter_list[k] = dict(
wheel_friction=self._random_state.uniform(
self.config["friction_min"], self.config["friction_max"]))
def _update_destination_for(self, vehicle_id):
vehicle = self.vehicles[vehicle_id]
# when agent re-joined to the game, call this to set the new route to destination
end_roads = copy.deepcopy(self.in_spawn_roads)
if vehicle.routing_localization.current_road in end_roads:
end_road = self.current_map.parking_space_manager.get_parking_space(vehicle_id)
else:
end_road = -get_np_random(self._DEBUG_RANDOM_SEED).choice(end_roads) # Use negative road!
vehicle.routing_localization.set_route(vehicle.lane_index[0], end_road.end_node)
def __init__(self,
index: int,
kinematic_model: IDMVehicle,
enable_respawn: bool = False,
np_random=None):
"""
A traffic vehicle class.
:param index: Each Traffic vehicle has an unique index, and the name of this vehicle will contain this index
:param kinematic_model: IDM Model or other models
:param enable_respawn: It will be generated at the spawn place again when arriving at the destination
:param np_random: Random Engine
"""
kinematic_model.LENGTH = self.LENGTH
kinematic_model.WIDTH = self.WIDTH
super(PGTrafficVehicle, self).__init__()
self.vehicle_node = TrafficVehicleNode(
BodyName.Traffic_vehicle, IDMVehicle.create_from(kinematic_model))
chassis_shape = BulletBoxShape(
Vec3(self.LENGTH / 2, self.WIDTH / 2, self.HEIGHT / 2))
self.index = index
self.vehicle_node.addShape(
chassis_shape,
TransformState.makePos(Point3(0, 0, self.HEIGHT / 2)))
self.vehicle_node.setMass(800.0)
self.vehicle_node.setIntoCollideMask(BitMask32.bit(
self.COLLISION_MASK))
self.vehicle_node.setKinematic(False)
self.vehicle_node.setStatic(True)
self.enable_respawn = enable_respawn
self._initial_state = kinematic_model if enable_respawn else None
self.dynamic_nodes.append(self.vehicle_node)
self.node_path = NodePath(self.vehicle_node)
self.out_of_road = False
np_random = np_random or get_np_random()
[path, scale, x_y_z_offset,
H] = self.path[np_random.randint(0, len(self.path))]
if self.render:
if path not in PGTrafficVehicle.model_collection:
carNP = self.loader.loadModel(
AssetLoader.file_path("models", path))
PGTrafficVehicle.model_collection[path] = carNP
else:
carNP = PGTrafficVehicle.model_collection[path]
carNP.setScale(scale)
carNP.setH(H)
carNP.setPos(x_y_z_offset)
carNP.instanceTo(self.node_path)
self.step(1e-1)
def _update_map(self, episode_data: dict = None, force_seed=None):
if episode_data is not None:
# Since in episode data map data only contains one map, values()[0] is the map_parameters
map_data = episode_data["map_data"].values()
assert len(map_data) > 0, "Can not find map info in episode data"
for map in map_data:
blocks_info = map
map_config = self.config["map_config"].copy()
map_config[Map.GENERATE_TYPE] = MapGenerateMethod.PG_MAP_FILE
map_config[Map.GENERATE_CONFIG] = blocks_info
self.current_map = PGMap(self.pg_world, map_config)
return
if self.config["load_map_from_json"] and self.current_map is None:
assert self.config["_load_map_from_json"]
self.load_all_maps_from_json(self.config["_load_map_from_json"])
# remove map from world before adding
if self.current_map is not None:
self.current_map.unload_from_pg_world(self.pg_world)
# create map
if force_seed is not None:
self.current_seed = force_seed
elif self._pending_force_seed is not None:
self.current_seed = self._pending_force_seed
self._pending_force_seed = None
else:
self.current_seed = get_np_random(self._DEBUG_RANDOM_SEED).randint(
self.start_seed, self.start_seed + self.env_num)
if self.maps.get(self.current_seed, None) is None:
if self.config["load_map_from_json"]:
map_config = self.restored_maps.get(self.current_seed, None)
assert map_config is not None
else:
map_config = self.config["map_config"]
map_config.update({"seed": self.current_seed})
new_map = PGMap(self.pg_world, map_config)
self.maps[self.current_seed] = new_map
self.current_map = self.maps[self.current_seed]
else:
self.current_map = self.maps[self.current_seed]
assert isinstance(self.current_map,
Map), "Map should be an instance of Map() class"
self.current_map.load_to_pg_world(self.pg_world)
def _respawn_single_vehicle(self, randomize_position=False):
"""
Exclude destination parking space
"""
safe_places_dict = self.engine.spawn_manager.get_available_respawn_places(
self.current_map, randomize=randomize_position)
# ===== filter spawn places =====
filter_ret = {}
for id, config in safe_places_dict.items():
spawn_l_index = config["config"]["spawn_lane_index"]
spawn_road = Road(spawn_l_index[0], spawn_l_index[1])
if spawn_road in self.config["in_spawn_roads"]:
if len(self.engine.spawn_manager.parking_space_available) > 0:
filter_ret[id] = config
else:
# spawn in parking space
if ParkingLot.is_in_direction_parking_space(spawn_road):
# avoid sweep test bug
spawn_road = self.current_map.parking_lot.out_direction_parking_space(
spawn_road)
config["config"]["spawn_lane_index"] = (
spawn_road.start_node, spawn_road.end_node, 0)
if spawn_road in self.engine.spawn_manager.parking_space_available:
# not other vehicle's destination
filter_ret[id] = config
# ===== same as super() =====
safe_places_dict = filter_ret
if len(safe_places_dict) == 0 or not self.agent_manager.allow_respawn:
# No more run, just wait!
return None, None
assert len(safe_places_dict) > 0
bp_index = get_np_random(self._DEBUG_RANDOM_SEED).choice(
list(safe_places_dict.keys()), 1)[0]
new_spawn_place = safe_places_dict[bp_index]
new_agent_id, vehicle = self.agent_manager.propose_new_vehicle()
new_spawn_place_config = new_spawn_place["config"]
new_spawn_place_config = self.engine.spawn_manager.update_destination_for(
new_agent_id, new_spawn_place_config)
vehicle.config.update(new_spawn_place_config)
vehicle.reset()
vehicle.after_step()
self.dones[
new_agent_id] = False # Put it in the internal dead-tracking dict.
new_obs = self.observations[new_agent_id].observe(vehicle)
return new_agent_id, new_obs
def chase_another_v(self) -> (str, BaseVehicle):
if self.main_camera is None:
return
self.main_camera.reset()
vehicles = list(self.agent_manager.active_agents.values())
if not self.main_camera.is_bird_view_camera(self.pg_world):
if self.current_track_vehicle in vehicles:
vehicles.remove(self.current_track_vehicle)
if len(vehicles) == 0:
return
self.current_track_vehicle.remove_display_region()
new_v = get_np_random().choice(vehicles)
self.current_track_vehicle = new_v
self.current_track_vehicle.add_to_display()
self.main_camera.track(self.current_track_vehicle, self.pg_world)
return
def _respawn_single_vehicle(self, randomize_position=False):
"""
Exclude destination parking space
"""
safe_places_dict = self._spawn_manager.get_available_respawn_places(
self.pg_world, self.current_map, randomize=randomize_position
)
# ===== filter spawn places =====
filter_ret = {}
for id, config in safe_places_dict.items():
spawn_l_index = config["config"]["spawn_lane_index"]
spawn_road = Road(spawn_l_index[0], spawn_l_index[1])
if spawn_road in self.in_spawn_roads and len(self.current_map.parking_space_manager.parking_space_available
) > 0:
filter_ret[id] = config
continue
spawn_road = self.current_map.parking_lot.in_direction_parking_space(spawn_road)
if spawn_road in self.current_map.parking_space_manager.parking_space_available:
# not other vehicle's destination
filter_ret[id] = config
# ===== same as super() =====
safe_places_dict = filter_ret
if len(safe_places_dict) == 0 or not self.agent_manager.allow_respawn:
# No more run, just wait!
return None, None
assert len(safe_places_dict) > 0
bp_index = get_np_random(self._DEBUG_RANDOM_SEED).choice(list(safe_places_dict.keys()), 1)[0]
new_spawn_place = safe_places_dict[bp_index]
if new_spawn_place[self._spawn_manager.FORCE_AGENT_NAME] is not None:
if new_spawn_place[self._spawn_manager.FORCE_AGENT_NAME] != self.agent_manager.next_agent_id():
return None, None
new_agent_id, vehicle = self.agent_manager.propose_new_vehicle()
new_spawn_place_config = new_spawn_place["config"]
vehicle.vehicle_config.update(new_spawn_place_config)
vehicle.reset(self.current_map)
self._update_destination_for(new_agent_id)
vehicle.update_state(detector_mask=None)
self.dones[new_agent_id] = False # Put it in the internal dead-tracking dict.
new_obs = self.observations[new_agent_id].observe(vehicle)
return new_agent_id, new_obs
def chase_camera(self) -> (str, BaseVehicle):
if self.main_camera is None:
return
self.main_camera.reset()
if self.config["prefer_track_agent"] is not None and self.config["prefer_track_agent"] in self.vehicles.keys():
new_v = self.vehicles[self.config["prefer_track_agent"]]
current_track_vehicle = new_v
else:
if self.main_camera.is_bird_view_camera():
current_track_vehicle = self.current_track_vehicle
else:
vehicles = list(self.agent_manager.active_agents.values())
if len(vehicles) <= 1:
return
if self.current_track_vehicle in vehicles:
vehicles.remove(self.current_track_vehicle)
new_v = get_np_random().choice(vehicles)
current_track_vehicle = new_v
self.main_camera.track(current_track_vehicle)
return
def __init__(
self,
traffic_mgr: TrafficManager,
position: List,
heading: float = 0,
speed: float = 0,
np_random: np.random.RandomState = None,
):
self.traffic_mgr = traffic_mgr
self._position = np.array(position).astype('float')
self.heading = heading
self.speed = speed
self.lane_index, _ = self.traffic_mgr.map.road_network.get_closest_lane_index(
self.position) if self.traffic_mgr else (np.nan, np.nan)
self.lane = self.traffic_mgr.map.road_network.get_lane(
self.lane_index) if self.traffic_mgr else None
self.action = {'steering': 0, 'acceleration': 0}
self.crashed = False
self.log = []
self.history = deque(maxlen=30)
self.np_random = np_random if np_random else get_np_random()
def _respawn_single_vehicle(self, randomize_position=False):
"""
Arbitrary insert a new vehicle to a new spawn place if possible.
"""
safe_places_dict = self.engine.spawn_manager.get_available_respawn_places(
self.current_map, randomize=randomize_position
)
if len(safe_places_dict) == 0:
return None, None
born_place_index = get_np_random(self._DEBUG_RANDOM_SEED).choice(list(safe_places_dict.keys()), 1)[0]
new_spawn_place = safe_places_dict[born_place_index]
new_agent_id, vehicle = self.agent_manager.propose_new_vehicle()
new_spawn_place_config = new_spawn_place["config"]
new_spawn_place_config = self.engine.spawn_manager.update_destination_for(new_agent_id, new_spawn_place_config)
vehicle.config.update(new_spawn_place_config)
vehicle.reset()
vehicle.after_step()
self.dones[new_agent_id] = False # Put it in the internal dead-tracking dict.
new_obs = self.observations[new_agent_id].observe(vehicle)
return new_agent_id, new_obs
def __init__(
self,
vehicle_config: Union[dict, Config] = None,
name: str = None,
random_seed=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 vehicle_config: mostly, vehicle module config
:param random_seed: int
"""
# check
assert vehicle_config is not None, "Please specify the vehicle config."
assert engine_initialized(
), "Please make sure game engine is successfully initialized!"
# NOTE: it is the game engine, not vehicle drivetrain
self.engine = get_engine()
BaseObject.__init__(self, name, random_seed,
self.engine.global_config["vehicle_config"])
BaseVehicleState.__init__(self)
self.update_config(vehicle_config)
am_i_the_special_one = self.config["am_i_the_special_one"]
# build vehicle physics model
vehicle_chassis = self._create_vehicle_chassis()
self.add_body(vehicle_chassis.getChassis())
self.system = vehicle_chassis
self.chassis = self.origin
self.wheels = self._create_wheel()
# powertrain config
self.increment_steering = self.config["increment_steering"]
self.enable_reverse = self.config["enable_reverse"]
self.max_speed = self.config["max_speed"]
self.max_steering = self.config["max_steering"]
# visualization
color = sns.color_palette("colorblind")
idx = get_np_random().randint(len(color))
rand_c = color[idx]
if am_i_the_special_one:
rand_c = color[2] # A pretty green
self.top_down_color = (rand_c[0] * 255, rand_c[1] * 255,
rand_c[2] * 255)
self.panda_color = rand_c
self._add_visualization()
# modules, get observation by using these modules
self.lane: Optional[AbstractLane] = None
self.lane_index = None
self.navigation: Optional[Navigation] = None
self.lidar: Optional[Lidar] = None # detect surrounding vehicles
self.side_detector: Optional[SideDetector] = None # detect road side
self.lane_line_detector: Optional[
LaneLineDetector] = None # detect nearest lane lines
self.image_sensors = {}
# 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 = (0, 0)
self.last_heading_dir = self.heading
self.dist_to_left_side = None
self.dist_to_right_side = None
# step info
self.out_of_route = None
self.on_lane = None
self.spawn_place = (0, 0)
self._init_step_info()
# others
self._add_modules_for_vehicle()
self.takeover = False
self.expert_takeover = False
self.energy_consumption = 0
self.action_space = self.get_action_space_before_init(
extra_action_dim=self.config["extra_action_dim"])
self.break_down = False
# overtake_stat
self.front_vehicles = set()
self.back_vehicles = set()
if self.engine.current_map is not None:
self.reset()
def __init__(self,
engine,
show_navi_mark: bool = False,
random_navi_mark_color=False,
show_dest_mark=False,
show_line_to_dest=False):
"""
This class define a helper for localizing vehicles and retrieving navigation information.
It now only support from first block start to the end node, but can be extended easily.
"""
self.map = None
self.final_road = None
self.final_lane = None
self.checkpoints = None
self.current_ref_lanes = None
self.next_ref_lanes = None
self.current_road = None
self.next_road = None
self._target_checkpoints_index = None
self._navi_info = np.zeros(
(self.navigation_info_dim, )) # navi information res
# Vis
self._show_navi_info = (
engine.mode == RENDER_MODE_ONSCREEN
and not engine.global_config["debug_physics_world"])
self.origin = engine.render.attachNewNode(
"navigation_sign") if self._show_navi_info else None
self.navi_mark_color = (
0.6, 0.8,
0.5) if not random_navi_mark_color else get_np_random().rand(3)
self.navi_arrow_dir = None
self._dest_node_path = None
self._goal_node_path = None
self._line_to_dest = None
self._show_line_to_dest = show_line_to_dest
if self._show_navi_info:
# nodepath
self._line_to_dest = self.origin.attachNewNode("line")
self._goal_node_path = self.origin.attachNewNode("target")
self._dest_node_path = self.origin.attachNewNode("dest")
if show_navi_mark:
navi_point_model = AssetLoader.loader.loadModel(
AssetLoader.file_path("models", "box.bam"))
navi_point_model.reparentTo(self._goal_node_path)
if show_dest_mark:
dest_point_model = AssetLoader.loader.loadModel(
AssetLoader.file_path("models", "box.bam"))
dest_point_model.reparentTo(self._dest_node_path)
if show_line_to_dest:
line_seg = LineSegs("line_to_dest")
line_seg.setColor(self.navi_mark_color[0],
self.navi_mark_color[1],
self.navi_mark_color[2], 0.7)
line_seg.setThickness(2)
self._dynamic_line_np = NodePath(line_seg.create(True))
self._dynamic_line_np.reparentTo(self.origin)
self._line_to_dest = line_seg
self._goal_node_path.setTransparency(TransparencyAttrib.M_alpha)
self._dest_node_path.setTransparency(TransparencyAttrib.M_alpha)
self._goal_node_path.setColor(self.navi_mark_color[0],
self.navi_mark_color[1],
self.navi_mark_color[2], 0.7)
self._dest_node_path.setColor(self.navi_mark_color[0],
self.navi_mark_color[1],
self.navi_mark_color[2], 0.7)
self._goal_node_path.hide(CamMask.AllOn)
self._dest_node_path.hide(CamMask.AllOn)
self._goal_node_path.show(CamMask.MainCam)
self._dest_node_path.show(CamMask.MainCam)
logging.debug("Load Vehicle Module: {}".format(
self.__class__.__name__))
def get_parking_space(self, v_id):
parking_space_idx = get_np_random().choice([i for i in range(len(self.parking_space_available))])
parking_space = list(self.parking_space_available)[parking_space_idx]
self.parking_space_available.remove(parking_space)
self.v_dest_pair[v_id] = parking_space
return parking_space
def seed(self, seed=None):
"""Seed the PRNG of this space. """
self.np_random, seed = get_np_random(seed, return_seed=True)
return [seed]
def __init__(self,
pg_world,
show_navi_mark: bool = False,
random_navi_mark_color=False,
show_dest_mark=False,
show_line_to_dest=False):
"""
This class define a helper for localizing vehicles and retrieving navigation information.
It now only support from first block start to the end node, but can be extended easily.
"""
self.map = None
self.final_road = None
self.checkpoints = None
self.final_lane = None
self.current_ref_lanes = None
self.current_road = None
self._target_checkpoints_index = None
self._navi_info = np.zeros(
(self.navigation_info_dim, )) # navi information res
self.navi_mark_color = (
0.6, 0.8,
0.5) if not random_navi_mark_color else get_np_random().rand(3)
# Vis
self._is_showing = True # store the state of navigation mark
self._show_navi_info = (
pg_world.mode == RENDER_MODE_ONSCREEN
and not pg_world.world_config["debug_physics_world"])
self._dest_node_path = None
self._goal_node_path = None
self._arrow_node_path = None
self._line_to_dest = None
self._show_line_to_dest = show_line_to_dest
if self._show_navi_info:
# nodepath
self._line_to_dest = pg_world.render.attachNewNode("line")
self._goal_node_path = pg_world.render.attachNewNode("target")
self._dest_node_path = pg_world.render.attachNewNode("dest")
self._arrow_node_path = pg_world.aspect2d.attachNewNode("arrow")
navi_arrow_model = AssetLoader.loader.loadModel(
AssetLoader.file_path("models", "navi_arrow.gltf"))
navi_arrow_model.setScale(0.1, 0.12, 0.2)
navi_arrow_model.setPos(2, 1.15, -0.221)
self._left_arrow = self._arrow_node_path.attachNewNode(
"left arrow")
self._left_arrow.setP(180)
self._right_arrow = self._arrow_node_path.attachNewNode(
"right arrow")
self._left_arrow.setColor(self.MARK_COLOR)
self._right_arrow.setColor(self.MARK_COLOR)
navi_arrow_model.instanceTo(self._left_arrow)
navi_arrow_model.instanceTo(self._right_arrow)
self._arrow_node_path.setPos(0, 0, 0.08)
self._arrow_node_path.hide(BitMask32.allOn())
self._arrow_node_path.show(CamMask.MainCam)
self._arrow_node_path.setQuat(
LQuaternionf(np.cos(-np.pi / 4), 0, 0, np.sin(-np.pi / 4)))
# the transparency attribute of gltf model is invalid on windows
# self._arrow_node_path.setTransparency(TransparencyAttrib.M_alpha)
if show_navi_mark:
navi_point_model = AssetLoader.loader.loadModel(
AssetLoader.file_path("models", "box.bam"))
navi_point_model.reparentTo(self._goal_node_path)
if show_dest_mark:
dest_point_model = AssetLoader.loader.loadModel(
AssetLoader.file_path("models", "box.bam"))
dest_point_model.reparentTo(self._dest_node_path)
if show_line_to_dest:
line_seg = LineSegs("line_to_dest")
line_seg.setColor(self.navi_mark_color[0],
self.navi_mark_color[1],
self.navi_mark_color[2], 0.7)
line_seg.setThickness(2)
self._dynamic_line_np = NodePath(line_seg.create(True))
self._dynamic_line_np.reparentTo(pg_world.render)
self._line_to_dest = line_seg
self._goal_node_path.setTransparency(TransparencyAttrib.M_alpha)
self._dest_node_path.setTransparency(TransparencyAttrib.M_alpha)
self._goal_node_path.setColor(self.navi_mark_color[0],
self.navi_mark_color[1],
self.navi_mark_color[2], 0.7)
self._dest_node_path.setColor(self.navi_mark_color[0],
self.navi_mark_color[1],
self.navi_mark_color[2], 0.7)
self._goal_node_path.hide(BitMask32.allOn())
self._dest_node_path.hide(BitMask32.allOn())
self._goal_node_path.show(CamMask.MainCam)
self._dest_node_path.show(CamMask.MainCam)
logging.debug("Load Vehicle Module: {}".format(
self.__class__.__name__))
