def __init__(self, pg_world: PGWorld, map_config: dict = None): """ Map can be stored and recover to save time when we access the map encountered before """ parent_node_path, pg_physics_world = pg_world.worldNP, pg_world.physics_world self.config = self.default_config() if map_config: self.config.update(map_config) self.film_size = (self.config["draw_map_resolution"], self.config["draw_map_resolution"]) self.lane_width = self.config[self.LANE_WIDTH] self.lane_num = self.config[self.LANE_NUM] self.random_seed = self.config[self.SEED] self.road_network = RoadNetwork() self.blocks = [] generate_type = self.config[self.GENERATE_METHOD] if generate_type == BigGenerateMethod.BLOCK_NUM or generate_type == BigGenerateMethod.BLOCK_SEQUENCE: self._big_generate(parent_node_path, pg_physics_world) elif generate_type == MapGenerateMethod.PG_MAP_FILE: # other config such as lane width, num and seed will be valid, since they will be read from file blocks_config = self.read_map(self.config[self.GENERATE_PARA]) self._config_generate(blocks_config, parent_node_path, pg_physics_world) else: raise ValueError( "Map can not be created by {}".format(generate_type)) # a trick to optimize performance self.road_network.update_indices() self.road_network.build_helper() self._load_to_highway_render(pg_world)
def __init__(self, block_index: int, pre_block_socket: BlockSocket, global_network: RoadNetwork, random_seed): super(Block, self).__init__(random_seed) # block information assert self.ID is not None, "Each Block must has its unique ID When define Block" assert self.SOCKET_NUM is not None, "The number of Socket should be specified when define a new block" if block_index == 0: from pgdrive.scene_creator.blocks import FirstBlock assert isinstance(self, FirstBlock), "only first block can use block index 0" elif block_index < 0: logging.debug("It is recommended that block index should > 1") self._block_name = str(block_index) + self.ID self.block_index = block_index self.number_of_sample_trial = 0 # each block contains its own road network and a global network self._global_network = global_network self.block_network = RoadNetwork() # used to spawn npc self._reborn_roads = [] # own sockets, one block derives from a socket, but will have more sockets to connect other blocks self._sockets = [] # used to connect previous blocks, save its info here self._pre_block_socket = pre_block_socket self.pre_block_socket_index = pre_block_socket.index # a bounding box used to improve efficiency x_min, x_max, y_min, y_max self.bounding_box = None # used to create this block, but for first block it is nonsense if block_index != 0: self.positive_lanes = self._pre_block_socket.positive_road.get_lanes(self._global_network) self.negative_lanes = self._pre_block_socket.negative_road.get_lanes(self._global_network) self.positive_lane_num = len(self.positive_lanes) self.negative_lane_num = len(self.negative_lanes) self.positive_basic_lane = self.positive_lanes[-1] # most right or outside lane is the basic lane self.negative_basic_lane = self.negative_lanes[-1] # most right or outside lane is the basic lane self.lane_width = self.positive_basic_lane.width_at(0) if self.render: # render pre-load self.road_texture = self.loader.loadTexture(AssetLoader.file_path("textures", "sci", "color.jpg")) self.road_texture.setMinfilter(SamplerState.FT_linear_mipmap_linear) self.road_texture.setAnisotropicDegree(8) self.road_normal = self.loader.loadTexture(AssetLoader.file_path("textures", "sci", "normal.jpg")) self.ts_color = TextureStage("color") self.ts_normal = TextureStage("normal") self.side_texture = self.loader.loadTexture(AssetLoader.file_path("textures", "side_walk", "color.png")) self.side_texture.setMinfilter(SamplerState.FT_linear_mipmap_linear) self.side_texture.setAnisotropicDegree(8) self.side_normal = self.loader.loadTexture(AssetLoader.file_path("textures", "side_walk", "normal.png")) self.side_walk = self.loader.loadModel(AssetLoader.file_path("models", "box.bam"))
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 __init__(self, pg_world: PGWorld, map_config: dict = None): """ Map can be stored and recover to save time when we access the map encountered before """ self.config = PGConfig(map_config) self.film_size = (self.config["draw_map_resolution"], self.config["draw_map_resolution"]) self.random_seed = self.config[self.SEED] self.road_network = RoadNetwork() # A flatten representation of blocks, might cause chaos in city-level generation. self.blocks = [] # Generate map and insert blocks self._generate(pg_world) assert self.blocks, "The generate methods does not fill blocks!" # a trick to optimize performance self.road_network.after_init()
def vis_big(debug: bool = False): test = TestBlock(debug=debug) test.cam.setPos(-200, -350, 2000) initialize_asset_loader(test) global_network = RoadNetwork() big = BIG(2, 3.5, global_network, test.render, test.world, 888) test.vis_big(big) test.big.block_num = 40 # big.generate(BigGenerateMethod.BLOCK_NUM, 10) test.run()
def vis_big(debug: bool = False, block_type_version="v1", random_seed=None): test = TestBlock(debug=debug) if block_type_version == "v1": random_seed = random_seed or 888 test.cam.setPos(-200, -350, 2000) elif block_type_version == "v2": random_seed = random_seed or 333 test.cam.setPos(300, 400, 2000) initialize_asset_loader(test) global_network = RoadNetwork() big = BIG( 2, 3.5, global_network, test.render, test.world, random_seed=random_seed, block_type_version=block_type_version ) test.vis_big(big) test.big.block_num = 40 # big.generate(BigGenerateMethod.BLOCK_NUM, 10) test.run()
from pgdrive.scene_creator.blocks.bottleneck import Merge, Split from pgdrive.scene_creator.blocks.first_block import FirstBlock from pgdrive.scene_creator.road.road_network import RoadNetwork from pgdrive.tests.vis_block.vis_block_base import TestBlock from pgdrive.utils.asset_loader import initialize_asset_loader if __name__ == "__main__": test = TestBlock() initialize_asset_loader(test) global_network = RoadNetwork() b = FirstBlock(global_network, 3.0, 1, test.render, test.world, 1) for i in range(1, 13): tp = Merge if i % 3 == 0 else Split b = tp(i, b.get_socket(0), global_network, i) b.construct_block(test.render, test.world) test.show_bounding_box(global_network) test.run()
class Block(Element, BlockDefault): """ Abstract class of Block, BlockSocket: a part of previous block connecting this block <---------------------------------------------- road_2_end <---------------------- road_2_start <~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~> road_1_start ----------------------> road_1_end ----------------------------------------------> BlockSocket = tuple(road_1, road_2) When single-direction block created, road_2 in block socket is useless. But it's helpful when a town is created. """ def __init__(self, block_index: int, pre_block_socket: BlockSocket, global_network: RoadNetwork, random_seed): super(Block, self).__init__(random_seed) # block information assert self.ID is not None, "Each Block must has its unique ID When define Block" assert len(self.ID) == 1, "Block ID must be a character " assert self.SOCKET_NUM is not None, "The number of Socket should be specified when define a new block" if block_index == 0: from pgdrive.scene_creator.blocks import FirstBlock assert isinstance( self, FirstBlock), "only first block can use block index 0" elif block_index < 0: logging.debug("It is recommended that block index should > 1") self._block_name = str(block_index) + self.ID self.block_index = block_index self.number_of_sample_trial = 0 # each block contains its own road network and a global network self._global_network = global_network self.block_network = RoadNetwork() # used to spawn npc self._respawn_roads = [] # own sockets, one block derives from a socket, but will have more sockets to connect other blocks self._sockets = OrderedDict() # used to connect previous blocks, save its info here self.pre_block_socket = pre_block_socket self.pre_block_socket_index = pre_block_socket.index # a bounding box used to improve efficiency x_min, x_max, y_min, y_max self.bounding_box = None # used to create this block, but for first block it is nonsense if block_index != 0: self.positive_lanes = self.pre_block_socket.positive_road.get_lanes( self._global_network) self.negative_lanes = self.pre_block_socket.negative_road.get_lanes( self._global_network) self.positive_lane_num = len(self.positive_lanes) self.negative_lane_num = len(self.negative_lanes) self.positive_basic_lane = self.positive_lanes[ -1] # most right or outside lane is the basic lane self.negative_basic_lane = self.negative_lanes[ -1] # most right or outside lane is the basic lane self.lane_width = self.positive_basic_lane.width_at(0) if self.render: # render pre-load self.road_texture = self.loader.loadTexture( AssetLoader.file_path("textures", "sci", "color.jpg")) self.road_texture.setMinfilter( SamplerState.FT_linear_mipmap_linear) self.road_texture.setAnisotropicDegree(8) self.road_normal = self.loader.loadTexture( AssetLoader.file_path("textures", "sci", "normal.jpg")) self.ts_color = TextureStage("color") self.ts_normal = TextureStage("normal") self.side_texture = self.loader.loadTexture( AssetLoader.file_path("textures", "sidewalk", "color.png")) self.side_texture.setMinfilter( SamplerState.FT_linear_mipmap_linear) self.side_texture.setAnisotropicDegree(8) self.side_normal = self.loader.loadTexture( AssetLoader.file_path("textures", "sidewalk", "normal.png")) self.sidewalk = self.loader.loadModel( AssetLoader.file_path("models", "box.bam")) def construct_block(self, root_render_np: NodePath, pg_physics_world: PGPhysicsWorld, extra_config: Dict = None) -> bool: """ Randomly Construct a block, if overlap return False """ self.set_config(self.PARAMETER_SPACE.sample()) if extra_config: self.set_config(extra_config) success = self._sample_topology() self._create_in_world() self.attach_to_pg_world(root_render_np, pg_physics_world) return success def destruct_block(self, pg_physics_world: PGPhysicsWorld): self._clear_topology() self.detach_from_pg_world(pg_physics_world) self.node_path.removeNode() self.dynamic_nodes.clear() self.static_nodes.clear() def _sample_topology(self) -> bool: """ Sample a new topology, clear the previous settings at first """ self.number_of_sample_trial += 1 self._clear_topology() no_cross = self._try_plug_into_previous_block() self._global_network.add(self.block_network) return no_cross def construct_from_config(self, config: Dict, root_render_np: NodePath, pg_physics_world: PGPhysicsWorld): assert set(config.keys()) == self.PARAMETER_SPACE.parameters, \ "Make sure the parameters' name are as same as what defined in pg_space.py" self.set_config(config) success = self._sample_topology() self._create_in_world() self.attach_to_pg_world(root_render_np, pg_physics_world) return success def get_socket(self, index: Union[str, int]) -> BlockSocket: if isinstance(index, int): if index < 0 or index >= len(self._sockets): raise ValueError("Socket of {}: index out of range".format( self.class_name)) socket_index = list(self._sockets)[index] else: assert index.startswith(self._block_name) socket_index = index assert socket_index in self._sockets, (socket_index, self._sockets.keys()) return self._sockets[socket_index] def add_respawn_roads(self, respawn_roads: Union[List[Road], Road]): """ Use this to add spawn roads instead of modifying the list directly """ if isinstance(respawn_roads, List): for road in respawn_roads: self._add_one_respawn_road(road) elif isinstance(respawn_roads, Road): self._add_one_respawn_road(respawn_roads) else: raise ValueError("Only accept List[Road] or Road in this func") def get_respawn_roads(self): return self._respawn_roads def get_respawn_lanes(self): """ return a 2-dim array [[]] to keep the lane index """ ret = [] for road in self._respawn_roads: lanes = road.get_lanes(self.block_network) ret.append(lanes) return ret def add_sockets(self, sockets: Union[List[BlockSocket], BlockSocket]): """ Use this to add sockets instead of modifying the list directly """ if isinstance(sockets, BlockSocket): self._add_one_socket(sockets) elif isinstance(sockets, List): for socket in sockets: self._add_one_socket(socket) def set_part_idx(self, x): """ It is necessary to divide block to some parts in complex block and give them unique id according to part idx """ self.PART_IDX = x self.ROAD_IDX = 0 # clear the road idx when create new part def add_road_node(self): """ Call me to get a new node name of this block. It is more accurate and recommended to use road_node() to get a node name """ self.ROAD_IDX += 1 return self.road_node(self.PART_IDX, self.ROAD_IDX - 1) def road_node(self, part_idx: int, road_idx: int) -> str: """ return standard road node name """ return self.node(self.block_index, part_idx, road_idx) @classmethod def node(cls, block_idx: int, part_idx: int, road_idx: int) -> str: return str(block_idx) + cls.ID + str(part_idx) + cls.DASH + str( road_idx) + cls.DASH def _add_one_socket(self, socket: BlockSocket): assert isinstance( socket, BlockSocket), "Socket list only accept BlockSocket Type" if socket.index is not None and not socket.index.startswith( self._block_name): logging.warning( "The adding socket has index {}, which is not started with this block name {}. This is dangerous! " "Current block has sockets: {}.".format( socket.index, self._block_name, self.get_socket_indices())) if socket.index is None: # if this socket is self block socket socket.set_index(self._block_name, len(self._sockets)) self._sockets[socket.index] = socket def _add_one_respawn_road(self, respawn_road: Road): assert isinstance(respawn_road, Road), "Spawn roads list only accept Road Type" self._respawn_roads.append(respawn_road) def _clear_topology(self): self._global_network -= self.block_network self.block_network.graph.clear() self.PART_IDX = 0 self.ROAD_IDX = 0 self._respawn_roads.clear() self._sockets.clear() def _try_plug_into_previous_block(self) -> bool: """ Try to plug this Block to previous block's socket, return True for success, False for road cross """ raise NotImplementedError """------------------------------------- For Render and Physics Calculation ---------------------------------- """ def _create_in_world(self): """ Create NodePath and Geom node to perform both collision detection and render """ self.lane_line_node_path = NodePath( RigidBodyCombiner(self._block_name + "_lane_line")) self.sidewalk_node_path = NodePath( RigidBodyCombiner(self._block_name + "_sidewalk")) self.lane_node_path = NodePath( RigidBodyCombiner(self._block_name + "_lane")) self.lane_vis_node_path = NodePath( RigidBodyCombiner(self._block_name + "_lane_vis")) graph = self.block_network.graph for _from, to_dict in graph.items(): for _to, lanes in to_dict.items(): self._add_lane_surface(_from, _to, lanes) for _id, l in enumerate(lanes): line_color = l.line_color self._add_lane(l, _id, line_color) self.lane_line_node_path.flattenStrong() self.lane_line_node_path.node().collect() self.sidewalk_node_path.flattenStrong() self.sidewalk_node_path.node().collect() self.sidewalk_node_path.hide(CamMask.ScreenshotCam) # only bodies reparent to this node self.lane_node_path.flattenStrong() self.lane_node_path.node().collect() self.lane_vis_node_path.flattenStrong() self.lane_vis_node_path.node().collect() self.lane_vis_node_path.hide(CamMask.DepthCam | CamMask.ScreenshotCam) self.node_path = NodePath(self._block_name) self.node_path.hide(CamMask.Shadow) self.sidewalk_node_path.reparentTo(self.node_path) self.lane_line_node_path.reparentTo(self.node_path) self.lane_node_path.reparentTo(self.node_path) self.lane_vis_node_path.reparentTo(self.node_path) self.bounding_box = self.block_network.get_bounding_box() def _add_lane(self, lane: AbstractLane, lane_id: int, colors: List[Vec4]): parent_np = self.lane_line_node_path lane_width = lane.width_at(0) for k, i in enumerate([-1, 1]): line_color = colors[k] if lane.line_types[k] == LineType.NONE or (lane_id != 0 and k == 0): if isinstance(lane, StraightLane): continue elif isinstance( lane, CircularLane) and lane.radius != lane_width / 2: # for ramp render continue if lane.line_types[k] == LineType.CONTINUOUS or lane.line_types[ k] == LineType.SIDE: if isinstance(lane, StraightLane): lane_start = lane.position(0, i * lane_width / 2) lane_end = lane.position(lane.length, i * lane_width / 2) middle = lane.position(lane.length / 2, i * lane_width / 2) self._add_lane_line2bullet(lane_start, lane_end, middle, parent_np, line_color, lane.line_types[k]) elif isinstance(lane, CircularLane): segment_num = int(lane.length / Block.CIRCULAR_SEGMENT_LENGTH) for segment in range(segment_num): lane_start = lane.position( segment * Block.CIRCULAR_SEGMENT_LENGTH, i * lane_width / 2) lane_end = lane.position( (segment + 1) * Block.CIRCULAR_SEGMENT_LENGTH, i * lane_width / 2) middle = (lane_start + lane_end) / 2 self._add_lane_line2bullet(lane_start, lane_end, middle, parent_np, line_color, lane.line_types[k]) # for last part lane_start = lane.position( segment_num * Block.CIRCULAR_SEGMENT_LENGTH, i * lane_width / 2) lane_end = lane.position(lane.length, i * lane_width / 2) middle = (lane_start + lane_end) / 2 self._add_lane_line2bullet(lane_start, lane_end, middle, parent_np, line_color, lane.line_types[k]) if lane.line_types[k] == LineType.SIDE: radius = lane.radius if isinstance(lane, CircularLane) else 0.0 segment_num = int(lane.length / Block.SIDEWALK_LENGTH) for segment in range(segment_num): lane_start = lane.position( segment * Block.SIDEWALK_LENGTH, i * lane_width / 2) lane_end = lane.position( (segment + 1) * Block.SIDEWALK_LENGTH, i * lane_width / 2) middle = (lane_start + lane_end) / 2 self._add_sidewalk2bullet(lane_start, lane_end, middle, radius, lane.direction) # for last part lane_start = lane.position( segment_num * Block.SIDEWALK_LENGTH, i * lane_width / 2) lane_end = lane.position(lane.length, i * lane_width / 2) middle = (lane_start + lane_end) / 2 if norm(lane_start[0] - lane_end[0], lane_start[1] - lane_end[1]) > 1e-1: self._add_sidewalk2bullet(lane_start, lane_end, middle, radius, lane.direction) elif lane.line_types[k] == LineType.BROKEN: straight = True if isinstance(lane, StraightLane) else False segment_num = int(lane.length / (2 * Block.STRIPE_LENGTH)) for segment in range(segment_num): lane_start = lane.position( segment * Block.STRIPE_LENGTH * 2, i * lane_width / 2) lane_end = lane.position( segment * Block.STRIPE_LENGTH * 2 + Block.STRIPE_LENGTH, i * lane_width / 2) middle = lane.position( segment * Block.STRIPE_LENGTH * 2 + Block.STRIPE_LENGTH / 2, i * lane_width / 2) self._add_lane_line2bullet(lane_start, lane_end, middle, parent_np, line_color, lane.line_types[k], straight) lane_start = lane.position( segment_num * Block.STRIPE_LENGTH * 2, i * lane_width / 2) lane_end = lane.position(lane.length + Block.STRIPE_LENGTH, i * lane_width / 2) middle = (lane_end[0] + lane_start[0]) / 2, (lane_end[1] + lane_start[1]) / 2 if not straight: self._add_lane_line2bullet(lane_start, lane_end, middle, parent_np, line_color, lane.line_types[k], straight) if straight: lane_start = lane.position(0, i * lane_width / 2) lane_end = lane.position(lane.length, i * lane_width / 2) middle = lane.position(lane.length / 2, i * lane_width / 2) self._add_box_body(lane_start, lane_end, middle, parent_np, lane.line_types[k], line_color) def _add_box_body(self, lane_start, lane_end, middle, parent_np: NodePath, line_type, line_color): length = norm(lane_end[0] - lane_start[0], lane_end[1] - lane_start[1]) if LineType.prohibit(line_type): node_name = BodyName.White_continuous_line if line_color == LineColor.GREY else BodyName.Yellow_continuous_line else: node_name = BodyName.Broken_line body_node = BulletGhostNode(node_name) body_node.set_active(False) body_node.setKinematic(False) body_node.setStatic(True) body_np = parent_np.attachNewNode(body_node) shape = BulletBoxShape( Vec3(length / 2, Block.LANE_LINE_WIDTH / 2, Block.LANE_LINE_GHOST_HEIGHT)) body_np.node().addShape(shape) mask = Block.CONTINUOUS_COLLISION_MASK if line_type != LineType.BROKEN else Block.BROKEN_COLLISION_MASK body_np.node().setIntoCollideMask(BitMask32.bit(mask)) self.static_nodes.append(body_np.node()) body_np.setPos(panda_position(middle, Block.LANE_LINE_GHOST_HEIGHT / 2)) direction_v = lane_end - lane_start theta = -numpy.arctan2(direction_v[1], direction_v[0]) body_np.setQuat( LQuaternionf(numpy.cos(theta / 2), 0, 0, numpy.sin(theta / 2))) def _add_lane_line2bullet(self, lane_start, lane_end, middle, parent_np: NodePath, color: Vec4, line_type: LineType, straight_stripe=False): length = norm(lane_end[0] - lane_start[0], lane_end[1] - lane_start[1]) if length <= 0: return if LineType.prohibit(line_type): node_name = BodyName.White_continuous_line if color == LineColor.GREY else BodyName.Yellow_continuous_line else: node_name = BodyName.Broken_line # add bullet body for it if straight_stripe: body_np = parent_np.attachNewNode(node_name) else: body_node = BulletGhostNode(node_name) body_node.set_active(False) body_node.setKinematic(False) body_node.setStatic(True) body_np = parent_np.attachNewNode(body_node) # its scale will change by setScale body_height = Block.LANE_LINE_GHOST_HEIGHT shape = BulletBoxShape( Vec3(length / 2 if line_type != LineType.BROKEN else length, Block.LANE_LINE_WIDTH / 2, body_height)) body_np.node().addShape(shape) mask = Block.CONTINUOUS_COLLISION_MASK if line_type != LineType.BROKEN else Block.BROKEN_COLLISION_MASK body_np.node().setIntoCollideMask(BitMask32.bit(mask)) self.static_nodes.append(body_np.node()) # position and heading body_np.setPos(panda_position(middle, Block.LANE_LINE_GHOST_HEIGHT / 2)) direction_v = lane_end - lane_start theta = -numpy.arctan2(direction_v[1], direction_v[0]) body_np.setQuat( LQuaternionf(numpy.cos(theta / 2), 0, 0, numpy.sin(theta / 2))) if self.render: # For visualization lane_line = self.loader.loadModel( AssetLoader.file_path("models", "box.bam")) lane_line.setScale(length, Block.LANE_LINE_WIDTH, Block.LANE_LINE_THICKNESS) lane_line.setPos(Vec3(0, 0 - Block.LANE_LINE_GHOST_HEIGHT / 2)) lane_line.reparentTo(body_np) body_np.set_color(color) def _add_sidewalk2bullet(self, lane_start, lane_end, middle, radius=0.0, direction=0): length = norm(lane_end[0] - lane_start[0], lane_end[1] - lane_start[1]) body_node = BulletRigidBodyNode(BodyName.Sidewalk) body_node.set_active(False) body_node.setKinematic(False) body_node.setStatic(True) side_np = self.sidewalk_node_path.attachNewNode(body_node) shape = BulletBoxShape(Vec3(1 / 2, 1 / 2, 1 / 2)) body_node.addShape(shape) body_node.setIntoCollideMask( BitMask32.bit(self.CONTINUOUS_COLLISION_MASK)) self.dynamic_nodes.append(body_node) if radius == 0: factor = 1 else: if direction == 1: factor = (1 - self.SIDEWALK_LINE_DIST / radius) else: factor = (1 + self.SIDEWALK_WIDTH / radius) * ( 1 + self.SIDEWALK_LINE_DIST / radius) direction_v = lane_end - lane_start vertical_v = PGVector( (-direction_v[1], direction_v[0])) / norm(*direction_v) middle += vertical_v * (self.SIDEWALK_WIDTH / 2 + self.SIDEWALK_LINE_DIST) side_np.setPos(panda_position(middle, 0)) theta = -numpy.arctan2(direction_v[1], direction_v[0]) side_np.setQuat( LQuaternionf(numpy.cos(theta / 2), 0, 0, numpy.sin(theta / 2))) side_np.setScale( length * factor, self.SIDEWALK_WIDTH, self.SIDEWALK_THICKNESS * (1 + 0.1 * numpy.random.rand())) if self.render: side_np.setTexture(self.ts_color, self.side_texture) self.sidewalk.instanceTo(side_np) def _add_lane_surface(self, from_: str, to_: str, lanes: List): """ Add the land surface to world, this surface will record the lane information, like index :param from_: From node :param to_: To Node :param lanes: All lanes of this road :return: None """ # decoration only has vis properties need_body = False if (from_, to_) == (Decoration.start, Decoration.end) else True if isinstance(lanes[0], StraightLane): for index, lane in enumerate(lanes): middle = lane.position(lane.length / 2, 0) end = lane.position(lane.length, 0) direction_v = end - middle theta = -numpy.arctan2(direction_v[1], direction_v[0]) width = lane.width_at(0) + self.SIDEWALK_LINE_DIST * 2 length = lane.length self._add_lane2bullet(middle, width, length, theta, lane, (from_, to_, index)) else: for index, lane in enumerate(lanes): segment_num = int(lane.length / self.CIRCULAR_SEGMENT_LENGTH) for i in range(segment_num): middle = lane.position( lane.length * (i + .5) / segment_num, 0) end = lane.position(lane.length * (i + 1) / segment_num, 0) direction_v = end - middle theta = -numpy.arctan2(direction_v[1], direction_v[0]) width = lane.width_at(0) + self.SIDEWALK_LINE_DIST * 2 length = lane.length self._add_lane2bullet(middle, width, length * 1.3 / segment_num, theta, lane, (from_, to_, index)) def _add_lane2bullet(self, middle, width, length, theta, lane: Union[StraightLane, CircularLane], lane_index): """ Add lane visualization and body for it :param middle: Middle point :param width: Lane width :param length: Segment length :param theta: Rotate theta :param lane: Lane info :return: None """ segment_np = NodePath(LaneNode(BodyName.Lane, lane, lane_index)) segment_node = segment_np.node() segment_node.set_active(False) segment_node.setKinematic(False) segment_node.setStatic(True) shape = BulletBoxShape(Vec3(length / 2, 0.1, width / 2)) segment_node.addShape(shape) self.static_nodes.append(segment_node) segment_np.setPos(panda_position(middle, -0.1)) segment_np.setQuat( LQuaternionf( numpy.cos(theta / 2) * numpy.cos(-numpy.pi / 4), numpy.cos(theta / 2) * numpy.sin(-numpy.pi / 4), -numpy.sin(theta / 2) * numpy.cos(-numpy.pi / 4), numpy.sin(theta / 2) * numpy.cos(-numpy.pi / 4))) segment_np.reparentTo(self.lane_node_path) if self.render: cm = CardMaker('card') cm.setFrame(-length / 2, length / 2, -width / 2, width / 2) cm.setHasNormals(True) cm.setUvRange((0, 0), (length / 20, width / 10)) card = self.lane_vis_node_path.attachNewNode(cm.generate()) card.setPos( panda_position(middle, numpy.random.rand() * 0.01 - 0.01)) card.setQuat( LQuaternionf( numpy.cos(theta / 2) * numpy.cos(-numpy.pi / 4), numpy.cos(theta / 2) * numpy.sin(-numpy.pi / 4), -numpy.sin(theta / 2) * numpy.cos(-numpy.pi / 4), numpy.sin(theta / 2) * numpy.cos(-numpy.pi / 4))) card.setTransparency(TransparencyAttrib.MMultisample) card.setTexture(self.ts_color, self.road_texture) @staticmethod 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 get_socket_indices(self): return list(self._sockets.keys()) def get_socket_list(self): return list(self._sockets.values())
class Map: """ Base class for Map generation! """ # only used to save and read maps FILE_SUFFIX = ".json" # define string in json and config SEED = "seed" LANE_WIDTH = "lane_width" LANE_WIDTH_RAND_RANGE = "lane_width_rand_range" LANE_NUM = "lane_num" BLOCK_ID = "id" BLOCK_SEQUENCE = "block_sequence" PRE_BLOCK_SOCKET_INDEX = "pre_block_socket_index" # generate_method GENERATE_CONFIG = "config" GENERATE_TYPE = "type" def __init__(self, pg_world: PGWorld, map_config: dict = None): """ Map can be stored and recover to save time when we access the map encountered before """ self.config = PGConfig(map_config) self.film_size = (self.config["draw_map_resolution"], self.config["draw_map_resolution"]) self.random_seed = self.config[self.SEED] self.road_network = RoadNetwork() # A flatten representation of blocks, might cause chaos in city-level generation. self.blocks = [] # Generate map and insert blocks self._generate(pg_world) assert self.blocks, "The generate methods does not fill blocks!" # a trick to optimize performance self.road_network.after_init() def _generate(self, pg_world): """Key function! Please overwrite it!""" raise NotImplementedError( "Please use child class like PGMap to replace Map!") def load_to_pg_world(self, pg_world: PGWorld): parent_node_path, pg_physics_world = pg_world.worldNP, pg_world.physics_world for block in self.blocks: block.attach_to_pg_world(parent_node_path, pg_physics_world) def unload_from_pg_world(self, pg_world: PGWorld): for block in self.blocks: block.detach_from_pg_world(pg_world.physics_world) def destroy(self, pg_world: PGWorld): for block in self.blocks: block.destroy(pg_world=pg_world) def save_map(self): assert self.blocks is not None and len( self.blocks) > 0, "Please generate Map before saving it" map_config = [] for b in self.blocks: assert isinstance(b, Block), "None Set can not be saved to json file" b_config = b.get_config() json_config = b_config.get_serializable_dict() json_config[self.BLOCK_ID] = b.ID json_config[self.PRE_BLOCK_SOCKET_INDEX] = b.pre_block_socket_index map_config.append(json_config) saved_data = copy.deepcopy({ self.SEED: self.random_seed, self.BLOCK_SEQUENCE: map_config }) return saved_data def read_map(self, map_config: dict): """ Load the map from a dict. Note that we don't provide a restore function in the base class. """ self.config[self.SEED] = map_config[self.SEED] blocks_config = map_config[self.BLOCK_SEQUENCE] for b_id, b in enumerate(blocks_config): blocks_config[b_id] = { k: np.array(v) if isinstance(v, list) else v for k, v in b.items() } # update the property self.random_seed = self.config[self.SEED] return blocks_config @property def num_blocks(self): return len(self.blocks) def __del__(self): describe = self.random_seed if self.random_seed is not None else "custom" logging.debug("Scene {} is destroyed".format(describe))
class Map: # only used to save and read maps FILE_SUFFIX = ".json" # define string in json and config SEED = "seed" LANE_WIDTH = "lane_width" LANE_NUM = "lane_num" BLOCK_ID = "id" BLOCK_SEQUENCE = "block_sequence" PRE_BLOCK_SOCKET_INDEX = "pre_block_socket_index" # generate_method GENERATE_PARA = "config" GENERATE_METHOD = "type" def __init__(self, pg_world: PGWorld, map_config: dict = None): """ Map can be stored and recover to save time when we access the map encountered before """ parent_node_path, pg_physics_world = pg_world.worldNP, pg_world.physics_world self.config = self.default_config() if map_config: self.config.update(map_config) self.film_size = (self.config["draw_map_resolution"], self.config["draw_map_resolution"]) self.lane_width = self.config[self.LANE_WIDTH] self.lane_num = self.config[self.LANE_NUM] self.random_seed = self.config[self.SEED] self.road_network = RoadNetwork() self.blocks = [] generate_type = self.config[self.GENERATE_METHOD] if generate_type == BigGenerateMethod.BLOCK_NUM or generate_type == BigGenerateMethod.BLOCK_SEQUENCE: self._big_generate(parent_node_path, pg_physics_world) elif generate_type == MapGenerateMethod.PG_MAP_FILE: # other config such as lane width, num and seed will be valid, since they will be read from file blocks_config = self.read_map(self.config[self.GENERATE_PARA]) self._config_generate(blocks_config, parent_node_path, pg_physics_world) else: raise ValueError( "Map can not be created by {}".format(generate_type)) # a trick to optimize performance self.road_network.update_indices() self.road_network.build_helper() self._load_to_highway_render(pg_world) @staticmethod def default_config(): return PGConfig({ Map.GENERATE_METHOD: MapGenerateMethod.BIG_BLOCK_NUM, Map.GENERATE_PARA: None, # it can be a file path / block num / block ID sequence Map.LANE_WIDTH: 3.5, Map.LANE_NUM: 3, Map.SEED: 10, "draw_map_resolution": 1024 # Drawing the map in a canvas of (x, x) pixels. }) def _big_generate(self, parent_node_path: NodePath, pg_physics_world: PGPhysicsWorld): big_map = BIG(self.lane_num, self.lane_width, self.road_network, parent_node_path, pg_physics_world, self.random_seed) big_map.generate(self.config[self.GENERATE_METHOD], self.config[self.GENERATE_PARA]) self.blocks = big_map.blocks def _config_generate(self, blocks_config: List, parent_node_path: NodePath, pg_physics_world: PGPhysicsWorld): assert len( self.road_network.graph ) == 0, "These Map is not empty, please create a new map to read config" last_block = FirstBlock(self.road_network, self.lane_width, self.lane_num, parent_node_path, pg_physics_world, 1) self.blocks.append(last_block) for block_index, b in enumerate(blocks_config[1:], 1): block_type = PGBlock.get_block(b.pop(self.BLOCK_ID)) pre_block_socket_index = b.pop(self.PRE_BLOCK_SOCKET_INDEX) last_block = block_type( block_index, last_block.get_socket(pre_block_socket_index), self.road_network, self.random_seed) last_block.construct_from_config(b, parent_node_path, pg_physics_world) self.blocks.append(last_block) def load_to_pg_world(self, pg_world: PGWorld): parent_node_path, pg_physics_world = pg_world.worldNP, pg_world.physics_world for block in self.blocks: block.attach_to_pg_world(parent_node_path, pg_physics_world) self._load_to_highway_render(pg_world) def _load_to_highway_render(self, pg_world: PGWorld): if pg_world.highway_render is not None: pg_world.highway_render.set_map(self) def unload_from_pg_world(self, pg_world: PGWorld): for block in self.blocks: block.detach_from_pg_world(pg_world.physics_world) def destroy(self, pg_world: PGWorld): for block in self.blocks: block.destroy(pg_world=pg_world) def save_map(self): assert self.blocks is not None and len( self.blocks) > 0, "Please generate Map before saving it" map_config = [] for b in self.blocks: assert isinstance(b, Block), "None Set can not be saved to json file" b_config = b.get_config() json_config = {} for k, v in b_config._config.items(): json_config[k] = v.tolist()[0] if isinstance(v, np.ndarray) else v json_config[self.BLOCK_ID] = b.ID json_config[self.PRE_BLOCK_SOCKET_INDEX] = b.pre_block_socket_index map_config.append(json_config) saved_data = copy.deepcopy({ self.SEED: self.random_seed, self.LANE_NUM: self.lane_num, self.LANE_WIDTH: self.lane_width, self.BLOCK_SEQUENCE: map_config }) return saved_data def save_map_to_json(self, map_name: str, save_dir: str = os.path.dirname(__file__)): """ This func will generate a json file named 'map_name.json', in 'save_dir' """ data = self.save_map() with open(AssetLoader.file_path(save_dir, map_name + self.FILE_SUFFIX), 'w') as outfile: json.dump(data, outfile) def read_map(self, map_config: dict): """ Load the map from a dict """ self.config[self.LANE_NUM] = map_config[self.LANE_NUM] self.config[self.LANE_WIDTH] = map_config[self.LANE_WIDTH] self.config[self.SEED] = map_config[self.SEED] blocks_config = map_config[self.BLOCK_SEQUENCE] # update the property self.lane_width = self.config[self.LANE_WIDTH] self.lane_num = self.config[self.LANE_NUM] self.random_seed = self.config[self.SEED] return blocks_config def read_map_from_json(self, map_file_path: str): """ Create map from a .json file, read it to map config and update default properties """ with open(map_file_path, "r") as map_file: map_config = json.load(map_file) ret = self.read_map(map_config) return ret def draw_maximum_surface(self, simple_draw=True) -> pygame.Surface: surface = WorldSurface(self.film_size, 0, pygame.Surface(self.film_size)) b_box = self.road_network.get_bounding_box() x_len = b_box[1] - b_box[0] y_len = b_box[3] - b_box[2] max_len = max(x_len, y_len) # scaling and center can be easily found by bounding box scaling = self.film_size[1] / max_len - 0.1 surface.scaling = scaling centering_pos = ((b_box[0] + b_box[1]) / 2, (b_box[2] + b_box[3]) / 2) surface.move_display_window_to(centering_pos) for _from in self.road_network.graph.keys(): decoration = True if _from == Decoration.start else False for _to in self.road_network.graph[_from].keys(): for l in self.road_network.graph[_from][_to]: if simple_draw: LaneGraphics.simple_draw(l, surface) else: two_side = True if l is self.road_network.graph[_from][ _to][-1] or decoration else False LaneGraphics.display(l, surface, two_side) return surface def get_map_image_array( self, resolution: Iterable = (512, 512) ) -> Optional[Union[np.ndarray, pygame.Surface]]: import cv2 surface = self.draw_maximum_surface() ret = cv2.resize(pygame.surfarray.pixels_red(surface), resolution, interpolation=cv2.INTER_LINEAR) return ret def draw_navi_line(self, vehicle, dest_resolution=(512, 512), save=False, navi_line_color=(255, 0, 0)): """ Use this function to draw the whole map, with a navigation line. :param vehicle: ego vehicle, BaseVehicle Class :param dest_resolution: image resolution :param save: save this image :param navi_line_color: color of navigation line :return: pygame.Surface """ self.film_size = (2 * dest_resolution[0], 2 * dest_resolution[1]) checkpoints = vehicle.routing_localization.checkpoints max_surface = self.draw_maximum_surface(simple_draw=False) map_surface = pygame.Surface(dest_resolution) pygame.transform.scale(max_surface, dest_resolution, map_surface) surface = WorldSurface(self.film_size, 0, pygame.Surface(self.film_size)) b_box = self.road_network.get_bounding_box() x_len = b_box[1] - b_box[0] y_len = b_box[3] - b_box[2] max_len = max(x_len, y_len) # scaling and center can be easily found by bounding box scaling = self.film_size[1] / max_len - 0.1 surface.scaling = scaling centering_pos = ((b_box[0] + b_box[1]) / 2, (b_box[2] + b_box[3]) / 2) surface.move_display_window_to(centering_pos) for i, c in enumerate(checkpoints[:-1]): lanes = self.road_network.graph[c][checkpoints[i + 1]] for lane in lanes: LaneGraphics.simple_draw(lane, surface, color=navi_line_color) dest_surface = pygame.Surface(dest_resolution) pygame.transform.scale(surface, dest_resolution, dest_surface) dest_surface.set_alpha(100) map_surface.blit(dest_surface, (0, 0)) if save: pygame.image.save(map_surface, "map_{}.png".format(self.random_seed)) self.film_size = (self.config["draw_map_resolution"], self.config["draw_map_resolution"]) return map_surface def __del__(self): describe = self.random_seed if self.random_seed is not None else "custom" logging.debug("Scene {} is destroyed".format(describe))