def find_right_most_successor(self, lane_id):
lane = self.get_lane_by_id(lane_id)
if lane is None:
return False, None
if lane.successor_id is None or len(lane.successor_id) <= 0:
return False, None
successor_endpoints = []
c_lane_center = lane.centerline
current_lane_end_point = Point.init_from_other_type(
c_lane_center.point[-1])
for i in range(len(lane.successor_id)):
next_curve = self.get_lane_by_id(lane.successor_id[i])
if next_curve is None:
continue
next_curve_center = next_curve.centerline
end_point = Point.init_from_other_type(next_curve_center.point[-1])
successor_endpoints.append(end_point)
successor_endpoints[i] = successor_endpoints[i] - \
current_lane_end_point
rightmost_index = 0
if len(successor_endpoints) > 1:
for j in range(1, len(successor_endpoints)):
if successor_endpoints[j].cross_product(successor_endpoints[rightmost_index]) > 0:
rightmost_index = j
successor_id = lane.successor_id[rightmost_index]
return True, successor_id
def get_accumulate_distance(self, lane_id, n):
lane = self.get_lane_by_id(lane_id)
if lane is None:
return 0
lane_line = self.__lanes_table[lane_id].centerline
l = 0.
points_slice = min(n, len(lane_line.point) - 1) + 1
for point, point_next in zip(lane_line.point[:points_slice], lane_line.point[1:points_slice]):
l += Point.init_from_other_type(point).distance_to(
Point.init_from_other_type(point_next))
return l
def get_nearest_crosswalk(self, point):
min_distance = sys.float_info.max
flag = False
min_crosswalk_id = -1
point = Point.init_from_other_type(point)
for crosswalk_id in self.__crosswalks_table.keys():
center_point = [0., 0.]
count = 0.
for croswalk_point in self.__crosswalks_table[crosswalk_id].polygon.point:
center_point[0] += croswalk_point.x
center_point[1] += croswalk_point.y
count += 1.
center_point[0] = center_point[0] / count
center_point[1] = center_point[1] / count
center_point = Point.init_from_other_type(center_point)
square_distance = center_point.square_distance_to(point)
if square_distance < min_distance:
min_distance = square_distance
min_crosswalk_id = crosswalk_id
flag = True
return flag, min_crosswalk_id
def get_point_and_lane_heading_by_s(self, id, s):
lane = self.get_lane_by_id(id)
if lane is None:
return False, None
if lane.centerline is None or len(lane.centerline.point) < 1:
return False, None
if s <= self.centerline_s_dict[id][0]:
return True, Point.init_from_other_type(lane.centerline.point[0])
if s >= self.centerline_s_dict[id][-1]:
return True, Point.init_from_other_type(lane.centerline.point[-1])
index = np.array(self.centerline_s_dict[id]).searchsorted(s)
delta_s = self.centerline_s_dict[id][index] - s
if delta_s < 1e-10:
return True, Point.init_from_other_type(lane.centerline.point[index])
p2 = Point.init_from_other_type(lane.centerline.point[index])
p1 = Point.init_from_other_type(lane.centerline.point[index - 1])
unit_directions_ = Line(p1, p2).unit_v
smooth_point = p2 - unit_directions_ * delta_s
return True, smooth_point
def get_visible_traffic_lights(self, point, heading):
max_range = 150
result = []
can_get, lane_id, sl = self.get_nearest_lane_with_heading(
point, heading)
if not can_get:
return result
while not self.get_forward_signals_on_lane(lane_id, sl[0])[0]:
sl[0] = 0
center_line = self.get_lane_by_id(lane_id).centerline
end_point = Point.init_from_other_type(center_line.point[-1])
if point.distance_to(end_point) > max_range:
break
can_find, successor_id = self.find_right_most_successor(lane_id)
if not can_find:
break
lane_id = successor_id
return self.get_forward_signals_on_lane(lane_id, sl[0])[1]
def get_rough_lane_right_width(self, id, s):
lane = self.get_lane_by_id(id)
if lane is None:
return False, 100000.0
has_found, lane_point = self.get_point_and_lane_heading_by_s(id, s)
if not has_found:
return False, 100000.0
if lane.right_boundary_id == 0:
return False, 100000.0
right_lane_id = lane.right_boundary_id
right_boundary = self.get_lane_boundary_by_id(right_lane_id)
if right_boundary is None:
return False, 100000.0
right_boundary_points = right_boundary.boundary.point
min_square_distance = 100000
min_index = -1
for idx, boundary_point in enumerate(right_boundary_points):
p = Point.init_from_other_type(boundary_point)
square_distance = p.square_distance_to(lane_point)
if square_distance < min_square_distance:
min_square_distance = square_distance
min_index = idx
if min_index == -1:
return False, 100000.0
if min_index == 0 and len(right_boundary_points) == 1:
return True, math.sqrt(min_square_distance)
flag = False
square_right_width = 100000.0
if min_index != 0:
line = Line.init_from_pb(
right_boundary_points[min_index], right_boundary_points[min_index - 1])
square_right_width = line.get_mini_square_distance(lane_point)
flag = True
if min_index < len(right_boundary_points) - 1:
line = Line.init_from_pb(
right_boundary_points[min_index], right_boundary_points[min_index + 1])
square_distance = line.get_mini_square_distance(lane_point)
if (flag and square_distance < square_right_width) or (not flag):
square_right_width = square_distance
flag = True
return flag, math.sqrt(square_right_width)
def get_nearest_lane_with_heading(self, point, heading):
output = self.kdtree.query_with_k(point, k=4)
if len(output[0]) <= 0:
return False, [], None
heading_vec = Point(np.cos(heading), np.sin(heading))
fix_data = {"pt": point,
"func_name": "combo_inner_product",
"heading": heading_vec}
pfunc = partial(self.find_line, **fix_data)
distance_list = list(map(pfunc, output[0]))
min_pack = distance_list.index(min(distance_list))
min_id, min_index = output[0][min_pack][0]
lane_line = self.__lanes_table[min_id].centerline
line = Line.init_from_pb(
lane_line.point[min_index], lane_line.point[min_index + 1])
s, l = line.get_sl(point)
s = self.get_accumulate_distance(min_id, min_index) + s
return True, [s, l], min_id