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 _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 get_negative_lanes(self):
"""
In order to remain the lane index, ret is a 2-dim array structure like like [Road_lanes[lane_1, lane_2]]
"""
ret = []
for _from, _to_dict in self.graph:
for _to, lanes in _to_dict:
road = Road(_from, _to)
if road.is_negative_road() and road.is_valid_road():
ret.append(lanes)
return ret
def reset(self):
"""
Generate an accident scene or construction scene on block
:return: None
"""
self.accident_lanes = []
engine = get_engine()
accident_prob = self.accident_prob
if abs(accident_prob - 0.0) < 1e-2:
return
for block in engine.current_map.blocks:
if type(block) not in [Straight, Curve, InRampOnStraight, OutRampOnStraight]:
# blocks with exists do not generate accident scene
continue
if self.np_random.rand() > accident_prob:
# prob filter
continue
road_1 = Road(block.pre_block_socket.positive_road.end_node, block.road_node(0, 0))
road_2 = Road(block.road_node(0, 0), block.road_node(0, 1)) if not isinstance(block, Straight) else None
if self.np_random.rand() > self.PROHIBIT_SCENE_PROB:
accident_road = self.np_random.choice([road_1, road_2]) if not isinstance(block, Curve) else road_2
accident_road = road_1 if accident_road is None else accident_road
is_ramp = isinstance(block, InRampOnStraight) or isinstance(block, OutRampOnStraight)
on_left = True if self.np_random.rand() > 0.5 or (accident_road is road_2 and is_ramp) else False
accident_lane_idx = 0 if on_left else -1
lane = accident_road.get_lanes(engine.current_map.road_network)[accident_lane_idx]
longitude = lane.length - self.ACCIDENT_AREA_LEN
lateral_len = engine.current_map.config[engine.current_map.LANE_WIDTH]
lane = engine.current_map.road_network.get_lane(accident_road.lane_index(accident_lane_idx))
self.accident_lanes += accident_road.get_lanes(engine.current_map.road_network)
self.prohibit_scene(lane, longitude, lateral_len, on_left)
else:
accident_road = self.np_random.choice([road_1, road_2])
accident_road = road_1 if accident_road is None else accident_road
is_ramp = isinstance(block, InRampOnStraight) or isinstance(block, OutRampOnStraight)
on_left = True if self.np_random.rand() > 0.5 or (accident_road is road_2 and is_ramp) else False
lanes = accident_road.get_lanes(engine.current_map.road_network)
accident_lane_idx = self.np_random.randint(0, len(lanes) - 1) if on_left else -1
lane = lanes[accident_lane_idx]
longitude = self.np_random.rand() * lane.length / 2 + lane.length / 2
if self.np_random.rand() > 0.5:
self.break_down_scene(lane, longitude)
else:
self.barrier_scene(lane, longitude)
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 __init__(self, map_config: dict = None, random_seed=None):
"""
Map can be stored and recover to save time when we access the map encountered before
"""
assert random_seed is None
# assert random_seed == map_config[
# self.SEED
# ], "Global seed {} should equal to seed in map config {}".format(random_seed, map_config[self.SEED])
super(BaseMap, self).__init__(config=map_config)
self.film_size = (get_global_config()["draw_map_resolution"],
get_global_config()["draw_map_resolution"])
self.road_network = RoadNetwork()
# A flatten representation of blocks, might cause chaos in city-level generation.
self.blocks = []
# Generate map and insert blocks
self.engine = get_engine()
self._generate()
assert self.blocks, "The generate methods does not fill blocks!"
# a trick to optimize performance
self.road_network.after_init()
self.spawn_roads = [Road(FirstPGBlock.NODE_2, FirstPGBlock.NODE_3)]
self.detach_from_world()
def replay_frame(self, target_vehicles, last_step=False):
assert self.restore_episode_info is not None, "No frame data in episode info"
if len(self.restore_episode_info) == 0:
return True
frame = self.restore_episode_info.pop(-1)
vehicles_to_remove = []
for index, state in frame.items():
if index == TARGET_VEHICLES:
for t_v_idx, t_v_s in state.items():
agent_idx = self._init_obj_to_agent[t_v_idx]
vehicle_to_set = target_vehicles[agent_idx]
vehicle_to_set.set_state(t_v_s)
if vehicle_to_set.navigation.final_road != Road(
*t_v_s["destination"]):
vehicle_to_set.navigation.set_route(
t_v_s["spawn_road"], t_v_s["destination"][-1])
vehicle_to_set.after_step()
elif index == TRAFFIC_VEHICLES:
for t_v_idx, t_v_s in state.items():
vehicle_to_set = self.restore_vehicles[t_v_idx]
vehicle_to_set.set_state(t_v_s)
if t_v_s["done"] and not vehicle_to_set.enable_respawn:
vehicles_to_remove.append(vehicle_to_set)
if last_step:
for v in vehicles_to_remove:
v.destroy()
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
def update(self,
map: BaseMap,
current_lane_index,
final_road_node=None,
random_seed=None):
# TODO(pzh): We should not determine the destination of a vehicle in the navigation module.
start_road_node = current_lane_index[0]
self.map = map
if start_road_node is None:
start_road_node = FirstPGBlock.NODE_1
if final_road_node is None:
# auto find if PGMap
current_road_negative = Road(
*current_lane_index[:-1]).is_negative_road()
# choose first block when born on negative road
block = map.blocks[0] if current_road_negative else map.blocks[-1]
sockets = block.get_socket_list()
socket = get_np_random(random_seed).choice(sockets)
while True:
if not socket.is_socket_node(start_road_node) or len(
sockets) == 1:
break
else:
sockets.remove(socket)
if len(sockets) == 0:
raise ValueError("Can not set a destination!")
# choose negative road end node when current road is negative road
final_road_node = socket.negative_road.end_node if current_road_negative else socket.positive_road.end_node
self.set_route(current_lane_index, final_road_node)
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 create_socket_from_positive_road(road: Road) -> PGBlockSocket:
"""
We usually create road from positive road, thus this func can get socket easily.
Note: it is not recommended to generate socket from negative road
"""
assert road.start_node[0] != Road.NEGATIVE_DIR and road.end_node[0] != Road.NEGATIVE_DIR, \
"Socket can only be created from positive road"
positive_road = Road(road.start_node, road.end_node)
return PGBlockSocket(positive_road, -positive_road)
def update_destination_for(self, vehicle_id, vehicle_config):
# when agent re-joined to the game, call this to set the new route to destination
end_roads = copy.deepcopy(self.engine.global_config["in_spawn_roads"])
if Road(*vehicle_config["spawn_lane_index"][:-1]) in end_roads:
end_road = self.engine.spawn_manager.get_parking_space(vehicle_id)
else:
end_road = -self.np_random.choice(end_roads) # Use negative road!
vehicle_config["destination_node"] = end_road.end_node
return vehicle_config
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)
def set_route(self, current_lane_index: str, end_road_node: str):
"""
Find a shortest path from start road to end road
:param current_lane_index: start road node
:param end_road_node: end road node
:return: None
"""
start_road_node = current_lane_index[0]
self.checkpoints = self.map.road_network.shortest_path(
start_road_node, end_road_node)
self._target_checkpoints_index = [0, 1]
# update routing info
if len(self.checkpoints) <= 2:
self.checkpoints = [current_lane_index[0], current_lane_index[1]]
self._target_checkpoints_index = [0, 0]
assert len(self.checkpoints
) >= 2, "Can not find a route from {} to {}".format(
start_road_node, end_road_node)
self.final_road = Road(self.checkpoints[-2], self.checkpoints[-1])
final_lanes = self.final_road.get_lanes(self.map.road_network)
self.final_lane = final_lanes[-1]
self._navi_info.fill(0.0)
target_road_1_start = self.checkpoints[0]
target_road_1_end = self.checkpoints[1]
self.current_ref_lanes = self.map.road_network.graph[
target_road_1_start][target_road_1_end]
self.next_ref_lanes = self.map.road_network.graph[self.checkpoints[1]][
self.checkpoints[2]] if len(self.checkpoints) > 2 else None
self.current_road = Road(target_road_1_start, target_road_1_end)
self.next_road = Road(
self.checkpoints[1],
self.checkpoints[2]) if len(self.checkpoints) > 2 else None
if self._dest_node_path is not None:
ref_lane = final_lanes[0]
later_middle = (float(self.get_current_lane_num()) / 2 -
0.5) * self.get_current_lane_width()
check_point = ref_lane.position(ref_lane.length, later_middle)
self._dest_node_path.setPos(check_point[0], -check_point[1], 1.8)
def remove_all_roads(self, start_node: str, end_node: str):
"""
Remove all road between two road nodes
:param start_node: start node name
:param end_node: end node name
:return: roads removed
"""
ret = []
paths = self.bfs_paths(start_node, end_node)
for path in paths:
for next_idx, node in enumerate(path[:-1], 1):
road_removed = self.remove_road(Road(node, path[next_idx]))
ret += road_removed
return ret
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 _respawn_single_vehicle(self, randomize_position=False):
"""
Exclude destination parking space
"""
safe_places_dict = self.engine.spawn_manager.get_available_respawn_places(
self.current_map, randomize=randomize_position)
# ===== filter spawn places =====
filter_ret = {}
for id, config in safe_places_dict.items():
spawn_l_index = config["config"]["spawn_lane_index"]
spawn_road = Road(spawn_l_index[0], spawn_l_index[1])
if spawn_road in self.config["in_spawn_roads"]:
if len(self.engine.spawn_manager.parking_space_available) > 0:
filter_ret[id] = config
else:
# spawn in parking space
if ParkingLot.is_in_direction_parking_space(spawn_road):
# avoid sweep test bug
spawn_road = self.current_map.parking_lot.out_direction_parking_space(
spawn_road)
config["config"]["spawn_lane_index"] = (
spawn_road.start_node, spawn_road.end_node, 0)
if spawn_road in self.engine.spawn_manager.parking_space_available:
# not other vehicle's destination
filter_ret[id] = config
# ===== same as super() =====
safe_places_dict = filter_ret
if len(safe_places_dict) == 0 or not self.agent_manager.allow_respawn:
# No more run, just wait!
return None, None
assert len(safe_places_dict) > 0
bp_index = get_np_random(self._DEBUG_RANDOM_SEED).choice(
list(safe_places_dict.keys()), 1)[0]
new_spawn_place = safe_places_dict[bp_index]
new_agent_id, vehicle = self.agent_manager.propose_new_vehicle()
new_spawn_place_config = new_spawn_place["config"]
new_spawn_place_config = self.engine.spawn_manager.update_destination_for(
new_agent_id, new_spawn_place_config)
vehicle.config.update(new_spawn_place_config)
vehicle.reset()
vehicle.after_step()
self.dones[
new_agent_id] = False # Put it in the internal dead-tracking dict.
new_obs = self.observations[new_agent_id].observe(vehicle)
return new_agent_id, new_obs
def get_roads(self, *, direction="all", lane_num=None) -> List:
"""
Return all roads in road_network
:param direction: "positive"/"negative"
:param lane_num: only roads with lane_num lanes will be returned
:return: List[Road]
"""
assert direction in ["positive", "negative",
"all"], "incorrect road direction"
ret = []
for _from, _to_dict in self.graph.items():
if direction == "all" or (direction == "positive" and _from[0] !=
"-") or (direction == "negative"
and _from[0] == "-"):
for _to, lanes in _to_dict.items():
if lane_num is None or len(lanes) == lane_num:
ret.append(Road(_from, _to))
return ret
def before_step(self):
"""
All traffic vehicles make driving decision here
:return: None
"""
# trigger vehicles
engine = self.engine
if self.mode != TrafficMode.Respawn:
for v in engine.agent_manager.active_objects.values():
ego_lane_idx = v.lane_index[:-1]
ego_road = Road(ego_lane_idx[0], ego_lane_idx[1])
if len(self.block_triggered_vehicles) > 0 and \
ego_road == self.block_triggered_vehicles[-1].trigger_road:
block_vehicles = self.block_triggered_vehicles.pop()
self._traffic_vehicles += block_vehicles.vehicles
for v in self._traffic_vehicles:
p = self.engine.get_policy(v.name)
v.before_step(p.act())
return dict()
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 _sample_topology(self) -> bool:
for lane in self.argo_lanes:
self.block_network.add_road(Road(lane.start_node, lane.end_node),
[lane])
return True
class Navigation:
navigation_info_dim = 10
NAVI_POINT_DIST = 50
PRE_NOTIFY_DIST = 40
MIN_ALPHA = 0.15
CKPT_UPDATE_RANGE = 5
FORCE_CALCULATE = False
LINE_TO_DEST_HEIGHT = 0.6
def __init__(self,
engine,
show_navi_mark: bool = False,
random_navi_mark_color=False,
show_dest_mark=False,
show_line_to_dest=False):
"""
This class define a helper for localizing vehicles and retrieving navigation information.
It now only support from first block start to the end node, but can be extended easily.
"""
self.map = None
self.final_road = None
self.final_lane = None
self.checkpoints = None
self.current_ref_lanes = None
self.next_ref_lanes = None
self.current_road = None
self.next_road = None
self._target_checkpoints_index = None
self._navi_info = np.zeros(
(self.navigation_info_dim, )) # navi information res
# Vis
self._show_navi_info = (
engine.mode == RENDER_MODE_ONSCREEN
and not engine.global_config["debug_physics_world"])
self.origin = engine.render.attachNewNode(
"navigation_sign") if self._show_navi_info else None
self.navi_mark_color = (
0.6, 0.8,
0.5) if not random_navi_mark_color else get_np_random().rand(3)
self.navi_arrow_dir = None
self._dest_node_path = None
self._goal_node_path = None
self._line_to_dest = None
self._show_line_to_dest = show_line_to_dest
if self._show_navi_info:
# nodepath
self._line_to_dest = self.origin.attachNewNode("line")
self._goal_node_path = self.origin.attachNewNode("target")
self._dest_node_path = self.origin.attachNewNode("dest")
if show_navi_mark:
navi_point_model = AssetLoader.loader.loadModel(
AssetLoader.file_path("models", "box.bam"))
navi_point_model.reparentTo(self._goal_node_path)
if show_dest_mark:
dest_point_model = AssetLoader.loader.loadModel(
AssetLoader.file_path("models", "box.bam"))
dest_point_model.reparentTo(self._dest_node_path)
if show_line_to_dest:
line_seg = LineSegs("line_to_dest")
line_seg.setColor(self.navi_mark_color[0],
self.navi_mark_color[1],
self.navi_mark_color[2], 0.7)
line_seg.setThickness(2)
self._dynamic_line_np = NodePath(line_seg.create(True))
self._dynamic_line_np.reparentTo(self.origin)
self._line_to_dest = line_seg
self._goal_node_path.setTransparency(TransparencyAttrib.M_alpha)
self._dest_node_path.setTransparency(TransparencyAttrib.M_alpha)
self._goal_node_path.setColor(self.navi_mark_color[0],
self.navi_mark_color[1],
self.navi_mark_color[2], 0.7)
self._dest_node_path.setColor(self.navi_mark_color[0],
self.navi_mark_color[1],
self.navi_mark_color[2], 0.7)
self._goal_node_path.hide(CamMask.AllOn)
self._dest_node_path.hide(CamMask.AllOn)
self._goal_node_path.show(CamMask.MainCam)
self._dest_node_path.show(CamMask.MainCam)
logging.debug("Load Vehicle Module: {}".format(
self.__class__.__name__))
def update(self,
map: BaseMap,
current_lane_index,
final_road_node=None,
random_seed=None):
# TODO(pzh): We should not determine the destination of a vehicle in the navigation module.
start_road_node = current_lane_index[0]
self.map = map
if start_road_node is None:
start_road_node = FirstPGBlock.NODE_1
if final_road_node is None:
# auto find if PGMap
current_road_negative = Road(
*current_lane_index[:-1]).is_negative_road()
# choose first block when born on negative road
block = map.blocks[0] if current_road_negative else map.blocks[-1]
sockets = block.get_socket_list()
socket = get_np_random(random_seed).choice(sockets)
while True:
if not socket.is_socket_node(start_road_node) or len(
sockets) == 1:
break
else:
sockets.remove(socket)
if len(sockets) == 0:
raise ValueError("Can not set a destination!")
# choose negative road end node when current road is negative road
final_road_node = socket.negative_road.end_node if current_road_negative else socket.positive_road.end_node
self.set_route(current_lane_index, final_road_node)
def set_route(self, current_lane_index: str, end_road_node: str):
"""
Find a shortest path from start road to end road
:param current_lane_index: start road node
:param end_road_node: end road node
:return: None
"""
start_road_node = current_lane_index[0]
self.checkpoints = self.map.road_network.shortest_path(
start_road_node, end_road_node)
self._target_checkpoints_index = [0, 1]
# update routing info
if len(self.checkpoints) <= 2:
self.checkpoints = [current_lane_index[0], current_lane_index[1]]
self._target_checkpoints_index = [0, 0]
assert len(self.checkpoints
) >= 2, "Can not find a route from {} to {}".format(
start_road_node, end_road_node)
self.final_road = Road(self.checkpoints[-2], self.checkpoints[-1])
final_lanes = self.final_road.get_lanes(self.map.road_network)
self.final_lane = final_lanes[-1]
self._navi_info.fill(0.0)
target_road_1_start = self.checkpoints[0]
target_road_1_end = self.checkpoints[1]
self.current_ref_lanes = self.map.road_network.graph[
target_road_1_start][target_road_1_end]
self.next_ref_lanes = self.map.road_network.graph[self.checkpoints[1]][
self.checkpoints[2]] if len(self.checkpoints) > 2 else None
self.current_road = Road(target_road_1_start, target_road_1_end)
self.next_road = Road(
self.checkpoints[1],
self.checkpoints[2]) if len(self.checkpoints) > 2 else None
if self._dest_node_path is not None:
ref_lane = final_lanes[0]
later_middle = (float(self.get_current_lane_num()) / 2 -
0.5) * self.get_current_lane_width()
check_point = ref_lane.position(ref_lane.length, later_middle)
self._dest_node_path.setPos(check_point[0], -check_point[1], 1.8)
def update_localization(self, ego_vehicle):
position = ego_vehicle.position
lane, lane_index = self.get_current_lane(ego_vehicle)
if lane is None:
lane, lane_index = ego_vehicle.lane, ego_vehicle.lane_index
ego_vehicle.on_lane = False
if self.FORCE_CALCULATE:
lane_index, _ = self.map.road_network.get_closest_lane_index(
position)
lane = self.map.road_network.get_lane(lane_index)
long, _ = lane.local_coordinates(position)
self._update_target_checkpoints(lane_index, long)
assert len(self.checkpoints) >= 2
target_road_1_start = self.checkpoints[
self._target_checkpoints_index[0]]
target_road_1_end = self.checkpoints[self._target_checkpoints_index[0]
+ 1]
target_road_2_start = self.checkpoints[
self._target_checkpoints_index[1]]
target_road_2_end = self.checkpoints[self._target_checkpoints_index[1]
+ 1]
target_lanes_1 = self.map.road_network.graph[target_road_1_start][
target_road_1_end]
target_lanes_2 = self.map.road_network.graph[target_road_2_start][
target_road_2_end]
self.current_ref_lanes = target_lanes_1
self.current_road = Road(target_road_1_start, target_road_1_end)
if target_road_1_start == target_road_2_start:
self.next_road = None
self.next_ref_lanes = None
else:
self.next_road = Road(target_road_2_start, target_road_2_end)
self.next_ref_lanes = target_lanes_2
self._navi_info.fill(0.0)
half = self.navigation_info_dim // 2
self._navi_info[:
half], lanes_heading1, checkpoint = self._get_info_for_checkpoint(
lanes_id=0,
lanes=target_lanes_1,
ego_vehicle=ego_vehicle #, print_info=False
)
self._navi_info[
half:], lanes_heading2, _ = self._get_info_for_checkpoint(
lanes_id=1,
lanes=target_lanes_2,
ego_vehicle=ego_vehicle #, print_info=True
)
if self._show_navi_info:
pos_of_goal = checkpoint
self._goal_node_path.setPos(pos_of_goal[0], -pos_of_goal[1], 1.8)
self._goal_node_path.setH(self._goal_node_path.getH() + 3)
self.navi_arrow_dir = [lanes_heading1, lanes_heading2]
dest_pos = self._dest_node_path.getPos()
self._draw_line_to_dest(engine=ego_vehicle.engine,
start_position=ego_vehicle.position,
end_position=(dest_pos[0], -dest_pos[1]))
return lane, lane_index
def _get_info_for_checkpoint(self,
lanes_id,
lanes,
ego_vehicle,
print_info=False):
ref_lane = lanes[0]
later_middle = (float(self.get_current_lane_num()) / 2 -
0.5) * self.get_current_lane_width()
check_point = ref_lane.position(ref_lane.length, later_middle)
if lanes_id == 0:
# calculate ego v lane heading
lanes_heading = ref_lane.heading_at(
ref_lane.local_coordinates(ego_vehicle.position)[0])
else:
lanes_heading = ref_lane.heading_at(
min(self.PRE_NOTIFY_DIST, ref_lane.length))
dir_vec = check_point - ego_vehicle.position
dir_norm = norm(dir_vec[0], dir_vec[1])
if dir_norm > self.NAVI_POINT_DIST:
dir_vec = dir_vec / dir_norm * self.NAVI_POINT_DIST
proj_heading, proj_side = ego_vehicle.projection(dir_vec)
bendradius = 0.0
dir = 0.0
angle = 0.0
if isinstance(ref_lane, CircularLane):
bendradius = ref_lane.radius / (
BlockParameterSpace.CURVE[Parameter.radius].max +
self.get_current_lane_num() * self.get_current_lane_width())
dir = ref_lane.direction
if dir == 1:
angle = ref_lane.end_phase - ref_lane.start_phase
elif dir == -1:
angle = ref_lane.start_phase - ref_lane.end_phase
ret = []
ret.append(
clip((proj_heading / self.NAVI_POINT_DIST + 1) / 2, 0.0, 1.0))
ret.append(clip((proj_side / self.NAVI_POINT_DIST + 1) / 2, 0.0, 1.0))
ret.append(clip(bendradius, 0.0, 1.0))
ret.append(clip((dir + 1) / 2, 0.0, 1.0))
ret.append(
clip((np.rad2deg(angle) /
BlockParameterSpace.CURVE[Parameter.angle].max + 1) / 2, 0.0,
1.0))
# if print_info:
# print("[2ND] Distance to this navigation: {:.3f}, Angle: {:.3f}. Return value: {}".format(
# norm(proj_heading, proj_side),
# np.rad2deg(np.arctan2(proj_side, proj_heading)),
# ret
# ))
# else:
# print("[1ND] Distance to this navigation: {:.3f}, Angle: {:.3f}. Return value: {}".format(
# norm(proj_heading, proj_side),
# np.rad2deg(np.arctan2(proj_side, proj_heading)),
# ret
# ))
return ret, lanes_heading, check_point
def _update_target_checkpoints(self, ego_lane_index, ego_lane_longitude):
"""
Return should_update: True or False
"""
if self._target_checkpoints_index[0] == self._target_checkpoints_index[
1]: # on last road
return
# arrive to second checkpoint
current_road_start_point = ego_lane_index[0]
if current_road_start_point in self.checkpoints[self._target_checkpoints_index[1]:] \
and ego_lane_longitude < self.CKPT_UPDATE_RANGE:
if current_road_start_point not in self.checkpoints[
self._target_checkpoints_index[1]:-1]:
return
idx = self.checkpoints.index(current_road_start_point,
self._target_checkpoints_index[1], -1)
self._target_checkpoints_index = [idx]
if idx + 1 == len(self.checkpoints) - 1:
self._target_checkpoints_index.append(idx)
else:
self._target_checkpoints_index.append(idx + 1)
return
return
def get_navi_info(self):
return self._navi_info
def destroy(self):
if self._show_navi_info:
try:
self._line_to_dest.removeNode()
except AttributeError:
pass
self._dest_node_path.removeNode()
self._goal_node_path.removeNode()
self.next_road = None
self.current_road = None
self.next_ref_lanes = None
self.current_ref_lanes = None
def set_force_calculate_lane_index(self, force: bool):
self.FORCE_CALCULATE = force
def __del__(self):
logging.debug("{} is destroyed".format(self.__class__.__name__))
def get_current_lateral_range(self, current_position, engine) -> float:
"""Return the maximum lateral distance from left to right."""
# special process for special block
try:
current_block_id = self.current_road.block_ID()
except AttributeError:
return self.get_current_lane_width() * self.get_current_lane_num()
if current_block_id == Split.ID or current_block_id == Merge.ID:
left_lane = self.current_ref_lanes[0]
assert isinstance(
left_lane,
StraightLane), "Reference lane should be straight lane here"
long, lat = left_lane.local_coordinates(current_position)
current_position = left_lane.position(long, -left_lane.width / 2)
return self._ray_lateral_range(
engine, current_position,
self.current_ref_lanes[0].direction_lateral)
else:
return self.get_current_lane_width() * self.get_current_lane_num()
def get_current_lane_width(self) -> float:
return self.map._config[self.map.LANE_WIDTH]
def get_current_lane_num(self) -> float:
return len(self.current_ref_lanes)
def get_current_lane(self, ego_vehicle):
possible_lanes = ray_localization(ego_vehicle.heading,
ego_vehicle.position,
ego_vehicle.engine,
return_all_result=True)
for lane, index, l_1_dist in possible_lanes:
if lane in self.current_ref_lanes:
return lane, index
nx_ckpt = self._target_checkpoints_index[-1]
if nx_ckpt == self.checkpoints[-1] or self.next_road is None:
return possible_lanes[0][:-1] if len(possible_lanes) > 0 else (
None, None)
nx_nx_ckpt = nx_ckpt + 1
next_ref_lanes = self.map.road_network.graph[
self.checkpoints[nx_ckpt]][self.checkpoints[nx_nx_ckpt]]
for lane, index, l_1_dist in possible_lanes:
if lane in next_ref_lanes:
return lane, index
return possible_lanes[0][:-1] if len(possible_lanes) > 0 else (None,
None)
def _ray_lateral_range(self, engine, start_position, dir, length=50):
"""
It is used to measure the lateral range of special blocks
:param start_position: start_point
:param dir: ray direction
:param length: length of ray
:return: lateral range [m]
"""
end_position = start_position[0] + dir[0] * length, start_position[
1] + dir[1] * length
start_position = panda_position(start_position, z=0.15)
end_position = panda_position(end_position, z=0.15)
mask = FirstPGBlock.CONTINUOUS_COLLISION_MASK
res = engine.physics_world.static_world.rayTestClosest(start_position,
end_position,
mask=mask)
if not res.hasHit():
return length
else:
return res.getHitFraction() * length
def _draw_line_to_dest(self, engine, start_position, end_position):
if not self._show_line_to_dest:
return
line_seg = self._line_to_dest
line_seg.moveTo(
panda_position(start_position, self.LINE_TO_DEST_HEIGHT))
line_seg.drawTo(panda_position(end_position, self.LINE_TO_DEST_HEIGHT))
self._dynamic_line_np.removeNode()
self._dynamic_line_np = NodePath(line_seg.create(False))
self._dynamic_line_np.hide(CamMask.Shadow | CamMask.RgbCam)
self._dynamic_line_np.reparentTo(self.origin)
def detach_from_world(self):
if isinstance(self.origin, NodePath):
self.origin.detachNode()
def attach_to_world(self, engine):
if isinstance(self.origin, NodePath):
self.origin.reparentTo(engine.render)
def current_road(self):
return Road(*self.lane_index[0:-1])
def _try_plug_into_previous_block(self) -> bool:
no_cross = True
sin_angle = math.sin(np.deg2rad(self.ANGLE))
cos_angle = math.cos(np.deg2rad(self.ANGLE))
longitude_len = sin_angle * self.RADIUS * 2 + cos_angle * self.CONNECT_PART_LEN + self.RAMP_LEN + self.EXTRA_LEN
self.set_part_idx(0)
# part 0 road 0
dec_lane_len = self.get_config()[Parameter.length]
dec_lane = ExtendStraightLane(
self.positive_basic_lane, dec_lane_len + self.lane_width,
[self.positive_basic_lane.line_types[0], LineType.SIDE]
)
dec_road = Road(self.pre_block_socket.positive_road.end_node, self.add_road_node())
no_cross = CreateRoadFrom(
dec_lane,
self.positive_lane_num,
dec_road,
self.block_network,
self._global_network,
side_lane_line_type=LineType.CONTINUOUS,
ignore_intersection_checking=self.ignore_intersection_checking
) and no_cross
no_cross = CreateAdverseRoad(
dec_road,
self.block_network,
self._global_network,
ignore_intersection_checking=self.ignore_intersection_checking
) and no_cross
dec_right_lane = dec_road.get_lanes(self.block_network)[-1]
left_line_type = LineType.CONTINUOUS if self.positive_lane_num == 1 else LineType.BROKEN
dec_right_lane.line_types = [left_line_type, LineType.NONE]
# part 0 road 1
extend_lane = ExtendStraightLane(
dec_right_lane, longitude_len, [dec_right_lane.line_types[0], LineType.CONTINUOUS]
)
extend_road = Road(dec_road.end_node, self.add_road_node())
no_cross = CreateRoadFrom(
extend_lane,
self.positive_lane_num,
extend_road,
self.block_network,
self._global_network,
side_lane_line_type=LineType.CONTINUOUS,
ignore_intersection_checking=self.ignore_intersection_checking
) and no_cross
no_cross = CreateAdverseRoad(
extend_road,
self.block_network,
self._global_network,
ignore_intersection_checking=self.ignore_intersection_checking
) and no_cross
_extend_road = -extend_road
_extend_road.get_lanes(self.block_network)[-1].line_types = [
LineType.NONE if self.positive_lane_num == 1 else LineType.BROKEN, LineType.SIDE
]
self.add_sockets(self.create_socket_from_positive_road(extend_road))
# part 1 road 0
self.set_part_idx(1)
dec_side_right_lane = self._get_deacc_lane(dec_right_lane)
self.block_network.add_lane(dec_road.start_node, dec_road.end_node, dec_side_right_lane)
no_cross = (
not check_lane_on_road(
self._global_network,
dec_side_right_lane,
0.95,
ignore_intersection_checking=self.ignore_intersection_checking
)
) and no_cross
bend_1, connect_part = create_bend_straight(
dec_side_right_lane,
self.CONNECT_PART_LEN,
self.RADIUS,
np.deg2rad(self.ANGLE),
True,
self.lane_width,
self.LANE_TYPE,
speed_limit=self.SPEED_LIMIT
)
bend_1_road = Road(dec_road.end_node, self.add_road_node())
connect_road = Road(bend_1_road.end_node, self.add_road_node())
self.block_network.add_lane(bend_1_road.start_node, bend_1_road.end_node, bend_1)
self.block_network.add_lane(connect_road.start_node, connect_road.end_node, connect_part)
no_cross = (
not check_lane_on_road(
self._global_network, bend_1, 0.95, ignore_intersection_checking=self.ignore_intersection_checking
)
) and no_cross
no_cross = (
not check_lane_on_road(
self._global_network,
connect_part,
0.95,
ignore_intersection_checking=self.ignore_intersection_checking
)
) and no_cross
bend_2, straight_part = create_bend_straight(
connect_part,
self.RAMP_LEN,
self.RADIUS,
np.deg2rad(self.ANGLE),
False,
self.lane_width,
self.LANE_TYPE,
speed_limit=self.SPEED_LIMIT
)
bend_2_road = Road(connect_road.end_node, self.add_road_node()) # end at part1 road 0, extend road
straight_road = Road(bend_2_road.end_node, self.add_road_node())
self.block_network.add_lane(bend_2_road.start_node, bend_2_road.end_node, bend_2)
self.block_network.add_lane(straight_road.start_node, straight_road.end_node, straight_part)
no_cross = (
not check_lane_on_road(
self._global_network, bend_2, 0.95, ignore_intersection_checking=self.ignore_intersection_checking
)
) and no_cross
no_cross = (
not check_lane_on_road(
self._global_network,
straight_part,
0.95,
ignore_intersection_checking=self.ignore_intersection_checking
)
) and no_cross
decoration_part = self._get_merge_part(dec_side_right_lane)
self.block_network.add_lane(Decoration.start, Decoration.end, decoration_part)
return no_cross
def update_localization(self, ego_vehicle):
position = ego_vehicle.position
lane, lane_index = self.get_current_lane(ego_vehicle)
if lane is None:
lane, lane_index = ego_vehicle.lane, ego_vehicle.lane_index
ego_vehicle.on_lane = False
if self.FORCE_CALCULATE:
lane_index, _ = self.map.road_network.get_closest_lane_index(
position)
lane = self.map.road_network.get_lane(lane_index)
long, _ = lane.local_coordinates(position)
self._update_target_checkpoints(lane_index, long)
assert len(self.checkpoints) >= 2
target_road_1_start = self.checkpoints[
self._target_checkpoints_index[0]]
target_road_1_end = self.checkpoints[self._target_checkpoints_index[0]
+ 1]
target_road_2_start = self.checkpoints[
self._target_checkpoints_index[1]]
target_road_2_end = self.checkpoints[self._target_checkpoints_index[1]
+ 1]
target_lanes_1 = self.map.road_network.graph[target_road_1_start][
target_road_1_end]
target_lanes_2 = self.map.road_network.graph[target_road_2_start][
target_road_2_end]
self.current_ref_lanes = target_lanes_1
self.current_road = Road(target_road_1_start, target_road_1_end)
if target_road_1_start == target_road_2_start:
self.next_road = None
self.next_ref_lanes = None
else:
self.next_road = Road(target_road_2_start, target_road_2_end)
self.next_ref_lanes = target_lanes_2
self._navi_info.fill(0.0)
half = self.navigation_info_dim // 2
self._navi_info[:
half], lanes_heading1, checkpoint = self._get_info_for_checkpoint(
lanes_id=0,
lanes=target_lanes_1,
ego_vehicle=ego_vehicle #, print_info=False
)
self._navi_info[
half:], lanes_heading2, _ = self._get_info_for_checkpoint(
lanes_id=1,
lanes=target_lanes_2,
ego_vehicle=ego_vehicle #, print_info=True
)
if self._show_navi_info:
pos_of_goal = checkpoint
self._goal_node_path.setPos(pos_of_goal[0], -pos_of_goal[1], 1.8)
self._goal_node_path.setH(self._goal_node_path.getH() + 3)
self.navi_arrow_dir = [lanes_heading1, lanes_heading2]
dest_pos = self._dest_node_path.getPos()
self._draw_line_to_dest(engine=ego_vehicle.engine,
start_position=ego_vehicle.position,
end_position=(dest_pos[0], -dest_pos[1]))
return lane, lane_index
def _try_plug_into_previous_block(self) -> bool:
acc_lane_len = self.get_config()[Parameter.length]
no_cross = True
# extend road and acc raod part, part 0
self.set_part_idx(0)
sin_angle = math.sin(np.deg2rad(self.ANGLE))
cos_angle = math.cos(np.deg2rad(self.ANGLE))
longitude_len = sin_angle * self.RADIUS * 2 + cos_angle * self.CONNECT_PART_LEN + self.RAMP_LEN
extend_lane = ExtendStraightLane(
self.positive_basic_lane, longitude_len + self.EXTRA_PART, [LineType.BROKEN, LineType.CONTINUOUS]
)
extend_road = Road(self.pre_block_socket.positive_road.end_node, self.add_road_node())
no_cross = CreateRoadFrom(
extend_lane,
self.positive_lane_num,
extend_road,
self.block_network,
self._global_network,
side_lane_line_type=LineType.CONTINUOUS,
ignore_intersection_checking=self.ignore_intersection_checking
) and no_cross
extend_road.get_lanes(self.block_network)[-1].line_types = [
LineType.BROKEN if self.positive_lane_num != 1 else LineType.CONTINUOUS, LineType.CONTINUOUS
]
no_cross = CreateAdverseRoad(
extend_road,
self.block_network,
self._global_network,
ignore_intersection_checking=self.ignore_intersection_checking
) and no_cross
_extend_road = -extend_road
left_lane_line = LineType.NONE if self.positive_lane_num == 1 else LineType.BROKEN
_extend_road.get_lanes(self.block_network)[-1].line_types = [left_lane_line, LineType.SIDE]
# main acc part
acc_side_lane = ExtendStraightLane(
extend_lane, acc_lane_len + self.lane_width, [extend_lane.line_types[0], LineType.SIDE]
)
acc_road = Road(extend_road.end_node, self.add_road_node())
no_cross = CreateRoadFrom(
acc_side_lane,
self.positive_lane_num,
acc_road,
self.block_network,
self._global_network,
side_lane_line_type=LineType.CONTINUOUS,
ignore_intersection_checking=self.ignore_intersection_checking
) and no_cross
no_cross = CreateAdverseRoad(
acc_road,
self.block_network,
self._global_network,
ignore_intersection_checking=self.ignore_intersection_checking
) and no_cross
left_line_type = LineType.CONTINUOUS if self.positive_lane_num == 1 else LineType.BROKEN
acc_road.get_lanes(self.block_network)[-1].line_types = [left_line_type, LineType.BROKEN]
# socket part
socket_side_lane = ExtendStraightLane(acc_side_lane, self.SOCKET_LEN, acc_side_lane.line_types)
socket_road = Road(acc_road.end_node, self.add_road_node())
no_cross = CreateRoadFrom(
socket_side_lane,
self.positive_lane_num,
socket_road,
self.block_network,
self._global_network,
side_lane_line_type=LineType.CONTINUOUS,
ignore_intersection_checking=self.ignore_intersection_checking
) and no_cross
no_cross = CreateAdverseRoad(
socket_road,
self.block_network,
self._global_network,
ignore_intersection_checking=self.ignore_intersection_checking
) and no_cross
self.add_sockets(PGBlock.create_socket_from_positive_road(socket_road))
# ramp part, part 1
self.set_part_idx(1)
lateral_dist = (1 - cos_angle) * self.RADIUS * 2 + sin_angle * self.CONNECT_PART_LEN
end_point = extend_lane.position(self.EXTRA_PART + self.RAMP_LEN, lateral_dist + self.lane_width)
start_point = extend_lane.position(self.EXTRA_PART, lateral_dist + self.lane_width)
straight_part = StraightLane(
start_point, end_point, self.lane_width, self.LANE_TYPE, speed_limit=self.SPEED_LIMIT
)
straight_road = Road(self.add_road_node(), self.add_road_node())
self.block_network.add_lane(straight_road.start_node, straight_road.end_node, straight_part)
no_cross = (
not check_lane_on_road(
self._global_network,
straight_part,
0.95,
ignore_intersection_checking=self.ignore_intersection_checking
)
) and no_cross
self.add_respawn_roads(straight_road)
# p1 road 0, 1
bend_1, connect_part = create_bend_straight(
straight_part,
self.CONNECT_PART_LEN,
self.RADIUS,
np.deg2rad(self.ANGLE),
False,
self.lane_width,
self.LANE_TYPE,
speed_limit=self.SPEED_LIMIT
)
bend_1_road = Road(straight_road.end_node, self.add_road_node())
connect_road = Road(bend_1_road.end_node, self.add_road_node())
self.block_network.add_lane(bend_1_road.start_node, bend_1_road.end_node, bend_1)
self.block_network.add_lane(connect_road.start_node, connect_road.end_node, connect_part)
no_cross = (
not check_lane_on_road(
self._global_network, bend_1, 0.95, ignore_intersection_checking=self.ignore_intersection_checking
)
) and no_cross
no_cross = (
not check_lane_on_road(
self._global_network,
connect_part,
0.95,
ignore_intersection_checking=self.ignore_intersection_checking
)
) and no_cross
# p1, road 2, 3
bend_2, acc_lane = create_bend_straight(
connect_part,
acc_lane_len,
self.RADIUS,
np.deg2rad(self.ANGLE),
True,
self.lane_width,
self.LANE_TYPE,
speed_limit=self.SPEED_LIMIT
)
acc_lane.line_types = [LineType.BROKEN, LineType.CONTINUOUS]
bend_2_road = Road(connect_road.end_node, self.road_node(0, 0)) # end at part1 road 0, extend road
self.block_network.add_lane(bend_2_road.start_node, bend_2_road.end_node, bend_2)
self.block_network.add_lane(acc_road.start_node, acc_road.end_node, acc_lane)
no_cross = (
not check_lane_on_road(
self._global_network, bend_2, 0.95, ignore_intersection_checking=self.ignore_intersection_checking
)
) and no_cross
no_cross = (
not check_lane_on_road(
self._global_network, acc_lane, 0.95, ignore_intersection_checking=self.ignore_intersection_checking
)
) and no_cross
# p1, road 4, small circular to decorate
merge_lane, _ = create_bend_straight(
acc_lane, 10, self.lane_width / 2, np.pi / 2, False, self.lane_width,
(LineType.BROKEN, LineType.CONTINUOUS)
)
self.block_network.add_lane(Decoration.start, Decoration.end, merge_lane)
return no_cross
import logging
from pgdrive.component.blocks.first_block import FirstPGBlock
from pgdrive.component.blocks.parking_lot import ParkingLot
from pgdrive.component.blocks.t_intersection import TInterSection
from pgdrive.component.map.pg_map import PGMap
from pgdrive.component.road.road import Road
from pgdrive.envs.marl_envs.marl_inout_roundabout import LidarStateObservationMARound
from pgdrive.envs.marl_envs.multi_agent_pgdrive import MultiAgentPGDrive, panda_replay
from pgdrive.manager.spawn_manager import SpawnManager
from pgdrive.obs.observation_base import ObservationBase
from pgdrive.utils import get_np_random, Config
MAParkingLotConfig = dict(
in_spawn_roads=[
Road(FirstPGBlock.NODE_2, FirstPGBlock.NODE_3),
-Road(TInterSection.node(2, 0, 0), TInterSection.node(2, 0, 1)),
-Road(TInterSection.node(2, 2, 0), TInterSection.node(2, 2, 1)),
],
out_spawn_roads=None, # auto fill
spawn_roads=None, # auto fill
num_agents=10,
parking_space_num=8,
map_config=dict(exit_length=20, lane_num=1),
top_down_camera_initial_x=80,
top_down_camera_initial_y=0,
top_down_camera_initial_z=120,
vehicle_config={
"enable_reverse": True,
"random_navi_mark_color": True,
"show_dest_mark": True,
def _try_plug_into_previous_block(self) -> bool:
no_cross = True
parameters = self.get_config()
self.BOTTLENECK_LEN = parameters["bottle_len"]
lane_num_changed = parameters[Parameter.lane_num]
start_ndoe = self.pre_block_socket.positive_road.end_node
straight_lane_num = int(self.positive_lane_num - lane_num_changed)
straight_lane_num = max(1, straight_lane_num)
circular_lane_num = self.positive_lane_num - straight_lane_num
# part 1, straight road 0
basic_lane = self.positive_lanes[straight_lane_num - 1]
ref_lane = ExtendStraightLane(basic_lane, self.BOTTLENECK_LEN,
[LineType.NONE, LineType.NONE])
straight_road = Road(start_ndoe, self.road_node(0, 0))
no_cross = CreateRoadFrom(ref_lane,
straight_lane_num,
straight_road,
self.block_network,
self._global_network,
center_line_type=LineType.CONTINUOUS,
side_lane_line_type=LineType.NONE,
inner_lane_line_type=LineType.NONE,
ignore_intersection_checking=self.
ignore_intersection_checking) and no_cross
no_cross = CreateAdverseRoad(straight_road,
self.block_network,
self._global_network,
inner_lane_line_type=LineType.NONE,
side_lane_line_type=LineType.NONE,
center_line_type=LineType.CONTINUOUS,
ignore_intersection_checking=self.
ignore_intersection_checking) and no_cross
# extend for socket ,part 1 road 1
ref_lane = ExtendStraightLane(ref_lane, parameters[Parameter.length],
[LineType.NONE, LineType.NONE])
socket_road = Road(self.road_node(0, 0), self.road_node(0, 1))
no_cross = CreateRoadFrom(ref_lane,
straight_lane_num,
socket_road,
self.block_network,
self._global_network,
center_line_type=LineType.CONTINUOUS,
side_lane_line_type=LineType.SIDE,
inner_lane_line_type=LineType.BROKEN,
ignore_intersection_checking=self.
ignore_intersection_checking) and no_cross
no_cross = CreateAdverseRoad(socket_road,
self.block_network,
self._global_network,
inner_lane_line_type=LineType.BROKEN,
side_lane_line_type=LineType.SIDE,
center_line_type=LineType.CONTINUOUS,
ignore_intersection_checking=self.
ignore_intersection_checking) and no_cross
negative_sockect_road = -socket_road
self.add_sockets(PGBlockSocket(socket_road, negative_sockect_road))
# part 2, circular part
for index, lane in enumerate(self.positive_lanes[straight_lane_num:],
1):
lateral_dist = index * self.lane_width / 2
inner_node = self.road_node(1, index)
side_line_type = LineType.SIDE if index == self.positive_lane_num - straight_lane_num else LineType.NONE
# positive part
circular_1, circular_2, _ = create_wave_lanes(
lane, lateral_dist, self.BOTTLENECK_LEN, 5, self.lane_width)
road_1 = Road(start_ndoe, inner_node)
no_cross = CreateRoadFrom(
circular_1,
1,
road_1,
self.block_network,
self._global_network,
center_line_type=LineType.NONE,
side_lane_line_type=side_line_type,
inner_lane_line_type=LineType.NONE,
ignore_intersection_checking=self.ignore_intersection_checking
) and no_cross
road_2 = Road(inner_node, self.road_node(0, 0))
no_cross = CreateRoadFrom(
circular_2,
1,
road_2,
self.block_network,
self._global_network,
center_line_type=LineType.NONE,
side_lane_line_type=side_line_type,
inner_lane_line_type=LineType.NONE,
ignore_intersection_checking=self.ignore_intersection_checking
) and no_cross
# adverse part
lane = negative_sockect_road.get_lanes(self.block_network)[-1]
circular_2, circular_1, _ = create_wave_lanes(
lane, lateral_dist, self.BOTTLENECK_LEN, 5, self.lane_width,
False)
road_2 = -road_2
no_cross = CreateRoadFrom(
circular_2,
1,
road_2,
self.block_network,
self._global_network,
center_line_type=LineType.NONE,
side_lane_line_type=side_line_type,
inner_lane_line_type=LineType.NONE,
ignore_intersection_checking=self.ignore_intersection_checking
) and no_cross
road_1 = -road_1
no_cross = CreateRoadFrom(
circular_1,
1,
road_1,
self.block_network,
self._global_network,
center_line_type=LineType.NONE,
side_lane_line_type=side_line_type,
inner_lane_line_type=LineType.NONE,
ignore_intersection_checking=self.ignore_intersection_checking
) and no_cross
return no_cross
import copy
from pgdrive.manager.spawn_manager import SpawnManager
from pgdrive.component.blocks.first_block import FirstPGBlock
from pgdrive.component.blocks.intersection import InterSection
from pgdrive.component.map.pg_map import PGMap
from pgdrive.component.road.road import Road
from pgdrive.envs.marl_envs.marl_inout_roundabout import LidarStateObservationMARound
from pgdrive.envs.marl_envs.multi_agent_pgdrive import MultiAgentPGDrive
from pgdrive.obs.observation_base import ObservationBase
from pgdrive.utils import get_np_random, Config
MAIntersectionConfig = dict(spawn_roads=[
Road(FirstPGBlock.NODE_2, FirstPGBlock.NODE_3),
-Road(InterSection.node(1, 0, 0), InterSection.node(1, 0, 1)),
-Road(InterSection.node(1, 1, 0), InterSection.node(1, 1, 1)),
-Road(InterSection.node(1, 2, 0), InterSection.node(1, 2, 1)),
],
num_agents=30,
map_config=dict(exit_length=60, lane_num=2),
top_down_camera_initial_x=80,
top_down_camera_initial_y=0,
top_down_camera_initial_z=120)
class MAIntersectionMap(PGMap):
def _generate(self):
length = self.config["exit_length"]
parent_node_path, physics_world = self.engine.worldNP, self.engine.physics_world
assert len(
def _get_out_spawn_roads(parking_space_num):
ret = []
for i in range(1, parking_space_num + 1):
ret.append(Road(ParkingLot.node(1, i, 5), ParkingLot.node(1, i,
6)))
return ret
