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