def _change_vis(self, t_type): # not good here, next_part_socket = self.get_socket_list()[(t_type + 1) % 4] # next_part_socket = self._sockets[(t_type + 1) % 4] # FIXME pzh: Help! @LQY What is in this part? next_positive = next_part_socket.positive_road next_negative = next_part_socket.negative_road last_part_socket = self.get_socket_list()[(t_type + 3) % 4] # last_part_socket = self._sockets[(t_type + 3) % 4] # FIXME pzh: Help! @LQY last_positive = last_part_socket.positive_road last_negative = last_part_socket.negative_road if t_type == Goal.LEFT: next_positive = next_part_socket.negative_road next_negative = next_part_socket.positive_road if t_type == Goal.RIGHT: last_positive = last_part_socket.negative_road last_negative = last_part_socket.positive_road for i, road in enumerate([ Road(last_negative.end_node, next_positive.start_node), Road(next_negative.end_node, last_positive.start_node) ]): lanes = road.get_lanes(self.block_network) outside_type = LineType.SIDE if i == 0 else LineType.NONE for k, lane in enumerate(lanes): line_types = [ LineType.BROKEN, LineType.BROKEN ] if k != len(lanes) - 1 else [LineType.BROKEN, outside_type] lane.line_types = line_types if k == 0: lane.line_color = [LineColor.YELLOW, LineColor.GREY] if i == 1: lane.line_types[0] = LineType.NONE
def __init__(self, global_network: RoadNetwork, lane_width: float, lane_num: int, render_root_np: NodePath, pg_physics_world: PGPhysicsWorld, random_seed): place_holder = BlockSocket(Road(Decoration.start, Decoration.end), Road(Decoration.start, Decoration.end)) super(FirstBlock, self).__init__(0, place_holder, global_network, random_seed) basic_lane = StraightLane([0, lane_width * (lane_num - 1)], [10, lane_width * (lane_num - 1)], line_types=(LineType.STRIPED, LineType.SIDE), width=lane_width) ego_v_born_road = Road(self.NODE_1, self.NODE_2) CreateRoadFrom(basic_lane, lane_num, ego_v_born_road, self.block_network, self._global_network) CreateAdverseRoad(ego_v_born_road, self.block_network, self._global_network) next_lane = ExtendStraightLane(basic_lane, 40, [LineType.STRIPED, LineType.SIDE]) other_v_born_road = Road(self.NODE_2, self.NODE_3) CreateRoadFrom(next_lane, lane_num, other_v_born_road, self.block_network, self._global_network) CreateAdverseRoad(other_v_born_road, self.block_network, self._global_network) self._create_in_world() global_network += self.block_network socket = self.create_socket_from_positive_road(other_v_born_road) socket.index = 0 self.add_sockets(socket) self.attach_to_pg_world(render_root_np, pg_physics_world) self._reborn_roads = [other_v_born_road]
def _try_plug_into_previous_block(self) -> bool: parameters = self.get_config() basic_lane = self.positive_basic_lane lane_num = self.positive_lane_num # part 1 start_node = self._pre_block_socket.positive_road.end_node end_node = self.add_road_node() positive_road = Road(start_node, end_node) length = parameters[Parameter.length] direction = parameters[Parameter.dir] angle = parameters[Parameter.angle] radius = parameters[Parameter.radius] curve, straight = sharpbend(basic_lane, length, radius, np.deg2rad(angle), direction, basic_lane.width, (LineType.STRIPED, LineType.SIDE)) no_cross = CreateRoadFrom(curve, lane_num, positive_road, self.block_network, self._global_network) no_cross = CreateAdverseRoad(positive_road, self.block_network, self._global_network) and no_cross # part 2 start_node = end_node end_node = self.add_road_node() positive_road = Road(start_node, end_node) no_cross = CreateRoadFrom(straight, lane_num, positive_road, self.block_network, self._global_network) and no_cross no_cross = CreateAdverseRoad(positive_road, self.block_network, self._global_network) and no_cross # common properties self.add_sockets(self.create_socket_from_positive_road(positive_road)) return no_cross
def set_route(self, start_road_node: str, end_road_node: str): """ Find a shorest path from start road to end road :param start_road_node: start road node :param end_road_node: end road node :return: None """ self.checkpoints = self.map.road_network.shortest_path( start_road_node, end_road_node) assert len( self.checkpoints) > 2, "Can not find a route from {} to {}".format( start_road_node, end_road_node) # update routing info self.final_road = Road(self.checkpoints[-2], end_road_node) final_lanes = self.final_road.get_lanes(self.map.road_network) self.final_lane = final_lanes[-1] self._target_checkpoints_index = [0, 1] self._navi_info.fill(0.0) target_road_1_start = self.checkpoints[0] target_road_1_end = self.checkpoints[1] self.current_ref_lanes = self.map.road_network.graph[ target_road_1_start][target_road_1_end] self.current_road = Road(target_road_1_start, target_road_1_end) if self._dest_node_path is not None: ref_lane = final_lanes[0] later_middle = (float(self.get_current_lane_num()) / 2 - 0.5) * self.get_current_lane_width() check_point = ref_lane.position(ref_lane.length, later_middle) self._dest_node_path.setPos(check_point[0], -check_point[1], 1.8)
def get_negative_lanes(self): """ In order to remain the lane index, ret is a 2-dim array structure like like [Road_lanes[lane_1, lane_2]] """ ret = [] for _from, _to_dict in self.graph: for _to, lanes in _to_dict: road = Road(_from, _to) if road.is_negative_road() and road.is_valid_road(): ret.append(lanes) return ret
def update_navigation_localization(self, ego_vehicle): position = ego_vehicle.position lane, lane_index = self.get_current_lane(ego_vehicle) if lane is None: lane, lane_index = ego_vehicle.lane, ego_vehicle.lane_index ego_vehicle.on_lane = False if self.FORCE_CALCULATE: lane_index, _ = self.map.road_network.get_closest_lane_index( position) lane = self.map.road_network.get_lane(lane_index) long, _ = lane.local_coordinates(position) self._update_target_checkpoints(lane_index, long) assert len(self.checkpoints) > 2 target_road_1_start = self.checkpoints[ self._target_checkpoints_index[0]] target_road_1_end = self.checkpoints[self._target_checkpoints_index[0] + 1] target_road_2_start = self.checkpoints[ self._target_checkpoints_index[1]] target_road_2_end = self.checkpoints[self._target_checkpoints_index[1] + 1] target_lanes_1 = self.map.road_network.graph[target_road_1_start][ target_road_1_end] target_lanes_2 = self.map.road_network.graph[target_road_2_start][ target_road_2_end] self.current_ref_lanes = target_lanes_1 self.current_road = Road(target_road_1_start, target_road_1_end) self._navi_info.fill(0.0) half = self.navigation_info_dim // 2 self._navi_info[: half], lanes_heading1, checkpoint = self._get_info_for_checkpoint( lanes_id=0, lanes=target_lanes_1, ego_vehicle=ego_vehicle) self._navi_info[ half:], lanes_heading2, _ = self._get_info_for_checkpoint( lanes_id=1, lanes=target_lanes_2, ego_vehicle=ego_vehicle) if self._show_navi_info: pos_of_goal = checkpoint self._goal_node_path.setPos(pos_of_goal[0], -pos_of_goal[1], 1.8) self._goal_node_path.setH(self._goal_node_path.getH() + 3) self._update_navi_arrow([lanes_heading1, lanes_heading2]) dest_pos = self._dest_node_path.getPos() self._draw_line_to_dest(pg_world=ego_vehicle.pg_world, start_position=ego_vehicle.position, end_position=(dest_pos[0], -dest_pos[1])) return lane, lane_index
def _create_left_turn(self, radius, lane_num, attach_left_lane, attach_road, intersect_nodes, part_idx): left_turn_radius = radius + lane_num * attach_left_lane.width_at(0) diff = self.lane_num_intersect - self.positive_lane_num # increase lane num if ((part_idx == 1 or part_idx == 3) and diff > 0) or ( (part_idx == 0 or part_idx == 2) and diff < 0): diff = abs(diff) left_bend, extra_part = create_bend_straight( attach_left_lane, self.lane_width * diff, left_turn_radius, np.deg2rad(self.ANGLE), False, attach_left_lane.width_at(0), (LineType.NONE, LineType.NONE)) left_road_start = intersect_nodes[2] pre_left_road_start = left_road_start + self.EXTRA_PART CreateRoadFrom(left_bend, min(self.positive_lane_num, self.lane_num_intersect), Road(attach_road.end_node, pre_left_road_start), self.block_network, self._global_network, toward_smaller_lane_index=False, center_line_type=LineType.NONE, side_lane_line_type=LineType.NONE, inner_lane_line_type=LineType.NONE) CreateRoadFrom(extra_part, min(self.positive_lane_num, self.lane_num_intersect), Road(pre_left_road_start, left_road_start), self.block_network, self._global_network, toward_smaller_lane_index=False, center_line_type=LineType.NONE, side_lane_line_type=LineType.NONE, inner_lane_line_type=LineType.NONE) else: left_bend, _ = create_bend_straight(attach_left_lane, self.EXIT_PART_LENGTH, left_turn_radius, np.deg2rad(self.ANGLE), False, attach_left_lane.width_at(0), (LineType.NONE, LineType.NONE)) left_road_start = intersect_nodes[2] CreateRoadFrom(left_bend, min(self.positive_lane_num, self.lane_num_intersect), Road(attach_road.end_node, left_road_start), self.block_network, self._global_network, toward_smaller_lane_index=False, center_line_type=LineType.NONE, side_lane_line_type=LineType.NONE, inner_lane_line_type=LineType.NONE)
def generate(self, scene_manager, pg_world: PGWorld): """ Generate an accident scene or construction scene on block :param scene_manager: SceneManager used to access other managers :param pg_world: PGWorld class :return: None """ accident_prob = self.accident_prob if abs(accident_prob - 0.0) < 1e-2: return for block in scene_manager.map.blocks: if type(block) not in [Straight, Curve, InRampOnStraight, OutRampOnStraight]: # blocks with exists do not generate accident scene continue if self.np_random.rand() > accident_prob: # prob filter continue road_1 = Road(block.pre_block_socket.positive_road.end_node, block.road_node(0, 0)) road_2 = Road(block.road_node(0, 0), block.road_node(0, 1)) if not isinstance(block, Straight) else None accident_road = self.np_random.choice([road_1, road_2]) if not isinstance(block, Curve) else road_2 accident_road = road_1 if accident_road is None else accident_road is_ramp = isinstance(block, InRampOnStraight) or isinstance(block, OutRampOnStraight) on_left = True if self.np_random.rand() > 0.5 or (accident_road is road_2 and is_ramp) else False accident_lane_idx = 0 if on_left else -1 _debug = pg_world.world_config["_debug_crash_object"] if _debug: on_left = True lane = accident_road.get_lanes(scene_manager.map.road_network)[accident_lane_idx] longitude = lane.length - self.ACCIDENT_AREA_LEN if lane.length < self.ACCIDENT_LANE_MIN_LEN: continue # generate scene block.PROHIBIT_TRAFFIC_GENERATION = True # TODO(pzh) This might not worked in MARL envs when the route is also has changeable lanes. lateral_len = scene_manager.map.config[scene_manager.map.LANE_WIDTH] lane = scene_manager.map.road_network.get_lane(accident_road.lane_index(accident_lane_idx)) if self.np_random.rand() > 0.5 or _debug: self.prohibit_scene( scene_manager, pg_world, lane, accident_road.lane_index(accident_lane_idx), longitude, lateral_len, on_left ) else: accident_lane_idx = self.np_random.randint(scene_manager.map.config[scene_manager.map.LANE_NUM]) self.break_down_scene( scene_manager, pg_world, lane, accident_road.lane_index(accident_lane_idx), longitude )
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 create_socket_from_positive_road(road: Road) -> BlockSocket: """ We usually create road from positive road, thus this func can get socket easily. Note: it is not recommended to generate socket from negative road """ assert road.start_node[0] != Road.NEGATIVE_DIR and road.end_node[0] != Road.NEGATIVE_DIR, \ "Socket can only be created from positive road" positive_road = Road(road.start_node, road.end_node) return BlockSocket(positive_road, -positive_road)
def __init__(self, global_network: RoadNetwork, lane_width: float, lane_num: int, render_root_np: NodePath, pg_physics_world: PGPhysicsWorld, random_seed, length: float = 50): place_holder = BlockSocket(Road(Decoration.start, Decoration.end), Road(Decoration.start, Decoration.end)) super(FirstBlock, self).__init__(0, place_holder, global_network, random_seed) assert length > self.ENTRANCE_LENGTH basic_lane = StraightLane( [0, lane_width * (lane_num - 1)], [self.ENTRANCE_LENGTH, lane_width * (lane_num - 1)], line_types=(LineType.BROKEN, LineType.SIDE), width=lane_width) ego_v_spawn_road = Road(self.NODE_1, self.NODE_2) CreateRoadFrom(basic_lane, lane_num, ego_v_spawn_road, self.block_network, self._global_network) CreateAdverseRoad(ego_v_spawn_road, self.block_network, self._global_network) next_lane = ExtendStraightLane(basic_lane, length - self.ENTRANCE_LENGTH, [LineType.BROKEN, LineType.SIDE]) other_v_spawn_road = Road(self.NODE_2, self.NODE_3) 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._create_in_world() # global_network += self.block_network global_network.add(self.block_network) socket = self.create_socket_from_positive_road(other_v_spawn_road) socket.set_index(self._block_name, 0) self.add_sockets(socket) self.attach_to_pg_world(render_root_np, pg_physics_world) self._respawn_roads = [other_v_spawn_road]
def out_direction_parking_space(road: Road): """ Give a parking space in in-direction, return out-direction road """ start_node = copy.deepcopy(road.start_node) end_node = copy.deepcopy(road.end_node) assert start_node[-2] == "1" and end_node[-2] == "2", "It is not in-direction of this parking space" start_node = start_node[:-2] + "5" + Block.DASH end_node = end_node[:-2] + "6" + Block.DASH return Road(start_node, end_node)
def _create_part(self, attach_lanes, attach_road: Road, radius: float, intersect_nodes: deque, part_idx) -> (StraightLane, bool): lane_num = self.lane_num_intersect if part_idx == 0 or part_idx == 2 else self.positive_lane_num non_cross = True attach_left_lane = attach_lanes[0] # first left part assert isinstance( attach_left_lane, StraightLane ), "Can't create a intersection following a circular lane" self._create_left_turn(radius, lane_num, attach_left_lane, attach_road, intersect_nodes, part_idx) # u-turn if self.enable_u_turn: adverse_road = -attach_road self._create_u_turn(attach_road, part_idx) # go forward part lanes_on_road = copy.deepcopy(attach_lanes) straight_lane_len = 2 * radius + (2 * lane_num - 1) * lanes_on_road[0].width_at(0) for l in lanes_on_road: next_lane = ExtendStraightLane(l, straight_lane_len, (LineType.NONE, LineType.NONE)) self.block_network.add_lane(attach_road.end_node, intersect_nodes[1], next_lane) # right part length = self.EXIT_PART_LENGTH right_turn_lane = lanes_on_road[-1] assert isinstance( right_turn_lane, StraightLane ), "Can't create a intersection following a circular lane" right_bend, right_straight = create_bend_straight( right_turn_lane, length, radius, np.deg2rad(self.ANGLE), True, right_turn_lane.width_at(0), (LineType.NONE, LineType.SIDE)) non_cross = (not check_lane_on_road(self._global_network, right_bend, 1)) and non_cross CreateRoadFrom(right_bend, min(self.positive_lane_num, self.lane_num_intersect), Road(attach_road.end_node, intersect_nodes[0]), self.block_network, self._global_network, toward_smaller_lane_index=True, side_lane_line_type=LineType.SIDE, inner_lane_line_type=LineType.NONE, center_line_type=LineType.NONE) intersect_nodes.rotate(-1) right_straight.line_types = [LineType.BROKEN, LineType.SIDE] return right_straight, non_cross
def remove_all_roads(self, start_node: str, end_node: str): """ Remove all road between two road nodes :param start_node: start node name :param end_node: end node name :return: roads removed """ ret = [] paths = self.bfs_paths(start_node, end_node) for path in paths: for next_idx, node in enumerate(path[:-1], 1): road_removed = self.remove_road(Road(node, path[next_idx])) ret += road_removed return ret
class MultiAgentIntersectionEnv(MultiAgentPGDrive): spawn_roads = [ Road(FirstBlock.NODE_2, FirstBlock.NODE_3), -Road(InterSection.node(1, 0, 0), InterSection.node(1, 0, 1)), -Road(InterSection.node(1, 1, 0), InterSection.node(1, 1, 1)), -Road(InterSection.node(1, 2, 0), InterSection.node(1, 2, 1)), ] @staticmethod def default_config() -> PGConfig: return MultiAgentPGDrive.default_config().update(MAIntersectionConfig, 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 = MAIntersectionMap(self.pg_world, map_config) self.maps[self.current_seed] = new_map self.current_map = self.maps[self.current_seed] 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.spawn_roads) 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 get_single_observation( self, vehicle_config: "PGConfig") -> "ObservationType": return LidarStateObservationMARound(vehicle_config)
def prepare_step(self, scene_mgr, pg_world: PGWorld): """ All traffic vehicles make driving decision here :param scene_mgr: access other elements in scene :param pg_world: World :return: None """ if self.mode != TrafficMode.Reborn: ego_lane_idx = self.ego_vehicle.lane_index[:-1] ego_road = Road(ego_lane_idx[0], ego_lane_idx[1]) if len(self.block_triggered_vehicles) > 0 and ego_road == self.block_triggered_vehicles[-1].trigger_road: block_vehicles = self.block_triggered_vehicles.pop() self.traffic_vehicles += block_vehicles.vehicles for v in self.traffic_vehicles: v.prepare_step(scene_mgr=scene_mgr)
def get_roads(self, *, direction="all", lane_num=None) -> List: """ Return all roads in road_network :param direction: "positive"/"negative" :param lane_num: only roads with lane_num lanes will be returned :return: List[Road] """ assert direction in ["positive", "negative", "all"], "incorrect road direction" ret = [] for _from, _to_dict in self.graph.items(): if direction == "all" or (direction == "positive" and _from[0] != "-") or (direction == "negative" and _from[0] == "-"): for _to, lanes in _to_dict.items(): if lane_num is None or len(lanes) == lane_num: ret.append(Road(_from, _to)) return ret
def prepare_step(self): """ All traffic vehicles make driving decision here :return: None """ # trigger vehicles scene_manager = self._scene_mgr if self.mode != TrafficMode.Respawn: for v in scene_manager.agent_manager.active_objects.values(): ego_lane_idx = v.lane_index[:-1] ego_road = Road(ego_lane_idx[0], ego_lane_idx[1]) if len(self.block_triggered_vehicles) > 0 and \ ego_road == self.block_triggered_vehicles[-1].trigger_road: block_vehicles = self.block_triggered_vehicles.pop() self._traffic_vehicles += block_vehicles.vehicles for v in self._traffic_vehicles: v.prepare_step(scene_manager=scene_manager)
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] basic_lane = self.positive_basic_lane assert isinstance(basic_lane, StraightLane), "Straight road can only connect straight type" new_lane = ExtendStraightLane(basic_lane, length, [LineType.STRIPED, 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) # create negative road no_cross = CreateAdverseRoad(socket, self.block_network, self._global_network) and no_cross self.add_sockets(BlockSocket(socket, _socket)) return no_cross
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 _try_plug_into_previous_block(self) -> bool: para = self.get_config() decrease_increase = -1 if para[Parameter.decrease_increase] == 0 else 1 if self.positive_lane_num <= 1: decrease_increase = 1 elif self.positive_lane_num >= 4: decrease_increase = -1 self.lane_num_intersect = self.positive_lane_num + decrease_increase * para[ Parameter.change_lane_num] no_cross = True attach_road = self.pre_block_socket.positive_road _attach_road = self.pre_block_socket.negative_road attach_lanes = attach_road.get_lanes(self._global_network) # right straight left node name, rotate it to fit different part intersect_nodes = deque([ self.road_node(0, 0), self.road_node(1, 0), self.road_node(2, 0), _attach_road.start_node ]) for i in range(4): right_lane, success = self._create_part(attach_lanes, attach_road, para[Parameter.radius], intersect_nodes, i) no_cross = no_cross and success if i != 3: lane_num = self.positive_lane_num if i == 1 else self.lane_num_intersect exit_road = Road(self.road_node(i, 0), self.road_node(i, 1)) no_cross = CreateRoadFrom(right_lane, lane_num, exit_road, self.block_network, self._global_network) and no_cross no_cross = CreateAdverseRoad(exit_road, self.block_network, self._global_network) and no_cross socket = BlockSocket(exit_road, -exit_road) self.add_respawn_roads(socket.negative_road) self.add_sockets(socket) attach_road = -exit_road attach_lanes = attach_road.get_lanes(self.block_network) return no_cross
def _create_u_turn(self, attach_road, part_idx): # set to CONTINUOUS to debug line_type = LineType.NONE lanes = attach_road.get_lanes( self.block_network) if part_idx != 0 else self.positive_lanes attach_left_lane = lanes[0] lane_num = len(lanes) left_turn_radius = self.lane_width / 2 left_bend, _ = create_bend_straight(attach_left_lane, 0.1, left_turn_radius, np.deg2rad(180), False, attach_left_lane.width_at(0), (LineType.NONE, LineType.NONE)) left_road_start = (-attach_road).start_node CreateRoadFrom(left_bend, lane_num, Road(attach_road.end_node, left_road_start), self.block_network, self._global_network, toward_smaller_lane_index=False, center_line_type=line_type, side_lane_line_type=line_type, inner_lane_line_type=line_type)
def _try_plug_into_previous_block(self) -> bool: acc_lane_len = self.get_config()[Parameter.length] no_cross = True fork_lane_num = self.get_config()[Parameter.lane_num] + 1 # extend road and acc raod part, part 0 self.set_part_idx(0) sin_angle = np.sin(np.deg2rad(self.ANGLE)) cos_angle = np.cos(np.deg2rad(self.ANGLE)) longitude_len = sin_angle * self.RADIUS * 2 + cos_angle * self.CONNECT_PART_LEN + self.RAMP_LEN extend_lane = ExtendStraightLane( self.positive_basic_lane, longitude_len + self.EXTRA_PART, [LineType.STRIPED, LineType.CONTINUOUS]) extend_road = Road(self._pre_block_socket.positive_road.end_node, self.add_road_node()) no_cross = CreateRoadFrom(extend_lane, self.positive_lane_num, extend_road, self.block_network, self._global_network) and no_cross no_cross = CreateAdverseRoad(extend_road, self.block_network, self._global_network) and no_cross acc_side_lane = ExtendStraightLane( extend_lane, acc_lane_len + self.lane_width, [LineType.STRIPED, LineType.CONTINUOUS]) acc_road = Road(extend_road.end_node, self.add_road_node()) no_cross = CreateRoadFrom(acc_side_lane, self.positive_lane_num, acc_road, self.block_network, self._global_network) and no_cross no_cross = CreateAdverseRoad(acc_road, self.block_network, self._global_network) and no_cross acc_road.get_lanes(self.block_network)[-1].line_types = [ LineType.STRIPED, LineType.STRIPED ] self.add_sockets(Block.create_socket_from_positive_road(acc_road)) # ramp part, part 1 self.set_part_idx(1) lateral_dist = (1 - cos_angle ) * self.RADIUS * 2 + sin_angle * self.CONNECT_PART_LEN end_point = extend_lane.position(self.EXTRA_PART + self.RAMP_LEN, lateral_dist + self.lane_width) start_point = extend_lane.position(self.EXTRA_PART, lateral_dist + self.lane_width) straight_part = StraightLane(start_point, end_point, self.lane_width, self.LANE_TYPE, speed_limit=self.SPEED_LIMIT) straight_road = Road(self.add_road_node(), self.add_road_node()) self.block_network.add_lane(straight_road.start_node, straight_road.end_node, straight_part) no_cross = (not check_lane_on_road(self._global_network, straight_part, 0.95)) and no_cross self.add_reborn_roads(straight_road) # p1 road 0, 1 bend_1, connect_part = sharpbend(straight_part, self.CONNECT_PART_LEN, self.RADIUS, np.deg2rad(self.ANGLE), False, self.lane_width, self.LANE_TYPE, speed_limit=self.SPEED_LIMIT) bend_1_road = Road(straight_road.end_node, self.add_road_node()) connect_road = Road(bend_1_road.end_node, self.add_road_node()) self.block_network.add_lane(bend_1_road.start_node, bend_1_road.end_node, bend_1) self.block_network.add_lane(connect_road.start_node, connect_road.end_node, connect_part) no_cross = CreateRoadFrom(bend_1, fork_lane_num, bend_1_road, self.block_network, self._global_network, False) and no_cross no_cross = CreateRoadFrom(connect_part, fork_lane_num, connect_road, self.block_network, self._global_network, False) and no_cross # p1, road 2, 3 bend_2, acc_lane = sharpbend(connect_part, acc_lane_len, self.RADIUS, np.deg2rad(self.ANGLE), True, self.lane_width, self.LANE_TYPE, speed_limit=self.SPEED_LIMIT) acc_lane.line_types = [LineType.STRIPED, LineType.CONTINUOUS] bend_2_road = Road(connect_road.end_node, self.road_node( 0, 0)) # end at part1 road 0, extend road acc_road = Road(self.road_node(0, 0), self.road_node(0, 1)) self.block_network.add_lane(bend_2_road.start_node, bend_2_road.end_node, bend_2) self.block_network.add_lane(acc_road.start_node, acc_road.end_node, acc_lane) no_cross = CreateRoadFrom(bend_2, fork_lane_num, bend_2_road, self.block_network, self._global_network, False) and no_cross no_cross = CreateRoadFrom(acc_lane, fork_lane_num, acc_road, self.block_network, self._global_network, False) and no_cross return no_cross
def _try_plug_into_previous_block(self) -> bool: no_cross = True sin_angle = np.sin(np.deg2rad(self.ANGLE)) cos_angle = np.cos(np.deg2rad(self.ANGLE)) longitude_len = sin_angle * self.RADIUS * 2 + cos_angle * self.CONNECT_PART_LEN + self.RAMP_LEN self.set_part_idx(0) # part 0 road 0 dec_lane_len = self.PARAMETER_SPACE.sample()[Parameter.length] dec_lane = ExtendStraightLane(self.positive_basic_lane, dec_lane_len + self.lane_width, [LineType.STRIPED, LineType.CONTINUOUS]) dec_road = Road(self._pre_block_socket.positive_road.end_node, self.add_road_node()) no_cross = CreateRoadFrom(dec_lane, self.positive_lane_num, dec_road, self.block_network, self._global_network) and no_cross no_cross = CreateAdverseRoad(dec_road, self.block_network, self._global_network) and no_cross dec_right_lane = dec_road.get_lanes(self.block_network)[-1] dec_right_lane.line_types = [LineType.STRIPED, LineType.STRIPED] # part 0 road 1 extend_lane = ExtendStraightLane( dec_right_lane, longitude_len, [LineType.STRIPED, LineType.CONTINUOUS]) extend_road = Road(dec_road.end_node, self.add_road_node()) no_cross = CreateRoadFrom(extend_lane, self.positive_lane_num, extend_road, self.block_network, self._global_network) and no_cross no_cross = CreateAdverseRoad(extend_road, self.block_network, self._global_network) and no_cross self.add_sockets(self.create_socket_from_positive_road(extend_road)) # part 1 road 0 self.set_part_idx(1) dec_side_right_lane = self._get_deacc_lane(dec_right_lane) self.block_network.add_lane(dec_road.start_node, self.add_road_node(), dec_side_right_lane) no_cross = (not check_lane_on_road( self._global_network, dec_side_right_lane, 0.95)) and no_cross bend_1, connect_part = sharpbend(dec_side_right_lane, self.CONNECT_PART_LEN, self.RADIUS, np.deg2rad(self.ANGLE), True, self.lane_width, self.LANE_TYPE, speed_limit=self.SPEED_LIMIT) bend_1_road = Road(self.road_node(1, 0), self.add_road_node()) connect_road = Road(bend_1_road.end_node, self.add_road_node()) self.block_network.add_lane(bend_1_road.start_node, bend_1_road.end_node, bend_1) self.block_network.add_lane(connect_road.start_node, connect_road.end_node, connect_part) no_cross = (not check_lane_on_road(self._global_network, bend_1, 0.95)) and no_cross no_cross = (not check_lane_on_road(self._global_network, connect_part, 0.95)) and no_cross bend_2, straight_part = sharpbend(connect_part, self.RAMP_LEN, self.RADIUS, np.deg2rad(self.ANGLE), False, self.lane_width, self.LANE_TYPE, speed_limit=self.SPEED_LIMIT) bend_2_road = Road( connect_road.end_node, self.add_road_node()) # end at part1 road 0, extend road straight_road = Road(bend_2_road.end_node, self.add_road_node()) self.block_network.add_lane(bend_2_road.start_node, bend_2_road.end_node, bend_2) self.block_network.add_lane(straight_road.start_node, straight_road.end_node, straight_part) no_cross = (not check_lane_on_road(self._global_network, bend_2, 0.95)) and no_cross no_cross = (not check_lane_on_road(self._global_network, straight_part, 0.95)) and no_cross decoration_part = self._get_merge_part(dec_side_right_lane) self.block_network.add_lane(Decoration.start, Decoration.end, decoration_part) return no_cross
def _try_plug_into_previous_block(self) -> bool: no_cross = True parameters = self.get_config() lane_num_changed = parameters[Parameter.lane_num] start_ndoe = self.pre_block_socket.positive_road.end_node straight_lane_num = int(self.positive_lane_num - lane_num_changed) straight_lane_num = max(1, straight_lane_num) circular_lane_num = self.positive_lane_num - straight_lane_num # part 1, straight road 0 basic_lane = self.positive_lanes[straight_lane_num - 1] ref_lane = ExtendStraightLane(basic_lane, self.BOTTLENECK_LEN, [LineType.NONE, LineType.NONE]) straight_road = Road(start_ndoe, self.road_node(0, 0)) no_cross = CreateRoadFrom( ref_lane, straight_lane_num, straight_road, self.block_network, self._global_network, center_line_type=LineType.CONTINUOUS, side_lane_line_type=LineType.NONE, inner_lane_line_type=LineType.NONE) and no_cross no_cross = CreateAdverseRoad( straight_road, self.block_network, self._global_network, inner_lane_line_type=LineType.NONE, side_lane_line_type=LineType.NONE, center_line_type=LineType.CONTINUOUS) and no_cross # extend for socket ,part 1 road 1 ref_lane = ExtendStraightLane(ref_lane, parameters[Parameter.length], [LineType.NONE, LineType.NONE]) socket_road = Road(self.road_node(0, 0), self.road_node(0, 1)) no_cross = CreateRoadFrom( ref_lane, straight_lane_num, socket_road, self.block_network, self._global_network, center_line_type=LineType.CONTINUOUS, side_lane_line_type=LineType.SIDE, inner_lane_line_type=LineType.BROKEN) and no_cross no_cross = CreateAdverseRoad( socket_road, self.block_network, self._global_network, inner_lane_line_type=LineType.BROKEN, side_lane_line_type=LineType.SIDE, center_line_type=LineType.CONTINUOUS) and no_cross negative_sockect_road = -socket_road self.add_sockets(BlockSocket(socket_road, negative_sockect_road)) # part 2, circular part for index, lane in enumerate(self.positive_lanes[straight_lane_num:], 1): lateral_dist = index * self.lane_width / 2 inner_node = self.road_node(1, index) side_line_type = LineType.SIDE if index == self.positive_lane_num - straight_lane_num else LineType.NONE # positive part circular_1, circular_2, _ = create_wave_lanes( lane, lateral_dist, self.BOTTLENECK_LEN, 5, self.lane_width) road_1 = Road(start_ndoe, inner_node) no_cross = CreateRoadFrom( circular_1, 1, road_1, self.block_network, self._global_network, center_line_type=LineType.NONE, side_lane_line_type=side_line_type, inner_lane_line_type=LineType.NONE) and no_cross road_2 = Road(inner_node, self.road_node(0, 0)) no_cross = CreateRoadFrom( circular_2, 1, road_2, self.block_network, self._global_network, center_line_type=LineType.NONE, side_lane_line_type=side_line_type, inner_lane_line_type=LineType.NONE) and no_cross # adverse part lane = negative_sockect_road.get_lanes(self.block_network)[-1] circular_2, circular_1, _ = create_wave_lanes( lane, lateral_dist, self.BOTTLENECK_LEN, 5, self.lane_width, False) road_2 = -road_2 no_cross = CreateRoadFrom( circular_2, 1, road_2, self.block_network, self._global_network, center_line_type=LineType.NONE, side_lane_line_type=side_line_type, inner_lane_line_type=LineType.NONE) and no_cross road_1 = -road_1 no_cross = CreateRoadFrom( circular_1, 1, road_1, self.block_network, self._global_network, center_line_type=LineType.NONE, side_lane_line_type=side_line_type, inner_lane_line_type=LineType.NONE) and no_cross return no_cross
class RoutingLocalizationModule: navigation_info_dim = 10 NAVI_POINT_DIST = 50 PRE_NOTIFY_DIST = 40 MARK_COLOR = COLLISION_INFO_COLOR["green"][1] MIN_ALPHA = 0.15 CKPT_UPDATE_RANGE = 5 FORCE_CALCULATE = False LINE_TO_DEST_HEIGHT = 0.6 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__)) 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 set_route(self, start_road_node: str, end_road_node: str): """ Find a shorest path from start road to end road :param start_road_node: start road node :param end_road_node: end road node :return: None """ self.checkpoints = self.map.road_network.shortest_path( start_road_node, end_road_node) assert len( self.checkpoints) > 2, "Can not find a route from {} to {}".format( start_road_node, end_road_node) # update routing info self.final_road = Road(self.checkpoints[-2], end_road_node) final_lanes = self.final_road.get_lanes(self.map.road_network) self.final_lane = final_lanes[-1] self._target_checkpoints_index = [0, 1] self._navi_info.fill(0.0) target_road_1_start = self.checkpoints[0] target_road_1_end = self.checkpoints[1] self.current_ref_lanes = self.map.road_network.graph[ target_road_1_start][target_road_1_end] self.current_road = Road(target_road_1_start, target_road_1_end) if self._dest_node_path is not None: ref_lane = final_lanes[0] later_middle = (float(self.get_current_lane_num()) / 2 - 0.5) * self.get_current_lane_width() check_point = ref_lane.position(ref_lane.length, later_middle) self._dest_node_path.setPos(check_point[0], -check_point[1], 1.8) def update_navigation_localization(self, ego_vehicle): position = ego_vehicle.position lane, lane_index = self.get_current_lane(ego_vehicle) if lane is None: lane, lane_index = ego_vehicle.lane, ego_vehicle.lane_index ego_vehicle.on_lane = False if self.FORCE_CALCULATE: lane_index, _ = self.map.road_network.get_closest_lane_index( position) lane = self.map.road_network.get_lane(lane_index) long, _ = lane.local_coordinates(position) self._update_target_checkpoints(lane_index, long) assert len(self.checkpoints) > 2 target_road_1_start = self.checkpoints[ self._target_checkpoints_index[0]] target_road_1_end = self.checkpoints[self._target_checkpoints_index[0] + 1] target_road_2_start = self.checkpoints[ self._target_checkpoints_index[1]] target_road_2_end = self.checkpoints[self._target_checkpoints_index[1] + 1] target_lanes_1 = self.map.road_network.graph[target_road_1_start][ target_road_1_end] target_lanes_2 = self.map.road_network.graph[target_road_2_start][ target_road_2_end] self.current_ref_lanes = target_lanes_1 self.current_road = Road(target_road_1_start, target_road_1_end) self._navi_info.fill(0.0) half = self.navigation_info_dim // 2 self._navi_info[: half], lanes_heading1, checkpoint = self._get_info_for_checkpoint( lanes_id=0, lanes=target_lanes_1, ego_vehicle=ego_vehicle) self._navi_info[ half:], lanes_heading2, _ = self._get_info_for_checkpoint( lanes_id=1, lanes=target_lanes_2, ego_vehicle=ego_vehicle) if self._show_navi_info: pos_of_goal = checkpoint self._goal_node_path.setPos(pos_of_goal[0], -pos_of_goal[1], 1.8) self._goal_node_path.setH(self._goal_node_path.getH() + 3) self._update_navi_arrow([lanes_heading1, lanes_heading2]) dest_pos = self._dest_node_path.getPos() self._draw_line_to_dest(pg_world=ego_vehicle.pg_world, start_position=ego_vehicle.position, end_position=(dest_pos[0], -dest_pos[1])) return lane, lane_index def _get_info_for_checkpoint(self, lanes_id, lanes, ego_vehicle): ref_lane = lanes[0] later_middle = (float(self.get_current_lane_num()) / 2 - 0.5) * self.get_current_lane_width() check_point = ref_lane.position(ref_lane.length, later_middle) if lanes_id == 0: # calculate ego v lane heading lanes_heading = ref_lane.heading_at( ref_lane.local_coordinates(ego_vehicle.position)[0]) else: lanes_heading = ref_lane.heading_at( min(self.PRE_NOTIFY_DIST, ref_lane.length)) dir_vec = check_point - ego_vehicle.position dir_norm = norm(dir_vec[0], dir_vec[1]) if dir_norm > self.NAVI_POINT_DIST: dir_vec = dir_vec / dir_norm * self.NAVI_POINT_DIST proj_heading, proj_side = ego_vehicle.projection(dir_vec) bendradius = 0.0 dir = 0.0 angle = 0.0 if isinstance(ref_lane, CircularLane): bendradius = ref_lane.radius / ( BlockParameterSpace.CURVE[Parameter.radius].max + self.get_current_lane_num() * self.get_current_lane_width()) dir = ref_lane.direction if dir == 1: angle = ref_lane.end_phase - ref_lane.start_phase elif dir == -1: angle = ref_lane.start_phase - ref_lane.end_phase return (clip((proj_heading / self.NAVI_POINT_DIST + 1) / 2, 0.0, 1.0), clip((proj_side / self.NAVI_POINT_DIST + 1) / 2, 0.0, 1.0), clip(bendradius, 0.0, 1.0), clip((dir + 1) / 2, 0.0, 1.0), clip((np.rad2deg(angle) / BlockParameterSpace.CURVE[Parameter.angle].max + 1) / 2, 0.0, 1.0)), lanes_heading, check_point def _update_navi_arrow(self, lanes_heading): lane_0_heading = lanes_heading[0] lane_1_heading = lanes_heading[1] if abs(lane_0_heading - lane_1_heading) < 0.01: if self._is_showing: self._left_arrow.detachNode() self._right_arrow.detachNode() self._is_showing = False else: dir_0 = np.array( [math.cos(lane_0_heading), math.sin(lane_0_heading), 0]) dir_1 = np.array( [math.cos(lane_1_heading), math.sin(lane_1_heading), 0]) cross_product = np.cross(dir_1, dir_0) left = False if cross_product[-1] < 0 else True if not self._is_showing: self._is_showing = True if left: if not self._left_arrow.hasParent(): self._left_arrow.reparentTo(self._arrow_node_path) if self._right_arrow.hasParent(): self._right_arrow.detachNode() else: if not self._right_arrow.hasParent(): self._right_arrow.reparentTo(self._arrow_node_path) if self._left_arrow.hasParent(): self._left_arrow.detachNode() def _update_target_checkpoints(self, ego_lane_index, ego_lane_longitude): """ Return should_update: True or False """ if self._target_checkpoints_index[0] == self._target_checkpoints_index[ 1]: # on last road return False # arrive to second checkpoint current_road_start_point = ego_lane_index[0] if current_road_start_point in self.checkpoints[self._target_checkpoints_index[1]:] \ and ego_lane_longitude < self.CKPT_UPDATE_RANGE: if current_road_start_point not in self.checkpoints[ self._target_checkpoints_index[1]:-1]: return False idx = self.checkpoints.index(current_road_start_point, self._target_checkpoints_index[1], -1) self._target_checkpoints_index = [idx] if idx + 1 == len(self.checkpoints) - 1: self._target_checkpoints_index.append(idx) else: self._target_checkpoints_index.append(idx + 1) return True return False def get_navi_info(self): return self._navi_info def destroy(self): if self._show_navi_info: self._arrow_node_path.removeNode() self._line_to_dest.removeNode() self._dest_node_path.removeNode() self._goal_node_path.removeNode() def set_force_calculate_lane_index(self, force: bool): self.FORCE_CALCULATE = force def __del__(self): logging.debug("{} is destroyed".format(self.__class__.__name__)) def get_current_lateral_range(self, current_position, pg_world) -> float: """Return the maximum lateral distance from left to right.""" # special process for special block current_block_id = self.current_road.block_ID() if current_block_id == Split.ID or current_block_id == Merge.ID: left_lane = self.current_ref_lanes[0] assert isinstance( left_lane, StraightLane), "Reference lane should be straight lane here" long, lat = left_lane.local_coordinates(current_position) current_position = left_lane.position(long, -left_lane.width / 2) return self._ray_lateral_range( pg_world, current_position, self.current_ref_lanes[0].direction_lateral) else: return self.get_current_lane_width() * self.get_current_lane_num() def get_current_lane_width(self) -> float: return self.map.config[self.map.LANE_WIDTH] def get_current_lane_num(self) -> float: return len(self.current_ref_lanes) def get_current_lane(self, ego_vehicle): possible_lanes = ray_localization(np.array( ego_vehicle.heading.tolist()), ego_vehicle.position, ego_vehicle.pg_world, return_all_result=True) for lane, index, l_1_dist in possible_lanes: if lane in self.current_ref_lanes: return lane, index return possible_lanes[0][:-1] if len(possible_lanes) > 0 else (None, None) def _ray_lateral_range(self, pg_world, start_position, dir, length=50): """ It is used to measure the lateral range of special blocks :param start_position: start_point :param dir: ray direction :param length: length of ray :return: lateral range [m] """ end_position = start_position[0] + dir[0] * length, start_position[ 1] + dir[1] * length start_position = panda_position(start_position, z=0.15) end_position = panda_position(end_position, z=0.15) mask = BitMask32.bit(FirstBlock.CONTINUOUS_COLLISION_MASK) res = pg_world.physics_world.static_world.rayTestClosest( start_position, end_position, mask=mask) if not res.hasHit(): return length else: return res.getHitFraction() * length def _draw_line_to_dest(self, pg_world, start_position, end_position): if not self._show_line_to_dest: return line_seg = self._line_to_dest line_seg.moveTo( panda_position(start_position, self.LINE_TO_DEST_HEIGHT)) line_seg.drawTo(panda_position(end_position, self.LINE_TO_DEST_HEIGHT)) self._dynamic_line_np.removeNode() self._dynamic_line_np = NodePath(line_seg.create(False)) self._dynamic_line_np.hide(CamMask.Shadow | CamMask.RgbCam) self._dynamic_line_np.reparentTo(pg_world.render)
class MultiAgentParkingLotEnv(MultiAgentPGDrive): """ Env will be done when vehicle is on yellow or white continuous lane line! """ in_spawn_roads = [ Road(FirstBlock.NODE_2, FirstBlock.NODE_3), -Road(TInterSection.node(2, 0, 0), TInterSection.node(2, 0, 1)), -Road(TInterSection.node(2, 2, 0), TInterSection.node(2, 2, 1)), ] out_spawn_roads = None @property def spawn_roads(self): return self.in_spawn_roads + self.out_spawn_roads @staticmethod def default_config() -> PGConfig: return MultiAgentPGDrive.default_config().update(MAParkingLotConfig, allow_overwrite=True) @staticmethod def _get_out_spawn_roads(parking_space_num): ret = [] for i in range(1, parking_space_num + 1): ret.append(Road(ParkingLot.node(1, i, 5), ParkingLot.node(1, i, 6))) return ret def _process_extra_config(self, config) -> "PGConfig": ret_config = self.default_config().update( config, allow_overwrite=False, stop_recursive_update=["target_vehicle_configs"] ) if not ret_config["crash_done"] and ret_config["crash_vehicle_penalty"] > 2: logging.warning( "Are you sure you wish to set crash_vehicle_penalty={} when crash_done=False?".format( ret_config["crash_vehicle_penalty"] ) ) if ret_config["use_render"] and ret_config["fast"]: logging.warning("Turn fast=False can accelerate Multi-agent rendering performance!") # add extra assert parking_space_num = ret_config["parking_space_num"] assert parking_space_num % 2 == 0, "number of parking spaces must be multiples of 2" assert parking_space_num >= 4, "minimal number of parking space is 4" self.out_spawn_roads = self._get_out_spawn_roads(parking_space_num) ret_config["map_config"]["parking_space_num"] = ret_config["parking_space_num"] # Workaround if ret_config["target_vehicle_configs"]: for k, v in ret_config["target_vehicle_configs"].items(): old = ret_config["vehicle_config"].copy() new = old.update(v) ret_config["target_vehicle_configs"][k] = new self._spawn_manager = SpawnManager( exit_length=ret_config["map_config"]["exit_length"], lane_num=ret_config["map_config"]["lane_num"], num_agents=ret_config["num_agents"], vehicle_config=ret_config["vehicle_config"], target_vehicle_configs=ret_config["target_vehicle_configs"], seed=self._DEBUG_RANDOM_SEED ) self._spawn_manager.set_spawn_roads(self.spawn_roads) ret_config = self._update_agent_pos_configs(ret_config) return ret_config 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 = MAParkingLotMap(self.pg_world, map_config) self.maps[self.current_seed] = new_map self.current_map = self.maps[self.current_seed] 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 _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 get_single_observation(self, vehicle_config: "PGConfig") -> "ObservationType": return LidarStateObservationMARound(vehicle_config) def _reset_agents(self): self.current_map.parking_space_manager.reset() super(MultiAgentParkingLotEnv, self)._reset_agents() def done_function(self, vehicle_id): done, info = super(MultiAgentParkingLotEnv, self).done_function(vehicle_id) if done: self.current_map.parking_space_manager.after_vehicle_done(vehicle_id) return done, 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.on_white_continuous_line or \ (not vehicle.on_lane) or vehicle.crash_sidewalk return ret
def current_road(self): return Road(*self.lane_index[0:-1])
def _get_out_spawn_roads(parking_space_num): ret = [] for i in range(1, parking_space_num + 1): ret.append(Road(ParkingLot.node(1, i, 5), ParkingLot.node(1, i, 6))) return ret
def _create_circular_part(self, road: Road, part_idx: int, radius_exit: float, radius_inner: float, angle: float) -> (str, str, StraightLane, bool): """ Create a part of roundabout according to a straight road """ none_cross = True self.set_part_idx(part_idx) radius_big = (self.positive_lane_num * 2 - 1) * self.lane_width + radius_inner # circular part 0 segment_start_node = road.end_node segment_end_node = self.add_road_node() segment_road = Road(segment_start_node, segment_end_node) lanes = road.get_lanes( self._global_network) if part_idx == 0 else road.get_lanes( self.block_network) right_lane = lanes[-1] bend, straight = sharpbend(right_lane, 10, radius_exit, np.deg2rad(angle), True, self.lane_width, (LineType.STRIPED, LineType.SIDE)) ignore_last_2_part_start = self.road_node((part_idx + 3) % 4, 0) ignore_last_2_part_end = self.road_node((part_idx + 3) % 4, 0) none_cross = CreateRoadFrom( bend, self.positive_lane_num, segment_road, self.block_network, self._global_network, ignore_start=ignore_last_2_part_start, ignore_end=ignore_last_2_part_end) and none_cross # set circular part 0 visualization for k, lane in enumerate(segment_road.get_lanes(self.block_network)): if k == self.positive_lane_num - 1: lane.line_types = [LineType.NONE, LineType.SIDE] else: lane.line_types = [LineType.NONE, LineType.NONE] # circular part 1 tool_lane_start = straight.position(-5, 0) tool_lane_end = straight.position(0, 0) tool_lane = StraightLane(tool_lane_start, tool_lane_end) bend, straight_to_next_iter_part = sharpbend( tool_lane, 10, radius_big, np.deg2rad(2 * angle - 90), False, self.lane_width, (LineType.STRIPED, LineType.SIDE)) segment_start_node = segment_end_node segment_end_node = self.add_road_node() segment_road = Road(segment_start_node, segment_end_node) none_cross = CreateRoadFrom(bend, self.positive_lane_num, segment_road, self.block_network, self._global_network) and none_cross # circular part 2 and exit straight part length = self.EXIT_PART_LENGTH tool_lane_start = straight_to_next_iter_part.position(-5, 0) tool_lane_end = straight_to_next_iter_part.position(0, 0) tool_lane = StraightLane(tool_lane_start, tool_lane_end) bend, straight = sharpbend(tool_lane, length, radius_exit, np.deg2rad(angle), True, self.lane_width, (LineType.STRIPED, LineType.SIDE)) segment_start_node = segment_end_node segment_end_node = self.add_road_node( ) if part_idx < 3 else self._pre_block_socket.negative_road.start_node segment_road = Road(segment_start_node, segment_end_node) none_cross = CreateRoadFrom(bend, self.positive_lane_num, segment_road, self.block_network, self._global_network) and none_cross # set circular part 2 (curve) visualization for k, lane in enumerate(segment_road.get_lanes(self.block_network)): if k == self.positive_lane_num - 1: lane.line_types = [LineType.NONE, LineType.SIDE] else: lane.line_types = [LineType.NONE, LineType.NONE] exit_start = segment_end_node exit_end = self.add_road_node() segment_road = Road(exit_start, exit_end) if part_idx < 3: none_cross = CreateRoadFrom(straight, self.positive_lane_num, segment_road, self.block_network, self._global_network) and none_cross self.add_sockets( self.create_socket_from_positive_road(segment_road)) # add circular part 3 at last segment_start = self.road_node(part_idx, 1) segment_end = self.road_node((part_idx + 1) % 4, 0) segment_road = Road(segment_start, segment_end) tool_lane_start = straight_to_next_iter_part.position(-6, 0) tool_lane_end = straight_to_next_iter_part.position(0, 0) tool_lane = StraightLane(tool_lane_start, tool_lane_end) beneath = (self.positive_lane_num * 2 - 1) * self.lane_width / 2 + radius_exit cos = np.cos(np.deg2rad(angle)) radius_this_seg = beneath / cos - radius_exit bend, _ = sharpbend(tool_lane, 5, radius_this_seg, np.deg2rad(180 - 2 * angle), False, self.lane_width, (LineType.STRIPED, LineType.SIDE)) CreateRoadFrom(bend, self.positive_lane_num, segment_road, self.block_network, self._global_network) # set circular part 2 visualization for k, lane in enumerate(segment_road.get_lanes(self.block_network)): if k == 0: if self.positive_lane_num > 1: lane.line_types = [LineType.CONTINUOUS, LineType.STRIPED] else: lane.line_types = [LineType.CONTINUOUS, LineType.NONE] else: lane.line_types = [LineType.STRIPED, LineType.STRIPED] return Road(exit_start, exit_end), none_cross