github/PythonRobotics
1
1
Fork 0
mirror of https://github.com/AtsushiSakai/PythonRobotics.git synced 2026-02-11 05:55:43 -05:00
Code Issues Packages Projects Releases Wiki Activity

code clean up

Browse Source
This commit is contained in:
Atsushi Sakai
2019-02-02 09:50:58 +09:00
parent 58b13e4253
commit 9c626d3f45
10 changed files with 124 additions and 100 deletions
Download Patch File Download Diff File Expand all files Collapse all files

83
PathPlanning/Eta3SplineTrajectory/eta3_spline_trajectory.py
View File

@@ -17,14 +17,19 @@ from matplotlib.collections import LineCollection
import sys import sys
import os import os
sys.path.append(os.path.relpath("../Eta3SplinePath")) sys.path.append(os.path.relpath("../Eta3SplinePath"))
try:
from eta3_spline_path import eta3_path, eta3_path_segment from eta3_spline_path import eta3_path, eta3_path_segment
except:
raise
show_animation = True show_animation = True
class MaxVelocityNotReached(Exception): class MaxVelocityNotReached(Exception):
def __init__(self, actual_vel, max_vel): def __init__(self, actual_vel, max_vel):
self.message = 'Actual velocity {} does not equal desired max velocity {}!'.format(actual_vel, max_vel) self.message = 'Actual velocity {} does not equal desired max velocity {}!'.format(
actual_vel, max_vel)
class eta3_trajectory(eta3_path): class eta3_trajectory(eta3_path):
@@ -34,6 +39,7 @@ class eta3_trajectory(eta3_path):
input input
segments: list of `eta3_trajectory_segment` instances defining a continuous trajectory segments: list of `eta3_trajectory_segment` instances defining a continuous trajectory
""" """
def __init__(self, segments, max_vel, v0=0.0, a0=0.0, max_accel=2.0, max_jerk=5.0): def __init__(self, segments, max_vel, v0=0.0, a0=0.0, max_accel=2.0, max_jerk=5.0):
# ensure that all inputs obey the assumptions of the model # ensure that all inputs obey the assumptions of the model
assert max_vel > 0 and v0 >= 0 and a0 >= 0 and max_accel > 0 and max_jerk > 0 \ assert max_vel > 0 and v0 >= 0 and a0 >= 0 and max_accel > 0 and max_jerk > 0 \
@@ -47,8 +53,9 @@ class eta3_trajectory(eta3_path):
self.max_jerk = float(max_jerk) self.max_jerk = float(max_jerk)
length_array = np.array([s.segment_length for s in self.segments]) length_array = np.array([s.segment_length for s in self.segments])
# add a zero to the beginning for finding the correct segment_id # add a zero to the beginning for finding the correct segment_id
self.cum_lengths = np.concatenate((np.array([0]), np.cumsum(length_array))) self.cum_lengths = np.concatenate(
## compute velocity profile on top of the path (np.array([0]), np.cumsum(length_array)))
# compute velocity profile on top of the path
self.velocity_profile() self.velocity_profile()
self.ui_prev = 0 self.ui_prev = 0
self.prev_seg_id = 0 self.prev_seg_id = 0
@@ -84,16 +91,17 @@ class eta3_trajectory(eta3_path):
# solve for the maximum achievable velocity based on the kinematic limits imposed by max_accel and max_jerk # solve for the maximum achievable velocity based on the kinematic limits imposed by max_accel and max_jerk
# this leads to a quadratic equation in v_max: a*v_max**2 + b*v_max + c = 0 # this leads to a quadratic equation in v_max: a*v_max**2 + b*v_max + c = 0
a = 1 / self.max_accel a = 1 / self.max_accel
b = 3. * self.max_accel / (2. * self.max_jerk) + v_s1 / self.max_accel - (self.max_accel**2 / self.max_jerk + v_s1) / self.max_accel b = 3. * self.max_accel / (2. * self.max_jerk) + v_s1 / self.max_accel - (
self.max_accel**2 / self.max_jerk + v_s1) / self.max_accel
c = s_s1 + s_sf - self.total_length - 7. * self.max_accel**3 / (3. * self.max_jerk**2) \ c = s_s1 + s_sf - self.total_length - 7. * self.max_accel**3 / (3. * self.max_jerk**2) \
- v_s1 * (self.max_accel / self.max_jerk + v_s1 / self.max_accel) \ - v_s1 * (self.max_accel / self.max_jerk + v_s1 / self.max_accel) \
+ (self.max_accel**2 / self.max_jerk + v_s1 / self.max_accel)**2 / (2. * self.max_accel) + (self.max_accel**2 / self.max_jerk + v_s1 /
self.max_accel)**2 / (2. * self.max_accel)
v_max = (-b + np.sqrt(b**2 - 4. * a * c)) / (2. * a) v_max = (-b + np.sqrt(b**2 - 4. * a * c)) / (2. * a)
# v_max represents the maximum velocity that could be attained if there was no cruise period # v_max represents the maximum velocity that could be attained if there was no cruise period
# (i.e. driving at constant speed without accelerating or jerking) # (i.e. driving at constant speed without accelerating or jerking)
# if this velocity is less than our desired max velocity, the max velocity needs to be updated # if this velocity is less than our desired max velocity, the max velocity needs to be updated
# XXX the way to handle this `if` condition needs to be more thoroughly worked through
if self.max_vel > v_max: if self.max_vel > v_max:
# when this condition is tripped, there will be no cruise period (s_cruise=0) # when this condition is tripped, there will be no cruise period (s_cruise=0)
self.max_vel = v_max self.max_vel = v_max
@@ -112,10 +120,13 @@ class eta3_trajectory(eta3_path):
# Section 1: accelerate at max_accel # Section 1: accelerate at max_accel
index = 1 index = 1
# compute change in velocity over the section # compute change in velocity over the section
delta_v = (self.max_vel - self.max_jerk * (self.max_accel / self.max_jerk)**2 / 2.) - self.vels[index-1] delta_v = (self.max_vel - self.max_jerk * (self.max_accel /
self.max_jerk)**2 / 2.) - self.vels[index - 1]
self.times[index] = delta_v / self.max_accel self.times[index] = delta_v / self.max_accel
self.vels[index] = self.vels[index-1] + self.max_accel * self.times[index] self.vels[index] = self.vels[index - 1] + \
self.seg_lengths[index] = self.vels[index-1] * self.times[index] + self.max_accel * self.times[index]**2 / 2. self.max_accel * self.times[index]
self.seg_lengths[index] = self.vels[index - 1] * \
self.times[index] + self.max_accel * self.times[index]**2 / 2.
# Section 2: decrease acceleration (down to 0) until max speed is hit # Section 2: decrease acceleration (down to 0) until max speed is hit
index = 2 index = 2
@@ -135,16 +146,20 @@ class eta3_trajectory(eta3_path):
# Section 4: apply min jerk until min acceleration is hit # Section 4: apply min jerk until min acceleration is hit
index = 4 index = 4
self.times[index] = self.max_accel / self.max_jerk self.times[index] = self.max_accel / self.max_jerk
self.vels[index] = self.max_vel - self.max_jerk * self.times[index]**2 / 2. self.vels[index] = self.max_vel - \
self.seg_lengths[index] = self.max_vel * self.times[index] - self.max_jerk * self.times[index]**3 / 6. self.max_jerk * self.times[index]**2 / 2.
self.seg_lengths[index] = self.max_vel * self.times[index] - \
self.max_jerk * self.times[index]**3 / 6.
# Section 5: continue deceleration at max rate # Section 5: continue deceleration at max rate
index = 5 index = 5
# compute velocity change over sections # compute velocity change over sections
delta_v = self.vels[index - 1] - v_sf delta_v = self.vels[index - 1] - v_sf
self.times[index] = delta_v / self.max_accel self.times[index] = delta_v / self.max_accel
self.vels[index] = self.vels[index-1] - self.max_accel * self.times[index] self.vels[index] = self.vels[index - 1] - \
self.seg_lengths[index] = self.vels[index-1] * self.times[index] - self.max_accel * self.times[index]**2 / 2. self.max_accel * self.times[index]
self.seg_lengths[index] = self.vels[index - 1] * \
self.times[index] - self.max_accel * self.times[index]**2 / 2.
# Section 6(final): max jerk to get to zero velocity and zero acceleration simultaneously # Section 6(final): max jerk to get to zero velocity and zero acceleration simultaneously
index = 6 index = 6
@@ -164,7 +179,8 @@ class eta3_trajectory(eta3_path):
# the length of the cruise section is whatever length hasn't already been accounted for # the length of the cruise section is whatever length hasn't already been accounted for
# NOTE: the total array self.seg_lengths is summed because the entry for the cruise segment is # NOTE: the total array self.seg_lengths is summed because the entry for the cruise segment is
# initialized to 0! # initialized to 0!
self.seg_lengths[index] = self.total_length - self.seg_lengths.sum() self.seg_lengths[index] = self.total_length - \
self.seg_lengths.sum()
self.vels[index] = self.max_vel self.vels[index] = self.max_vel
self.times[index] = self.seg_lengths[index] / self.max_vel self.times[index] = self.seg_lengths[index] / self.max_vel
@@ -174,8 +190,9 @@ class eta3_trajectory(eta3_path):
self.total_time = self.times.sum() self.total_time = self.times.sum()
def get_interp_param(self, seg_id, s, ui, tol=0.001): def get_interp_param(self, seg_id, s, ui, tol=0.001):
f = lambda u: self.segments[seg_id].f_length(u)[0] - s def f(u): return self.segments[seg_id].f_length(u)[0] - s
fprime = lambda u: self.segments[seg_id].s_dot(u)
def fprime(u): return self.segments[seg_id].s_dot(u)
while (ui >= 0 and ui <= 1) and abs(f(ui)) > tol: while (ui >= 0 and ui <= 1) and abs(f(ui)) > tol:
ui -= f(ui) / fprime(ui) ui -= f(ui) / fprime(ui)
ui = max(0, min(ui, 1)) ui = max(0, min(ui, 1))
@@ -190,11 +207,13 @@ class eta3_trajectory(eta3_path):
elif time <= self.times[:2].sum(): elif time <= self.times[:2].sum():
delta_t = time - self.times[0] delta_t = time - self.times[0]
linear_velocity = self.vels[0] + self.max_accel * delta_t linear_velocity = self.vels[0] + self.max_accel * delta_t
s = self.seg_lengths[0] + self.vels[0] * delta_t + self.max_accel * delta_t**2 / 2. s = self.seg_lengths[0] + self.vels[0] * \
delta_t + self.max_accel * delta_t**2 / 2.
linear_accel = self.max_accel linear_accel = self.max_accel
elif time <= self.times[:3].sum(): elif time <= self.times[:3].sum():
delta_t = time - self.times[:2].sum() delta_t = time - self.times[:2].sum()
linear_velocity = self.vels[1] + self.max_accel * delta_t - self.max_jerk * delta_t**2 / 2. linear_velocity = self.vels[1] + self.max_accel * \
delta_t - self.max_jerk * delta_t**2 / 2.
s = self.seg_lengths[:2].sum() + self.vels[1] * delta_t + self.max_accel * delta_t**2 / 2. \ s = self.seg_lengths[:2].sum() + self.vels[1] * delta_t + self.max_accel * delta_t**2 / 2. \
- self.max_jerk * delta_t**3 / 6. - self.max_jerk * delta_t**3 / 6.
linear_accel = self.max_accel - self.max_jerk * delta_t linear_accel = self.max_accel - self.max_jerk * delta_t
@@ -206,16 +225,19 @@ class eta3_trajectory(eta3_path):
elif time <= self.times[:5].sum(): elif time <= self.times[:5].sum():
delta_t = time - self.times[:4].sum() delta_t = time - self.times[:4].sum()
linear_velocity = self.vels[3] - self.max_jerk * delta_t**2 / 2. linear_velocity = self.vels[3] - self.max_jerk * delta_t**2 / 2.
s = self.seg_lengths[:4].sum() + self.vels[3] * delta_t - self.max_jerk * delta_t**3 / 6. s = self.seg_lengths[:4].sum() + self.vels[3] * \
delta_t - self.max_jerk * delta_t**3 / 6.
linear_accel = -self.max_jerk * delta_t linear_accel = -self.max_jerk * delta_t
elif time <= self.times[:-1].sum(): elif time <= self.times[:-1].sum():
delta_t = time - self.times[:5].sum() delta_t = time - self.times[:5].sum()
linear_velocity = self.vels[4] - self.max_accel * delta_t linear_velocity = self.vels[4] - self.max_accel * delta_t
s = self.seg_lengths[:5].sum() + self.vels[4] * delta_t - self.max_accel * delta_t**2 / 2. s = self.seg_lengths[:5].sum() + self.vels[4] * \
delta_t - self.max_accel * delta_t**2 / 2.
linear_accel = -self.max_accel linear_accel = -self.max_accel
elif time < self.times.sum(): elif time < self.times.sum():
delta_t = time - self.times[:-1].sum() delta_t = time - self.times[:-1].sum()
linear_velocity = self.vels[5] - self.max_accel * delta_t + self.max_jerk * delta_t**2 / 2. linear_velocity = self.vels[5] - self.max_accel * \
delta_t + self.max_jerk * delta_t**2 / 2.
s = self.seg_lengths[:-1].sum() + self.vels[5] * delta_t - self.max_accel * delta_t**2 / 2. \ s = self.seg_lengths[:-1].sum() + self.vels[5] * delta_t - self.max_accel * delta_t**2 / 2. \
+ self.max_jerk * delta_t**3 / 6. + self.max_jerk * delta_t**3 / 6.
linear_accel = -self.max_accel + self.max_jerk * delta_t linear_accel = -self.max_accel + self.max_jerk * delta_t
@@ -232,7 +254,8 @@ class eta3_trajectory(eta3_path):
else: else:
# compute interpolation parameter using length from current segment's starting point # compute interpolation parameter using length from current segment's starting point
curr_segment_length = s - self.cum_lengths[seg_id] curr_segment_length = s - self.cum_lengths[seg_id]
ui = self.get_interp_param(seg_id=seg_id, s=curr_segment_length, ui=self.ui_prev) ui = self.get_interp_param(
seg_id=seg_id, s=curr_segment_length, ui=self.ui_prev)
if not seg_id == self.prev_seg_id: if not seg_id == self.prev_seg_id:
self.ui_prev = 0 self.ui_prev = 0
@@ -240,7 +263,6 @@ class eta3_trajectory(eta3_path):
self.ui_prev = ui self.ui_prev = ui
self.prev_seg_id = seg_id self.prev_seg_id = seg_id
# TODO(jwd): normalize!
# compute angular velocity of current point= (ydd*xd - xdd*yd) / (xd**2 + yd**2) # compute angular velocity of current point= (ydd*xd - xdd*yd) / (xd**2 + yd**2)
d = self.segments[seg_id].calc_deriv(ui, order=1) d = self.segments[seg_id].calc_deriv(ui, order=1)
dd = self.segments[seg_id].calc_deriv(ui, order=2) dd = self.segments[seg_id].calc_deriv(ui, order=2)
@@ -250,7 +272,8 @@ class eta3_trajectory(eta3_path):
# ut - time-derivative of interpolation parameter u # ut - time-derivative of interpolation parameter u
ut = linear_velocity / su ut = linear_velocity / su
# utt - time-derivative of ut # utt - time-derivative of ut
utt = linear_accel / su - (d[0] * dd[0] + d[1] * dd[1]) / su**2 * ut utt = linear_accel / su - \
(d[0] * dd[0] + d[1] * dd[1]) / su**2 * ut
xt = d[0] * ut xt = d[0] * ut
yt = d[1] * ut yt = d[1] * ut
xtt = dd[0] * ut**2 + d[0] * utt xtt = dd[0] * ut**2 + d[0] * utt
@@ -261,7 +284,8 @@ class eta3_trajectory(eta3_path):
# combine path point with orientation and velocities # combine path point with orientation and velocities
pos = self.segments[seg_id].calc_point(ui) pos = self.segments[seg_id].calc_point(ui)
state = np.array([pos[0], pos[1], np.arctan2(d[1], d[0]), linear_velocity, angular_velocity]) state = np.array([pos[0], pos[1], np.arctan2(
d[1], d[0]), linear_velocity, angular_velocity])
return state return state
@@ -278,7 +302,8 @@ def test1(max_vel=0.5):
trajectory_segments.append(eta3_path_segment( trajectory_segments.append(eta3_path_segment(
start_pose=start_pose, end_pose=end_pose, eta=eta, kappa=kappa)) start_pose=start_pose, end_pose=end_pose, eta=eta, kappa=kappa))
traj = eta3_trajectory(trajectory_segments, max_vel=max_vel, max_accel=0.5) traj = eta3_trajectory(trajectory_segments,
max_vel=max_vel, max_accel=0.5)
# interpolate at several points along the path # interpolate at several points along the path
times = np.linspace(0, traj.total_time, 101) times = np.linspace(0, traj.total_time, 101)
@@ -311,7 +336,8 @@ def test2(max_vel=0.5):
trajectory_segments.append(eta3_path_segment( trajectory_segments.append(eta3_path_segment(
start_pose=start_pose, end_pose=end_pose, eta=eta, kappa=kappa)) start_pose=start_pose, end_pose=end_pose, eta=eta, kappa=kappa))
traj = eta3_trajectory(trajectory_segments, max_vel=max_vel, max_accel=0.5) traj = eta3_trajectory(trajectory_segments,
max_vel=max_vel, max_accel=0.5)
# interpolate at several points along the path # interpolate at several points along the path
times = np.linspace(0, traj.total_time, 101) times = np.linspace(0, traj.total_time, 101)
@@ -376,7 +402,8 @@ def test3(max_vel=2.0):
start_pose=start_pose, end_pose=end_pose, eta=eta, kappa=kappa)) start_pose=start_pose, end_pose=end_pose, eta=eta, kappa=kappa))
# construct the whole path # construct the whole path
traj = eta3_trajectory(trajectory_segments, max_vel=max_vel, max_accel=0.5, max_jerk=1) traj = eta3_trajectory(trajectory_segments,
max_vel=max_vel, max_accel=0.5, max_jerk=1)
# interpolate at several points along the path # interpolate at several points along the path
times = np.linspace(0, traj.total_time, 1001) times = np.linspace(0, traj.total_time, 1001)

