def generateTrack(self, z=0.0):
"""Generates the left and right boundaries from the centerline"""
left, right = [], []
import matplotlib.pyplot as plt
for i in range(self.center.length - 1):
ix, iy = self.center.x[i], self.center.y[i]
dx, dy = self.center.dx[i], self.center.dy[i]
length = np.linalg.norm(np.array([dx, dy]))
dx = dx * self.width / (2 * length)
dy = dy * self.width / (2 * length)
left.append([ix - dy, iy + dx])
right.append([ix + dy, iy - dx])
if self.closed:
if left[0] != left[-1] and right[0] != right[-1]:
left.append(left[0])
right.append(right[0])
self.left = Path(left,
self.num_points,
closed=self.closed,
raw_mode=self.raw_mode)
self.right = Path(right,
self.num_points,
closed=self.closed,
raw_mode=self.raw_mode)
self.left_waypoints = left
self.right_waypoints = right
def generate_final_path(self):
points = []
for i in range(self.size):
points.append([self.x[i], self.y[i]])
final_path = Path(points, num_points=500)
points = []
for i in range(final_path.length):
points.append([final_path.x[i], final_path.y[i]])
final_track_path = Path(points, raw_mode=True)
self.final_path = final_track_path
def __init__(self,
center,
width=10,
num_points=1000,
closed=True,
raw_mode=False):
"""
Parameters
----------
center : ChBezierCurve
the centerline
width : int, optional
constant distance from the centerline to each boundary
num_points : int, optional
num points to interpolate along path
"""
self.raw_mode = raw_mode
self.closed = closed
self.center = Path(center,
num_points,
closed=closed,
raw_mode=raw_mode)
self.width = width
self.num_points = num_points
def create_segmentations(self):
"""Create the segmentations"""
sk_sum = 0.0
cum_distance = 0.0
right_points = []
left_points = []
width = []
for i in range(self.track.center.length):
distance = 0 if i == 0 else self.track.center.ps[i - 1]
cum_distance += distance
k = np.abs(self.track.center.k[i])
sk_sum += distance * k
# The first segmentation wouldn't be recorded until condition is meet,
# so we need to ensure the last point is recorded.
if sk_sum >= self.precision or \
cum_distance >= self.max_distance or \
i == self.track.center.length-1:
sk_sum = 0
cum_distance = 0
left = self.track.left_waypoints[i]
right = self.track.right_waypoints[i]
left_points.append(left)
right_points.append(right)
width.append(
np.sqrt(
np.square(left[0] - right[0]) +
np.square(left[1] - right[1])))
self.size += 1
self.left = Path(left_points, closed=True, raw_mode=True)
self.right = Path(right_points, closed=True, raw_mode=True)
self.width = np.array(width)
def Advance(self, step, vehicle):
state = vehicle.GetState()
self.sentinel = chrono.ChVectorD(
self.lookDist * math.cos(state.yaw) + state.x,
self.lookDist * math.sin(state.yaw) + state.y,
0,
)
self.target = self.path.calcClosestPoint(self.sentinel)
targ = chrono.ChVectorD(self.target.x, self.target.y, 0.5)
self.sentinel.z = 0.5
io = self.path.calcIndex(state) + self.obsDist
ist = self.path.calcIndex(state)
it = self.path.calcIndex(targ)
loc = 0
#print(ist)
self.obstacle = ()
for i in range(ist, io + 1):
try:
self.obstacle = self.obstacles[i]
loc = i
self.obstacleInRange = True
if self.gap == 0:
self.gap = self.path.getDistance(
loc) - self.path.getDistance(it)
elif self.path.getDistance(loc) - self.path.getDistance(
it) > self.gap:
self.gap = self.path.getDistance(
loc) - self.path.getDistance(it)
except KeyError:
x = 0
if ist > loc and self.obstacleInRange:
self.obstacleInRange = False
self.gap = 0
self.tempObstacleAvoidancePath = ()
tempObstacleAvoidancePath = ()
if self.obstacleInRange:
self.och_time = self.obstacle.ch_time
self.distanceToObstacle = self.path.getDistance(
loc) - self.path.getDistance(it)
if self.distanceToObstacle < 1000:
#self.gap = 50
left = True
if self.path.getCurvature(it) < 0:
left = False
self.alterTargetForObstacle(
it=it,
left=left,
s=5,
a=0.75,
dist=-((self.distanceToObstacle / self.gap) * 10 - 5),
targ=targ,
state=state)
tempObstacleAvoidancePathArray = self.generateTempObstacleAvoidancePath(
loc, ist, it, left, 5, 0.75, targ, state)
#self.tempObstacleAvoidancePath = Path(tempObstacleAvoidancePathArray, loc-it + 10)
self.tempObstacleAvoidancePath = Path(
tempObstacleAvoidancePathArray, 100, per=False)
#print("Index: " + str(self.obstacle.i))
#print("Predict: " + str(self.predictObstacleIndex()))
#print(self.obstacle.ch_time)
else:
self.obstacleInRange = False
#print("NEW Target X: " + str(targ.x))
#print("NEW Target Y: " + str(targ.y))
# The "error" vector is the projection onto the horizontal plane (z=0) of
# vector between sentinel and target
err_vec = targ - self.sentinel
err_vec.z = 0
# Calculate the sign of the angle between the projections of the sentinel
# vector and the target vector (with origin at vehicle location).
sign = self.calcSign(state, targ)
# Calculate current error (magnitude)
err = sign * err_vec.Length()
# Estimate error derivative (backward FD approximation).
self.errd = (err - self.err) / step
# Calculate current error integral (trapezoidal rule).
self.erri += (err + self.err) * step / 2
# Cache new error
self.err = err
# Return PID output (steering value)
self.steering = np.clip(
self.Kp * self.err + self.Ki * self.erri + self.Kd * self.errd,
-1.0, 1.0)
vehicle.driver.SetTargetSteering(self.steering)
return self.steering, self.obstacleInRange, self.tempObstacleAvoidancePath
class Track:
"""
Track class that has a center, left and right path
...
Attributes
----------
center : Path
the path object that describes the centerline
right : Path
the path object that describes the right boundary
left : Path
the path object that describes the left boundary
width : int
constant width of the track
num_points : int
number of points to interpolate along path
Methods
-------
generateTrack()
generates the left and right boundaries from the centerline
plot(show=True)
plots the track using matplotlib
"""
def __init__(self,
center,
width=10,
num_points=1000,
closed=True,
raw_mode=False):
"""
Parameters
----------
center : ChBezierCurve
the centerline
width : int, optional
constant distance from the centerline to each boundary
num_points : int, optional
num points to interpolate along path
"""
self.raw_mode = raw_mode
self.closed = closed
self.center = Path(center,
num_points,
closed=closed,
raw_mode=raw_mode)
self.width = width
self.num_points = num_points
def generateTrack(self, z=0.0):
"""Generates the left and right boundaries from the centerline"""
left, right = [], []
import matplotlib.pyplot as plt
for i in range(self.center.length - 1):
ix, iy = self.center.x[i], self.center.y[i]
dx, dy = self.center.dx[i], self.center.dy[i]
length = np.linalg.norm(np.array([dx, dy]))
dx = dx * self.width / (2 * length)
dy = dy * self.width / (2 * length)
left.append([ix - dy, iy + dx])
right.append([ix + dy, iy - dx])
if self.closed:
if left[0] != left[-1] and right[0] != right[-1]:
left.append(left[0])
right.append(right[0])
self.left = Path(left,
self.num_points,
closed=self.closed,
raw_mode=self.raw_mode)
self.right = Path(right,
self.num_points,
closed=self.closed,
raw_mode=self.raw_mode)
self.left_waypoints = left
self.right_waypoints = right
def checkBoundary(self, pos, n=20):
"""Checks if current position is within boundaries or not"""
track_pos = self.center.calcClosestPoint(pos, n=n)
dist = (track_pos - pos).Length()
return dist < (width / 2)
def plot(self, show=True, centerline=True):
"""Plots track using matplotlib
Parameters
----------
show : bool, optional
if plot.show() be called
"""
import matplotlib.pyplot as plt
plt.axis('equal')
if centerline:
self.center.plot(color='-r', show=False)
self.right.plot(color='-k', show=False)
self.left.plot(color='-k', show=False)
if show:
plt.show()
def update(val):
path = Path(generator.generatePath(seed=seed_slider.val))
plt.cla()
path.plot(color='-r', show=False)