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 heading_diff(self, target_lane):
lateral = None
if isinstance(target_lane, StraightLane):
lateral = np.asarray(
get_vertical_vector(target_lane.end - target_lane.start)[1])
elif isinstance(target_lane, CircularLane):
if target_lane.direction == -1:
lateral = self.position - target_lane.center
else:
lateral = target_lane.center - self.position
else:
raise ValueError("Unknown target lane type: {}".format(
type(target_lane)))
lateral_norm = norm(lateral[0], lateral[1])
forward_direction = self.heading
# print(f"Old forward direction: {self.forward_direction}, new heading {self.heading}")
forward_direction_norm = norm(forward_direction[0],
forward_direction[1])
if not lateral_norm * forward_direction_norm:
return 0
cos = ((forward_direction[0] * lateral[0] +
forward_direction[1] * lateral[1]) /
(lateral_norm * forward_direction_norm))
# return cos
# Normalize to 0, 1
return clip(cos, -1.0, 1.0) / 2 + 0.5
def test_utils():
from pgdrive.utils.math_utils import safe_clip, safe_clip_for_small_array, get_vertical_vector, distance_greater
arr = np.array([
np.nan, np.inf, -np.inf, 1000000000000000000000000000,
-10000000000000000, 0, 1
])
ans = np.array([0, 1, -1, 1, -1, 0, 1])
assert np.array_equal(safe_clip(arr, -1, 1), ans)
assert np.array_equal(safe_clip_for_small_array(arr, -1, 1), ans)
assert np.dot(get_vertical_vector([0, 1])[0], [0, 1]) == 0
assert np.dot(get_vertical_vector([0, 1])[1], [0, 1]) == 0
for _ in range(20):
pos = np.random.normal(0, 1, size=(2, ))
assert distance_greater(pos, (0, 0),
0.5) == (abs(np.linalg.norm(pos, ord=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 sharpbend(
previous_lane: "StraightLane",
following_lane_length,
radius: float,
angle: float,
clockwise: bool = True,
width: float = AbstractLane.DEFAULT_WIDTH,
line_types: Tuple[LineType, LineType] = None,
forbidden: bool = False,
speed_limit: float = 20,
priority: int = 0
):
bend_direction = 1 if clockwise else -1
center = previous_lane.position(previous_lane.length, bend_direction * radius)
p_lateral = previous_lane.direction_lateral
x, y = p_lateral
start_phase = 0
if y == 0:
start_phase = 0 if x < 0 else -np.pi
elif x == 0:
start_phase = np.pi / 2 if y < 0 else -np.pi / 2
else:
base_angel = np.arctan(y / x)
if x < 0:
start_phase = base_angel
elif y < 0:
start_phase = np.pi + base_angel
elif y > 0:
start_phase = -np.pi + base_angel
end_phase = start_phase + angle
if not clockwise:
start_phase = start_phase - np.pi
end_phase = start_phase - angle
bend = CircularLane(
center, radius, start_phase, end_phase, clockwise, width, line_types, forbidden, speed_limit, priority
)
length = 2 * radius * angle / 2
bend_end = bend.position(length, 0)
next_lane_heading = get_vertical_vector(bend_end - center)
nxt_dir = next_lane_heading[0] if not clockwise else next_lane_heading[1]
nxt_dir = np.asarray(nxt_dir)
following_lane_end = nxt_dir * following_lane_length + bend_end
following_lane = StraightLane(bend_end, following_lane_end, width, line_types, forbidden, speed_limit, priority)
return bend, following_lane
