class Straight(Block):
"""
Straight Road
----------------------------------------
----------------------------------------
----------------------------------------
"""
ID = "S"
SOCKET_NUM = 1
PARAMETER_SPACE = PGSpace(BlockParameterSpace.STRAIGHT)
def _try_plug_into_previous_block(self) -> bool:
self.set_part_idx(0) # only one part in simple block like straight, and curve
para = self.get_config()
length = para[Parameter.length]
basic_lane = self.positive_basic_lane
assert isinstance(basic_lane, StraightLane), "Straight road can only connect straight type"
new_lane = ExtendStraightLane(basic_lane, length, [LineType.STRIPED, LineType.SIDE])
start = self._pre_block_socket.positive_road.end_node
end = self.add_road_node()
socket = Road(start, end)
_socket = -socket
# create positive road
no_cross = CreateRoadFrom(new_lane, self.positive_lane_num, socket, self.block_network, self._global_network)
# create negative road
no_cross = CreateAdverseRoad(socket, self.block_network, self._global_network) and no_cross
self.add_sockets(BlockSocket(socket, _socket))
return no_cross
class Ramp(Block):
"""
InRamp OutRamp
start ----------- end ------------------ start ----------- end ------------------
start ----------- end ------------------ start ----------- end ------------------
(start ----------- end)[----------------] start ----------- end [----------------]
end -----------------} (start ---------- {end)
// \\
{ ---------------// \\---------------}
"""
PARAMETER_SPACE = PGSpace(BlockParameterSpace.RAMP_PARAMETER)
SOCKET_NUM = 1
# can be added to parameter space in the future
RADIUS = 40 # meters
ANGLE = 10 # degree
LANE_TYPE = (LineType.CONTINUOUS, LineType.CONTINUOUS)
SPEED_LIMIT = 12 # 12 m/s ~= 40 km/h
CONNECT_PART_LEN = 20
RAMP_LEN = 15
def __init__(self, block_index: int, pre_block_socket: BlockSocket,
global_network: RoadNetwork, random_seed):
super(Ramp, self).__init__(block_index, pre_block_socket,
global_network, random_seed)
class Curve(Block):
"""
2 - - - - - - - - - -
/ 3 - - - - - - - - - -
/ /
/ /
0 1
"""
ID = "C"
SOCKET_NUM = 1
PARAMETER_SPACE = PGSpace(BlockParameterSpace.CURVE)
def _try_plug_into_previous_block(self) -> bool:
parameters = self.get_config()
basic_lane = self.positive_basic_lane
lane_num = self.positive_lane_num
# part 1
start_node = self._pre_block_socket.positive_road.end_node
end_node = self.add_road_node()
positive_road = Road(start_node, end_node)
length = parameters[Parameter.length]
direction = parameters[Parameter.dir]
angle = parameters[Parameter.angle]
radius = parameters[Parameter.radius]
curve, straight = sharpbend(basic_lane, length, radius,
np.deg2rad(angle), direction,
basic_lane.width,
(LineType.STRIPED, LineType.SIDE))
no_cross = CreateRoadFrom(curve, lane_num, positive_road,
self.block_network, self._global_network)
no_cross = CreateAdverseRoad(positive_road, self.block_network,
self._global_network) and no_cross
# part 2
start_node = end_node
end_node = self.add_road_node()
positive_road = Road(start_node, end_node)
no_cross = CreateRoadFrom(straight, lane_num, positive_road,
self.block_network,
self._global_network) and no_cross
no_cross = CreateAdverseRoad(positive_road, self.block_network,
self._global_network) and no_cross
# common properties
self.add_sockets(self.create_socket_from_positive_road(positive_road))
return no_cross
class FirstBlock(Block):
"""
A special Set, only used to create the first block. One scene has only one first block!!!
"""
NODE_1 = ">"
NODE_2 = ">>"
NODE_3 = ">>>"
PARAMETER_SPACE = PGSpace({})
ID = "I"
SOCKET_NUM = 1
def __init__(self, global_network: RoadNetwork, lane_width: float,
lane_num: int, render_root_np: NodePath,
pg_physics_world: PGPhysicsWorld, random_seed):
place_holder = BlockSocket(Road(Decoration.start, Decoration.end),
Road(Decoration.start, Decoration.end))
super(FirstBlock, self).__init__(0, place_holder, global_network,
random_seed)
basic_lane = StraightLane([0, lane_width * (lane_num - 1)],
[10, lane_width * (lane_num - 1)],
line_types=(LineType.STRIPED, LineType.SIDE),
width=lane_width)
ego_v_born_road = Road(self.NODE_1, self.NODE_2)
CreateRoadFrom(basic_lane, lane_num, ego_v_born_road,
self.block_network, self._global_network)
CreateAdverseRoad(ego_v_born_road, self.block_network,
self._global_network)
next_lane = ExtendStraightLane(basic_lane, 40,
[LineType.STRIPED, LineType.SIDE])
other_v_born_road = Road(self.NODE_2, self.NODE_3)
CreateRoadFrom(next_lane, lane_num, other_v_born_road,
self.block_network, self._global_network)
CreateAdverseRoad(other_v_born_road, self.block_network,
self._global_network)
self._create_in_world()
global_network += self.block_network
socket = self.create_socket_from_positive_road(other_v_born_road)
socket.index = 0
self.add_sockets(socket)
self.attach_to_pg_world(render_root_np, pg_physics_world)
self._reborn_roads = [other_v_born_road]
class Fork(Ramp):
"""
Similar to Ramp
"""
PARAMETER_SPACE = PGSpace(BlockParameterSpace.FORK_PARAMETER)
class BaseVehicle(DynamicElement):
Ego_state_obs_dim = 9
"""
Vehicle chassis and its wheels index
0 1
II-----II
|
| <---chassis
|
II-----II
2 3
"""
PARAMETER_SPACE = PGSpace(
VehicleParameterSpace.BASE_VEHICLE
) # it will not sample config from parameter space
COLLISION_MASK = CollisionGroup.EgoVehicle
STEERING_INCREMENT = 0.05
default_vehicle_config = PGConfig(
dict(
lidar=(240, 50, 4), # laser num, distance, other vehicle info num
mini_map=(84, 84, 250), # buffer length, width
rgb_cam=(160, 120), # buffer length, width
depth_cam=(84, 84, True), # buffer length, width, view_ground
show_navi_mark=False,
increment_steering=False,
wheel_friction=0.6,
))
born_place = (5, 0)
LENGTH = None
WIDTH = None
def __init__(self,
pg_world: PGWorld,
vehicle_config: dict = None,
random_seed: int = 0,
config: dict = None):
"""
This Vehicle Config is different from self.get_config(), and it is used to define which modules to use, and
module parameters.
"""
super(BaseVehicle, self).__init__(random_seed)
self.pg_world = pg_world
self.node_path = NodePath("vehicle")
# config info
self.set_config(self.PARAMETER_SPACE.sample())
if config is not None:
self.set_config(config)
self.vehicle_config = self.get_vehicle_config(
vehicle_config
) if vehicle_config is not None else self.default_vehicle_config
self.increment_steering = self.vehicle_config["increment_steering"]
self.max_speed = self.get_config()[Parameter.speed_max]
self.max_steering = self.get_config()[Parameter.steering_max]
# create
self._add_chassis(pg_world.physics_world)
self.wheels = self._create_wheel()
# modules
self.image_sensors = {}
self.lidar = None
self.routing_localization = None
self.lane = None
self.lane_index = None
self.vehicle_panel = VehiclePanel(self.pg_world) if (
self.pg_world.mode == RENDER_MODE_ONSCREEN) else None
# other info
self.throttle_brake = 0.0
self.steering = 0
self.last_current_action = deque([(0.0, 0.0), (0.0, 0.0)], maxlen=2)
self.last_position = self.born_place
self.last_heading_dir = self.heading
# collision info render
self.collision_info_np = self._init_collision_info_render(pg_world)
self.collision_banners = {} # to save time
self.current_banner = None
# done info
self.crash = False
self.out_of_road = False
self.attach_to_pg_world(self.pg_world.pbr_render,
self.pg_world.physics_world)
@classmethod
def get_vehicle_config(cls, new_config):
default = copy.deepcopy(cls.default_vehicle_config)
default.update(new_config)
return default
def prepare_step(self, action):
"""
Save info and make decision before action
"""
self.last_position = self.position
self.last_heading_dir = self.heading
self.last_current_action.append(
action
) # the real step of physics world is implemented in taskMgr.step()
if self.increment_steering:
self.set_incremental_action(action)
else:
self.set_act(action)
if self.vehicle_panel is not None:
self.vehicle_panel.renew_2d_car_para_visualization(self)
def update_state(self, pg_world=None):
if self.lidar is not None:
self.lidar.perceive(self.position, self.heading_theta,
self.pg_world.physics_world)
if self.routing_localization is not None:
self.lane, self.lane_index = self.routing_localization.update_navigation_localization(
self)
return self._state_check()
def reset(self, map: Map, pos: np.ndarray, heading: float):
"""
pos is a 2-d array, and heading is a float (unit degree)
"""
heading = -np.deg2rad(heading) - np.pi / 2
self.chassis_np.setPos(Vec3(*pos, 1))
self.chassis_np.setQuat(
LQuaternionf(np.cos(heading / 2), 0, 0, np.sin(heading / 2)))
self.update_map_info(map)
self.chassis_np.node().clearForces()
self.chassis_np.node().setLinearVelocity(Vec3(0, 0, 0))
self.chassis_np.node().setAngularVelocity(Vec3(0, 0, 0))
self.system.resetSuspension()
# done info
self.crash = False
self.out_of_road = False
# other info
self.throttle_brake = 0.0
self.steering = 0
self.last_current_action = deque([(0.0, 0.0), (0.0, 0.0)], maxlen=2)
self.last_position = self.born_place
self.last_heading_dir = self.heading
if "depth_cam" in self.image_sensors and self.image_sensors[
"depth_cam"].view_ground:
for block in map.blocks:
block.node_path.hide(CamMask.DepthCam)
"""------------------------------------------- act -------------------------------------------------"""
def set_act(self, action):
para = self.get_config()
steering = action[0]
self.throttle_brake = action[1]
self.steering = steering
self.system.setSteeringValue(
self.steering * para[Parameter.steering_max], 0)
self.system.setSteeringValue(
self.steering * para[Parameter.steering_max], 1)
self._apply_throttle_brake(action[1])
def set_incremental_action(self, action: np.ndarray):
self.throttle_brake = action[1]
self.steering += action[0] * self.STEERING_INCREMENT
self.steering = clip(self.steering, -1, 1)
steering = self.steering * self.max_steering
self.system.setSteeringValue(steering, 0)
self.system.setSteeringValue(steering, 1)
self._apply_throttle_brake(action[1])
def _apply_throttle_brake(self, throttle_brake):
para = self.get_config()
max_engine_force = para[Parameter.engine_force_max]
max_brake_force = para[Parameter.brake_force_max]
for wheel_index in range(4):
if throttle_brake >= 0:
self.system.setBrake(2.0, wheel_index)
if self.speed > self.max_speed:
self.system.applyEngineForce(0.0, wheel_index)
else:
self.system.applyEngineForce(
max_engine_force * throttle_brake, wheel_index)
else:
self.system.applyEngineForce(0.0, wheel_index)
self.system.setBrake(
abs(throttle_brake) * max_brake_force, wheel_index)
"""---------------------------------------- vehicle info ----------------------------------------------"""
@property
def position(self):
return pgdrive_position(self.chassis_np.getPos())
@property
def speed(self):
"""
km/h
"""
speed = self.chassis_np.node().get_linear_velocity().length() * 3.6
return clip(speed, 0.0, 100000.0)
@property
def heading(self):
real_heading = self.heading_theta
heading = np.array([np.cos(real_heading), np.sin(real_heading)])
return heading
@property
def heading_theta(self):
"""
Get the heading theta of vehicle, unit [rad]
:return: heading in rad
"""
return (pgdrive_heading(self.chassis_np.getH()) - 90) / 180 * math.pi
@property
def velocity(self) -> np.ndarray:
return self.speed * self.velocity_direction
@property
def velocity_direction(self):
direction = self.system.get_forward_vector()
return np.asarray([direction[0], -direction[1]])
@property
def forward_direction(self):
raise ValueError("This function id deprecated.")
# print("This function id deprecated.")
# direction = self.vehicle.get_forward_vector()
# return np.array([direction[0], -direction[1]])
@property
def current_road(self):
return self.lane_index[0:-1]
"""---------------------------------------- some math tool ----------------------------------------------"""
def heading_diff(self, target_lane):
lateral = None
if isinstance(target_lane, StraightLane):
lateral = np.asarray(
get_vertical_vector(target_lane.end - target_lane.start)[1])
elif isinstance(target_lane, CircularLane):
if target_lane.direction == -1:
lateral = self.position - target_lane.center
else:
lateral = target_lane.center - self.position
else:
raise ValueError("Unknown target lane type: {}".format(
type(target_lane)))
lateral_norm = norm(lateral[0], lateral[1])
forward_direction = self.heading
# print(f"Old forward direction: {self.forward_direction}, new heading {self.heading}")
forward_direction_norm = norm(forward_direction[0],
forward_direction[1])
if not lateral_norm * forward_direction_norm:
return 0
# cos = self.forward_direction.dot(lateral) / (np.linalg.norm(lateral) * np.linalg.norm(self.forward_direction))
cos = ((forward_direction[0] * lateral[0] +
forward_direction[1] * lateral[1]) /
(lateral_norm * forward_direction_norm))
# return cos
# Normalize to 0, 1
return clip(cos, -1.0, 1.0) / 2 + 0.5
def projection(self, vector):
# Projected to the heading of vehicle
# forward_vector = self.vehicle.get_forward_vector()
# forward_old = (forward_vector[0], -forward_vector[1])
forward = self.heading
# print(f"[projection] Old forward {forward_old}, new heading {forward}")
norm_velocity = norm(forward[0], forward[1]) + 1e-6
project_on_heading = (vector[0] * forward[0] +
vector[1] * forward[1]) / norm_velocity
side_direction = get_vertical_vector(forward)[1]
side_norm = norm(side_direction[0], side_direction[1]) + 1e-6
project_on_side = (vector[0] * side_direction[0] +
vector[1] * side_direction[1]) / side_norm
return project_on_heading, project_on_side
def lane_distance_to(self, vehicle, lane: AbstractLane = None) -> float:
assert self.routing_localization is not None, "a routing and localization module should be added " \
"to interact with other vehicles"
if not vehicle:
return np.nan
if not lane:
lane = self.lane
return lane.local_coordinates(
vehicle.position)[0] - lane.local_coordinates(self.position)[0]
"""-------------------------------------- for vehicle making ------------------------------------------"""
def _add_chassis(self, pg_physics_world: PGPhysicsWorld):
para = self.get_config()
chassis = BulletRigidBodyNode(BodyName.Ego_vehicle_top)
chassis.setIntoCollideMask(BitMask32.bit(self.COLLISION_MASK))
chassis_shape = BulletBoxShape(
Vec3(para[Parameter.vehicle_width] / 2,
para[Parameter.vehicle_length] / 2,
para[Parameter.vehicle_height] / 2))
ts = TransformState.makePos(
Vec3(0, 0, para[Parameter.chassis_height] * 2))
chassis.addShape(chassis_shape, ts)
heading = np.deg2rad(-para[Parameter.heading] - 90)
chassis.setMass(para[Parameter.mass])
self.chassis_np = self.node_path.attachNewNode(chassis)
# not random born now
self.chassis_np.setPos(Vec3(*self.born_place, 1))
self.chassis_np.setQuat(
LQuaternionf(np.cos(heading / 2), 0, 0, np.sin(heading / 2)))
chassis.setDeactivationEnabled(False)
chassis.notifyCollisions(True) # advance collision check
self.pg_world.physics_world.dynamic_world.setContactAddedCallback(
PythonCallbackObject(self._collision_check))
self.dynamic_nodes.append(chassis)
chassis_beneath = BulletGhostNode(BodyName.Ego_vehicle)
chassis_beneath.setIntoCollideMask(BitMask32.bit(self.COLLISION_MASK))
chassis_beneath.addShape(chassis_shape)
self.chassis_beneath_np = self.chassis_np.attachNewNode(
chassis_beneath)
self.dynamic_nodes.append(chassis_beneath)
self.system = BulletVehicle(pg_physics_world.dynamic_world, chassis)
self.system.setCoordinateSystem(ZUp)
self.dynamic_nodes.append(
self.system
) # detach chassis will also detach system, so a waring will generate
self.LENGTH = para[Parameter.vehicle_length]
self.WIDTH = para[Parameter.vehicle_width]
if self.render:
model_path = 'models/ferra/scene.gltf'
self.chassis_vis = self.loader.loadModel(
AssetLoader.file_path(model_path))
self.chassis_vis.setZ(para[Parameter.vehicle_vis_z])
self.chassis_vis.setY(para[Parameter.vehicle_vis_y])
self.chassis_vis.setH(para[Parameter.vehicle_vis_h])
self.chassis_vis.set_scale(para[Parameter.vehicle_vis_scale])
self.chassis_vis.reparentTo(self.chassis_np)
def _create_wheel(self):
para = self.get_config()
f_l = para[Parameter.front_tire_longitude]
r_l = -para[Parameter.rear_tire_longitude]
lateral = para[Parameter.tire_lateral]
axis_height = para[Parameter.tire_radius] + 0.05
radius = para[Parameter.tire_radius]
wheels = []
for k, pos in enumerate([
Vec3(lateral, f_l, axis_height),
Vec3(-lateral, f_l, axis_height),
Vec3(lateral, r_l, axis_height),
Vec3(-lateral, r_l, axis_height)
]):
wheel = self._add_wheel(pos, radius, True if k < 2 else False,
True if k == 0 or k == 2 else False)
wheels.append(wheel)
return wheels
def _add_wheel(self, pos: Vec3, radius: float, front: bool, left):
wheel_np = self.node_path.attachNewNode("wheel")
if self.render:
model_path = 'models/yugo/yugotireR.egg' if left else 'models/yugo/yugotireL.egg'
wheel_model = self.loader.loadModel(
AssetLoader.file_path(model_path))
wheel_model.reparentTo(wheel_np)
wheel_model.set_scale(1.4, radius / 0.25, radius / 0.25)
wheel = self.system.create_wheel()
wheel.setNode(wheel_np.node())
wheel.setChassisConnectionPointCs(pos)
wheel.setFrontWheel(front)
wheel.setWheelDirectionCs(Vec3(0, 0, -1))
wheel.setWheelAxleCs(Vec3(1, 0, 0))
# TODO add them to PGConfig in the future
wheel.setWheelRadius(radius)
wheel.setMaxSuspensionTravelCm(40)
wheel.setSuspensionStiffness(30)
wheel.setWheelsDampingRelaxation(4.8)
wheel.setWheelsDampingCompression(1.2)
wheel.setFrictionSlip(self.vehicle_config["wheel_friction"])
wheel.setRollInfluence(1.5)
return wheel
def add_image_sensor(self, name: str, sensor: ImageBuffer):
self.image_sensors[name] = sensor
def add_lidar(self, laser_num=240, distance=50):
self.lidar = Lidar(self.pg_world.render, laser_num, distance)
def add_routing_localization(self, show_navi_point: bool):
self.routing_localization = RoutingLocalizationModule(
self.pg_world, show_navi_point)
def update_map_info(self, map):
"""
Update map info after reset()
:param map: new map
:return: None
"""
self.routing_localization.update(map)
lane, new_l_index = ray_localization((self.born_place), self.pg_world)
assert lane is not None, "Born place is not on road!"
self.lane_index = new_l_index
self.lane = lane
def _state_check(self):
"""
Check States and filter to update info
"""
result = self.pg_world.physics_world.dynamic_world.contactTest(
self.chassis_beneath_np.node(), True)
contacts = set()
for contact in result.getContacts():
node0 = contact.getNode0()
node1 = contact.getNode1()
name = [node0.getName(), node1.getName()]
name.remove(BodyName.Ego_vehicle)
if name[0] == "Ground" or name[0] == BodyName.Lane:
continue
elif name[0] == BodyName.Side_walk:
self.out_of_road = True
contacts.add(name[0])
if self.render:
self.render_collision_info(contacts)
return contacts
def _collision_check(self, contact):
"""
It may lower the performance if overdone
"""
node0 = contact.getNode0().getName()
node1 = contact.getNode1().getName()
name = [node0, node1]
name.remove(BodyName.Ego_vehicle_top)
if name[0] == BodyName.Traffic_vehicle:
self.crash = True
logging.debug("Crash with {}".format(name[0]))
def _init_collision_info_render(self, pg_world):
if pg_world.mode == "onscreen":
info_np = NodePath("Collision info nodepath")
info_np.reparentTo(pg_world.aspect2d)
else:
info_np = None
return info_np
def render_collision_info(self, contacts):
contacts = sorted(list(contacts),
key=lambda c: COLLISION_INFO_COLOR[COLOR[c]][0])
text = contacts[0] if len(contacts) != 0 else None
if text is None:
text = "Normal" if time.time(
) - self.pg_world._episode_start_time > 10 else "Press H to see help message"
self.render_banner(text, COLLISION_INFO_COLOR["green"][1])
else:
self.render_banner(text, COLLISION_INFO_COLOR[COLOR[text]][1])
def render_banner(self, text, color=COLLISION_INFO_COLOR["green"][1]):
"""
Render the banner in the left bottom corner.
"""
if self.collision_info_np is None:
return
if self.current_banner is not None:
self.current_banner.detachNode()
if text in self.collision_banners:
self.collision_banners[text].reparentTo(self.collision_info_np)
self.current_banner = self.collision_banners[text]
else:
new_banner = NodePath(TextNode("collision_info:{}".format(text)))
self.collision_banners[text] = new_banner
text_node = new_banner.node()
text_node.setCardColor(color)
text_node.setText(text)
text_node.setCardActual(-5 * self.pg_world.w_scale,
5.1 * self.pg_world.w_scale, -0.3, 1)
text_node.setCardDecal(True)
text_node.setTextColor(1, 1, 1, 1)
text_node.setAlign(TextNode.A_center)
new_banner.setScale(0.05)
new_banner.setPos(-0.75 * self.pg_world.w_scale, 0,
-0.8 * self.pg_world.h_scale)
new_banner.reparentTo(self.collision_info_np)
self.current_banner = new_banner
def destroy(self, _=None):
self.dynamic_nodes.remove(self.chassis_np.node())
super(BaseVehicle, self).destroy(self.pg_world)
self.pg_world.physics_world.dynamic_world.clearContactAddedCallback()
self.routing_localization.destroy()
self.routing_localization = None
if self.lidar is not None:
self.lidar.destroy()
self.lidar = None
if len(self.image_sensors) != 0:
for sensor in self.image_sensors.values():
sensor.destroy(self.pg_world)
self.image_sensors = None
if self.vehicle_panel is not None:
self.vehicle_panel.destroy(self.pg_world)
self.pg_world = None
def set_position(self, position):
"""
Should only be called when restore traffic from episode data
:param position: 2d array or list
:return: None
"""
self.chassis_np.setPos(panda_position(position, 0.4))
def set_heading(self, heading_theta) -> None:
"""
Should only be called when restore traffic from episode data
:param heading_theta: float in rad
:return: None
"""
self.chassis_np.setH((panda_heading(heading_theta) * 180 / np.pi) - 90)
def get_state(self):
return {
"heading": self.heading_theta,
"position": self.position.tolist(),
"done": self.crash or self.out_of_road
}
def set_state(self, state: dict):
self.set_heading(state["heading"])
self.set_position(state["position"])
def __del__(self):
super(BaseVehicle, self).__del__()
self.pg_world = None
self.lidar = None
self.mini_map = None
self.rgb_cam = None
self.routing_localization = None
self.wheels = None
class Roundabout(Block):
"""
roundabout class, the example is the same as Intersection
"""
ID = "O"
PARAMETER_SPACE = PGSpace(BlockParameterSpace.ROUNDABOUT)
SOCKET_NUM = 3
RADIUS_IN = 20
ANGLE = 60
EXIT_PART_LENGTH = 30
def _try_plug_into_previous_block(self) -> bool:
para = self.get_config()
no_cross = True
attach_road = self._pre_block_socket.positive_road
for i in range(4):
exit_road, success = self._create_circular_part(
attach_road, i, para[Parameter.radius_exit],
para[Parameter.radius_inner], para[Parameter.angle])
no_cross = no_cross and success
if i < 3:
no_cross = CreateAdverseRoad(exit_road, self.block_network,
self._global_network) and no_cross
attach_road = -exit_road
self.add_reborn_roads(
[socket.negative_road for socket in self._sockets])
return no_cross
def _create_circular_part(self, road: Road, part_idx: int,
radius_exit: float, radius_inner: float,
angle: float) -> (str, str, StraightLane, bool):
"""
Create a part of roundabout according to a straight road
"""
none_cross = True
self.set_part_idx(part_idx)
radius_big = (self.positive_lane_num * 2 -
1) * self.lane_width + radius_inner
# circular part 0
segment_start_node = road.end_node
segment_end_node = self.add_road_node()
segment_road = Road(segment_start_node, segment_end_node)
lanes = road.get_lanes(
self._global_network) if part_idx == 0 else road.get_lanes(
self.block_network)
right_lane = lanes[-1]
bend, straight = sharpbend(right_lane, 10, radius_exit,
np.deg2rad(angle), True, self.lane_width,
(LineType.STRIPED, LineType.SIDE))
ignore_last_2_part_start = self.road_node((part_idx + 3) % 4, 0)
ignore_last_2_part_end = self.road_node((part_idx + 3) % 4, 0)
none_cross = CreateRoadFrom(
bend,
self.positive_lane_num,
segment_road,
self.block_network,
self._global_network,
ignore_start=ignore_last_2_part_start,
ignore_end=ignore_last_2_part_end) and none_cross
# set circular part 0 visualization
for k, lane in enumerate(segment_road.get_lanes(self.block_network)):
if k == self.positive_lane_num - 1:
lane.line_types = [LineType.NONE, LineType.SIDE]
else:
lane.line_types = [LineType.NONE, LineType.NONE]
# circular part 1
tool_lane_start = straight.position(-5, 0)
tool_lane_end = straight.position(0, 0)
tool_lane = StraightLane(tool_lane_start, tool_lane_end)
bend, straight_to_next_iter_part = sharpbend(
tool_lane, 10, radius_big, np.deg2rad(2 * angle - 90), False,
self.lane_width, (LineType.STRIPED, LineType.SIDE))
segment_start_node = segment_end_node
segment_end_node = self.add_road_node()
segment_road = Road(segment_start_node, segment_end_node)
none_cross = CreateRoadFrom(bend, self.positive_lane_num, segment_road,
self.block_network,
self._global_network) and none_cross
# circular part 2 and exit straight part
length = self.EXIT_PART_LENGTH
tool_lane_start = straight_to_next_iter_part.position(-5, 0)
tool_lane_end = straight_to_next_iter_part.position(0, 0)
tool_lane = StraightLane(tool_lane_start, tool_lane_end)
bend, straight = sharpbend(tool_lane, length, radius_exit,
np.deg2rad(angle), True, self.lane_width,
(LineType.STRIPED, LineType.SIDE))
segment_start_node = segment_end_node
segment_end_node = self.add_road_node(
) if part_idx < 3 else self._pre_block_socket.negative_road.start_node
segment_road = Road(segment_start_node, segment_end_node)
none_cross = CreateRoadFrom(bend, self.positive_lane_num, segment_road,
self.block_network,
self._global_network) and none_cross
# set circular part 2 (curve) visualization
for k, lane in enumerate(segment_road.get_lanes(self.block_network)):
if k == self.positive_lane_num - 1:
lane.line_types = [LineType.NONE, LineType.SIDE]
else:
lane.line_types = [LineType.NONE, LineType.NONE]
exit_start = segment_end_node
exit_end = self.add_road_node()
segment_road = Road(exit_start, exit_end)
if part_idx < 3:
none_cross = CreateRoadFrom(straight, self.positive_lane_num,
segment_road, self.block_network,
self._global_network) and none_cross
self.add_sockets(
self.create_socket_from_positive_road(segment_road))
# add circular part 3 at last
segment_start = self.road_node(part_idx, 1)
segment_end = self.road_node((part_idx + 1) % 4, 0)
segment_road = Road(segment_start, segment_end)
tool_lane_start = straight_to_next_iter_part.position(-6, 0)
tool_lane_end = straight_to_next_iter_part.position(0, 0)
tool_lane = StraightLane(tool_lane_start, tool_lane_end)
beneath = (self.positive_lane_num * 2 -
1) * self.lane_width / 2 + radius_exit
cos = np.cos(np.deg2rad(angle))
radius_this_seg = beneath / cos - radius_exit
bend, _ = sharpbend(tool_lane, 5, radius_this_seg,
np.deg2rad(180 - 2 * angle), False,
self.lane_width, (LineType.STRIPED, LineType.SIDE))
CreateRoadFrom(bend, self.positive_lane_num, segment_road,
self.block_network, self._global_network)
# set circular part 2 visualization
for k, lane in enumerate(segment_road.get_lanes(self.block_network)):
if k == 0:
if self.positive_lane_num > 1:
lane.line_types = [LineType.CONTINUOUS, LineType.STRIPED]
else:
lane.line_types = [LineType.CONTINUOUS, LineType.NONE]
else:
lane.line_types = [LineType.STRIPED, LineType.STRIPED]
return Road(exit_start, exit_end), none_cross
def get_socket(self, index: int) -> BlockSocket:
socket = super(Roundabout, self).get_socket(index)
self._reborn_roads.remove(socket.negative_road)
return socket
class Element:
PARAMETER_SPACE = PGSpace({})
def __init__(self, random_seed=None):
"""
Config is a static conception, which specified the parameters of one element.
There parameters doesn't change, such as length of straight road, max speed of one vehicle, etc.
"""
assert isinstance(
self.PARAMETER_SPACE, PGSpace
) or random_seed is None, "Using PGSpace to define parameter spaces of " + self.class_name
self._config = PGConfig(
{k: None
for k in self.PARAMETER_SPACE.parameters})
self.random_seed = 0 if random_seed is None else random_seed
if self.PARAMETER_SPACE is not None:
self.PARAMETER_SPACE.seed(self.random_seed)
# Temporally store bullet nodes that have to place in bullet world (not NodePath)
self.dynamic_nodes = PhysicsNodeList()
# Nodes in this tuple didn't interact with other nodes! they only used to do rayTest or sweepTest
self.static_nodes = PhysicsNodeList()
self.render = False if AssetLoader.loader is None else True
self.node_path = None # each element has its node_path to render, physics node are child nodes of it
if self.render:
self.loader = AssetLoader.get_loader()
@property
def class_name(self):
return self.__class__.__name__
def get_config(self):
assert self._config is not None, "config of " + self.class_name + " is None, can not be read !"
return copy.copy(self._config)
def set_config(self, config: dict):
# logging.debug("Read config to " + self.class_name)
self._config.update(copy.copy(config))
def attach_to_pg_world(self, parent_node_path: NodePath,
pg_physics_world: PGPhysicsWorld):
if self.render:
# double check :-)
assert isinstance(
self.node_path,
NodePath), "No render model on node_path in this Element"
self.node_path.reparentTo(parent_node_path)
self.dynamic_nodes.attach_to_physics_world(
pg_physics_world.dynamic_world)
self.static_nodes.attach_to_physics_world(
pg_physics_world.static_world)
def detach_from_pg_world(self, pg_physics_world: PGPhysicsWorld):
"""
It is not fully remove, if this element is useless in the future, call Func delete()
"""
self.node_path.detachNode()
self.dynamic_nodes.detach_from_physics_world(
pg_physics_world.dynamic_world)
self.static_nodes.detach_from_physics_world(
pg_physics_world.static_world)
def destroy(self, pg_world):
"""
Fully delete this element and release the memory
"""
self.detach_from_pg_world(pg_world.physics_world)
self.node_path.removeNode()
self.dynamic_nodes.clear()
self.static_nodes.clear()
self._config.clear()
def __del__(self):
pass
class InterSection(Block):
"""
up(Goal:1)
||
||
||
||
||
_________________||_________________
left(Goal:2) -----------------||---------------- right(Goal:0)
__ ||
| ||
spawn_road <--| ||
| ||
|__ ||
down
It's an Intersection with two lanes on same direction, 4 lanes on both roads
"""
ID = "X"
EXTRA_PART = "extra"
PARAMETER_SPACE = PGSpace(BlockParameterSpace.INTERSECTION)
SOCKET_NUM = 3
ANGLE = 90 # may support other angle in the future
EXIT_PART_LENGTH = 30
# LEFT_TURN_NUM = 1 now it is useless
def _try_plug_into_previous_block(self) -> bool:
para = self.get_config()
decrease_increase = -1 if para[Parameter.decrease_increase] == 0 else 1
if self.positive_lane_num <= 1:
decrease_increase = 1
elif self.positive_lane_num >= 4:
decrease_increase = -1
self.lane_num_intersect = self.positive_lane_num + decrease_increase * para[Parameter.change_lane_num]
no_cross = True
attach_road = self._pre_block_socket.positive_road
_attach_road = self._pre_block_socket.negative_road
attach_lanes = attach_road.get_lanes(self._global_network)
# right straight left node name, rotate it to fit different part
intersect_nodes = deque(
[self.road_node(0, 0),
self.road_node(1, 0),
self.road_node(2, 0), _attach_road.start_node]
)
for i in range(4):
right_lane, success = self._create_part(
attach_lanes, attach_road, para[Parameter.radius], intersect_nodes, i
)
no_cross = no_cross and success
if i != 3:
lane_num = self.positive_lane_num if i == 1 else self.lane_num_intersect
exit_road = Road(self.road_node(i, 0), self.road_node(i, 1))
no_cross = CreateRoadFrom(
right_lane, lane_num, exit_road, self.block_network, self._global_network
) and no_cross
no_cross = CreateAdverseRoad(exit_road, self.block_network, self._global_network) and no_cross
socket = BlockSocket(exit_road, -exit_road)
self.add_reborn_roads(socket.negative_road)
self.add_sockets(socket)
attach_road = -exit_road
attach_lanes = attach_road.get_lanes(self.block_network)
return no_cross
def _create_part(self, attach_lanes, attach_road: Road, radius: float, intersect_nodes: deque,
part_idx) -> (StraightLane, bool):
lane_num = self.lane_num_intersect if part_idx == 0 or part_idx == 2 else self.positive_lane_num
non_cross = True
attach_left_lane = attach_lanes[0]
# first left part
assert isinstance(attach_left_lane, StraightLane), "Can't create a intersection following a circular lane"
self._create_left_turn(radius, lane_num, attach_left_lane, attach_road, intersect_nodes, part_idx)
# go forward part
lanes_on_road = copy.deepcopy(attach_lanes)
straight_lane_len = 2 * radius + (2 * lane_num - 1) * lanes_on_road[0].width_at(0)
for l in lanes_on_road:
next_lane = ExtendStraightLane(l, straight_lane_len, (LineType.NONE, LineType.NONE))
self.block_network.add_lane(attach_road.end_node, intersect_nodes[1], next_lane)
# right part
length = self.EXIT_PART_LENGTH
right_turn_lane = lanes_on_road[-1]
assert isinstance(right_turn_lane, StraightLane), "Can't create a intersection following a circular lane"
right_bend, right_straight = sharpbend(
right_turn_lane, length, radius, np.deg2rad(self.ANGLE), True, right_turn_lane.width_at(0),
(LineType.NONE, LineType.SIDE)
)
non_cross = (not check_lane_on_road(self._global_network, right_bend, 1)) and non_cross
CreateRoadFrom(
right_bend,
min(self.positive_lane_num, self.lane_num_intersect),
Road(attach_road.end_node, intersect_nodes[0]),
self.block_network,
self._global_network,
toward_smaller_lane_index=True,
side_lane_line_type=LineType.SIDE,
inner_lane_line_type=LineType.NONE,
center_line_type=LineType.NONE
)
intersect_nodes.rotate(-1)
return right_straight, non_cross
def get_socket(self, index: int) -> BlockSocket:
socket = super(InterSection, self).get_socket(index)
self._reborn_roads.remove(socket.negative_road)
return socket
def _create_left_turn(self, radius, lane_num, attach_left_lane, attach_road, intersect_nodes, part_idx):
left_turn_radius = radius + lane_num * attach_left_lane.width_at(0)
diff = self.lane_num_intersect - self.positive_lane_num # increase lane num
if ((part_idx == 1 or part_idx == 3) and diff > 0) or ((part_idx == 0 or part_idx == 2) and diff < 0):
diff = abs(diff)
left_bend, extra_part = sharpbend(
attach_left_lane, self.lane_width * diff, left_turn_radius, np.deg2rad(self.ANGLE), False,
attach_left_lane.width_at(0), (LineType.NONE, LineType.NONE)
)
left_road_start = intersect_nodes[2]
pre_left_road_start = left_road_start + self.EXTRA_PART
CreateRoadFrom(
left_bend,
min(self.positive_lane_num, self.lane_num_intersect),
Road(attach_road.end_node, pre_left_road_start),
self.block_network,
self._global_network,
toward_smaller_lane_index=False,
center_line_type=LineType.NONE,
side_lane_line_type=LineType.NONE,
inner_lane_line_type=LineType.NONE
)
CreateRoadFrom(
extra_part,
min(self.positive_lane_num, self.lane_num_intersect),
Road(pre_left_road_start, left_road_start),
self.block_network,
self._global_network,
toward_smaller_lane_index=False,
center_line_type=LineType.NONE,
side_lane_line_type=LineType.NONE,
inner_lane_line_type=LineType.NONE
)
else:
left_bend, _ = sharpbend(
attach_left_lane, self.EXIT_PART_LENGTH, left_turn_radius, np.deg2rad(self.ANGLE), False,
attach_left_lane.width_at(0), (LineType.NONE, LineType.NONE)
)
left_road_start = intersect_nodes[2]
CreateRoadFrom(
left_bend,
min(self.positive_lane_num, self.lane_num_intersect),
Road(attach_road.end_node, left_road_start),
self.block_network,
self._global_network,
toward_smaller_lane_index=False,
center_line_type=LineType.NONE,
side_lane_line_type=LineType.NONE,
inner_lane_line_type=LineType.NONE
)
class TInterSection(InterSection):
"""
A structure like X Intersection, code copied from it mostly
"""
ID = "T"
SOCKET_NUM = 2
PARAMETER_SPACE = PGSpace(BlockParameterSpace.T_INTERSECTION)
def _try_plug_into_previous_block(self) -> bool:
no_cross = super(TInterSection, self)._try_plug_into_previous_block()
self._exclude_lanes()
return no_cross
def _change_vis(self, t_type):
# not good here,
next_part_socket = self._sockets[(t_type + 1) % 4]
next_positive = next_part_socket.positive_road
next_negative = next_part_socket.negative_road
last_part_socket = self._sockets[(t_type + 3) % 4]
last_positive = last_part_socket.positive_road
last_negative = last_part_socket.negative_road
if t_type == Goal.LEFT:
next_positive = next_part_socket.negative_road
next_negative = next_part_socket.positive_road
if t_type == Goal.RIGHT:
last_positive = last_part_socket.negative_road
last_negative = last_part_socket.positive_road
for i, road in enumerate([
Road(last_negative.end_node, next_positive.start_node),
Road(next_negative.end_node, last_positive.start_node)
]):
lanes = road.get_lanes(self.block_network)
outside_type = LineType.SIDE if i == 0 else LineType.NONE
for k, lane in enumerate(lanes):
line_types = [
LineType.STRIPED, LineType.STRIPED
] if k != len(lanes) - 1 else [LineType.STRIPED, outside_type]
lane.line_types = line_types
if k == 0:
lane.line_color = [LineColor.YELLOW, LineColor.GREY]
if i == 1:
lane.line_types[0] = LineType.NONE
def _exclude_lanes(self):
para = self.get_config()
t_type = para[Parameter.t_intersection_type]
self.add_sockets(self._pre_block_socket)
start_node = self._sockets[t_type].negative_road.end_node
end_node = self._sockets[t_type].positive_road.start_node
for i in range(4):
if i == t_type:
continue
exit_node = self._sockets[
i].positive_road.start_node if i != Goal.ADVERSE else self._sockets[
i].negative_road.start_node
pos_lanes = self.block_network.remove_all_roads(
start_node, exit_node)
entry_node = self._sockets[
i].negative_road.end_node if i != Goal.ADVERSE else self._sockets[
i].positive_road.end_node
neg_lanes = self.block_network.remove_all_roads(
entry_node, end_node)
# TODO small vis bug may raise in a corner case,
# these code can fix it but will introduce a new get_closest_lane_index bug
# if (i + 2) % 4 == t_type:
# # for vis only, solve a bug existing in a corner case,
# for lane in neg_lanes:
# lane.reset_start_end(lane.start, lane.position(lane.length / 2, 0))
# self.block_network.add_road(Road(Decoration.start, Decoration.end), neg_lanes)
#
# for lane in pos_lanes:
# lane.reset_start_end(lane.position(lane.length / 2, 0), lane.end)
# self.block_network.add_road(Road(Decoration.start, Decoration.end), pos_lanes)
self._change_vis(t_type)
self._sockets.pop(-1)
socket = self._sockets.pop(t_type)
self.block_network.remove_all_roads(socket.positive_road.start_node,
socket.positive_road.end_node)
self.block_network.remove_all_roads(socket.negative_road.start_node,
socket.negative_road.end_node)
self._reborn_roads.remove(socket.negative_road)