def __init__(self, map_config: dict = None, random_seed=None):
"""
Map can be stored and recover to save time when we access the map encountered before
"""
assert random_seed is None
# assert random_seed == map_config[
# self.SEED
# ], "Global seed {} should equal to seed in map config {}".format(random_seed, map_config[self.SEED])
super(BaseMap, self).__init__(config=map_config)
self.film_size = (get_global_config()["draw_map_resolution"],
get_global_config()["draw_map_resolution"])
self.road_network = RoadNetwork()
# A flatten representation of blocks, might cause chaos in city-level generation.
self.blocks = []
# Generate map and insert blocks
self.engine = get_engine()
self._generate()
assert self.blocks, "The generate methods does not fill blocks!"
# a trick to optimize performance
self.road_network.after_init()
self.spawn_roads = [Road(FirstPGBlock.NODE_2, FirstPGBlock.NODE_3)]
self.detach_from_world()
def __init__(self,
block_index: int,
global_network: RoadNetwork,
random_seed,
ignore_intersection_checking=False):
super(BaseBlock, self).__init__(str(block_index) + self.ID,
random_seed,
escape_random_seed_assertion=True)
# 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 "
self.block_index = block_index
self.ignore_intersection_checking = ignore_intersection_checking
# each block contains its own road network and a global network
self._global_network = global_network
self.block_network = RoadNetwork()
# a bounding box used to improve efficiency x_min, x_max, y_min, y_max
self.bounding_box = None
# used to spawn npc
self._respawn_roads = []
self._block_objects = None
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 vis_big(debug: bool = False):
test = TestBlock(debug=debug)
test.cam.setPos(300, 400, 2000)
initialize_asset_loader(test)
set_global_random_seed(4)
global_network = RoadNetwork()
big = BIG(2, 3.5, global_network, test.render, test.world)
test.vis_big(big)
test.big.block_num = 40
# big.generate(BigGenerateMethod.BLOCK_NUM, 10)
test.run()
class BaseMap(BaseRunnable):
"""
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, map_config: dict = None, random_seed=None):
"""
Map can be stored and recover to save time when we access the map encountered before
"""
assert random_seed is None
# assert random_seed == map_config[
# self.SEED
# ], "Global seed {} should equal to seed in map config {}".format(random_seed, map_config[self.SEED])
super(BaseMap, self).__init__(config=map_config)
self.film_size = (get_global_config()["draw_map_resolution"],
get_global_config()["draw_map_resolution"])
self.road_network = RoadNetwork()
# A flatten representation of blocks, might cause chaos in city-level generation.
self.blocks = []
# Generate map and insert blocks
self.engine = get_engine()
self._generate()
assert self.blocks, "The generate methods does not fill blocks!"
# a trick to optimize performance
self.road_network.after_init()
self.spawn_roads = [Road(FirstPGBlock.NODE_2, FirstPGBlock.NODE_3)]
self.detach_from_world()
def _generate(self):
"""Key function! Please overwrite it! This func aims at fill the self.road_network adn self.blocks"""
raise NotImplementedError(
"Please use child class like PGMap to replace Map!")
def attach_to_world(self, parent_np=None, physics_world=None):
parent_node_path, physics_world = self.engine.worldNP or parent_np, self.engine.physics_world or physics_world
for block in self.blocks:
block.attach_to_world(parent_node_path, physics_world)
def detach_from_world(self, physics_world=None):
for block in self.blocks:
block.detach_from_world(self.engine.physics_world or physics_world)
def save_map(self):
"""
Save the generated map to map file
"""
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, PGBlock), "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.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
return blocks_config
@property
def num_blocks(self):
return len(self.blocks)
def destroy(self):
for block in self.blocks:
block.destroy()
super(BaseMap, self).destroy()
from pgdrive.component.blocks.first_block import FirstPGBlock
from pgdrive.component.blocks.std_intersection import StdInterSection
from pgdrive.component.blocks.std_t_intersection import StdTInterSection
from pgdrive.component.road.road_network import RoadNetwork
from pgdrive.tests.vis_block.vis_block_base import TestBlock
if __name__ == "__main__":
test = TestBlock()
from pgdrive.engine.asset_loader import initialize_asset_loader
initialize_asset_loader(test)
global_network = RoadNetwork()
first = FirstPGBlock(global_network, 3.0, 1, test.render, test.world, 1)
intersection = StdInterSection(3, first.get_socket(0), global_network, 1)
print(intersection.construct_block(test.render, test.world))
id = 4
for socket_idx in range(intersection.SOCKET_NUM):
block = StdTInterSection(id, intersection.get_socket(socket_idx),
global_network, id)
block.construct_block(test.render, test.world)
id += 1
test.show_bounding_box(global_network)
test.run()
class BaseBlock(BaseObject, DrivableAreaProperty):
"""
Block is a driving area consisting of several roads
Note: overriding the _sample() function to fill block_network/respawn_roads in subclass
Call Block.construct_block() to add it to world
"""
ID = "B"
def __init__(self,
block_index: int,
global_network: RoadNetwork,
random_seed,
ignore_intersection_checking=False):
super(BaseBlock, self).__init__(str(block_index) + self.ID,
random_seed,
escape_random_seed_assertion=True)
# 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 "
self.block_index = block_index
self.ignore_intersection_checking = ignore_intersection_checking
# each block contains its own road network and a global network
self._global_network = global_network
self.block_network = RoadNetwork()
# a bounding box used to improve efficiency x_min, x_max, y_min, y_max
self.bounding_box = None
# used to spawn npc
self._respawn_roads = []
self._block_objects = None
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 _sample_topology(self) -> bool:
"""
Sample a new topology to fill self.block_network
"""
raise NotImplementedError
def construct_block(self,
root_render_np: NodePath,
physics_world: PhysicsWorld,
extra_config: Dict = None,
no_same_node=True) -> bool:
"""
Randomly Construct a block, if overlap return False
"""
self.sample_parameters()
self.origin = NodePath(self.name)
self._block_objects = []
if extra_config:
assert set(extra_config.keys()).issubset(self.PARAMETER_SPACE.parameters), \
"Make sure the parameters' name are as same as what defined in pg_space.py"
raw_config = self.get_config()
raw_config.update(extra_config)
self.update_config(raw_config)
self._clear_topology()
success = self._sample_topology()
self._global_network.add(self.block_network, no_same_node)
self._create_in_world()
self.attach_to_world(root_render_np, physics_world)
return success
def destruct_block(self, physics_world: PhysicsWorld):
self._clear_topology()
self.detach_from_world(physics_world)
self.origin.removeNode()
self.dynamic_nodes.clear()
self.static_nodes.clear()
for obj in self._block_objects:
obj.destroy()
self._block_objects = None
def construct_from_config(self, config: Dict, root_render_np: NodePath,
physics_world: PhysicsWorld):
success = self.construct_block(root_render_np, physics_world, config)
return success
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 get_intermediate_spawn_lanes(self):
"""Return all lanes that can be used to generate spawn intermediate vehicles."""
raise NotImplementedError()
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()
"""------------------------------------- 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.name + "_lane_line"))
self.sidewalk_node_path = NodePath(
RigidBodyCombiner(self.name + "_sidewalk"))
self.lane_node_path = NodePath(RigidBodyCombiner(self.name + "_lane"))
self.lane_vis_node_path = NodePath(
RigidBodyCombiner(self.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.origin.hide(CamMask.Shadow)
self.sidewalk_node_path.reparentTo(self.origin)
self.lane_line_node_path.reparentTo(self.origin)
self.lane_node_path.reparentTo(self.origin)
self.lane_vis_node_path.reparentTo(self.origin)
self.bounding_box = self.block_network.get_bounding_box()
def _add_pgdrive_lanes(self, lane, lane_id, lane_width, colors, parent_np):
# for pgdrive structure
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 /
DrivableAreaProperty.CIRCULAR_SEGMENT_LENGTH)
for segment in range(segment_num):
lane_start = lane.position(
segment *
DrivableAreaProperty.CIRCULAR_SEGMENT_LENGTH,
i * lane_width / 2)
lane_end = lane.position(
(segment + 1) *
DrivableAreaProperty.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 *
DrivableAreaProperty.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 /
DrivableAreaProperty.SIDEWALK_LENGTH)
for segment in range(segment_num):
lane_start = lane.position(
segment * DrivableAreaProperty.SIDEWALK_LENGTH,
i * lane_width / 2)
lane_end = lane.position(
(segment + 1) *
DrivableAreaProperty.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 * DrivableAreaProperty.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 * DrivableAreaProperty.STRIPE_LENGTH))
for segment in range(segment_num):
lane_start = lane.position(
segment * DrivableAreaProperty.STRIPE_LENGTH * 2,
i * lane_width / 2)
lane_end = lane.position(
segment * DrivableAreaProperty.STRIPE_LENGTH * 2 +
DrivableAreaProperty.STRIPE_LENGTH, i * lane_width / 2)
middle = lane.position(
segment * DrivableAreaProperty.STRIPE_LENGTH * 2 +
DrivableAreaProperty.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 * DrivableAreaProperty.STRIPE_LENGTH * 2,
i * lane_width / 2)
lane_end = lane.position(
lane.length + DrivableAreaProperty.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_lane(self, lane: AbstractLane, lane_id: int, colors: List[Vec4]):
parent_np = self.lane_line_node_path
lane_width = lane.width_at(0)
if isinstance(lane, CircularLane) or isinstance(lane, StraightLane):
self._add_pgdrive_lanes(lane, lane_id, lane_width, colors,
parent_np)
elif isinstance(lane, WayPointLane):
for c, i in enumerate([-1, 1]):
line_color = colors[c]
acc_length = 0
if lane.line_types[c] != LineType.NONE:
for segment in lane.segment_property:
lane_start = lane.position(acc_length,
i * lane_width / 2)
acc_length += segment["length"]
lane_end = lane.position(acc_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[c])
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, DrivableAreaProperty.LANE_LINE_WIDTH / 2,
DrivableAreaProperty.LANE_LINE_GHOST_HEIGHT))
body_np.node().addShape(shape)
mask = DrivableAreaProperty.CONTINUOUS_COLLISION_MASK if line_type != LineType.BROKEN else DrivableAreaProperty.BROKEN_COLLISION_MASK
body_np.node().setIntoCollideMask(mask)
self.static_nodes.append(body_np.node())
body_np.setPos(
panda_position(middle,
DrivableAreaProperty.LANE_LINE_GHOST_HEIGHT / 2))
direction_v = lane_end - lane_start
# theta = -numpy.arctan2(direction_v[1], direction_v[0])
theta = -math.atan2(direction_v[1], direction_v[0])
body_np.setQuat(
LQuaternionf(math.cos(theta / 2), 0, 0, math.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 = DrivableAreaProperty.LANE_LINE_GHOST_HEIGHT
shape = BulletBoxShape(
Vec3(length / 2 if line_type != LineType.BROKEN else length,
DrivableAreaProperty.LANE_LINE_WIDTH / 2, body_height))
body_np.node().addShape(shape)
mask = DrivableAreaProperty.CONTINUOUS_COLLISION_MASK if line_type != LineType.BROKEN else DrivableAreaProperty.BROKEN_COLLISION_MASK
body_np.node().setIntoCollideMask(mask)
self.static_nodes.append(body_np.node())
# position and heading
body_np.setPos(
panda_position(middle,
DrivableAreaProperty.LANE_LINE_GHOST_HEIGHT / 2))
direction_v = lane_end - lane_start
# theta = -numpy.arctan2(direction_v[1], direction_v[0])
theta = -math.atan2(direction_v[1], direction_v[0])
body_np.setQuat(
LQuaternionf(math.cos(theta / 2), 0, 0, math.sin(theta / 2)))
if self.render:
# For visualization
lane_line = self.loader.loadModel(
AssetLoader.file_path("models", "box.bam"))
lane_line.setScale(length, DrivableAreaProperty.LANE_LINE_WIDTH,
DrivableAreaProperty.LANE_LINE_THICKNESS)
lane_line.setPos(
Vec3(0, 0 - DrivableAreaProperty.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.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(self.SIDEWALK_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 = Vector(
(-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 = -math.atan2(direction_v[1], direction_v[0])
side_np.setQuat(
LQuaternionf(math.cos(theta / 2), 0, 0, math.sin(theta / 2)))
side_np.setScale(
length * factor, self.SIDEWALK_WIDTH,
self.SIDEWALK_THICKNESS * (1 + 0.1 * np.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
"""
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 = -math.atan2(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))
elif isinstance(lanes[0], CircularLane):
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 = -math.atan2(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))
elif isinstance(lanes[0], WayPointLane):
for index, lane in enumerate(lanes):
for segment in lane.segment_property:
lane_start = segment["start_point"]
lane_end = segment["end_point"]
middle = (lane_start + lane_end) / 2
direction_v = lane_end - middle
theta = -math.atan2(direction_v[1], direction_v[0])
width = lane.width_at(0)
length = segment["length"]
self._add_lane2bullet(middle, width, length, 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
"""
length += 0.1
lane.index = lane_index
segment_np = NodePath(BaseRigidBodyNode(lane, BodyName.Lane))
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(
math.cos(theta / 2) * math.cos(-math.pi / 4),
math.cos(theta / 2) * math.sin(-math.pi / 4),
-math.sin(theta / 2) * math.cos(-math.pi / 4),
math.sin(theta / 2) * math.cos(-math.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, np.random.rand() * 0.01 - 0.01))
card.setQuat(
LQuaternionf(
math.cos(theta / 2) * math.cos(-math.pi / 4),
math.cos(theta / 2) * math.sin(-math.pi / 4),
-math.sin(theta / 2) * math.cos(-math.pi / 4),
math.sin(theta / 2) * math.cos(-math.pi / 4)))
card.setTransparency(TransparencyAttrib.MMultisample)
card.setTexture(self.ts_color, self.road_texture)
def add_body(self, physics_body):
raise DeprecationWarning(
"Different from common objects like vehicle/traffic sign, Block has several bodies!"
"Therefore, you should create BulletBody and then add them to self.dynamics_nodes "
"manually. See in construct() method")
def __init__(self,
global_network: RoadNetwork,
lane_width: float,
lane_num: int,
render_root_np: NodePath,
physics_world: PhysicsWorld,
length: float = 50,
ignore_intersection_checking=False):
place_holder = PGBlockSocket(Road(Decoration.start, Decoration.end),
Road(Decoration.start, Decoration.end))
super(FirstPGBlock, self).__init__(
0,
place_holder,
global_network,
random_seed=0,
ignore_intersection_checking=ignore_intersection_checking)
assert length > self.ENTRANCE_LENGTH, (length, self.ENTRANCE_LENGTH)
self._block_objects = []
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,
ignore_intersection_checking=self.ignore_intersection_checking)
CreateAdverseRoad(
ego_v_spawn_road,
self.block_network,
self._global_network,
ignore_intersection_checking=self.ignore_intersection_checking)
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,
ignore_intersection_checking=self.ignore_intersection_checking)
CreateAdverseRoad(
other_v_spawn_road,
self.block_network,
self._global_network,
ignore_intersection_checking=self.ignore_intersection_checking)
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.name, 0)
self.add_sockets(socket)
self.attach_to_world(render_root_np, physics_world)
self._respawn_roads = [other_v_spawn_road]