"""Library for track filtering to calculate centerline and track widths based on the landmarks estimated by SLAM."""
from enum import Enum
from typing import Tuple
import time
import matplotlib.pyplot as plt
import numpy as np
import numpy.typing as npt
from scipy.spatial import Delaunay
from scipy.interpolate import CubicSpline
[docs]class LogLevel(Enum):
"""
Enum to describe which LogLevel to use.
TRACE - the most fine-grained information only used in rare cases where you need the full visibility
of what is happening in your application and inside the third-party libraries that you use. You can
expect the TRACE logging level to be very verbose. You can use it for example to annotate each step
in the algorithm or each individual query with parameters in your code.
DEBUG - less granular compared to the TRACE level, but it is more than you will need in everyday use. The DEBUG
log level should be used for information that may be needed for diagnosing issues and troubleshooting or when
running application in the test environment for the purpose of making sure everything is running correctly
INFO - the standard log level indicating that something happened, the application entered a certain
state, etc. For example, a controller of your authorization API may include an INFO log level with
information on which user requested authorization if the authorization was successful or not. The
information logged using the INFO log level should be purely informative and not looking into them
on a regular basis shouldnt result in missing any important information.
WARN - the log level that indicates that something unexpected happened in the application, a problem,
or a situation that might disturb one of the processes. But that doesnt mean that the application failed.
The WARN level should be used in situations that are unexpected, but the code can continue the work.
For example, a parsing error occurred that resulted in a certain document not being processed.
ERROR - the log level that should be used when the application hits an issue preventing one or more
functionalities from properly functioning. The ERROR log level can be used when one of the payment
systems is not available, but there is still the option to check out the basket in the e-commerce
application or when your social media logging option is not working for some reason.
FATAL - the log level that tells that the application encountered an event or entered a state in which
one of the crucial business functionality is no longer working. A FATAL log level may be used when the
application is not able to connect to a crucial data store like a database or all the payment systems
are not available and users cant checkout their baskets in your e-commerce.
"""
TRACE = 0
DEBUG = 1
INFO = 2
WARN = 3
ERROR = 4
FATAL = 5
[docs]class ConeID(Enum):
"""Enum to describe the different cone IDs."""
YELLOW = 0
BLUE = 1
ORANGE = 2
ORANGE_BIG = 3
[docs]class TrackFiltering():
"""
TrackFiltering instance implementing functionality to calculate centerline and track widths for given landmarks.
Attributes
----------
log_level : LogLevel
Log level to use.
track_width_min : float
Expected minimal track width.
"""
def __init__(self, local_fov_angle_rad: float, local_fov_range_m: float,
log_level: LogLevel = LogLevel.FATAL, track_width_min: float = 3.):
"""
Initialize TrackFiltering object.
Parameters
----------
local_fov_angle_rad : float
Field of view angle [rad] for limiting landmarks for local track filtering.
local_fov_range_m : float
Field of view range [m] limiting landmarks for local track filtering.
log_level : LogLevel, optional
Log level to use, by default LogLevel.FATAL
track_width_min : float, optional
Expected minimal track width, by default 3.
local_fov_angle_rad : float
Field of view angle [rad] for limiting landmarks for local track filtering.
local_fov_range_m : float
Field of view range [m] limiting landmarks for local track filtering.
vehicle_pose : Optional[npt.NDArray]
Current vehicle pose (x, y, psi).
left_cones : Optional[npt.NDArray]
All cones which are on the left side of the track, shape: (n, 2) with n equal number of left cones.
right_cones : Optional[npt.NDArray]
All cones which are on the right side of the track, shape: (m, 2) with m equal number of right cones.
orange_cones : Optional[npt.NDArray]
All small cones, shape: (k, 2) with k equal number of small orange cones.
left_not_empty : Optional[bool]
Indicates whether there are no left cones.
right_not_empty : Optional[bool]
Indicates whether there are no right cones.
orange_cones_not_empty : Optional[bool]
Indicates whether there are no small orange cones.
global_track : Optional[bool]
Indicates whether the inputed cones build a closed track.
runtime : Optional[float]
Time it took to run filtering.
centerpoints : Optional[npt.NDArray]
Calculated centerpoints of the filtered track, shape: (c, 2) with c equal number of centerpoints.
track_widths : Optional[npt.NDArray]
Calculated trackwidths of the filtered track, shape: (c, 2) with c equal number of centerpoints.
curvature_centerline : Optional[npt.NDArray]
Curvature of the calculated centerpoints of the filtered track,
shape: (c, 2) with c equal number of centerpoints.
points_unorganized : Optional[npt.NDArray]
Helper class attribute for ordering points, buffers the points which are not ordered yet, shape: (i, 2)
min_distance : Optional[npt.NDArray]
Helper class attribute for ordering points, buffers the minimal distances (??)m shape: (l, )
"""
self.log_level = log_level
self.set_fov(local_fov_angle_rad=local_fov_angle_rad, local_fov_range_m=local_fov_range_m)
self.track_width_min = track_width_min
[docs] def set_fov(self, local_fov_angle_rad: float, local_fov_range_m: float):
"""
Set field of view for local track filtering.
Parameters
----------
local_fov_angle_rad : float
Field of view angle [rad] for limiting landmarks for local track filtering.
local_fov_range_m : float
Field of view range [m] limiting landmarks for local track filtering.
"""
if local_fov_angle_rad > np.pi:
self.local_fov_angle_rad = np.pi
else:
self.local_fov_angle_rad = local_fov_angle_rad
self.local_fov_range_m = local_fov_range_m
[docs] def filtering_main(self, left_cones: npt.NDArray, right_cones: npt.NDArray, orange_cones: npt.NDArray, x_m: float,
y_m: float, psi_rad: float, global_track: bool) -> Tuple[npt.NDArray, npt.NDArray, bool]:
"""
Filter given cones to calculate centerline and track widths.
Parameters
----------
left_cones : npt.NDArray
All cones which are on the left side of the track, shape: (n, 2) with n equal number of left cones.
right_cones : npt.NDArray
All cones which are on the right side of the track, shape: (m, 2) with m equal number of right cones.
orange_cones : npt.NDArray
All small cones, shape: (k, 2) with k equal number of small orange cones.
x_m : float
X position [m] of the vehicle.
y_m : float
Y position [m] of the vehicle.
psi_rad : float
Heading [rad] of the vehicle.
global_track : bool
Indicates whether the inputed cones build a closed track.
Returns
-------
Tuple[npt.NDArray, npt.NDArray, bool]
**Centerline** (shape: (l, 2) with l equal number of centerpoints),
**Track widths** (shape: (l, 2) with l equal number of centerpoints) and
**Closed track** indicating whether the resulting centerline is closed.
"""
if (self.log_level == LogLevel.INFO):
start_time = time.time()
# save vehicle state as class member
self.vehicle_pose = np.array([x_m, y_m, psi_rad])
# save cone arrays as class member
self.left_cones = left_cones
self.right_cones = right_cones
self.orange_cones = orange_cones
# check if right or left array is empty
if (self.left_cones.size != 0):
self.left_not_empty = True
else:
self.left_not_empty = False
if (self.right_cones.size != 0):
self.right_not_empty = True
else:
self.right_not_empty = False
if (self.orange_cones.size != 0):
self.orange_cones_not_empty = True
else:
self.orange_cones_not_empty = False
# save global track flag as class member
self.global_track = global_track
# Execution
# difference between local and gloabl approach
if (self.global_track is False):
self.local_filtering()
else:
# if global filtering failes run local filtering again
if (self.global_filtering() is False):
self.global_track = False
self.local_filtering()
if (self.log_level == LogLevel.INFO):
end_time = time.time()
self.runtime = (end_time - start_time) * 1e03
return self.centerpoints, self.track_widths, self.global_track
[docs] def local_filtering(self) -> bool:
"""
Filter track locally.
Input are left and right cones, Output are centerline of filtered track and legal area of the track.
The algorithm follows 3 steps (steps 2 & 3 are currently part of motion planning):
#. Find the midpoints of the track. algorithm seperates between the secen cases below.
#. Interpolate and approximate centerline from fround mitpoints. Calculate normalvectors.
#. Defines the legal track width for each point and calculates borderlines on the right and left.
For the first step the algorithm has to decide which kind of midpoint generator should be used.
#. Only one cone:
a. Only one cone on the left: DynamicWindow
b. Only one cone on the right: DynamicWindow
#. Only one color:
a. Only left cones: BorderShift
a. Only right cones: BorderShift
#. Two colors:
a. One cone each: GateFallback
b. One cone on one side: BorderShift
c. All other: delauney
.. todo::
For case 3.b the threshold should be modified. Also a other stepsize
for interpolating and approximation could serve better results.
Returns
-------
bool
Indicates whether filtering was successful.
"""
# get left and right cones from local fov
try:
self.get_cones_in_local_fov()
except Exception:
self.centerpoints = np.array([])
self.track_widths = np.array([])
return False
#####################################
# #
# Case Filtering #
# #
#####################################
#####################################
# 1.1 ONE left CONE #
#####################################
if ((len(self.right_cones) == 0) and (len(self.left_cones) == 1)):
# run Dynamic Window
if (self.advanced_dynamic_window(ConeID.BLUE) is False):
# print error message
if (self.log_level == LogLevel.ERROR):
print('1.1 ERROR: Dynamic Window failed.')
return False
#####################################
# 1.2 ONE right CONE #
#####################################
elif ((len(self.right_cones) == 1) and (len(self.left_cones) == 0)):
# run Dynamic Window
if (self.advanced_dynamic_window(ConeID.YELLOW) is False):
# print error message
if (self.log_level == LogLevel.ERROR):
print('1.2 ERROR: Dynamic Window failed.')
return False
#####################################
# 2.1 MULTIPLE left CONES #
#####################################
elif ((len(self.right_cones) == 0) and (len(self.left_cones) != 0)):
# run Bordershift
if (self.border_shift(ConeID.BLUE) is False):
# print error message
if (self.log_level == LogLevel.ERROR):
print('2.1 ERROR: Bordershift failed.')
return False
#####################################
# 2.2 MULTIPLE right CONES #
#####################################
elif ((len(self.left_cones) == 0) and (len(self.right_cones) != 0)):
# run Bordershift
if (self.border_shift(ConeID.YELLOW) is False):
# print error message
if (self.log_level == LogLevel.ERROR):
print('2.2 ERROR: Bordershift failed.')
return False
#####################################
# 3.1 ONE CONE EACH #
#####################################
elif ((len(self.left_cones) == 1) and (len(self.right_cones) == 1)):
# run gate fallback
if (self.gate_fallback() is False):
# print error message
if (self.log_level == LogLevel.ERROR):
print('2.2 ERROR: Gate Fallback failed.')
return False
#####################################
# 3.2 ONE CONE ON ONE SIDE #
#####################################
elif (len(self.right_cones) == 1):
# run Bordershift
if (self.border_shift(ConeID.BLUE) is False):
# print error message
if (self.log_level == LogLevel.ERROR):
print('3.2 ERROR: Bordershift failed.')
return False
elif (len(self.left_cones) == 1):
# run Bordershift
if (self.border_shift(ConeID.YELLOW) is False):
# print error message
if (self.log_level == LogLevel.ERROR):
print('3.2 ERROR: Bordershift failed.')
return False
#####################################
# 3.3 MULTIPLE CONES #
#####################################
elif ((len(self.left_cones) > 1) and (len(self.right_cones) > 1)):
# run full pathplanner
if (self.full_path_filter() is False):
# logging message
if (self.log_level == LogLevel.ERROR):
print('3.3 ERROR: Full Pathfilter failed.')
return False
#####################################
# 0 ERROR #
#####################################
else:
# print error message
if (self.full_path_filter() is False):
print('0 ERROR: Youre car is blind. Stop the car!')
# return empty arrays
self.centerpoints = np.array([])
self.track_widths = np.array([])
return False
# reconvert local to global coordinates
self.centerpoints = self.convert_local_to_global(self.centerpoints)
# self.track_widths = self.convert_local_to_global(self.track_widths) # noqa: E800
self.left_cones = self.left_cones_global
self.right_cones = self.right_cones_global
return True
[docs] def global_filtering(self) -> bool:
"""
Filter track globally.
Input are left and right cones, Output are centerline of filtered track and legal area of the track.
Algorithm has the following steps:
#. Find midpoints with delauney triangulation.
#. Sort mitpoints with generate tree algorithm (first is the one
nearest to start and finish line, second one have to calculated with the direction).
#. Calculate trackwidth to left and right for each midpoint.
Returns
-------
bool
Indicates whether filtering was successful.
"""
try:
# call delauny traingulation
self.delaunay()
# find the midpoint between the start/finish cones
midpoint_start = np.mean(self.orange_cones, axis=0)
# sort centerline, left cones and right cones
self.centerpoints = self.generate_tree(self.centerpoints, midpoint_start)
self.left_cones = self.generate_tree(self.left_cones, midpoint_start)
self.right_cones = self.generate_tree(self.right_cones, midpoint_start)
# check if right direction is choosen
vector_to_left_cone = self.left_cones[0] - midpoint_start
vector_to_centerpoint = self.centerpoints[0] - midpoint_start
angle_1 = np.arctan2(vector_to_left_cone[1], vector_to_left_cone[0])
angle_2 = np.arctan2(vector_to_centerpoint[1], vector_to_centerpoint[0])
delta_angle = (angle_1 - angle_2)
if delta_angle < 0:
delta_angle += 2 * np.pi
if (delta_angle > np.pi):
self.centerpoints = np.append(self.centerpoints[0], np.flip(self.centerpoints, 0)[:-1]).reshape((-1, 2))
# create high resolution splines of left and right cones
left_cones_spline = self.cubic_spline(np.append(self.left_cones, self.left_cones[0]).reshape((-1, 2)),
factor=10, endpoint=False)[0]
right_cones_spline = self.cubic_spline(np.append(self.right_cones, self.right_cones[0]).reshape((-1, 2)),
factor=10, endpoint=False)[0]
# find nearest left and right border point for trackwidth calculation
self.track_widths = np.array([])
for point in self.centerpoints:
# calculate distance from midpoint to ervery border point
distance_left = np.max([0, np.min(np.sqrt(
(left_cones_spline[:, 0] - point[0])**2 + (left_cones_spline[:, 1] - point[1])**2))])
distance_right = np.max([0, np.min(np.sqrt(
(right_cones_spline[:, 0] - point[0])**2 + (right_cones_spline[:, 1] - point[1])**2))])
# append to trackwidth array
self.track_widths = np.append(self.track_widths, [distance_right, distance_left])
# resahpe trackwidth
self.track_widths = self.track_widths.reshape((-1, 2))
return True
except Exception:
return False
[docs] def get_cones_in_local_fov(self):
"""
Limit all cones to the visuals ones according to the field of view.
Also rotate coordinate system to vehicle coordinate system.
.. todo:: Check Rotation from scipy.spatial.transform instead of own implementation.
"""
self.left_cones_global = self.left_cones
self.right_cones_global = self.right_cones
if (self.left_not_empty):
# Translation of KOS
left_x_strich = self.left_cones[:, 0] - self.vehicle_pose[0]
left_y_strich = self.left_cones[:, 1] - self.vehicle_pose[1]
# Rotation of KOS, so that Psi of the vehicle equals zero
left_x = left_x_strich * np.cos(-self.vehicle_pose[2]) - left_y_strich * np.sin(-self.vehicle_pose[2])
left_y = left_x_strich * np.sin(-self.vehicle_pose[2]) + left_y_strich * np.cos(-self.vehicle_pose[2])
# calculate distance to each cone
distance_left = np.sqrt(left_x**2 + left_y**2)
# calculate angle [rad] for each cone
angle_left = np.arctan2(left_y, left_x)
# check if cones are in range in the fov angle
left_cones_in_fov_range = distance_left <= self.local_fov_range_m
left_cones_fov_angle_pos = angle_left <= self.local_fov_angle_rad / 2
left_cones_fov_angle_neg = angle_left >= -self.local_fov_angle_rad / 2
left_cones_fov_angle = np.logical_and(left_cones_fov_angle_pos, left_cones_fov_angle_neg)
# filter out all cones that are not in the local fov
left_x = left_x[np.logical_and(left_cones_in_fov_range, left_cones_fov_angle)]
left_y = left_y[np.logical_and(left_cones_in_fov_range, left_cones_fov_angle)]
# save results in class member
self.left_cones = np.c_[left_x, left_y]
if (self.right_not_empty):
# Translation of KOS
right_x_strich = self.right_cones[:, 0] - self.vehicle_pose[0]
right_y_strich = self.right_cones[:, 1] - self.vehicle_pose[1]
# Rotation of KOS, so that Psi of the vehicle equals zero
right_x = right_x_strich * np.cos(-self.vehicle_pose[2]) - right_y_strich * np.sin(-self.vehicle_pose[2])
right_y = right_x_strich * np.sin(-self.vehicle_pose[2]) + right_y_strich * np.cos(-self.vehicle_pose[2])
# calculate distance to each cone
distance_right = np.sqrt(right_x**2 + right_y**2)
# calculate angle [rad] for each cone
angle_right = np.arctan2(right_y, right_x)
# check if cones are in range in the fov angle
right_cones_in_fov_range = distance_right <= self.local_fov_range_m
right_cones_fov_angle_pos = angle_right <= self.local_fov_angle_rad / 2
right_cones_fov_angle_neg = angle_right >= -self.local_fov_angle_rad / 2
right_cones_fov_angle = np.logical_and(right_cones_fov_angle_pos, right_cones_fov_angle_neg)
# filter out all cones that are not in the local fov
right_x = right_x[np.logical_and(right_cones_in_fov_range, right_cones_fov_angle)]
right_y = right_y[np.logical_and(right_cones_in_fov_range, right_cones_fov_angle)]
# save results in class member
self.right_cones = np.c_[right_x, right_y]
[docs] def convert_local_to_global(self, local_points: npt.NDArray) -> npt.NDArray:
"""
Convert local coordinates (rear axle) back to global (map) coordinates.
Rotate and translate vehicle coordinate system to global coordinate system.
.. todo:: Check Rotation from scipy.spatial.transform instead of own implementation.
Parameters
----------
local_points : npt.NDArray
Points in local (rear axle) coordinates system.
Returns
-------
npt.NDArray
Points in global (map) coordinate system.
"""
# Rotation of KOS, so that Psi of the vehicle is global again
cos = np.cos(self.vehicle_pose[2])
sin = np.sin(self.vehicle_pose[2])
global_points_x = local_points[:, 0] * cos - local_points[:, 1] * sin
global_points_y = local_points[:, 0] * sin + local_points[:, 1] * cos
# Translation of KOS
global_points_x = global_points_x + self.vehicle_pose[0]
global_points_y = global_points_y + self.vehicle_pose[1]
# put arrays for x and y together
global_points = np.c_[global_points_x, global_points_y]
return global_points
[docs] def dynamic_window(self, cone_id: int) -> bool:
"""
Generate midpoints if there is only one single cone detected.
Calculate the normalvector on the support vector to the cone.
With the the minimum half track width one can estimate the midpoint.
Name of the methode is based on the 'dynamic window approach'
where a robot finds its path by avoiding a crash with each obstacle.
.. note:: See implementation of advanced dynamic window
Parameters
----------
cone_id :
ID of one and only detected cone.
Returns
-------
bool
Indicates whether algorithm was successful.
"""
try:
# check if it's a left cone
if (cone_id == ConeID.BLUE):
# normalvector on line to cone
normalvector = (np.array([-self.left_cones[0, 1], self.left_cones[0, 0]])
/ np.linalg.norm(self.left_cones[0]))
# append to midpoints
self.centerpoints = (self.left_cones[0] - self.track_width_min / 2 * normalvector).reshape((1, 2))
# calculate trackwidth (left and right both the same) -> see notebook for documentation
width = np.max([0, np.abs((np.linalg.norm(self.left_cones[0]) * self.track_width_min / 2)
/ np.linalg.norm(self.centerpoints[0]))])
self.track_widths = np.array([width, width]).reshape((-1, 2))
# it's a right cone:
elif (cone_id == ConeID.YELLOW):
# normalvector on line to cone
normalvector = (np.array([-self.right_cones[0, 1], self.right_cones[0, 0]])
/ np.linalg.norm(self.right_cones[0]))
# append to midpoints
self.centerpoints = (self.right_cones[0] + self.track_width_min / 2 * normalvector).reshape((1, 2))
# calculate trackwidth (left and right both the same) -> see notebook for documentation
width = np.max([0, np.abs((np.linalg.norm(self.right_cones[0]) * self.track_width_min / 2)
/ np.linalg.norm(self.centerpoints[0]))])
self.track_widths = np.array([width, width]).reshape((-1, 2))
# it's a orange cone
else:
# print error message
if (self.log_level == LogLevel.ERROR):
print('ERROR: You see only one cone and its a god damn orange one. BRAKE!')
# save empty arrays
self.centerpoints = np.array([])
self.track_widths = np.array([])
return False
except Exception:
# save empty arrays
self.centerpoints = np.array([])
self.track_widths = np.array([])
return False
return True
[docs] def advanced_dynamic_window(self, cone_id: int) -> bool:
"""
Generate midpoints if there is only one single cone detected.
Calculate the normalvector on the support vector to the cone.
With the the minimum half track width one can estimate the midpoint.
Name of the methode is based on the 'dynamic window approach'
where a robot finds its path by avoiding a crash with each obstacle.
.. note:: See implementation of dynamic window
.. todo:: What is the difference to the dynamic window approach.
Parameters
----------
cone_id :
ID of one and only detected cone.
Returns
-------
bool
Indicates whether algorithm was successful
"""
try:
# check if it's a left cone
if (cone_id == ConeID.BLUE):
# append to midpoints
self.centerpoints = np.array([self.left_cones[0, 0], self.left_cones[0, 1] - self.track_width_min / 2]
).reshape((1, 2))
# calculate trackwidth (left and right both the same)
self.track_widths = np.array([self.track_width_min / 2, self.track_width_min / 2]).reshape((-1, 2))
# it's a right cone:
elif (cone_id == ConeID.YELLOW):
# append to midpoints
self.centerpoints = np.array([self.right_cones[0, 0], self.right_cones[0, 1] + self.track_width_min / 2]
).reshape((1, 2))
# calculate trackwidth (left and right both the same)
self.track_widths = np.array([self.track_width_min / 2, self.track_width_min / 2]).reshape((-1, 2))
# it's a orange cone
else:
# print error message
if (self.log_level == LogLevel.ERROR):
print('ERROR: You see only one cone and its a god damn orange one. BRAKE!')
# save empty arrays
self.centerpoints = np.array([])
self.track_widths = np.array([])
return False
except Exception:
# save empty arrays
self.centerpoints = np.array([])
self.track_widths = np.array([])
return False
[docs] def border_shift(self, cone_id: int) -> bool:
"""
Shifts trackborder points to the estimated middle of the track.
Resulting in new centerline and a legal trackwidth for each centerpoint.
.. todo:: Reqrite generateTree function.
.. todo:: Add Polyfit and CubicSpline Methode.
Parameters
----------
cone_id :
ID of the detected cones.
Returns
-------
bool
Indicates whether algorithm was successful.
"""
try:
# check if left or right border given
if (cone_id == ConeID.BLUE):
# bring border points in true order
points = self.generate_tree(self.left_cones, np.array([0, 0]))
elif (cone_id == ConeID.YELLOW):
# bring border points in true order
points = self.generate_tree(self.right_cones, np.array([0, 0]))
else:
# print error message
if (self.log_level == LogLevel.ERROR):
print('ERROR: You got a bunch of orange cones. Better stop the car!')
return
# get high resolution spline of borderpoints -> Doesn't work if array got less than 3 points
if (points.shape[0] > 2):
points, normalvectors = self.cubic_spline(points, factor=2)
else:
points, normalvectors = self.polyfit(points, order=1, factor=2)
# define trackwidth from known data
track_width = np.linspace(np.abs(points[0, 1]), self.track_width_min / 2, len(points))
if (cone_id == ConeID.BLUE):
# calculate new midpoints for left border cones
self.centerpoints = points - normalvectors * track_width[:, None]
if (cone_id == ConeID.YELLOW):
# calculate new midpoints for right border cones
self.centerpoints = points + normalvectors * track_width[:, None]
# calculate track widths array
self.track_widths = np.c_[track_width, track_width]
except Exception:
# return empty arrays
self.centerpoints = np.array([])
self.track_widths = np.array([])
return False
return True
[docs] def gate_fallback(self) -> bool:
"""
Calculate centerpoint between one yellow and one blue cone.
.. todo:: One could return False if cones are inverted (vehicle has to turn 180 degree to pass gate).
Returns
-------
bool
Indicates whether algorithm was successful.
"""
try:
# calculate centerpoint
x = (self.left_cones[0, 0] + self.right_cones[0, 0]) / 2
y = (self.left_cones[0, 1] + self.right_cones[0, 1]) / 2
self.centerpoints = np.array([x, y]).reshape((-1, 2))
# calculate trackwidth
vector = self.left_cones[0] - self.right_cones[0]
width = np.sqrt(vector[0]**2 + vector[1]**2) / 2
self.track_widths = np.array([width, width]).reshape((-1, 2))
except Exception:
# save empty arrays
self.centerpoints = np.array([])
self.track_widths = np.array([])
return False
return True
[docs] def full_path_filter(self) -> bool:
"""
Filter the full path.
.. todo:: Improve documentation of this method.
Returns
-------
bool
Indicates whether algorithm was successful.
"""
try:
self.delaunay()
# generate Tree of midpoints and cones
self.left_cones = self.generate_tree(self.left_cones, np.array([0, 0]))
self.right_cones = self.generate_tree(self.right_cones, np.array([0, 0]))
self.centerpoints = self.generate_tree(self.centerpoints, np.array([0, 0]), centerpoints=True)
# filter out centerpoints that are 70% more far away to its neighbor
# get high resolution spline of left cones -> Doesn't work if array got less than 3 points
if (len(self.left_cones) > 2):
left_cones, left_normalvectors = self.cubic_spline(self.left_cones, factor=10)
else:
left_cones, left_normalvectors = self.polyfit(self.left_cones, order=1, factor=10)
# get high resolution spline of right cones -> Doesn't work if array got less than 3 points
if (len(self.right_cones) > 2):
right_cones, right_normalvectors = self.cubic_spline(self.right_cones, factor=10)
else:
right_cones, right_normalvectors = self.polyfit(self.right_cones, order=1, factor=10)
# find nearest left and right border point for trackwidth calculation
self.track_widths = np.array([])
for point in self.centerpoints:
# calculate distance from midpoint to ervery border point
# distance_left = np.linalg.norm(left_cones - point) - self.vehicle_width/2 # noqa: E800
# distance_right = np.linalg.norm(right_cones - point) - self.vehicle_width/2 # noqa: E800
distance_left = np.max([0, np.min(np.sqrt((left_cones[:, 0] - point[0])**2
+ (left_cones[:, 1] - point[1])**2))])
distance_right = np.max([0, np.min(np.sqrt((right_cones[:, 0] - point[0])**2
+ (right_cones[:, 1] - point[1])**2))])
# append to trackwidth array
self.track_widths = np.append(self.track_widths, [distance_left, distance_right])
# resahpe trackwidth
self.track_widths = self.track_widths.reshape((-1, 2))
except Exception:
# save empty arrays
self.centerpoints = np.array([])
self.track_widths = np.array([])
return False
return True
[docs] def delaunay(self):
"""Run delaunex triangulation. Works for local and global tracks."""
if (self.log_level == LogLevel.INFO):
start = time.time()
coordinates = np.append(self.left_cones, self.right_cones, axis=0)
# delaunay
tri = Delaunay(coordinates)
triangles = tri.simplices
if (self.log_level == LogLevel.INFO):
del_time = time.time()
# get all lines
result = np.empty((3 * triangles.shape[0], 2), dtype=int)
result[::3] = triangles[:, [0, 1]]
result[1::3] = triangles[:, [1, 2]]
result[2::3] = triangles[:, [0, 2]]
# sort lines and filter out duplicates and lines between same color
result = np.sort(result)
result = np.unique(result, axis=0)
result = result[np.logical_not(np.logical_or(np.logical_and(result[:, 0] < len(self.left_cones),
result[:, 1] < len(self.left_cones)),
np.logical_and(result[:, 0] >= len(self.left_cones),
result[:, 1] >= len(self.left_cones))))]
# get waypoints
indizes_1 = np.hstack((2 * result[:, 0].reshape(-1, 1), 2 * result[:, 0].reshape(-1, 1) + 1))
indizes_2 = np.hstack((2 * result[:, 1].reshape(-1, 1), 2 * result[:, 1].reshape(-1, 1) + 1))
self.centerpoints = (np.take(coordinates, indizes_1) + np.take(coordinates, indizes_2)) / 2
if (self.log_level == LogLevel.INFO):
end = time.time()
print('INFO: delaunay Time', del_time - start, ' sec')
print('INFO: Filter delaunay Time', end - start, ' sec')
[docs] def cubic_spline(self, points: npt.NDArray, factor: int = 1,
endpoint: bool = True) -> Tuple[npt.NDArray, npt.NDArray]:
"""
Calculate cubic spline of the input points.
Also calculates the curvature of the spline.
.. todo:: What's the smallest number of points to get the first and second deriviate.
Parameters
----------
points : npt.NDArray
Array if points as input for cubicspline.
factor : int, optional
Factor for interpolation between points, by default 1
endpoint : bool, optional
Whether to use the last point or not, by default True
Returns
-------
Tuple[npt.NDArray, npt.NDArray]
**new points** of the spline interpolation (shape: (m, 2) with m equal (factor * n) + 1) and
**normal vectors** of the spline interpolation (shape: (m, 2) with m equal (factor * n) + 1).
"""
# create step-itterator array
i = np.arange(0, len(points), 1)
i_new = np.linspace(0, len(points) - 1, (len(points) - 1) * factor + 1, endpoint=endpoint)
# generate cubic Spline
self.spline_x = CubicSpline(i, points[:, 0], bc_type='natural')
self.spline_y = CubicSpline(i, points[:, 1], bc_type='natural')
# get spline arrays
x_new = self.spline_x(i_new)
y_new = self.spline_y(i_new)
x_1_new = self.spline_x(i_new, 1)
y_1_new = self.spline_y(i_new, 1)
# create resulting points
new_points = np.c_[x_new, y_new]
# calculate normalvector: n= [-y', x']/np.sqrt(x'**2 + y'**2)
normalvectors = [-y_1_new, x_1_new] / np.sqrt(x_1_new**2 + y_1_new**2)
normalvectors = np.c_[normalvectors[0], normalvectors[1]]
return new_points, normalvectors
[docs] def polyfit(self, points: npt.NDArray, order: int = 5, factor: int = 1) -> Tuple[npt.NDArray, npt.NDArray]:
"""
Calculate a polyfit for a given set of points. Also calculate normal vectors for each point.
Parameters
----------
points : npt.NDArray
Array containing the position of every cone in the right order, shape: (n, 2) with n equal number of points.
order : int, optional
Highest order of ponynomial, needs to be odd number, by default 5
factor : int, optional
Scaling factor for approximation, by default 1
Returns
-------
Tuple[npt.NDArray, npt.NDArray]
**new points** of the fitted polynomial (shape: (m, 2) with m equal (factor * n) + 1) and
**normal vectors** of the fitted polynomial (shape: (m, 2) with m equal (factor * n) + 1).
"""
# create itterator and array for x and y
i = np.arange(0, len(points), 1)
i_new = np.linspace(0, len(points) - 1, (len(points) - 1) * factor + 1, endpoint=True)
x = points[:, 0]
y = points[:, 1]
# limit order of polyfit to a maximum of 5
if (order > 5):
order = 5
# create Polynomial fit for x and y
poly_x = np.poly1d(np.polyfit(i, x, deg=order))
poly_y = np.poly1d(np.polyfit(i, y, deg=order))
# calculate new points
x_new = poly_x(i_new)
y_new = poly_y(i_new)
points_new = np.c_[x_new, y_new]
# calculate normalvector: n= [-y', x']/np.sqrt(x'**2 + y'**2)
x_1_new = poly_x.deriv()(i_new)
y_1_new = poly_y.deriv()(i_new)
normalvectors = [-y_1_new, x_1_new] / np.sqrt(x_1_new**2 + y_1_new**2)
normalvectors = np.c_[normalvectors[0], normalvectors[1]]
# calculate curvature
x_2_new = poly_x.deriv().deriv()(i_new)
y_2_new = poly_y.deriv().deriv()(i_new)
self.curvature_centerline = (x_1_new * y_2_new - x_2_new * y_1_new) / np.sqrt(x_1_new**2 + y_1_new**2)**3
return points_new, normalvectors
[docs] def generate_tree(self, points: npt.NDArray, start_point: npt.NDArray, centerpoints: bool = False) -> npt.NDArray:
"""
Bring points intro right order.
Start with vehicle position and try to find nearest points for each upcoming point.
Parameters
----------
points : npt.NDArray
Array containing position of points user wants to order, shape: (n, 2) with n equal number of points.
start_point : npt.NDArray
Position of first point, e.g. vehicle position, shape: (2, ).
centerpoints : bool, optional
Indicates whether points are centerpoints, by default False
Returns
-------
npt.NDArray
Order points, shape: (n, 2) with n equal number of points.
"""
if (self.orange_cones_not_empty is False) and (self.global_track is True):
start_point = np.array([0, 0])
# create one source and one result array for filtering of points. Origin will be deleted later!
points_organized = np.copy(start_point)
self.points_unorganized = np.copy(points)
self.min_distance = np.array([])
# itterate trough all points
while (len(self.points_unorganized) != 0):
# call Nearest Neigbor Methode with last organized point
nearest_neigbor = self.find_nearest_point(np.array([points_organized[-2], points_organized[-1]]))
if centerpoints and len(points_organized) >= 8:
if (1.5 * np.mean(self.min_distance[1:-1]) >= self.min_distance[-1]):
points_organized = np.append(points_organized, nearest_neigbor)
else:
self.min_distance = self.min_distance[:-1]
else:
points_organized = np.append(points_organized, nearest_neigbor)
# reshape result array to [[x,y],[x,y], ... ]
points_organized = points_organized.reshape((-1, 2))
# Delete duplicated values
# points_organized = np.unique(points_organized, axis=0) # noqa: E800
# delet vehicle position
points_organized = np.delete(points_organized, 0, 0)
return points_organized
[docs] def find_nearest_point(self, origin: npt.NDArray) -> npt.NDArray:
"""
Find nearest point to last origin point. Delete used points from points_unorgaized class attribute.
Parameters
----------
origin : npt.NDArray
Position of last known point, shape: (2, ).
Returns
-------
npt.NDArray
Position of next ordered point.
"""
result = origin
# check if it's the last point
if (len(self.points_unorganized) == 1):
result = self.points_unorganized[0]
self.points_unorganized = np.delete(self.points_unorganized, 0, 0)
# calculate distance to new origin point
new_x = result[0] - origin[0]
new_y = result[1] - origin[1]
new_distance = np.sqrt(new_x**2 + new_y**2)
min_distance = np.min(new_distance)
self.min_distance = np.append(self.min_distance, min_distance)
return result
else:
# calculate distance to new origin point
new_x = self.points_unorganized[:, 0] - origin[0]
new_y = self.points_unorganized[:, 1] - origin[1]
new_distance = np.sqrt(new_x**2 + new_y**2)
# get index of points with minimum distance to new origin
i_min = np.argmin(new_distance)
min_distance = np.min(new_distance)
# ToDo: maybe check if there are more than one nearest point. If true delete both points
if (i_min.size == 1):
# append nearest point to result array
result = self.points_unorganized[i_min]
self.min_distance = np.append(self.min_distance, min_distance)
# delete all used points out of source array
self.points_unorganized = np.delete(self.points_unorganized, i_min, 0)
return result
[docs] def draw(self):
"""Plot the final results."""
alpha = np.linspace(0, 2 * np.pi, 50)
plt.figure(figsize=(8, 8))
colors = ['blue', 'orange', 'black', 'red']
for i in range(len(self.track_widths)):
x_1 = self.track_widths[i, 0] * np.cos(alpha) + self.centerpoints[i, 0]
x_2 = self.track_widths[i, 1] * np.cos(alpha) + self.centerpoints[i, 0]
y_1 = self.track_widths[i, 0] * np.sin(alpha) + self.centerpoints[i, 1]
y_2 = self.track_widths[i, 1] * np.sin(alpha) + self.centerpoints[i, 1]
plt.plot(x_1, y_1, '--', color='green', alpha=0.7)
plt.plot(x_2, y_2, '--', color='yellow', alpha=0.7)
if (self.left_not_empty):
plt.plot(self.left_cones[:, 0], self.left_cones[:, 1], 'o', color=colors[0], label='left')
if (self.right_not_empty):
plt.plot(self.right_cones[:, 0], self.right_cones[:, 1], 'o', color=colors[1], label='right')
if (len(self.centerpoints) > 0):
plt.plot(self.centerpoints[:, 0], self.centerpoints[:, 1], '.', color=colors[2], label='centerpoints')
plt.plot(self.vehicle_pose[0], self.vehicle_pose[1], '+', color=colors[3], label='vehicle pos')
plt.plot([self.vehicle_pose[0], self.vehicle_pose[0] + np.cos(self.vehicle_pose[2]) * 3],
[self.vehicle_pose[1], self.vehicle_pose[1] + np.sin(self.vehicle_pose[2]) * 3], '-', color=colors[3])
plt.legend()
plt.grid()
plt.xlabel('x [m]')
plt.ylabel('y [m]')
plt.xlim([-30, 30])
plt.ylim([-30, 30])
plt.show()