class Bottleneck(PGBlock):
"""
This block is used to change thr lane num
"""
ID = None
SOCKET_NUM = 1
PARAMETER_SPACE = ParameterSpace(BlockParameterSpace.BOTTLENECK_PARAMETER)
# property of bottleneck
BOTTLENECK_LEN = None
class DefaultVehicle(BaseVehicle):
PARAMETER_SPACE = ParameterSpace(VehicleParameterSpace.DEFAULT_VEHICLE)
LENGTH = 4.51
WIDTH = 1.852
HEIGHT = 1.19
TIRE_RADIUS = 0.313
MASS = 1100
LATERAL_TIRE_TO_CENTER = 0.815
FRONT_WHEELBASE = 1.05234
REAR_WHEELBASE = 1.4166
path = [['ferra/scene.gltf', (factor, factor, factor), (0, 0.0, 0.), 0]]
class LVehicle(BaseVehicle):
PARAMETER_SPACE = ParameterSpace(VehicleParameterSpace.L_VEHICLE)
LENGTH = 4.5
WIDTH = 1.86
HEIGHT = 1.85
TIRE_RADIUS = 0.39
REAR_WHEELBASE = 1.10751
FRONT_WHEELBASE = 1.391
LATERAL_TIRE_TO_CENTER = 0.75
MASS = 1300
path = [
['new/lada/scene.gltf', (factor, factor, factor), (0, -0.25, 0.07), 0],
]
class MVehicle(BaseVehicle):
PARAMETER_SPACE = ParameterSpace(VehicleParameterSpace.M_VEHICLE)
LENGTH = 4.4
WIDTH = 1.85
HEIGHT = 1.37
TIRE_RADIUS = 0.39
REAR_WHEELBASE = 1.203
FRONT_WHEELBASE = 1.285
LATERAL_TIRE_TO_CENTER = 0.803
MASS = 1200
path = [
['new/130/scene.gltf', (factor, factor, factor), (0, -0.05, 0.07), 0],
]
class XLVehicle(BaseVehicle):
PARAMETER_SPACE = ParameterSpace(VehicleParameterSpace.XL_VEHICLE)
LENGTH = 5.8
WIDTH = 2.3
HEIGHT = 2.8
TIRE_RADIUS = 0.37
REAR_WHEELBASE = 1.075
FRONT_WHEELBASE = 1.726
LATERAL_TIRE_TO_CENTER = 0.831
CHASSIS_TO_WHEEL_AXIS = 0.3
MASS = 1600
path = [[
'new/truck/scene.gltf', (factor, factor, factor), (0, 0.3, 0.04), 0
]]
class SVehicle(BaseVehicle):
PARAMETER_SPACE = ParameterSpace(VehicleParameterSpace.S_VEHICLE)
LENGTH = 4.25
WIDTH = 1.7
HEIGHT = 1.7
LATERAL_TIRE_TO_CENTER = 0.7
FRONT_WHEELBASE = 1.4126
REAR_WHEELBASE = 1.07
TIRE_RADIUS = 0.376
MASS = 800
path = [
[
'new/beetle/scene.gltf', (factor, factor, factor), (0, -0.2, 0.03),
0
],
]
class Straight(PGBlock):
"""
Straight Road
----------------------------------------
----------------------------------------
----------------------------------------
"""
ID = "S"
SOCKET_NUM = 1
PARAMETER_SPACE = ParameterSpace(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,
ignore_intersection_checking=self.ignore_intersection_checking)
# create negative road
no_cross = CreateAdverseRoad(socket,
self.block_network,
self._global_network,
ignore_intersection_checking=self.
ignore_intersection_checking) and no_cross
self.add_sockets(PGBlockSocket(socket, _socket))
return no_cross
class Ramp(PGBlock):
"""
InRamp OutRamp
start ----------- end ------------------ start ----------- end ------------------
start ----------- end ------------------ start ----------- end ------------------
(start ----------- end)[----------------] start ----------- end [----------------]
end -----------------} (start ---------- {end)
// \\
{ ---------------// \\---------------}
"""
PARAMETER_SPACE = ParameterSpace(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
class Fork(Ramp):
"""
Similar to Ramp
"""
PARAMETER_SPACE = ParameterSpace(BlockParameterSpace.FORK_PARAMETER)
class ParkingLot(PGBlock):
"""
Parking Lot
"""
ID = "P"
PARAMETER_SPACE = ParameterSpace(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,
ignore_intersection_checking=self.
ignore_intersection_checking) 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,
ignore_intersection_checking=self.
ignore_intersection_checking) 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,
ignore_intersection_checking=self.
ignore_intersection_checking) 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,
ignore_intersection_checking=self.
ignore_intersection_checking) 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: PGBlockSocket,
out_socket: PGBlockSocket, 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,
ignore_intersection_checking=self.ignore_intersection_checking)
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,
ignore_intersection_checking=self.ignore_intersection_checking)
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,
ignore_intersection_checking=self.ignore_intersection_checking)
# 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,
ignore_intersection_checking=self.ignore_intersection_checking)
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,
ignore_intersection_checking=self.ignore_intersection_checking)
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,
ignore_intersection_checking=self.ignore_intersection_checking)
# 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,
ignore_intersection_checking=self.ignore_intersection_checking)
# 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,
ignore_intersection_checking=self.ignore_intersection_checking)
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,
ignore_intersection_checking=self.ignore_intersection_checking)
# 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,
ignore_intersection_checking=self.ignore_intersection_checking)
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,
ignore_intersection_checking=self.ignore_intersection_checking)
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,
ignore_intersection_checking=self.ignore_intersection_checking)
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:{}, end_node:{}".format(
start_node, end_node)
start_node = start_node[:-2] + "1" + PGBlock.DASH
end_node = end_node[:-2] + "2" + PGBlock.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:{}, end_node:{}".format(
start_node, end_node)
start_node = start_node[:-2] + "5" + PGBlock.DASH
end_node = end_node[:-2] + "6" + PGBlock.DASH
return Road(start_node, end_node)
@staticmethod
def is_out_direction_parking_space(road: Road):
start_node = road.start_node
end_node = road.end_node
assert (start_node[-2] == "1" and end_node[-2] == "2") or (
start_node[-2] == "5"
and end_node[-2] == "6"), "{} to {} is not parking space".format(
start_node, end_node)
if start_node[-2] == "5" and end_node[-2] == "6":
return True
else:
return False
@staticmethod
def is_in_direction_parking_space(road: Road):
start_node = road.start_node
end_node = road.end_node
assert (start_node[-2] == "1" and end_node[-2] == "2") or (
start_node[-2] == "5"
and end_node[-2] == "6"), "{} to {} is not parking space".format(
start_node, end_node)
if start_node[-2] == "1" and end_node[-2] == "2":
return True
else:
return False
class BaseRunnable(Configurable, Nameable, Randomizable):
"""
Abstract class, all sub class must implement all methods to participate in the program running loop.
The BaseRunnable instance can be everything which don't interact with game engine.
If you need an element have visualization property or physics property, using BaseObject() instead
"""
PARAMETER_SPACE = ParameterSpace({})
def __init__(self, name=None, random_seed=None, config=None):
Nameable.__init__(self, name)
Randomizable.__init__(self, random_seed)
Configurable.__init__(
self, {k: None
for k in self.PARAMETER_SPACE.parameters})
# Parameter check
assert isinstance(
self.PARAMETER_SPACE, ParameterSpace
), "Using PGSpace to define parameter spaces of " + self.class_name
self.sample_parameters()
# use external config update to overwrite sampled parameters, except None
self.update_config(copy.copy(config), allow_add_new_key=True)
def get_state(self) -> Dict:
"""
Store current state, for example if this runnable instance is an object in the 3D-world state can be heading,
position, etc. This function can be used to to store the movement and change history trajectory.
:return: state dict
"""
raise NotImplementedError
def set_state(self, state: Dict):
"""
Set state for this runnable instance, restore the instance to a certain state, For example, if this runnable
instance is a policy, it can restore the policy to a certain state to make sure it do the same decision as
before
:param state: dict
"""
raise NotImplementedError
def before_step(self, *args, **kwargs):
"""
Do Information fusion and then analyze and wait for decision
"""
pass
def set_action(self, *args, **kwargs):
"""
Set action for this object, and the action will last for the minimal simulation interval
"""
raise NotImplementedError
def step(self, *args, **kwargs):
"""
Call this function to implement the decision set by set_action() for a period of time. This function is usually
useless, since the result of action, mostly force, is calculated bu game engine via force calculation respect to
time. However some runnable instances who don't belong to the physics world and their actions are not force need
to implement this function to get the action accumulated result respect to time.
"""
pass
def after_step(self, *args, **kwargs):
"""
After advancing all objects for a time period, their state should be updated for statistic or other purpose
"""
def reset(self, random_seed, *args, **kwargs):
"""
Call this function to re-init objects. Since some __init__ process of creating objects is redundant, reset can
help us reuse this object by resetting some necessary attributes
"""
self.__init__(random_seed=random_seed, *args, **kwargs)
def sample_parameters(self):
"""
Fix a value of the random parameters in PARAMETER_SPACE
"""
random_seed = self.np_random.randint(low=0, high=int(1e6))
self.PARAMETER_SPACE.seed(random_seed)
ret = self.PARAMETER_SPACE.sample()
self.update_config(ret)
def destroy(self):
Configurable.destroy(self)
class TollGate(PGBlock):
"""
Toll, like Straight, but has speed limit
"""
SOCKET_NUM = 1
PARAMETER_SPACE = ParameterSpace(BlockParameterSpace.BOTTLENECK_PARAMETER)
ID = "$"
SPEED_LIMIT = 3 # m/s ~= 5 miles per hour https://bestpass.com/feed/61-speeding-through-tolls
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]
self.BUILDING_LENGTH = length
basic_lane = self.positive_basic_lane
new_lane = ExtendStraightLane(basic_lane, length, [LineType.CONTINUOUS, 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,
center_line_color=LineColor.YELLOW,
center_line_type=LineType.CONTINUOUS,
inner_lane_line_type=LineType.CONTINUOUS,
side_lane_line_type=LineType.SIDE,
ignore_intersection_checking=self.ignore_intersection_checking
)
# create negative road
no_cross = CreateAdverseRoad(
socket,
self.block_network,
self._global_network,
center_line_color=LineColor.YELLOW,
center_line_type=LineType.CONTINUOUS,
inner_lane_line_type=LineType.CONTINUOUS,
side_lane_line_type=LineType.SIDE,
ignore_intersection_checking=self.ignore_intersection_checking
) and no_cross
self.add_sockets(PGBlockSocket(socket, _socket))
self._add_building_and_speed_limit(socket)
self._add_building_and_speed_limit(_socket)
return no_cross
def _add_building_and_speed_limit(self, road):
# add house
lanes = road.get_lanes(self.block_network)
for idx, lane in enumerate(lanes):
lane.set_speed_limit(self.SPEED_LIMIT)
if idx % 2 == 1:
# add toll
position = lane.position(lane.length / 2, 0)
building = get_engine().spawn_object(
TollGateBuilding, lane=lane, position=position, heading=lane.heading_at(0)
)
self.dynamic_nodes.append(building.body)
self._block_objects.append(building)
class FirstPGBlock(PGBlock):
"""
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 = ParameterSpace({})
ID = "I"
SOCKET_NUM = 1
ENTRANCE_LENGTH = 10
def __init__(self,
global_network: RoadNetwork,
lane_width: float,
lane_num: int,
render_root_np: NodePath,
physics_world: PhysicsWorld,
length: float = 50,
ignore_intersection_checking=False):
place_holder = PGBlockSocket(Road(Decoration.start, Decoration.end),
Road(Decoration.start, Decoration.end))
super(FirstPGBlock, self).__init__(
0,
place_holder,
global_network,
random_seed=0,
ignore_intersection_checking=ignore_intersection_checking)
assert length > self.ENTRANCE_LENGTH, (length, self.ENTRANCE_LENGTH)
self._block_objects = []
basic_lane = StraightLane(
[0, lane_width * (lane_num - 1)],
[self.ENTRANCE_LENGTH, lane_width * (lane_num - 1)],
line_types=(LineType.BROKEN, LineType.SIDE),
width=lane_width)
ego_v_spawn_road = Road(self.NODE_1, self.NODE_2)
CreateRoadFrom(
basic_lane,
lane_num,
ego_v_spawn_road,
self.block_network,
self._global_network,
ignore_intersection_checking=self.ignore_intersection_checking)
CreateAdverseRoad(
ego_v_spawn_road,
self.block_network,
self._global_network,
ignore_intersection_checking=self.ignore_intersection_checking)
next_lane = ExtendStraightLane(basic_lane,
length - self.ENTRANCE_LENGTH,
[LineType.BROKEN, LineType.SIDE])
other_v_spawn_road = Road(self.NODE_2, self.NODE_3)
CreateRoadFrom(
next_lane,
lane_num,
other_v_spawn_road,
self.block_network,
self._global_network,
ignore_intersection_checking=self.ignore_intersection_checking)
CreateAdverseRoad(
other_v_spawn_road,
self.block_network,
self._global_network,
ignore_intersection_checking=self.ignore_intersection_checking)
self._create_in_world()
# global_network += self.block_network
global_network.add(self.block_network)
socket = self.create_socket_from_positive_road(other_v_spawn_road)
socket.set_index(self.name, 0)
self.add_sockets(socket)
self.attach_to_world(render_root_np, physics_world)
self._respawn_roads = [other_v_spawn_road]
class Roundabout(PGBlock):
"""
roundabout class, the example is the same as Intersection
"""
ID = "O"
PARAMETER_SPACE = ParameterSpace(BlockParameterSpace.ROUNDABOUT)
SOCKET_NUM = 3
RADIUS_IN = 20
ANGLE = 60
EXIT_PART_LENGTH = 30
def __init__(self, *args, **kwargs):
super(Roundabout, self).__init__(*args, **kwargs)
self.intermediate_spawn_places = []
def _try_plug_into_previous_block(self) -> bool:
self.intermediate_spawn_places = []
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,
ignore_intersection_checking=self.
ignore_intersection_checking) 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,
ignore_intersection_checking=self.ignore_intersection_checking
) 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,
ignore_intersection_checking=self.ignore_intersection_checking
) and none_cross
self.intermediate_spawn_places.append(
segment_road.get_lanes(self.block_network))
# 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,
ignore_intersection_checking=self.ignore_intersection_checking
) 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,
ignore_intersection_checking=self.ignore_intersection_checking
) 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,
ignore_intersection_checking=self.ignore_intersection_checking)
# 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) -> PGBlockSocket:
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
def get_intermediate_spawn_lanes(self):
"""Filter out half of the vehicles."""
return self.get_respawn_lanes() + self.intermediate_spawn_places
class InterSection(PGBlock):
"""
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 = ParameterSpace(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,
ignore_intersection_checking=self.
ignore_intersection_checking) and no_cross
no_cross = CreateAdverseRoad(
exit_road,
self.block_network,
self._global_network,
ignore_intersection_checking=self.
ignore_intersection_checking) and no_cross
socket = PGBlockSocket(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,
ignore_intersection_checking=self.ignore_intersection_checking)
) 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,
ignore_intersection_checking=self.ignore_intersection_checking)
intersect_nodes.rotate(-1)
right_straight.line_types = [LineType.BROKEN, LineType.SIDE]
return right_straight, non_cross
def get_socket(self, index: int) -> PGBlockSocket:
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,
ignore_intersection_checking=self.ignore_intersection_checking)
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,
ignore_intersection_checking=self.ignore_intersection_checking)
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,
ignore_intersection_checking=self.ignore_intersection_checking)
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,
ignore_intersection_checking=self.ignore_intersection_checking)
def add_u_turn(self, enable_u_turn: bool):
self.enable_u_turn = enable_u_turn
def get_intermediate_spawn_lanes(self):
"""Override this function for intersection so that we won't spawn vehicles in the center of intersection."""
respawn_lanes = self.get_respawn_lanes()
return respawn_lanes
class BaseVehicle(BaseObject, BaseVehicleState):
"""
Vehicle chassis and its wheels index
0 1
II-----II
II-----II
|
| <---chassis/wheelbase
|
II-----II
2 3
"""
COLLISION_MASK = CollisionGroup.Vehicle
PARAMETER_SPACE = ParameterSpace(VehicleParameterSpace.BASE_VEHICLE)
MAX_LENGTH = 10
MAX_WIDTH = 2.5
MAX_STEERING = 60
LENGTH = None
WIDTH = None
HEIGHT = None
TIRE_RADIUS = None
LATERAL_TIRE_TO_CENTER = None
FRONT_WHEELBASE = None
REAR_WHEELBASE = None
MASS = None
CHASSIS_TO_WHEEL_AXIS = 0.2
SUSPENSION_LENGTH = 40
SUSPENSION_STIFFNESS = 30
# for random color choosing
MATERIAL_COLOR_COEFF = 10 # to resist other factors, since other setting may make color dark
MATERIAL_METAL_COEFF = 1 # 0-1
MATERIAL_ROUGHNESS = 0.8 # smaller to make it more smooth, and reflect more light
MATERIAL_SHININESS = 1 # 0-128 smaller to make it more smooth, and reflect more light
MATERIAL_SPECULAR_COLOR = (3, 3, 3, 3)
# control
STEERING_INCREMENT = 0.05
# save memory, load model once
model_collection = {}
path = None
def __init__(
self,
vehicle_config: Union[dict, Config] = None,
name: str = None,
random_seed=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 vehicle_config: mostly, vehicle module config
:param random_seed: int
"""
# check
assert vehicle_config is not None, "Please specify the vehicle config."
assert engine_initialized(
), "Please make sure game engine is successfully initialized!"
# NOTE: it is the game engine, not vehicle drivetrain
self.engine = get_engine()
BaseObject.__init__(self, name, random_seed,
self.engine.global_config["vehicle_config"])
BaseVehicleState.__init__(self)
self.update_config(vehicle_config)
am_i_the_special_one = self.config["am_i_the_special_one"]
# build vehicle physics model
vehicle_chassis = self._create_vehicle_chassis()
self.add_body(vehicle_chassis.getChassis())
self.system = vehicle_chassis
self.chassis = self.origin
self.wheels = self._create_wheel()
# powertrain config
self.increment_steering = self.config["increment_steering"]
self.enable_reverse = self.config["enable_reverse"]
self.max_speed = self.config["max_speed"]
self.max_steering = self.config["max_steering"]
# visualization
color = sns.color_palette("colorblind")
idx = get_np_random().randint(len(color))
rand_c = color[idx]
if am_i_the_special_one:
rand_c = color[2] # A pretty green
self.top_down_color = (rand_c[0] * 255, rand_c[1] * 255,
rand_c[2] * 255)
self.panda_color = rand_c
self._add_visualization()
# modules, get observation by using these modules
self.lane: Optional[AbstractLane] = None
self.lane_index = None
self.navigation: Optional[Navigation] = None
self.lidar: Optional[Lidar] = None # detect surrounding vehicles
self.side_detector: Optional[SideDetector] = None # detect road side
self.lane_line_detector: Optional[
LaneLineDetector] = None # detect nearest lane lines
self.image_sensors = {}
# 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 = (0, 0)
self.last_heading_dir = self.heading
self.dist_to_left_side = None
self.dist_to_right_side = None
# step info
self.out_of_route = None
self.on_lane = None
self.spawn_place = (0, 0)
self._init_step_info()
# others
self._add_modules_for_vehicle()
self.takeover = False
self.expert_takeover = False
self.energy_consumption = 0
self.action_space = self.get_action_space_before_init(
extra_action_dim=self.config["extra_action_dim"])
self.break_down = False
# overtake_stat
self.front_vehicles = set()
self.back_vehicles = set()
if self.engine.current_map is not None:
self.reset()
def _add_modules_for_vehicle(self, ):
config = self.config
# add routing module
self.add_navigation() # default added
# add distance detector/lidar
self.side_detector = SideDetector(
config["side_detector"]["num_lasers"],
config["side_detector"]["distance"],
self.engine.global_config["vehicle_config"]["show_side_detector"])
self.lane_line_detector = LaneLineDetector(
config["lane_line_detector"]["num_lasers"],
config["lane_line_detector"]["distance"],
self.engine.global_config["vehicle_config"]
["show_lane_line_detector"])
self.lidar = Lidar(
config["lidar"]["num_lasers"], config["lidar"]["distance"],
self.engine.global_config["vehicle_config"]["show_lidar"])
# vision modules
self.add_image_sensor("rgb_camera", RGBCamera())
self.add_image_sensor("mini_map", MiniMap())
self.add_image_sensor("depth_camera", DepthCamera())
def _init_step_info(self):
# done info will be initialized every frame
self.init_state_info()
self.out_of_route = False # re-route is required if is false
self.on_lane = True # on lane surface or not
def _preprocess_action(self, action):
if self.config["action_check"]:
assert self.action_space.contains(
action
), "Input {} is not compatible with action space {}!".format(
action, self.action_space)
return action, {'raw_action': (action[0], action[1])}
def before_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_action(action)
return step_info
def after_step(self):
if self.navigation is not None:
self.lane, self.lane_index, = self.navigation.update_localization(
self)
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,
random_seed=None,
vehicle_config=None,
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 random_seed is not None:
self.seed(random_seed)
self.sample_parameters()
if vehicle_config is not None:
self.update_config(vehicle_config)
map = self.engine.current_map
if pos is None:
lane = map.road_network.get_lane(self.config["spawn_lane_index"])
pos = lane.position(self.config["spawn_longitude"],
self.config["spawn_lateral"])
heading = np.rad2deg(
lane.heading_at(self.config["spawn_longitude"]))
self.spawn_place = pos
heading = -np.deg2rad(heading) - np.pi / 2
self.set_static(False)
self.origin.setPos(panda_position(Vec3(*pos, self.HEIGHT / 2 + 1)))
self.origin.setQuat(
LQuaternionf(math.cos(heading / 2), 0, 0, math.sin(heading / 2)))
self.update_map_info(map)
self.body.clearForces()
self.body.setLinearVelocity(Vec3(0, 0, 0))
self.body.setAngularVelocity(Vec3(0, 0, 0))
self.system.resetSuspension()
self._apply_throttle_brake(0.0)
# 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()
assert self.navigation
"""------------------------------------------- act -------------------------------------------------"""
def _set_action(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.config["max_engine_force"]
max_brake_force = self.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.navigation.current_ref_lanes[0]
_, lateral_to_reference = current_reference_lane.local_coordinates(
self.position)
lateral_to_left = lateral_to_reference + self.navigation.get_current_lane_width(
) / 2
lateral_to_right = self.navigation.get_current_lateral_range(
self.position, self.engine) - lateral_to_left
return lateral_to_left, lateral_to_right
@property
def position(self):
return pgdrive_position(self.origin.getPos())
@property
def speed(self):
"""
km/h
"""
velocity = self.body.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 = Vector((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.origin.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
elif isinstance(target_lane, WayPointLane):
lane_segment = target_lane.segment(
target_lane.local_coordinates(self.position)[0])
lateral = lane_segment["lateral_direction"]
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.navigation 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 _create_vehicle_chassis(self):
self.LENGTH = type(self).LENGTH
self.WIDTH = type(self).WIDTH
self.HEIGHT = type(self).HEIGHT
assert self.LENGTH < BaseVehicle.MAX_LENGTH, "Vehicle is too large!"
assert self.WIDTH < BaseVehicle.MAX_WIDTH, "Vehicle is too large!"
chassis = BaseRigidBodyNode(self.name, BodyName.Vehicle)
chassis.setIntoCollideMask(CollisionGroup.Vehicle)
chassis_shape = BulletBoxShape(
Vec3(self.WIDTH / 2, self.LENGTH / 2, self.HEIGHT / 2))
ts = TransformState.makePos(Vec3(0, 0, self.HEIGHT / 2))
chassis.addShape(chassis_shape, ts)
chassis.setMass(self.MASS)
chassis.setDeactivationEnabled(False)
chassis.notifyCollisions(
True
) # advance collision check, do callback in pg_collision_callback
physics_world = get_engine().physics_world
vehicle_chassis = BulletVehicle(physics_world.dynamic_world, chassis)
vehicle_chassis.setCoordinateSystem(ZUp)
self.dynamic_nodes.append(vehicle_chassis)
return vehicle_chassis
def _add_visualization(self):
if self.render:
[path, scale, x_y_z_offset,
H] = self.path[self.np_random.randint(0, len(self.path))]
if path not in BaseVehicle.model_collection:
car_model = self.loader.loadModel(
AssetLoader.file_path("models", path))
BaseVehicle.model_collection[path] = car_model
else:
car_model = BaseVehicle.model_collection[path]
car_model.setScale(scale)
car_model.setH(H)
car_model.setPos(x_y_z_offset)
car_model.setZ(-self.TIRE_RADIUS - self.CHASSIS_TO_WHEEL_AXIS +
x_y_z_offset[-1])
car_model.instanceTo(self.origin)
if self.config["random_color"]:
material = Material()
material.setBaseColor(
(self.panda_color[0] * self.MATERIAL_COLOR_COEFF,
self.panda_color[1] * self.MATERIAL_COLOR_COEFF,
self.panda_color[2] * self.MATERIAL_COLOR_COEFF, 0.2))
material.setMetallic(self.MATERIAL_METAL_COEFF)
material.setSpecular(self.MATERIAL_SPECULAR_COLOR)
material.setRefractiveIndex(1.5)
material.setRoughness(self.MATERIAL_ROUGHNESS)
material.setShininess(self.MATERIAL_SHININESS)
material.setTwoside(False)
self.origin.setMaterial(material, True)
def _create_wheel(self):
f_l = self.FRONT_WHEELBASE
r_l = -self.REAR_WHEELBASE
lateral = self.LATERAL_TIRE_TO_CENTER
axis_height = self.TIRE_RADIUS - self.CHASSIS_TO_WHEEL_AXIS
radius = self.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.origin.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))
wheel.setWheelRadius(radius)
wheel.setMaxSuspensionTravelCm(self.SUSPENSION_LENGTH)
wheel.setSuspensionStiffness(self.SUSPENSION_STIFFNESS)
wheel.setWheelsDampingRelaxation(4.8)
wheel.setWheelsDampingCompression(1.2)
wheel.setFrictionSlip(self.config["wheel_friction"])
wheel.setRollInfluence(0.5)
return wheel
def add_image_sensor(self, name: str, sensor: ImageBuffer):
self.image_sensors[name] = sensor
def add_navigation(self):
self.navigation = Navigation(
self.engine,
show_navi_mark=self.engine.global_config["vehicle_config"]
["show_navi_mark"],
random_navi_mark_color=self.engine.global_config["vehicle_config"]
["random_navi_mark_color"],
show_dest_mark=self.engine.global_config["vehicle_config"]
["show_dest_mark"],
show_line_to_dest=self.engine.global_config["vehicle_config"]
["show_line_to_dest"])
def update_map_info(self, map):
"""
Update map info after reset()
:param map: new map
:return: None
"""
possible_lanes = ray_localization(self.heading,
self.spawn_place,
self.engine,
return_all_result=True)
possible_lane_indexes = [
lane_index for lane, lane_index, dist in possible_lanes
]
try:
idx = possible_lane_indexes.index(self.config["spawn_lane_index"])
except ValueError:
lane, new_l_index = possible_lanes[0][:-1]
else:
lane, new_l_index = possible_lanes[idx][:-1]
dest = self.config["destination_node"]
self.navigation.update(
map,
current_lane_index=new_l_index,
final_road_node=dest if dest is not None else None,
random_seed=self.engine.global_random_seed)
assert lane is not None, "spawn place is not on road!"
self.navigation.update_localization(self)
self.lane_index = new_l_index
self.lane = lane
def _state_check(self):
"""
Check States and filter to update info
"""
result_1 = self.engine.physics_world.static_world.contactTest(
self.chassis.node(), True)
result_2 = self.engine.physics_world.dynamic_world.contactTest(
self.chassis.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.Vehicle)
if name[0] == BodyName.White_continuous_line:
self.on_white_continuous_line = True
elif name[0] == BodyName.Yellow_continuous_line:
self.on_yellow_continuous_line = True
elif name[0] == BodyName.Broken_line:
self.on_broken_line = True
else:
# didn't add
continue
contacts.add(name[0])
# side walk detect
res = rect_region_detection(self.engine, self.position,
np.rad2deg(self.heading_theta),
self.LENGTH, self.WIDTH,
CollisionGroup.Sidewalk)
if res.hasHit() and res.getNode().getName() == BodyName.Sidewalk:
self.crash_sidewalk = True
contacts.add(BodyName.Sidewalk)
self.contact_results = contacts
def destroy(self):
super(BaseVehicle, self).destroy()
self.navigation.destroy()
self.navigation = None
if self.side_detector is not None:
self.side_detector.destroy()
self.side_detector = None
if self.lane_line_detector is not None:
self.lane_line_detector.destroy()
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.image_sensors = None
self.engine = 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.origin.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.origin.setH((panda_heading(heading_theta) * 180 / np.pi) - 90)
def get_state(self):
final_road = self.navigation.final_road
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,
"speed":
self.speed,
"spawn_road":
self.config["spawn_lane_index"][:-1],
"destination": (final_road.start_node, final_road.end_node)
}
def set_state(self, state: dict):
self.set_heading(state["heading"])
self.set_position(state["position"])
self._replay_done = state["done"]
self.set_position(state["position"], height=0.28)
def _update_overtake_stat(self):
if self.config["overtake_stat"] and self.lidar.available:
surrounding_vs = self.lidar.get_surrounding_vehicles()
routing = self.navigation
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,
discrete_action=False,
steering_dim=5,
throttle_dim=5):
if not discrete_action:
return gym.spaces.Box(-1.0,
1.0,
shape=(2 + extra_action_dim, ),
dtype=np.float32)
else:
return gym.spaces.MultiDiscrete([steering_dim, throttle_dim])
def __del__(self):
super(BaseVehicle, self).__del__()
self.engine = None
self.lidar = None
self.mini_map = None
self.rgb_camera = None
self.navigation = None
self.wheels = None
@property
def arrive_destination(self):
long, lat = self.navigation.final_lane.local_coordinates(self.position)
flag = (self.navigation.final_lane.length - 5 < long <
self.navigation.final_lane.length + 5) and (
self.navigation.get_current_lane_width() / 2 >= lat >=
(0.5 - self.navigation.get_current_lane_num()) *
self.navigation.get_current_lane_width())
return flag
def set_static(self, flag):
self.body.setStatic(flag)
@property
def reference_lanes(self):
return self.navigation.current_ref_lanes
def set_wheel_friction(self, new_friction):
raise DeprecationWarning("Bug exists here")
for wheel in self.wheels:
wheel.setFrictionSlip(new_friction)
@property
def overspeed(self):
return True if self.lane.speed_limit < self.speed else False
@property
def replay_done(self):
return self._replay_done if hasattr(self, "_replay_done") else (
self.crash_building or self.crash_vehicle or
# self.on_white_continuous_line or
self.on_yellow_continuous_line)
@property
def current_action(self):
return self.last_current_action[-1]
@property
def last_current(self):
return self.last_current_action[0]
def detach_from_world(self, physics_world):
self.navigation.detach_from_world()
if self.lidar is not None:
self.lidar.detach_from_world()
if self.side_detector is not None:
self.side_detector.detach_from_world()
if self.lane_line_detector is not None:
self.lane_line_detector.detach_from_world()
super(BaseVehicle, self).detach_from_world(physics_world)
def attach_to_world(self, parent_node_path, physics_world):
if self.config["show_navi_mark"]:
self.navigation.attach_to_world(self.engine)
if self.lidar is not None and self.config["show_lidar"]:
self.lidar.attach_to_world(self.engine)
if self.side_detector is not None and self.config["show_side_detector"]:
self.side_detector.attach_to_world(self.engine)
if self.lane_line_detector is not None and self.config[
"show_lane_line_detector"]:
self.lane_line_detector.attach_to_world(self.engine)
super(BaseVehicle, self).attach_to_world(parent_node_path,
physics_world)
def set_break_down(self, break_down=True):
self.break_down = break_down
class TInterSection(InterSection):
"""
A structure like X Intersection, code copied from it mostly
"""
ID = "T"
SOCKET_NUM = 2
PARAMETER_SPACE = ParameterSpace(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[PGBlockSocket.get_real_index(self.name, t_type)].negative_road.end_node
end_node = self._sockets[PGBlockSocket.get_real_index(self.name, t_type)].positive_road.start_node
for i in range(4):
if i == t_type:
continue
index_i = PGBlockSocket.get_real_index(self.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(PGBlockSocket.get_real_index(self.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: PGBlockSocket):
assert isinstance(socket, PGBlockSocket), "Socket list only accept BlockSocket Type"
# mute warning in T interection
# if socket.index is not None and not socket.index.startswith(self.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.name, self.get_socket_indices())
# )
if socket.index is None:
# if this socket is self block socket
socket.set_index(self.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")
