def _try_plug_into_previous_block(self) -> bool:
acc_lane_len = self.get_config()[Parameter.length]
no_cross = True
fork_lane_num = self.get_config()[Parameter.lane_num] + 1
# extend road and acc raod part, part 0
self.set_part_idx(0)
sin_angle = np.sin(np.deg2rad(self.ANGLE))
cos_angle = np.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.STRIPED, 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) and no_cross
no_cross = CreateAdverseRoad(extend_road, self.block_network,
self._global_network) and no_cross
acc_side_lane = ExtendStraightLane(
extend_lane, acc_lane_len + self.lane_width,
[LineType.STRIPED, LineType.CONTINUOUS])
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) and no_cross
no_cross = CreateAdverseRoad(acc_road, self.block_network,
self._global_network) and no_cross
acc_road.get_lanes(self.block_network)[-1].line_types = [
LineType.STRIPED, LineType.STRIPED
]
self.add_sockets(Block.create_socket_from_positive_road(acc_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)) and no_cross
self.add_reborn_roads(straight_road)
# p1 road 0, 1
bend_1, connect_part = sharpbend(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 = CreateRoadFrom(bend_1, fork_lane_num, bend_1_road,
self.block_network, self._global_network,
False) and no_cross
no_cross = CreateRoadFrom(connect_part, fork_lane_num, connect_road,
self.block_network, self._global_network,
False) and no_cross
# p1, road 2, 3
bend_2, acc_lane = sharpbend(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.STRIPED, LineType.CONTINUOUS]
bend_2_road = Road(connect_road.end_node, self.road_node(
0, 0)) # end at part1 road 0, extend road
acc_road = Road(self.road_node(0, 0), self.road_node(0, 1))
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 = CreateRoadFrom(bend_2, fork_lane_num, bend_2_road,
self.block_network, self._global_network,
False) and no_cross
no_cross = CreateRoadFrom(acc_lane, fork_lane_num, acc_road,
self.block_network, self._global_network,
False) and no_cross
return no_cross
def _try_plug_into_previous_block(self) -> bool:
no_cross = True
sin_angle = np.sin(np.deg2rad(self.ANGLE))
cos_angle = np.cos(np.deg2rad(self.ANGLE))
longitude_len = sin_angle * self.RADIUS * 2 + cos_angle * self.CONNECT_PART_LEN + self.RAMP_LEN
self.set_part_idx(0)
# part 0 road 0
dec_lane_len = self.PARAMETER_SPACE.sample()[Parameter.length]
dec_lane = ExtendStraightLane(self.positive_basic_lane,
dec_lane_len + self.lane_width,
[LineType.STRIPED, LineType.CONTINUOUS])
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) and no_cross
no_cross = CreateAdverseRoad(dec_road, self.block_network,
self._global_network) and no_cross
dec_right_lane = dec_road.get_lanes(self.block_network)[-1]
dec_right_lane.line_types = [LineType.STRIPED, LineType.STRIPED]
# part 0 road 1
extend_lane = ExtendStraightLane(
dec_right_lane, longitude_len,
[LineType.STRIPED, 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) and no_cross
no_cross = CreateAdverseRoad(extend_road, self.block_network,
self._global_network) and no_cross
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, self.add_road_node(),
dec_side_right_lane)
no_cross = (not check_lane_on_road(
self._global_network, dec_side_right_lane, 0.95)) and no_cross
bend_1, connect_part = sharpbend(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(self.road_node(1, 0), 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)) and no_cross
no_cross = (not check_lane_on_road(self._global_network, connect_part,
0.95)) and no_cross
bend_2, straight_part = sharpbend(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)) and no_cross
no_cross = (not check_lane_on_road(self._global_network, straight_part,
0.95)) 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 _create_circular_part(self, road: Road, part_idx: int,
radius_exit: float, radius_inner: float,
angle: float) -> (str, str, StraightLane, bool):
"""
Create a part of roundabout according to a straight road
"""
none_cross = True
self.set_part_idx(part_idx)
radius_big = (self.positive_lane_num * 2 -
1) * self.lane_width + radius_inner
# circular part 0
segment_start_node = road.end_node
segment_end_node = self.add_road_node()
segment_road = Road(segment_start_node, segment_end_node)
lanes = road.get_lanes(
self._global_network) if part_idx == 0 else road.get_lanes(
self.block_network)
right_lane = lanes[-1]
bend, straight = sharpbend(right_lane, 10, radius_exit,
np.deg2rad(angle), True, self.lane_width,
(LineType.STRIPED, LineType.SIDE))
ignore_last_2_part_start = self.road_node((part_idx + 3) % 4, 0)
ignore_last_2_part_end = self.road_node((part_idx + 3) % 4, 0)
none_cross = CreateRoadFrom(
bend,
self.positive_lane_num,
segment_road,
self.block_network,
self._global_network,
ignore_start=ignore_last_2_part_start,
ignore_end=ignore_last_2_part_end) and none_cross
# set circular part 0 visualization
for k, lane in enumerate(segment_road.get_lanes(self.block_network)):
if k == self.positive_lane_num - 1:
lane.line_types = [LineType.NONE, LineType.SIDE]
else:
lane.line_types = [LineType.NONE, LineType.NONE]
# circular part 1
tool_lane_start = straight.position(-5, 0)
tool_lane_end = straight.position(0, 0)
tool_lane = StraightLane(tool_lane_start, tool_lane_end)
bend, straight_to_next_iter_part = sharpbend(
tool_lane, 10, radius_big, np.deg2rad(2 * angle - 90), False,
self.lane_width, (LineType.STRIPED, LineType.SIDE))
segment_start_node = segment_end_node
segment_end_node = self.add_road_node()
segment_road = Road(segment_start_node, segment_end_node)
none_cross = CreateRoadFrom(bend, self.positive_lane_num, segment_road,
self.block_network,
self._global_network) and none_cross
# circular part 2 and exit straight part
length = self.EXIT_PART_LENGTH
tool_lane_start = straight_to_next_iter_part.position(-5, 0)
tool_lane_end = straight_to_next_iter_part.position(0, 0)
tool_lane = StraightLane(tool_lane_start, tool_lane_end)
bend, straight = sharpbend(tool_lane, length, radius_exit,
np.deg2rad(angle), True, self.lane_width,
(LineType.STRIPED, LineType.SIDE))
segment_start_node = segment_end_node
segment_end_node = self.add_road_node(
) if part_idx < 3 else self._pre_block_socket.negative_road.start_node
segment_road = Road(segment_start_node, segment_end_node)
none_cross = CreateRoadFrom(bend, self.positive_lane_num, segment_road,
self.block_network,
self._global_network) and none_cross
# set circular part 2 (curve) visualization
for k, lane in enumerate(segment_road.get_lanes(self.block_network)):
if k == self.positive_lane_num - 1:
lane.line_types = [LineType.NONE, LineType.SIDE]
else:
lane.line_types = [LineType.NONE, LineType.NONE]
exit_start = segment_end_node
exit_end = self.add_road_node()
segment_road = Road(exit_start, exit_end)
if part_idx < 3:
none_cross = CreateRoadFrom(straight, self.positive_lane_num,
segment_road, self.block_network,
self._global_network) and none_cross
self.add_sockets(
self.create_socket_from_positive_road(segment_road))
# add circular part 3 at last
segment_start = self.road_node(part_idx, 1)
segment_end = self.road_node((part_idx + 1) % 4, 0)
segment_road = Road(segment_start, segment_end)
tool_lane_start = straight_to_next_iter_part.position(-6, 0)
tool_lane_end = straight_to_next_iter_part.position(0, 0)
tool_lane = StraightLane(tool_lane_start, tool_lane_end)
beneath = (self.positive_lane_num * 2 -
1) * self.lane_width / 2 + radius_exit
cos = np.cos(np.deg2rad(angle))
radius_this_seg = beneath / cos - radius_exit
bend, _ = sharpbend(tool_lane, 5, radius_this_seg,
np.deg2rad(180 - 2 * angle), False,
self.lane_width, (LineType.STRIPED, LineType.SIDE))
CreateRoadFrom(bend, self.positive_lane_num, segment_road,
self.block_network, self._global_network)
# set circular part 2 visualization
for k, lane in enumerate(segment_road.get_lanes(self.block_network)):
if k == 0:
if self.positive_lane_num > 1:
lane.line_types = [LineType.CONTINUOUS, LineType.STRIPED]
else:
lane.line_types = [LineType.CONTINUOUS, LineType.NONE]
else:
lane.line_types = [LineType.STRIPED, LineType.STRIPED]
return Road(exit_start, exit_end), none_cross
class RoutingLocalizationModule:
navigation_info_dim = 10
NAVI_POINT_DIST = 50
PRE_NOTIFY_DIST = 40
MARK_COLOR = COLLISION_INFO_COLOR["green"][1]
MIN_ALPHA = 0.15
CKPT_UPDATE_RANGE = 5
FORCE_CALCULATE = False
LINE_TO_DEST_HEIGHT = 0.6
def __init__(self,
pg_world,
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.checkpoints = None
self.final_lane = None
self.current_ref_lanes = None
self.current_road = None
self._target_checkpoints_index = None
self._navi_info = np.zeros(
(self.navigation_info_dim, )) # navi information res
self.navi_mark_color = (
0.6, 0.8,
0.5) if not random_navi_mark_color else get_np_random().rand(3)
# Vis
self._is_showing = True # store the state of navigation mark
self._show_navi_info = (
pg_world.mode == RENDER_MODE_ONSCREEN
and not pg_world.world_config["debug_physics_world"])
self._dest_node_path = None
self._goal_node_path = None
self._arrow_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 = pg_world.render.attachNewNode("line")
self._goal_node_path = pg_world.render.attachNewNode("target")
self._dest_node_path = pg_world.render.attachNewNode("dest")
self._arrow_node_path = pg_world.aspect2d.attachNewNode("arrow")
navi_arrow_model = AssetLoader.loader.loadModel(
AssetLoader.file_path("models", "navi_arrow.gltf"))
navi_arrow_model.setScale(0.1, 0.12, 0.2)
navi_arrow_model.setPos(2, 1.15, -0.221)
self._left_arrow = self._arrow_node_path.attachNewNode(
"left arrow")
self._left_arrow.setP(180)
self._right_arrow = self._arrow_node_path.attachNewNode(
"right arrow")
self._left_arrow.setColor(self.MARK_COLOR)
self._right_arrow.setColor(self.MARK_COLOR)
navi_arrow_model.instanceTo(self._left_arrow)
navi_arrow_model.instanceTo(self._right_arrow)
self._arrow_node_path.setPos(0, 0, 0.08)
self._arrow_node_path.hide(BitMask32.allOn())
self._arrow_node_path.show(CamMask.MainCam)
self._arrow_node_path.setQuat(
LQuaternionf(np.cos(-np.pi / 4), 0, 0, np.sin(-np.pi / 4)))
# the transparency attribute of gltf model is invalid on windows
# self._arrow_node_path.setTransparency(TransparencyAttrib.M_alpha)
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(pg_world.render)
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(BitMask32.allOn())
self._dest_node_path.hide(BitMask32.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: Map,
current_lane_index,
final_road_node=None,
random_seed=False):
start_road_node = current_lane_index[0]
self.map = map
if start_road_node is None:
start_road_node = FirstBlock.NODE_1
if final_road_node is None:
current_road_negative = Road(
*current_lane_index[:-1]).is_negative_road()
random_seed = random_seed if random_seed is not False else map.random_seed
# 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()
while True:
socket = get_np_random(random_seed).choice(sockets)
if not socket.is_socket_node(start_road_node):
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(start_road_node, final_road_node)
def set_route(self, start_road_node: str, end_road_node: str):
"""
Find a shorest path from start road to end road
:param start_road_node: start road node
:param end_road_node: end road node
:return: None
"""
self.checkpoints = self.map.road_network.shortest_path(
start_road_node, end_road_node)
assert len(
self.checkpoints) > 2, "Can not find a route from {} to {}".format(
start_road_node, end_road_node)
# update routing info
self.final_road = Road(self.checkpoints[-2], end_road_node)
final_lanes = self.final_road.get_lanes(self.map.road_network)
self.final_lane = final_lanes[-1]
self._target_checkpoints_index = [0, 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.current_road = Road(target_road_1_start, target_road_1_end)
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_navigation_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)
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)
self._navi_info[
half:], lanes_heading2, _ = self._get_info_for_checkpoint(
lanes_id=1, lanes=target_lanes_2, ego_vehicle=ego_vehicle)
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._update_navi_arrow([lanes_heading1, lanes_heading2])
dest_pos = self._dest_node_path.getPos()
self._draw_line_to_dest(pg_world=ego_vehicle.pg_world,
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):
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
return (clip((proj_heading / self.NAVI_POINT_DIST + 1) / 2, 0.0, 1.0),
clip((proj_side / self.NAVI_POINT_DIST + 1) / 2, 0.0,
1.0), clip(bendradius, 0.0,
1.0), clip((dir + 1) / 2, 0.0, 1.0),
clip((np.rad2deg(angle) /
BlockParameterSpace.CURVE[Parameter.angle].max + 1) / 2,
0.0, 1.0)), lanes_heading, check_point
def _update_navi_arrow(self, lanes_heading):
lane_0_heading = lanes_heading[0]
lane_1_heading = lanes_heading[1]
if abs(lane_0_heading - lane_1_heading) < 0.01:
if self._is_showing:
self._left_arrow.detachNode()
self._right_arrow.detachNode()
self._is_showing = False
else:
dir_0 = np.array(
[math.cos(lane_0_heading),
math.sin(lane_0_heading), 0])
dir_1 = np.array(
[math.cos(lane_1_heading),
math.sin(lane_1_heading), 0])
cross_product = np.cross(dir_1, dir_0)
left = False if cross_product[-1] < 0 else True
if not self._is_showing:
self._is_showing = True
if left:
if not self._left_arrow.hasParent():
self._left_arrow.reparentTo(self._arrow_node_path)
if self._right_arrow.hasParent():
self._right_arrow.detachNode()
else:
if not self._right_arrow.hasParent():
self._right_arrow.reparentTo(self._arrow_node_path)
if self._left_arrow.hasParent():
self._left_arrow.detachNode()
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 False
# 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 False
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 True
return False
def get_navi_info(self):
return self._navi_info
def destroy(self):
if self._show_navi_info:
self._arrow_node_path.removeNode()
self._line_to_dest.removeNode()
self._dest_node_path.removeNode()
self._goal_node_path.removeNode()
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, pg_world) -> float:
"""Return the maximum lateral distance from left to right."""
# special process for special block
current_block_id = self.current_road.block_ID()
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(
pg_world, 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(np.array(
ego_vehicle.heading.tolist()),
ego_vehicle.position,
ego_vehicle.pg_world,
return_all_result=True)
for lane, index, l_1_dist in possible_lanes:
if lane in self.current_ref_lanes:
return lane, index
return possible_lanes[0][:-1] if len(possible_lanes) > 0 else (None,
None)
def _ray_lateral_range(self, pg_world, 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 = BitMask32.bit(FirstBlock.CONTINUOUS_COLLISION_MASK)
res = pg_world.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, pg_world, 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(pg_world.render)
def _add_one_parking_space(
self, in_socket: BlockSocket, out_socket: BlockSocket, part_idx: int, radius, dist_to_in, dist_to_out
) -> bool:
no_cross = True
# lane into parking space and parking space, 1
if in_socket.is_same_socket(self.pre_block_socket) or in_socket.is_same_socket(
self.pre_block_socket.get_socket_in_reverse()):
net = self._global_network
else:
net = self.block_network
in_lane = in_socket.positive_road.get_lanes(net)[0]
start_node = in_socket.positive_road.end_node
if dist_to_in > 1e-3:
# a straight part will be added
in_lane = ExtendStraightLane(in_lane, dist_to_in, [LineType.NONE, LineType.NONE])
in_road = Road(in_socket.positive_road.end_node, self.road_node(part_idx, 0))
CreateRoadFrom(
in_lane,
self.positive_lane_num,
in_road,
self.block_network,
self._global_network,
center_line_type=LineType.NONE,
inner_lane_line_type=LineType.NONE,
side_lane_line_type=LineType.NONE
)
start_node = self.road_node(part_idx, 0)
bend, straight = create_bend_straight(
in_lane, self.parking_space_length, radius, self.ANGLE, True, self.parking_space_width
)
bend_road = Road(start_node, self.road_node(part_idx, 1))
bend_no_cross = CreateRoadFrom(
bend,
self.positive_lane_num,
bend_road,
self.block_network,
self._global_network,
center_line_type=LineType.NONE,
inner_lane_line_type=LineType.NONE,
side_lane_line_type=LineType.SIDE if dist_to_in < 1e-3 else LineType.NONE
)
if dist_to_in < 1e-3:
no_cross = no_cross and bend_no_cross
straight_road = Road(self.road_node(part_idx, 1), self.road_node(part_idx, 2))
self.dest_roads.append(straight_road)
no_cross = no_cross and CreateRoadFrom(
straight,
self.positive_lane_num,
straight_road,
self.block_network,
self._global_network,
center_line_type=LineType.CONTINUOUS,
inner_lane_line_type=LineType.NONE,
side_lane_line_type=LineType.SIDE if dist_to_in < 1e-3 else LineType.NONE,
center_line_color=LineColor.GREY
)
# lane into parking space and parking space, 2
neg_road: Road = out_socket.negative_road
if out_socket.is_same_socket(self.pre_block_socket) or out_socket.is_same_socket(
self.pre_block_socket.get_socket_in_reverse()):
net = self._global_network
else:
net = self.block_network
neg_lane = \
neg_road.get_lanes(net)[0]
start_node = neg_road.end_node
if dist_to_out > 1e-3:
# a straight part will be added
neg_lane = ExtendStraightLane(neg_lane, dist_to_out, [LineType.NONE, LineType.NONE])
neg_road = Road(neg_road.end_node, self.road_node(part_idx, 3))
CreateRoadFrom(
neg_lane,
self.positive_lane_num,
neg_road,
self.block_network,
self._global_network,
center_line_type=LineType.NONE,
inner_lane_line_type=LineType.NONE,
side_lane_line_type=LineType.NONE
)
start_node = self.road_node(part_idx, 3)
bend, straight = create_bend_straight(
neg_lane, self.lane_width, radius, self.ANGLE, False, self.parking_space_width
)
bend_road = Road(start_node, self.road_node(part_idx, 4))
CreateRoadFrom(
bend,
self.positive_lane_num,
bend_road,
self.block_network,
self._global_network,
center_line_type=LineType.NONE,
inner_lane_line_type=LineType.NONE,
side_lane_line_type=LineType.NONE
)
straight_road = Road(self.road_node(part_idx, 4), self.road_node(part_idx, 1))
CreateRoadFrom(
straight,
self.positive_lane_num,
straight_road,
self.block_network,
self._global_network,
center_line_type=LineType.NONE,
inner_lane_line_type=LineType.NONE,
side_lane_line_type=LineType.NONE
)
# give it a new road name to make it be a two way road (1,2) = (5,6) = parking space !
parking_road = Road(self.road_node(part_idx, 5), self.road_node(part_idx, 6))
self.spawn_roads.append(parking_road)
CreateTwoWayRoad(
Road(self.road_node(part_idx, 1), self.road_node(part_idx, 2)),
self.block_network,
self._global_network,
parking_road,
center_line_type=LineType.NONE,
inner_lane_line_type=LineType.NONE,
side_lane_line_type=LineType.SIDE if dist_to_out < 1e-3 else LineType.NONE
)
# out part
parking_lane = parking_road.get_lanes(self.block_network)[0]
# out part 1
bend, straight = create_bend_straight(
parking_lane, 0.1 if dist_to_out < 1e-3 else dist_to_out, radius, self.ANGLE, True, parking_lane.width
)
out_bend_road = Road(
self.road_node(part_idx, 6),
self.road_node(part_idx, 7) if dist_to_out > 1e-3 else out_socket.positive_road.start_node
)
bend_success = CreateRoadFrom(
bend,
self.positive_lane_num,
out_bend_road,
self.block_network,
self._global_network,
center_line_type=LineType.NONE,
inner_lane_line_type=LineType.NONE,
side_lane_line_type=LineType.SIDE if dist_to_out < 1e-3 else LineType.NONE
)
if dist_to_out < 1e-3:
no_cross = no_cross and bend_success
if dist_to_out > 1e-3:
out_straight_road = Road(self.road_node(part_idx, 7), out_socket.positive_road.start_node)
no_cross = no_cross and CreateRoadFrom(
straight,
self.positive_lane_num,
out_straight_road,
self.block_network,
self._global_network,
center_line_type=LineType.NONE,
inner_lane_line_type=LineType.NONE,
side_lane_line_type=LineType.NONE
)
# out part 2
extend_lane = ExtendStraightLane(parking_lane, self.lane_width, [LineType.NONE, LineType.NONE])
CreateRoadFrom(
extend_lane,
self.positive_lane_num,
Road(self.road_node(part_idx, 6), self.road_node(part_idx, 8)),
self.block_network,
self._global_network,
center_line_type=LineType.NONE,
inner_lane_line_type=LineType.NONE,
side_lane_line_type=LineType.NONE
)
bend, straight = create_bend_straight(
extend_lane, 0.1 if dist_to_in < 1e-3 else dist_to_in, radius, self.ANGLE, False, parking_lane.width
)
out_bend_road = Road(
self.road_node(part_idx, 8),
self.road_node(part_idx, 9) if dist_to_in > 1e-3 else in_socket.negative_road.start_node
)
CreateRoadFrom(
bend,
self.positive_lane_num,
out_bend_road,
self.block_network,
self._global_network,
center_line_type=LineType.NONE,
inner_lane_line_type=LineType.NONE,
side_lane_line_type=LineType.NONE
)
if dist_to_in > 1e-3:
out_straight_road = Road(self.road_node(part_idx, 9), in_socket.negative_road.start_node)
CreateRoadFrom(
straight,
self.positive_lane_num,
out_straight_road,
self.block_network,
self._global_network,
center_line_type=LineType.NONE,
inner_lane_line_type=LineType.NONE,
side_lane_line_type=LineType.NONE
)
return no_cross