20
PathPlanning/FrenetOptimalTrajectory/cubic_spline_planner.py
View File

@@ -40,7 +40,7 @@ class Spline:
self.b.append(tb) self.b.append(tb)
def calc(self, t): def calc(self, t):
u""" """
Calc position Calc position
if t is outside of the input x, return None if t is outside of the input x, return None
@@ -60,7 +60,7 @@ class Spline:
return result return result
def calcd(self, t): def calcd(self, t):
u""" """
Calc first derivative Calc first derivative
if t is outside of the input x, return None if t is outside of the input x, return None
@@ -77,7 +77,7 @@ class Spline:
return result return result
def calcdd(self, t): def calcdd(self, t):
u""" """
Calc second derivative Calc second derivative
""" """
@@ -92,13 +92,13 @@ class Spline:
return result return result
def __search_index(self, x): def __search_index(self, x):
u""" """
search data segment index search data segment index
""" """
return bisect.bisect(self.x, x) - 1 return bisect.bisect(self.x, x) - 1
def __calc_A(self, h): def __calc_A(self, h):
u""" """
calc matrix A for spline coefficient c calc matrix A for spline coefficient c
""" """
A = np.zeros((self.nx, self.nx)) A = np.zeros((self.nx, self.nx))
@@ -116,7 +116,7 @@ class Spline:
return A return A
def __calc_B(self, h): def __calc_B(self, h):
u""" """
calc matrix B for spline coefficient c calc matrix B for spline coefficient c
""" """
B = np.zeros(self.nx) B = np.zeros(self.nx)
@@ -128,7 +128,7 @@ class Spline:
class Spline2D: class Spline2D:
u""" """
2D Cubic Spline class 2D Cubic Spline class
""" """
@@ -148,7 +148,7 @@ class Spline2D:
return s return s
def calc_position(self, s): def calc_position(self, s):
u""" """
calc position calc position
""" """
x = self.sx.calc(s) x = self.sx.calc(s)
@@ -157,7 +157,7 @@ class Spline2D:
return x, y return x, y
def calc_curvature(self, s): def calc_curvature(self, s):
u""" """
calc curvature calc curvature
""" """
dx = self.sx.calcd(s) dx = self.sx.calcd(s)
@@ -168,7 +168,7 @@ class Spline2D:
return k return k
def calc_yaw(self, s): def calc_yaw(self, s):
u""" """
calc yaw calc yaw
""" """
dx = self.sx.calcd(s) dx = self.sx.calcd(s)

