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__))