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.BROKEN, 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 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
ENTRANCE_LENGTH = 10
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]
class Bottleneck(Block):
"""
This block is used to change thr lane num
"""
ID = None
SOCKET_NUM = 1
PARAMETER_SPACE = PGSpace(BlockParameterSpace.BOTTLENECK_PARAMETER)
# property of bottleneck
BOTTLENECK_LEN = 20 # Add to parameter sapce in the future
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 = create_bend_straight(
basic_lane, length, radius, np.deg2rad(angle), direction,
basic_lane.width, (LineType.BROKEN, 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 Element:
"""
Element class are base class of all static objects, whose properties are fixed after calling init().
They have no any desire to change its state, e.g. moving, bouncing, changing color.
Instead, only other Elements or DynamicElements can affect them and change their states.
"""
PARAMETER_SPACE = PGSpace({})
def __init__(self, random_seed=None, name=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.
"""
self.name = random_string() if name is None else name
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)
self.render = False if AssetLoader.loader is None else True
# each element has its node_path to render, physics node are child nodes of it
self.node_path = None
# 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()
if self.render:
self.loader = AssetLoader.get_loader()
if not hasattr(self.loader, "loader"):
# It is closed before!
self.loader.__init__()
@property
def class_name(self):
return self.__class__.__name__
def get_config(self, copy=True):
if copy:
return self._config.copy()
return self._config
def set_config(self, config: dict):
# logging.debug("Read config to " + self.class_name)
self._config.update(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):
logging.debug("{} is destroyed".format(self.class_name))
@property
def heading_theta(self):
return 0.0 # Not used!
class BaseVehicle(DynamicElement):
MODEL = None
"""
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
LENGTH = None
WIDTH = None
def __init__(
self,
pg_world: PGWorld,
vehicle_config: Union[dict, PGConfig] = None,
physics_config: dict = None,
random_seed: int = 0,
name: str = None,
):
"""
This Vehicle Config is different from self.get_config(), and it is used to define which modules to use, and
module parameters. And self.physics_config defines the physics feature of vehicles, such as length/width
:param pg_world: PGWorld
:param vehicle_config: mostly, vehicle module config
:param physics_config: vehicle height/width/length, find more physics para in VehicleParameterSpace
:param random_seed: int
"""
self.vehicle_config = PGConfig(vehicle_config)
# self.vehicle_config = self.get_vehicle_config(vehicle_config) \
# if vehicle_config is not None else self._default_vehicle_config()
# observation, action
self.action_space = self.get_action_space_before_init(
extra_action_dim=self.vehicle_config["extra_action_dim"])
super(BaseVehicle, self).__init__(random_seed, name=name)
# config info
self.set_config(self.PARAMETER_SPACE.sample())
if physics_config is not None:
self.set_config(physics_config)
self.increment_steering = self.vehicle_config["increment_steering"]
self.enable_reverse = self.vehicle_config["enable_reverse"]
self.max_speed = self.vehicle_config["max_speed"]
self.max_steering = self.vehicle_config["max_steering"]
self.pg_world = pg_world
self.node_path = NodePath("vehicle")
# create
self.spawn_place = (0, 0)
self._add_chassis(pg_world.physics_world)
self.wheels = self._create_wheel()
# modules
self.image_sensors = {}
self.lidar: Optional[Lidar] = None
self.side_detector: Optional[SideDetector] = None
self.lane_line_detector: Optional[LaneLineDetector] = None
self.routing_localization: Optional[RoutingLocalizationModule] = None
self.lane: Optional[AbstractLane] = None
self.lane_index = None
self.vehicle_panel = VehiclePanel(self.pg_world) if (
self.pg_world.mode == RENDER_MODE_ONSCREEN) else None
# state 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.spawn_place
self.last_heading_dir = self.heading
self.dist_to_left_side = None
self.dist_to_right_side = None
# collision info render
self.collision_info_np = self._init_collision_info_render(pg_world)
self.collision_banners = {} # to save time
self.current_banner = None
self.attach_to_pg_world(self.pg_world.pbr_render,
self.pg_world.physics_world)
# step info
self.out_of_route = None
self.on_lane = None
# self.step_info = None
self._init_step_info()
# others
self._add_modules_for_vehicle(self.vehicle_config["use_render"])
self.takeover = False
self._expert_takeover = False
self.energy_consumption = 0
# overtake_stat
self.front_vehicles = set()
self.back_vehicles = set()
def _add_modules_for_vehicle(self, use_render: bool):
# add self module for training according to config
vehicle_config = self.vehicle_config
self.add_routing_localization(
vehicle_config["show_navi_mark"]) # default added
if self.vehicle_config["side_detector"]["num_lasers"] > 0:
self.side_detector = SideDetector(
self.pg_world.render,
self.vehicle_config["side_detector"]["num_lasers"],
self.vehicle_config["side_detector"]["distance"],
self.vehicle_config["show_side_detector"])
if self.vehicle_config["lane_line_detector"]["num_lasers"] > 0:
self.lane_line_detector = LaneLineDetector(
self.pg_world.render,
self.vehicle_config["lane_line_detector"]["num_lasers"],
self.vehicle_config["lane_line_detector"]["distance"],
self.vehicle_config["show_lane_line_detector"])
if not self.vehicle_config["use_image"]:
if vehicle_config["lidar"]["num_lasers"] > 0 and vehicle_config[
"lidar"]["distance"] > 0:
self.add_lidar(
num_lasers=vehicle_config["lidar"]["num_lasers"],
distance=vehicle_config["lidar"]["distance"],
show_lidar_point=vehicle_config["show_lidar"])
else:
import logging
logging.warning(
"You have set the lidar config to: {}, which seems to be invalid!"
.format(vehicle_config["lidar"]))
if use_render:
rgb_cam_config = vehicle_config["rgb_cam"]
rgb_cam = RGBCamera(rgb_cam_config[0], rgb_cam_config[1],
self.chassis_np, self.pg_world)
self.add_image_sensor("rgb_cam", rgb_cam)
mini_map = MiniMap(vehicle_config["mini_map"], self.chassis_np,
self.pg_world)
self.add_image_sensor("mini_map", mini_map)
return
if vehicle_config["use_image"]:
# 3 types image observation
if vehicle_config["image_source"] == "rgb_cam":
rgb_cam_config = vehicle_config["rgb_cam"]
rgb_cam = RGBCamera(rgb_cam_config[0], rgb_cam_config[1],
self.chassis_np, self.pg_world)
self.add_image_sensor("rgb_cam", rgb_cam)
elif vehicle_config["image_source"] == "mini_map":
mini_map = MiniMap(vehicle_config["mini_map"], self.chassis_np,
self.pg_world)
self.add_image_sensor("mini_map", mini_map)
elif vehicle_config["image_source"] == "depth_cam":
cam_config = vehicle_config["depth_cam"]
depth_cam = DepthCamera(*cam_config, self.chassis_np,
self.pg_world)
self.add_image_sensor("depth_cam", depth_cam)
else:
raise ValueError("No module named {}".format(
vehicle_config["image_source"]))
# load more sensors for visualization when render, only for beauty...
if use_render:
if vehicle_config["image_source"] == "mini_map":
rgb_cam_config = vehicle_config["rgb_cam"]
rgb_cam = RGBCamera(rgb_cam_config[0], rgb_cam_config[1],
self.chassis_np, self.pg_world)
self.add_image_sensor("rgb_cam", rgb_cam)
else:
mini_map = MiniMap(vehicle_config["mini_map"], self.chassis_np,
self.pg_world)
self.add_image_sensor("mini_map", mini_map)
def _init_step_info(self):
# done info will be initialized every frame
self.chassis_np.node().getPythonTag(
BodyName.Base_vehicle).init_collision_info()
self.out_of_route = False # re-route is required if is false
self.on_lane = True # on lane surface or not
# self.step_info = {"reward": 0, "cost": 0}
def _preprocess_action(self, action):
if self.vehicle_config["action_check"]:
assert self.action_space.contains(
action
), "Input {} is not compatible with action space {}!".format(
action, self.action_space)
# protect agent from nan error
action = safe_clip_for_small_array(action,
min_val=self.action_space.low[0],
max_val=self.action_space.high[0])
return action, {'raw_action': (action[0], action[1])}
def prepare_step(self, action):
"""
Save info and make decision before action
"""
# init step info to store info before each step
self._init_step_info()
action, step_info = self._preprocess_action(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)
return step_info
def update_state(self, pg_world=None, detector_mask="WRONG"):
# lidar
if self.lidar is not None:
self.lidar.perceive(self.position,
self.heading_theta,
self.pg_world.physics_world.dynamic_world,
extra_filter_node={self.chassis_np.node()},
detector_mask=detector_mask)
if self.routing_localization is not None:
self.lane, self.lane_index, = self.routing_localization.update_navigation_localization(
self)
if self.side_detector is not None:
self.side_detector.perceive(
self.position, self.heading_theta,
self.pg_world.physics_world.static_world)
if self.lane_line_detector is not None:
self.lane_line_detector.perceive(
self.position, self.heading_theta,
self.pg_world.physics_world.static_world)
self._state_check()
self.update_dist_to_left_right()
step_energy, episode_energy = self._update_energy_consumption()
self.out_of_route = self._out_of_route()
step_info = self._update_overtake_stat()
step_info.update({
"velocity": float(self.speed),
"steering": float(self.steering),
"acceleration": float(self.throttle_brake),
"step_energy": step_energy,
"episode_energy": episode_energy
})
return step_info
def _out_of_route(self):
left, right = self._dist_to_route_left_right()
return True if right < 0 or left < 0 else False
def _update_energy_consumption(self):
"""
The calculation method is from
https://www.researchgate.net/publication/262182035_Reduction_of_Fuel_Consumption_and_Exhaust_Pollutant_Using_Intelligent_Transport_System
default: 3rd gear, try to use ae^bx to fit it, dp: (90, 8), (130, 12)
:return: None
"""
distance = norm(*(self.last_position - self.position)) / 1000 # km
step_energy = 3.25 * math.pow(np.e, 0.01 * self.speed) * distance / 100
# step_energy is in Liter, we return mL
step_energy = step_energy * 1000
self.energy_consumption += step_energy # L/100 km
return step_energy, self.energy_consumption
def reset(self, map: Map, pos: np.ndarray = None, heading: float = 0.0):
"""
pos is a 2-d array, and heading is a float (unit degree)
if pos is not None, vehicle will be reset to the position
else, vehicle will be reset to spawn place
"""
if pos is None:
lane = map.road_network.get_lane(
self.vehicle_config["spawn_lane_index"])
pos = lane.position(self.vehicle_config["spawn_longitude"],
self.vehicle_config["spawn_lateral"])
heading = np.rad2deg(
lane.heading_at(self.vehicle_config["spawn_longitude"]))
self.spawn_place = pos
heading = -np.deg2rad(heading) - np.pi / 2
self.set_static(False)
self.chassis_np.setPos(panda_position(Vec3(*pos, 1)))
self.chassis_np.setQuat(
LQuaternionf(math.cos(heading / 2), 0, 0, math.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()
# np.testing.assert_almost_equal(self.position, pos, decimal=4)
# done info
self._init_step_info()
# 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.spawn_place
self.last_heading_dir = self.heading
self.update_dist_to_left_right()
self.takeover = False
self.energy_consumption = 0
# overtake_stat
self.front_vehicles = set()
self.back_vehicles = set()
# for render
if self.vehicle_panel is not None:
self.vehicle_panel.renew_2d_car_para_visualization(self)
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)
assert self.routing_localization
# Please note that if you respawn agent to some new place and might have a new destination,
# you should reset the routing localization too! Via: vehicle.routing_localization.set_route or
# vehicle.update
"""------------------------------------------- act -------------------------------------------------"""
def set_act(self, action):
steering = action[0]
self.throttle_brake = action[1]
self.steering = steering
self.system.setSteeringValue(self.steering * self.max_steering, 0)
self.system.setSteeringValue(self.steering * self.max_steering, 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):
max_engine_force = self.vehicle_config["max_engine_force"]
max_brake_force = self.vehicle_config["max_brake_force"]
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:
if self.enable_reverse:
self.system.applyEngineForce(
max_engine_force * throttle_brake, wheel_index)
self.system.setBrake(0, wheel_index)
else:
self.system.applyEngineForce(0.0, wheel_index)
self.system.setBrake(
abs(throttle_brake) * max_brake_force, wheel_index)
"""---------------------------------------- vehicle info ----------------------------------------------"""
def update_dist_to_left_right(self):
self.dist_to_left_side, self.dist_to_right_side = self._dist_to_route_left_right(
)
def _dist_to_route_left_right(self):
current_reference_lane = self.routing_localization.current_ref_lanes[0]
_, lateral_to_reference = current_reference_lane.local_coordinates(
self.position)
lateral_to_left = lateral_to_reference + self.routing_localization.get_current_lane_width(
) / 2
lateral_to_right = self.routing_localization.get_current_lateral_range(
self.position, self.pg_world) - lateral_to_left
return lateral_to_left, lateral_to_right
@property
def position(self):
return pgdrive_position(self.chassis_np.getPos())
@property
def speed(self):
"""
km/h
"""
velocity = self.chassis_np.node().get_linear_velocity()
speed = norm(velocity[0], velocity[1]) * 3.6
return clip(speed, 0.0, 100000.0)
@property
def heading(self):
real_heading = self.heading_theta
# heading = np.array([math.cos(real_heading), math.sin(real_heading)])
heading = PGVector((math.cos(real_heading), math.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.getForwardVector()
return np.asarray([direction[0], -direction[1]])
@property
def current_road(self):
return Road(*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 = ((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()
self.LENGTH = self.vehicle_config["vehicle_length"]
self.WIDTH = self.vehicle_config["vehicle_width"]
chassis = BaseVehicleNode(BodyName.Base_vehicle, self)
chassis.setIntoCollideMask(BitMask32.bit(CollisionGroup.EgoVehicle))
chassis_shape = BulletBoxShape(
Vec3(self.WIDTH / 2, self.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 spawn now
self.chassis_np.setPos(Vec3(*self.spawn_place, 1))
self.chassis_np.setQuat(
LQuaternionf(math.cos(heading / 2), 0, 0, math.sin(heading / 2)))
chassis.setDeactivationEnabled(False)
chassis.notifyCollisions(
True
) # advance collision check, do callback in pg_collision_callback
self.dynamic_nodes.append(chassis)
chassis_beneath = BulletGhostNode(BodyName.Base_vehicle_beneath)
chassis_beneath.setIntoCollideMask(
BitMask32.bit(CollisionGroup.EgoVehicleBeneath))
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
if self.render:
if self.MODEL is None:
model_path = 'models/ferra/scene.gltf'
self.MODEL = self.loader.loadModel(
AssetLoader.file_path(model_path))
self.MODEL.setZ(para[Parameter.vehicle_vis_z])
self.MODEL.setY(para[Parameter.vehicle_vis_y])
self.MODEL.setH(para[Parameter.vehicle_vis_h])
self.MODEL.set_scale(para[Parameter.vehicle_vis_scale])
self.MODEL.instanceTo(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:
# TODO something wrong with the wheel 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, num_lasers=240, distance=50, show_lidar_point=False):
assert num_lasers > 0
assert distance > 0
self.lidar = Lidar(self.pg_world.render, num_lasers, distance,
show_lidar_point)
def add_routing_localization(self, show_navi_mark: bool = False):
config = self.vehicle_config
self.routing_localization = RoutingLocalizationModule(
self.pg_world,
show_navi_mark=show_navi_mark,
random_navi_mark_color=config["random_navi_mark_color"],
show_dest_mark=config["show_dest_mark"],
show_line_to_dest=config["show_line_to_dest"])
def update_map_info(self, map):
"""
Update map info after reset()
:param map: new map
:return: None
"""
lane, new_l_index = ray_localization(np.array(self.heading.tolist()),
np.array(self.spawn_place),
self.pg_world)
self.routing_localization.update(map, current_lane_index=new_l_index)
assert lane is not None, "spawn 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_1 = self.pg_world.physics_world.static_world.contactTest(
self.chassis_beneath_np.node(), True)
result_2 = self.pg_world.physics_world.dynamic_world.contactTest(
self.chassis_beneath_np.node(), True)
contacts = set()
for contact in result_1.getContacts() + result_2.getContacts():
node0 = contact.getNode0()
node1 = contact.getNode1()
name = [node0.getName(), node1.getName()]
name.remove(BodyName.Base_vehicle_beneath)
if name[0] == "Ground" or name[0] == BodyName.Lane:
continue
if name[0] == BodyName.Sidewalk:
self.chassis_np.node().getPythonTag(
BodyName.Base_vehicle).crash_sidewalk = True
elif name[0] == BodyName.White_continuous_line:
self.chassis_np.node().getPythonTag(
BodyName.Base_vehicle).on_white_continuous_line = True
elif name[0] == BodyName.Yellow_continuous_line:
self.chassis_np.node().getPythonTag(
BodyName.Base_vehicle).on_yellow_continuous_line = True
elif name[0] == BodyName.Broken_line:
self.chassis_np.node().getPythonTag(
BodyName.Base_vehicle).on_broken_line = True
contacts.add(name[0])
if self.render:
self.render_collision_info(contacts)
@staticmethod
def _init_collision_info_render(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:
if text == BodyName.Base_vehicle_beneath:
text = BodyName.Traffic_vehicle
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.chassis_np.node().getPythonTag(BodyName.Base_vehicle).destroy()
if self.chassis_np.node() in self.dynamic_nodes:
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.side_detector is not None:
self.side_detector.destroy()
if self.lane_line_detector is not None:
self.lane_line_detector.destroy()
self.side_detector = None
self.lane_line_detector = 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, height=0.4):
"""
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, height))
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_vehicle or self.out_of_route or self.crash_sidewalk
or not self.on_lane
}
def set_state(self, state: dict):
self.set_heading(state["heading"])
self.set_position(state["position"])
def _update_overtake_stat(self):
if self.vehicle_config["overtake_stat"]:
surrounding_vs = self.lidar.get_surrounding_vehicles()
routing = self.routing_localization
ckpt_idx = routing._target_checkpoints_index
for surrounding_v in surrounding_vs:
if surrounding_v.lane_index[:-1] == (
routing.checkpoints[ckpt_idx[0]],
routing.checkpoints[ckpt_idx[1]]):
if self.lane.local_coordinates(self.position)[0] - \
self.lane.local_coordinates(surrounding_v.position)[0] < 0:
self.front_vehicles.add(surrounding_v)
if surrounding_v in self.back_vehicles:
self.back_vehicles.remove(surrounding_v)
else:
self.back_vehicles.add(surrounding_v)
return {"overtake_vehicle_num": self.get_overtake_num()}
def get_overtake_num(self):
return len(self.front_vehicles.intersection(self.back_vehicles))
@classmethod
def get_action_space_before_init(cls, extra_action_dim: int = 0):
return gym.spaces.Box(-1.0,
1.0,
shape=(2 + extra_action_dim, ),
dtype=np.float32)
def remove_display_region(self):
if self.render:
self.vehicle_panel.remove_display_region(self.pg_world)
self.vehicle_panel.buffer.set_active(False)
self.collision_info_np.detachNode()
self.routing_localization._arrow_node_path.detachNode()
for sensor in self.image_sensors.values():
sensor.remove_display_region(self.pg_world)
sensor.buffer.set_active(False)
def add_to_display(self):
if self.render:
self.vehicle_panel.add_to_display(
self.pg_world, self.vehicle_panel.default_region)
self.vehicle_panel.buffer.set_active(True)
self.collision_info_np.reparentTo(self.pg_world.aspect2d)
self.routing_localization._arrow_node_path.reparentTo(
self.pg_world.aspect2d)
for sensor in self.image_sensors.values():
sensor.add_to_display(self.pg_world, sensor.default_region)
sensor.buffer.set_active(True)
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
@property
def arrive_destination(self):
long, lat = self.routing_localization.final_lane.local_coordinates(
self.position)
flag = (self.routing_localization.final_lane.length - 5 < long <
self.routing_localization.final_lane.length + 5) and (
self.routing_localization.get_current_lane_width() / 2 >=
lat >=
(0.5 - self.routing_localization.get_current_lane_num()) *
self.routing_localization.get_current_lane_width())
return flag
@property
def crash_vehicle(self):
return self.chassis_np.node().getPythonTag(
BodyName.Base_vehicle).crash_vehicle
@property
def crash_object(self):
return self.chassis_np.node().getPythonTag(
BodyName.Base_vehicle).crash_object
@property
def crash_sidewalk(self):
return self.chassis_np.node().getPythonTag(
BodyName.Base_vehicle).crash_sidewalk
@property
def on_yellow_continuous_line(self):
return self.chassis_np.node().getPythonTag(
BodyName.Base_vehicle).on_yellow_continuous_line
@property
def on_white_continuous_line(self):
return self.chassis_np.node().getPythonTag(
BodyName.Base_vehicle).on_white_continuous_line
@property
def on_broken_line(self):
return self.chassis_np.node().getPythonTag(
BodyName.Base_vehicle).on_broken_line
def set_static(self, flag):
self.chassis_np.node().setStatic(flag)
@property
def reference_lanes(self):
return self.routing_localization.current_ref_lanes
def set_wheel_friction(self, new_friction):
raise DeprecationWarning("Bug exists here")
for wheel in self.wheels:
wheel.setFrictionSlip(new_friction)
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.get_socket_list()[(t_type + 1) % 4]
# next_part_socket = self._sockets[(t_type + 1) % 4] # FIXME pzh: Help! @LQY What is in this part?
next_positive = next_part_socket.positive_road
next_negative = next_part_socket.negative_road
last_part_socket = self.get_socket_list()[(t_type + 3) % 4]
# last_part_socket = self._sockets[(t_type + 3) % 4] # FIXME pzh: Help! @LQY
last_positive = last_part_socket.positive_road
last_negative = last_part_socket.negative_road
if t_type == Goal.LEFT:
next_positive = next_part_socket.negative_road
next_negative = next_part_socket.positive_road
if t_type == Goal.RIGHT:
last_positive = last_part_socket.negative_road
last_negative = last_part_socket.positive_road
for i, road in enumerate([
Road(last_negative.end_node, next_positive.start_node),
Road(next_negative.end_node, last_positive.start_node)
]):
lanes = road.get_lanes(self.block_network)
outside_type = LineType.SIDE if i == 0 else LineType.NONE
for k, lane in enumerate(lanes):
line_types = [
LineType.BROKEN, LineType.BROKEN
] if k != len(lanes) - 1 else [LineType.BROKEN, outside_type]
lane.line_types = line_types
if k == 0:
lane.line_color = [LineColor.YELLOW, LineColor.GREY]
if i == 1:
lane.line_types[0] = LineType.NONE
def _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[BlockSocket.get_real_index(
self._block_name, t_type)].negative_road.end_node
end_node = self._sockets[BlockSocket.get_real_index(
self._block_name, t_type)].positive_road.start_node
for i in range(4):
if i == t_type:
continue
index_i = BlockSocket.get_real_index(
self._block_name, i) if i < 3 else self.pre_block_socket_index
exit_node = self._sockets[
index_i].positive_road.start_node if i != Goal.ADVERSE else self._sockets[
index_i].negative_road.start_node
pos_lanes = self.block_network.remove_all_roads(
start_node, exit_node)
entry_node = self._sockets[
index_i].negative_road.end_node if i != Goal.ADVERSE else self._sockets[
index_i].positive_road.end_node
neg_lanes = self.block_network.remove_all_roads(
entry_node, end_node)
# 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(self.pre_block_socket.index)
socket = self._sockets.pop(
BlockSocket.get_real_index(self._block_name, 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._respawn_roads.remove(socket.negative_road)
def _add_one_socket(self, socket: BlockSocket):
assert isinstance(
socket, BlockSocket), "Socket list only accept BlockSocket Type"
# mute warning in T interection
# 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_u_turn(self, enable_u_turn: bool):
raise ValueError("T intersection didn't support u_turn now")
class Fork(Ramp):
"""
Similar to Ramp
"""
PARAMETER_SPACE = PGSpace(BlockParameterSpace.FORK_PARAMETER)
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(copy=False)
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_respawn_roads(
[socket.negative_road for socket in self.get_socket_list()])
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 = create_bend_straight(right_lane, 10, radius_exit,
np.deg2rad(angle), True,
self.lane_width,
(LineType.BROKEN, 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 = create_bend_straight(
tool_lane, 10, radius_big, np.deg2rad(2 * angle - 90), False,
self.lane_width, (LineType.BROKEN, 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 = create_bend_straight(tool_lane, length, radius_exit,
np.deg2rad(angle), True,
self.lane_width,
(LineType.BROKEN, 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 = math.cos(np.deg2rad(angle))
radius_this_seg = beneath / cos - radius_exit
bend, _ = create_bend_straight(tool_lane, 5, radius_this_seg,
np.deg2rad(180 - 2 * angle), False,
self.lane_width,
(LineType.BROKEN, 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.BROKEN]
else:
lane.line_types = [LineType.CONTINUOUS, LineType.NONE]
else:
lane.line_types = [LineType.BROKEN, LineType.BROKEN]
return Road(exit_start, exit_end), none_cross
def get_socket(self, index: int) -> BlockSocket:
socket = super(Roundabout, self).get_socket(index)
if socket.negative_road in self.get_respawn_roads():
self._respawn_roads.remove(socket.negative_road)
return socket
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
enable_u_turn = False
# 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_respawn_roads(socket.negative_road)
self.add_sockets(socket)
attach_road = -exit_road
attach_lanes = attach_road.get_lanes(self.block_network)
return no_cross
def _create_part(self, attach_lanes, attach_road: Road, radius: float,
intersect_nodes: deque, part_idx) -> (StraightLane, bool):
lane_num = self.lane_num_intersect if part_idx == 0 or part_idx == 2 else self.positive_lane_num
non_cross = True
attach_left_lane = attach_lanes[0]
# first left part
assert isinstance(
attach_left_lane, StraightLane
), "Can't create a intersection following a circular lane"
self._create_left_turn(radius, lane_num, attach_left_lane, attach_road,
intersect_nodes, part_idx)
# u-turn
if self.enable_u_turn:
adverse_road = -attach_road
self._create_u_turn(attach_road, part_idx)
# go forward part
lanes_on_road = copy.deepcopy(attach_lanes)
straight_lane_len = 2 * radius + (2 * lane_num -
1) * lanes_on_road[0].width_at(0)
for l in lanes_on_road:
next_lane = ExtendStraightLane(l, straight_lane_len,
(LineType.NONE, LineType.NONE))
self.block_network.add_lane(attach_road.end_node,
intersect_nodes[1], next_lane)
# right part
length = self.EXIT_PART_LENGTH
right_turn_lane = lanes_on_road[-1]
assert isinstance(
right_turn_lane, StraightLane
), "Can't create a intersection following a circular lane"
right_bend, right_straight = create_bend_straight(
right_turn_lane, length, radius, np.deg2rad(self.ANGLE), True,
right_turn_lane.width_at(0), (LineType.NONE, LineType.SIDE))
non_cross = (not check_lane_on_road(self._global_network, right_bend,
1)) and non_cross
CreateRoadFrom(right_bend,
min(self.positive_lane_num, self.lane_num_intersect),
Road(attach_road.end_node, intersect_nodes[0]),
self.block_network,
self._global_network,
toward_smaller_lane_index=True,
side_lane_line_type=LineType.SIDE,
inner_lane_line_type=LineType.NONE,
center_line_type=LineType.NONE)
intersect_nodes.rotate(-1)
right_straight.line_types = [LineType.BROKEN, LineType.SIDE]
return right_straight, non_cross
def get_socket(self, index: int) -> BlockSocket:
socket = super(InterSection, self).get_socket(index)
if socket.negative_road in self.get_respawn_roads():
self._respawn_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 = create_bend_straight(
attach_left_lane, self.lane_width * diff, left_turn_radius,
np.deg2rad(self.ANGLE), False, attach_left_lane.width_at(0),
(LineType.NONE, LineType.NONE))
left_road_start = intersect_nodes[2]
pre_left_road_start = left_road_start + self.EXTRA_PART
CreateRoadFrom(left_bend,
min(self.positive_lane_num,
self.lane_num_intersect),
Road(attach_road.end_node, pre_left_road_start),
self.block_network,
self._global_network,
toward_smaller_lane_index=False,
center_line_type=LineType.NONE,
side_lane_line_type=LineType.NONE,
inner_lane_line_type=LineType.NONE)
CreateRoadFrom(extra_part,
min(self.positive_lane_num,
self.lane_num_intersect),
Road(pre_left_road_start, left_road_start),
self.block_network,
self._global_network,
toward_smaller_lane_index=False,
center_line_type=LineType.NONE,
side_lane_line_type=LineType.NONE,
inner_lane_line_type=LineType.NONE)
else:
left_bend, _ = create_bend_straight(attach_left_lane,
self.EXIT_PART_LENGTH,
left_turn_radius,
np.deg2rad(self.ANGLE), False,
attach_left_lane.width_at(0),
(LineType.NONE, LineType.NONE))
left_road_start = intersect_nodes[2]
CreateRoadFrom(left_bend,
min(self.positive_lane_num,
self.lane_num_intersect),
Road(attach_road.end_node, left_road_start),
self.block_network,
self._global_network,
toward_smaller_lane_index=False,
center_line_type=LineType.NONE,
side_lane_line_type=LineType.NONE,
inner_lane_line_type=LineType.NONE)
def _create_u_turn(self, attach_road, part_idx):
# set to CONTINUOUS to debug
line_type = LineType.NONE
lanes = attach_road.get_lanes(
self.block_network) if part_idx != 0 else self.positive_lanes
attach_left_lane = lanes[0]
lane_num = len(lanes)
left_turn_radius = self.lane_width / 2
left_bend, _ = create_bend_straight(attach_left_lane,
0.1, left_turn_radius,
np.deg2rad(180), False,
attach_left_lane.width_at(0),
(LineType.NONE, LineType.NONE))
left_road_start = (-attach_road).start_node
CreateRoadFrom(left_bend,
lane_num,
Road(attach_road.end_node, left_road_start),
self.block_network,
self._global_network,
toward_smaller_lane_index=False,
center_line_type=line_type,
side_lane_line_type=line_type,
inner_lane_line_type=line_type)
def add_u_turn(self, enable_u_turn: bool):
self.enable_u_turn = enable_u_turn
class ParkingLot(Block):
"""
Parking Lot
"""
ID = "P"
PARAMETER_SPACE = PGSpace(BlockParameterSpace.PARKING_LOT_PARAMETER)
ANGLE = np.deg2rad(90)
SOCKET_LENGTH = 4 # m
SOCKET_NUM = 1
def _try_plug_into_previous_block(self) -> bool:
self.spawn_roads = []
self.dest_roads = []
no_cross = True
para = self.get_config()
assert self.positive_lane_num == 1, "Lane number of previous block must be 1 in each direction"
self.parking_space_length = para[Parameter.length]
self.parking_space_width = self.lane_width
parking_space_num = para[Parameter.one_side_vehicle_num]
# parking_space_num = 10
radius = para[Parameter.radius]
main_straight_road_length = 2 * radius + (parking_space_num - 1) * self.parking_space_width
main_lane = ExtendStraightLane(
self.positive_lanes[0], main_straight_road_length, [LineType.BROKEN, LineType.NONE]
)
road = Road(self.pre_block_socket.positive_road.end_node, self.road_node(0, 0))
# main straight part
no_cross = CreateRoadFrom(
main_lane,
self.positive_lane_num,
road,
self.block_network,
self._global_network,
center_line_type=LineType.BROKEN,
inner_lane_line_type=LineType.BROKEN,
side_lane_line_type=LineType.NONE,
center_line_color=LineColor.GREY
) and no_cross
no_cross = CreateAdverseRoad(
road,
self.block_network,
self._global_network,
center_line_type=LineType.BROKEN,
inner_lane_line_type=LineType.BROKEN,
side_lane_line_type=LineType.NONE,
center_line_color=LineColor.GREY
) and no_cross
# socket part
parking_lot_out_lane = ExtendStraightLane(main_lane, self.SOCKET_LENGTH, [LineType.BROKEN, LineType.NONE])
parking_lot_out_road = Road(self.road_node(0, 0), self.road_node(0, 1))
# out socket part
no_cross = CreateRoadFrom(
parking_lot_out_lane,
self.positive_lane_num,
parking_lot_out_road,
self.block_network,
self._global_network,
center_line_type=LineType.BROKEN,
inner_lane_line_type=LineType.BROKEN,
side_lane_line_type=LineType.SIDE
) and no_cross
no_cross = CreateAdverseRoad(
parking_lot_out_road,
self.block_network,
self._global_network,
center_line_type=LineType.BROKEN,
inner_lane_line_type=LineType.BROKEN,
side_lane_line_type=LineType.SIDE
) and no_cross
socket = self.create_socket_from_positive_road(parking_lot_out_road)
self.add_sockets(socket)
# add parking space
for i in range(int(parking_space_num)):
no_cross = self._add_one_parking_space(
copy.copy(self.get_socket_list()[0]).get_socket_in_reverse(),
self.pre_block_socket.get_socket_in_reverse(), i + 1, radius, i * self.parking_space_width,
(parking_space_num - i - 1) * self.parking_space_width
) and no_cross
for i in range(parking_space_num, 2 * parking_space_num):
index = i + 1
i -= parking_space_num
no_cross = self._add_one_parking_space(
self.pre_block_socket, copy.copy(self.get_socket_list()[0]), index, radius,
i * self.parking_space_width, (parking_space_num - i - 1) * self.parking_space_width
) and no_cross
return no_cross
def _add_one_parking_space(
self, in_socket: BlockSocket, out_socket: BlockSocket, part_idx: int, radius, dist_to_in, dist_to_out
) -> bool:
no_cross = True
# lane into parking space and parking space, 1
if in_socket.is_same_socket(self.pre_block_socket) or in_socket.is_same_socket(
self.pre_block_socket.get_socket_in_reverse()):
net = self._global_network
else:
net = self.block_network
in_lane = in_socket.positive_road.get_lanes(net)[0]
start_node = in_socket.positive_road.end_node
if dist_to_in > 1e-3:
# a straight part will be added
in_lane = ExtendStraightLane(in_lane, dist_to_in, [LineType.NONE, LineType.NONE])
in_road = Road(in_socket.positive_road.end_node, self.road_node(part_idx, 0))
CreateRoadFrom(
in_lane,
self.positive_lane_num,
in_road,
self.block_network,
self._global_network,
center_line_type=LineType.NONE,
inner_lane_line_type=LineType.NONE,
side_lane_line_type=LineType.NONE
)
start_node = self.road_node(part_idx, 0)
bend, straight = create_bend_straight(
in_lane, self.parking_space_length, radius, self.ANGLE, True, self.parking_space_width
)
bend_road = Road(start_node, self.road_node(part_idx, 1))
bend_no_cross = CreateRoadFrom(
bend,
self.positive_lane_num,
bend_road,
self.block_network,
self._global_network,
center_line_type=LineType.NONE,
inner_lane_line_type=LineType.NONE,
side_lane_line_type=LineType.SIDE if dist_to_in < 1e-3 else LineType.NONE
)
if dist_to_in < 1e-3:
no_cross = no_cross and bend_no_cross
straight_road = Road(self.road_node(part_idx, 1), self.road_node(part_idx, 2))
self.dest_roads.append(straight_road)
no_cross = no_cross and CreateRoadFrom(
straight,
self.positive_lane_num,
straight_road,
self.block_network,
self._global_network,
center_line_type=LineType.CONTINUOUS,
inner_lane_line_type=LineType.NONE,
side_lane_line_type=LineType.SIDE if dist_to_in < 1e-3 else LineType.NONE,
center_line_color=LineColor.GREY
)
# lane into parking space and parking space, 2
neg_road: Road = out_socket.negative_road
if out_socket.is_same_socket(self.pre_block_socket) or out_socket.is_same_socket(
self.pre_block_socket.get_socket_in_reverse()):
net = self._global_network
else:
net = self.block_network
neg_lane = \
neg_road.get_lanes(net)[0]
start_node = neg_road.end_node
if dist_to_out > 1e-3:
# a straight part will be added
neg_lane = ExtendStraightLane(neg_lane, dist_to_out, [LineType.NONE, LineType.NONE])
neg_road = Road(neg_road.end_node, self.road_node(part_idx, 3))
CreateRoadFrom(
neg_lane,
self.positive_lane_num,
neg_road,
self.block_network,
self._global_network,
center_line_type=LineType.NONE,
inner_lane_line_type=LineType.NONE,
side_lane_line_type=LineType.NONE
)
start_node = self.road_node(part_idx, 3)
bend, straight = create_bend_straight(
neg_lane, self.lane_width, radius, self.ANGLE, False, self.parking_space_width
)
bend_road = Road(start_node, self.road_node(part_idx, 4))
CreateRoadFrom(
bend,
self.positive_lane_num,
bend_road,
self.block_network,
self._global_network,
center_line_type=LineType.NONE,
inner_lane_line_type=LineType.NONE,
side_lane_line_type=LineType.NONE
)
straight_road = Road(self.road_node(part_idx, 4), self.road_node(part_idx, 1))
CreateRoadFrom(
straight,
self.positive_lane_num,
straight_road,
self.block_network,
self._global_network,
center_line_type=LineType.NONE,
inner_lane_line_type=LineType.NONE,
side_lane_line_type=LineType.NONE
)
# give it a new road name to make it be a two way road (1,2) = (5,6) = parking space !
parking_road = Road(self.road_node(part_idx, 5), self.road_node(part_idx, 6))
self.spawn_roads.append(parking_road)
CreateTwoWayRoad(
Road(self.road_node(part_idx, 1), self.road_node(part_idx, 2)),
self.block_network,
self._global_network,
parking_road,
center_line_type=LineType.NONE,
inner_lane_line_type=LineType.NONE,
side_lane_line_type=LineType.SIDE if dist_to_out < 1e-3 else LineType.NONE
)
# out part
parking_lane = parking_road.get_lanes(self.block_network)[0]
# out part 1
bend, straight = create_bend_straight(
parking_lane, 0.1 if dist_to_out < 1e-3 else dist_to_out, radius, self.ANGLE, True, parking_lane.width
)
out_bend_road = Road(
self.road_node(part_idx, 6),
self.road_node(part_idx, 7) if dist_to_out > 1e-3 else out_socket.positive_road.start_node
)
bend_success = CreateRoadFrom(
bend,
self.positive_lane_num,
out_bend_road,
self.block_network,
self._global_network,
center_line_type=LineType.NONE,
inner_lane_line_type=LineType.NONE,
side_lane_line_type=LineType.SIDE if dist_to_out < 1e-3 else LineType.NONE
)
if dist_to_out < 1e-3:
no_cross = no_cross and bend_success
if dist_to_out > 1e-3:
out_straight_road = Road(self.road_node(part_idx, 7), out_socket.positive_road.start_node)
no_cross = no_cross and CreateRoadFrom(
straight,
self.positive_lane_num,
out_straight_road,
self.block_network,
self._global_network,
center_line_type=LineType.NONE,
inner_lane_line_type=LineType.NONE,
side_lane_line_type=LineType.NONE
)
# out part 2
extend_lane = ExtendStraightLane(parking_lane, self.lane_width, [LineType.NONE, LineType.NONE])
CreateRoadFrom(
extend_lane,
self.positive_lane_num,
Road(self.road_node(part_idx, 6), self.road_node(part_idx, 8)),
self.block_network,
self._global_network,
center_line_type=LineType.NONE,
inner_lane_line_type=LineType.NONE,
side_lane_line_type=LineType.NONE
)
bend, straight = create_bend_straight(
extend_lane, 0.1 if dist_to_in < 1e-3 else dist_to_in, radius, self.ANGLE, False, parking_lane.width
)
out_bend_road = Road(
self.road_node(part_idx, 8),
self.road_node(part_idx, 9) if dist_to_in > 1e-3 else in_socket.negative_road.start_node
)
CreateRoadFrom(
bend,
self.positive_lane_num,
out_bend_road,
self.block_network,
self._global_network,
center_line_type=LineType.NONE,
inner_lane_line_type=LineType.NONE,
side_lane_line_type=LineType.NONE
)
if dist_to_in > 1e-3:
out_straight_road = Road(self.road_node(part_idx, 9), in_socket.negative_road.start_node)
CreateRoadFrom(
straight,
self.positive_lane_num,
out_straight_road,
self.block_network,
self._global_network,
center_line_type=LineType.NONE,
inner_lane_line_type=LineType.NONE,
side_lane_line_type=LineType.NONE
)
return no_cross
@staticmethod
def in_direction_parking_space(road: Road):
"""
Give a parking space in out-direction, return in direction road
"""
start_node = copy.deepcopy(road.start_node)
end_node = copy.deepcopy(road.end_node)
assert start_node[-2] == "5" and end_node[-2] == "6", "It is not out-direction of this parking space"
start_node = start_node[:-2] + "1" + Block.DASH
end_node = end_node[:-2] + "2" + Block.DASH
return Road(start_node, end_node)
@staticmethod
def out_direction_parking_space(road: Road):
"""
Give a parking space in in-direction, return out-direction road
"""
start_node = copy.deepcopy(road.start_node)
end_node = copy.deepcopy(road.end_node)
assert start_node[-2] == "1" and end_node[-2] == "2", "It is not in-direction of this parking space"
start_node = start_node[:-2] + "5" + Block.DASH
end_node = end_node[:-2] + "6" + Block.DASH
return Road(start_node, end_node)