2
PathPlanning/ModelPredictiveTrajectoryGenerator/model_predictive_trajectory_generator.py
View File

@@ -20,7 +20,7 @@ show_animation = False
def plot_arrow(x, y, yaw, length=1.0, width=0.5, fc="r", ec="k"): def plot_arrow(x, y, yaw, length=1.0, width=0.5, fc="r", ec="k"):
u""" """
Plot arrow Plot arrow
""" """
plt.arrow(x, y, length * math.cos(yaw), length * math.sin(yaw), plt.arrow(x, y, length * math.cos(yaw), length * math.sin(yaw),

12
PathPlanning/ProbabilisticRoadMap/probabilistic_road_map.py
View File

@@ -45,7 +45,7 @@ class KDTree:
self.tree = scipy.spatial.cKDTree(data) self.tree = scipy.spatial.cKDTree(data)
def search(self, inp, k=1): def search(self, inp, k=1):
u""" """
Search NN Search NN
inp: input data, single frame or multi frame inp: input data, single frame or multi frame
@@ -62,12 +62,12 @@ class KDTree:
dist.append(idist) dist.append(idist)
return index, dist return index, dist
else:
dist, index = self.tree.query(inp, k=k) dist, index = self.tree.query(inp, k=k)
return index, dist return index, dist
def search_in_distance(self, inp, r): def search_in_distance(self, inp, r):
u""" """
find points with in a distance r find points with in a distance r
""" """
@@ -176,7 +176,7 @@ def dijkstra_planning(sx, sy, gx, gy, ox, oy, rr, road_map, sample_x, sample_y):
openset[len(road_map) - 2] = nstart openset[len(road_map) - 2] = nstart
while True: while True:
if len(openset) == 0: if not openset:
print("Cannot find path") print("Cannot find path")
break break
@@ -232,7 +232,7 @@ def dijkstra_planning(sx, sy, gx, gy, ox, oy, rr, road_map, sample_x, sample_y):
def plot_road_map(road_map, sample_x, sample_y): def plot_road_map(road_map, sample_x, sample_y):
for i in range(len(road_map)): for i, _ in enumerate(road_map):
for ii in range(len(road_map[i])): for ii in range(len(road_map[i])):
ind = road_map[i][ii] ind = road_map[i][ii]
@@ -307,7 +307,7 @@ def main():
rx, ry = PRM_planning(sx, sy, gx, gy, ox, oy, robot_size) rx, ry = PRM_planning(sx, sy, gx, gy, ox, oy, robot_size)
assert len(rx) != 0, 'Cannot found path' assert rx, 'Cannot found path'
if show_animation: if show_animation:
plt.plot(rx, ry, "-r") plt.plot(rx, ry, "-r")

2
PathPlanning/RRTStarDubins/dubins_path_planning.py
View File

@@ -259,7 +259,7 @@ def generate_course(length, mode, c):
def plot_arrow(x, y, yaw, length=1.0, width=0.5, fc="r", ec="k"): def plot_arrow(x, y, yaw, length=1.0, width=0.5, fc="r", ec="k"):
u""" """
Plot arrow Plot arrow
""" """

2
PathPlanning/RRTStarDubins/rrt_star_dubins.py
View File

@@ -203,7 +203,7 @@ class RRT():
self.nodeList[i] = tNode self.nodeList[i] = tNode
def DrawGraph(self, rnd=None): def DrawGraph(self, rnd=None):
u""" """
Draw Graph Draw Graph
""" """
plt.clf() plt.clf()

5
PathPlanning/RRTstar/rrt_star.py
View File

@@ -71,7 +71,7 @@ class RRT():
return path return path
def choose_parent(self, newNode, nearinds): def choose_parent(self, newNode, nearinds):
if len(nearinds) == 0: if not nearinds:
return newNode return newNode
dlist = [] dlist = []
@@ -130,9 +130,8 @@ class RRT():
disglist = [self.calc_dist_to_goal( disglist = [self.calc_dist_to_goal(
node.x, node.y) for node in self.nodeList] node.x, node.y) for node in self.nodeList]
goalinds = [disglist.index(i) for i in disglist if i <= self.expandDis] goalinds = [disglist.index(i) for i in disglist if i <= self.expandDis]
# print(goalinds)
if len(goalinds) == 0: if not goalinds:
return None return None
mincost = min([self.nodeList[i].cost for i in goalinds]) mincost = min([self.nodeList[i].cost for i in goalinds])

4
PathPlanning/ReedsSheppPath/reeds_shepp_path_planning.py
View File

@@ -357,14 +357,14 @@ def reeds_shepp_path_planning(sx, sy, syaw,
paths = calc_paths(sx, sy, syaw, gx, gy, gyaw, maxc, step_size) paths = calc_paths(sx, sy, syaw, gx, gy, gyaw, maxc, step_size)
if len(paths) == 0: if not paths:
# print("No path") # print("No path")
# print(sx, sy, syaw, gx, gy, gyaw) # print(sx, sy, syaw, gx, gy, gyaw)
return None, None, None, None, None return None, None, None, None, None
minL = float("Inf") minL = float("Inf")
best_path_index = -1 best_path_index = -1
for i in range(len(paths)): for i, _ in enumerate(paths):
if paths[i].L <= minL: if paths[i].L <= minL:
minL = paths[i].L minL = paths[i].L
best_path_index = i best_path_index = i

12
PathPlanning/VoronoiRoadMap/voronoi_road_map.py
View File

@@ -43,7 +43,7 @@ class KDTree:
self.tree = scipy.spatial.cKDTree(data) self.tree = scipy.spatial.cKDTree(data)
def search(self, inp, k=1): def search(self, inp, k=1):
u""" """
Search NN Search NN
inp: input data, single frame or multi frame inp: input data, single frame or multi frame
@@ -60,12 +60,12 @@ class KDTree:
dist.append(idist) dist.append(idist)
return index, dist return index, dist
else:
dist, index = self.tree.query(inp, k=k) dist, index = self.tree.query(inp, k=k)
return index, dist return index, dist
def search_in_distance(self, inp, r): def search_in_distance(self, inp, r):
u""" """
find points with in a distance r find points with in a distance r
""" """
@@ -175,7 +175,7 @@ def dijkstra_planning(sx, sy, gx, gy, ox, oy, rr, road_map, sample_x, sample_y):
openset[len(road_map) - 2] = nstart openset[len(road_map) - 2] = nstart
while True: while True:
if len(openset) == 0: if not openset:
print("Cannot find path") print("Cannot find path")
break break
@@ -231,7 +231,7 @@ def dijkstra_planning(sx, sy, gx, gy, ox, oy, rr, road_map, sample_x, sample_y):
def plot_road_map(road_map, sample_x, sample_y): def plot_road_map(road_map, sample_x, sample_y):
for i in range(len(road_map)): for i, _ in enumerate(road_map):
for ii in range(len(road_map[i])): for ii in range(len(road_map[i])):
ind = road_map[i][ii] ind = road_map[i][ii]
@@ -296,7 +296,7 @@ def main():
rx, ry = VRM_planning(sx, sy, gx, gy, ox, oy, robot_size) rx, ry = VRM_planning(sx, sy, gx, gy, ox, oy, robot_size)
assert len(rx) != 0, 'Cannot found path' assert rx, 'Cannot found path'
if show_animation: if show_animation:
plt.plot(rx, ry, "-r") plt.plot(rx, ry, "-r")

24
PathTracking/model_predictive_speed_and_steer_control/model_predictive_speed_and_steer_control.py
View File

@@ -5,14 +5,18 @@ Path tracking simulation with iterative linear model predictive control for spee
author: Atsushi Sakai (@Atsushi_twi) author: Atsushi Sakai (@Atsushi_twi)
""" """
import matplotlib.pyplot as plt
import cvxpy
import math
import numpy as np
import sys import sys
sys.path.append("../../PathPlanning/CubicSpline/") sys.path.append("../../PathPlanning/CubicSpline/")
import numpy as np try:
import math
import cvxpy
import matplotlib.pyplot as plt
import cubic_spline_planner import cubic_spline_planner
except:
raise
NX = 4 # x = x, y, v, yaw NX = 4 # x = x, y, v, yaw
NU = 2 # a = [accel, steer] NU = 2 # a = [accel, steer]
@@ -208,7 +212,7 @@ def calc_nearest_index(state, cx, cy, cyaw, pind):
def predict_motion(x0, oa, od, xref): def predict_motion(x0, oa, od, xref):
xbar = xref * 0.0 xbar = xref * 0.0
for i in range(len(x0)): for i, _ in enumerate(x0):
xbar[i, 0] = x0[i] xbar[i, 0] = x0[i]
state = State(x=x0[0], y=x0[1], yaw=x0[3], v=x0[2]) state = State(x=x0[0], y=x0[1], yaw=x0[3], v=x0[2])
@@ -346,18 +350,12 @@ def check_goal(state, goal, tind, nind):
dy = state.y - goal[1] dy = state.y - goal[1]
d = math.sqrt(dx ** 2 + dy ** 2) d = math.sqrt(dx ** 2 + dy ** 2)
if (d <= GOAL_DIS): isgoal = (d <= GOAL_DIS)
isgoal = True
else:
isgoal = False
if abs(tind - nind) >= 5: if abs(tind - nind) >= 5:
isgoal = False isgoal = False
if (abs(state.v) <= STOP_SPEED): isstop = (abs(state.v) <= STOP_SPEED)
isstop = True
else:
isstop = False
if isgoal and isstop: if isgoal and isstop:
return True return True