From 9c626d3f45d48ad719c82cb307b2252a3797a4ef Mon Sep 17 00:00:00 2001 From: Atsushi Sakai Date: Sat, 2 Feb 2019 09:50:58 +0900 Subject: [PATCH] code clean up --- .../eta3_spline_trajectory.py | 123 +++++++++++------- .../cubic_spline_planner.py | 20 +-- .../model_predictive_trajectory_generator.py | 2 +- .../probabilistic_road_map.py | 16 +-- .../RRTStarDubins/dubins_path_planning.py | 2 +- PathPlanning/RRTStarDubins/rrt_star_dubins.py | 2 +- PathPlanning/RRTstar/rrt_star.py | 5 +- .../reeds_shepp_path_planning.py | 4 +- .../VoronoiRoadMap/voronoi_road_map.py | 16 +-- ...odel_predictive_speed_and_steer_control.py | 34 +++-- 10 files changed, 124 insertions(+), 100 deletions(-) diff --git a/PathPlanning/Eta3SplineTrajectory/eta3_spline_trajectory.py b/PathPlanning/Eta3SplineTrajectory/eta3_spline_trajectory.py index cb1b06f2..1c78807c 100644 --- a/PathPlanning/Eta3SplineTrajectory/eta3_spline_trajectory.py +++ b/PathPlanning/Eta3SplineTrajectory/eta3_spline_trajectory.py @@ -17,14 +17,19 @@ from matplotlib.collections import LineCollection import sys import os sys.path.append(os.path.relpath("../Eta3SplinePath")) -from eta3_spline_path import eta3_path, eta3_path_segment + +try: + from eta3_spline_path import eta3_path, eta3_path_segment +except: + raise show_animation = True class MaxVelocityNotReached(Exception): 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): @@ -34,6 +39,7 @@ class eta3_trajectory(eta3_path): input 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): # 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 \ @@ -47,8 +53,9 @@ class eta3_trajectory(eta3_path): self.max_jerk = float(max_jerk) length_array = np.array([s.segment_length for s in self.segments]) # add a zero to the beginning for finding the correct segment_id - self.cum_lengths = np.concatenate((np.array([0]), np.cumsum(length_array))) - ## compute velocity profile on top of the path + self.cum_lengths = np.concatenate( + (np.array([0]), np.cumsum(length_array))) + # compute velocity profile on top of the path self.velocity_profile() self.ui_prev = 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 # this leads to a quadratic equation in v_max: a*v_max**2 + b*v_max + c = 0 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) \ - 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) - 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 + + (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 represents the maximum velocity that could be attained if there was no cruise period # (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 - # XXX the way to handle this `if` condition needs to be more thoroughly worked through if self.max_vel > v_max: # when this condition is tripped, there will be no cruise period (s_cruise=0) self.max_vel = v_max @@ -112,22 +120,25 @@ class eta3_trajectory(eta3_path): # Section 1: accelerate at max_accel index = 1 # 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.vels[index] = self.vels[index-1] + 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. + self.vels[index] = self.vels[index - 1] + \ + 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 index = 2 self.times[index] = self.max_accel / self.max_jerk - self.vels[index] = self.vels[index-1] + self.max_accel * self.times[index] \ + self.vels[index] = self.vels[index - 1] + self.max_accel * self.times[index] \ - self.max_jerk * self.times[index]**2 / 2. - + # as a check, the velocity at the end of the section should be self.max_vel if not np.isclose(self.vels[index], self.max_vel): raise MaxVelocityNotReached(self.vels[index], self.max_vel) - self.seg_lengths[index] = self.vels[index-1] * self.times[index] + self.max_accel * self.times[index]**2 / 2. \ + self.seg_lengths[index] = self.vels[index - 1] * self.times[index] + self.max_accel * self.times[index]**2 / 2. \ - self.max_jerk * self.times[index]**3 / 6. # Section 3: will be done last @@ -135,21 +146,25 @@ class eta3_trajectory(eta3_path): # Section 4: apply min jerk until min acceleration is hit index = 4 self.times[index] = self.max_accel / self.max_jerk - self.vels[index] = self.max_vel - 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. + self.vels[index] = self.max_vel - \ + 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 index = 5 # 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.vels[index] = self.vels[index-1] - 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. + self.vels[index] = self.vels[index - 1] - \ + 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 index = 6 self.times[index] = t_sf - self.vels[index] = self.vels[index-1] - self.max_jerk * t_sf**2 / 2. + self.vels[index] = self.vels[index - 1] - self.max_jerk * t_sf**2 / 2. try: assert np.isclose(self.vels[index], 0) @@ -164,7 +179,8 @@ class eta3_trajectory(eta3_path): # 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 # 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.times[index] = self.seg_lengths[index] / self.max_vel @@ -174,8 +190,9 @@ class eta3_trajectory(eta3_path): self.total_time = self.times.sum() def get_interp_param(self, seg_id, s, ui, tol=0.001): - f = lambda u: self.segments[seg_id].f_length(u)[0] - s - fprime = lambda u: self.segments[seg_id].s_dot(u) + def f(u): return self.segments[seg_id].f_length(u)[0] - s + + def fprime(u): return self.segments[seg_id].s_dot(u) while (ui >= 0 and ui <= 1) and abs(f(ui)) > tol: ui -= f(ui) / fprime(ui) ui = max(0, min(ui, 1)) @@ -190,12 +207,14 @@ class eta3_trajectory(eta3_path): elif time <= self.times[:2].sum(): delta_t = time - self.times[0] 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 elif time <= self.times[:3].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. - s = self.seg_lengths[:2].sum() + self.vels[1] * delta_t + self.max_accel * 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. \ - self.max_jerk * delta_t**3 / 6. linear_accel = self.max_accel - self.max_jerk * delta_t elif time <= self.times[:4].sum(): @@ -206,19 +225,22 @@ class eta3_trajectory(eta3_path): elif time <= self.times[:5].sum(): delta_t = time - self.times[:4].sum() 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 elif time <= self.times[:-1].sum(): delta_t = time - self.times[:5].sum() 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 elif time < self.times.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. \ + 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 else: linear_velocity = 0. s = self.total_length @@ -232,15 +254,15 @@ class eta3_trajectory(eta3_path): else: # compute interpolation parameter using length from current segment's starting point 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: self.ui_prev = 0 else: self.ui_prev = ui - + self.prev_seg_id = seg_id - # TODO(jwd): normalize! # compute angular velocity of current point= (ydd*xd - xdd*yd) / (xd**2 + yd**2) d = self.segments[seg_id].calc_deriv(ui, order=1) 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 = linear_velocity / su # 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 yt = d[1] * ut xtt = dd[0] * ut**2 + d[0] * utt @@ -261,7 +284,8 @@ class eta3_trajectory(eta3_path): # combine path point with orientation and velocities 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 @@ -278,8 +302,9 @@ def test1(max_vel=0.5): trajectory_segments.append(eta3_path_segment( 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 times = np.linspace(0, traj.total_time, 101) state = np.empty((5, times.size)) @@ -311,8 +336,9 @@ def test2(max_vel=0.5): trajectory_segments.append(eta3_path_segment( 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 times = np.linspace(0, traj.total_time, 101) state = np.empty((5, times.size)) @@ -346,7 +372,7 @@ def test3(max_vel=2.0): end_pose = [5.5, 1.5, 0] kappa = [0, 0, 0, 0] # NOTE: INTEGRATOR ERROR EXPLODES WHEN eta[:1] IS ZERO! - #was: eta = [0, 0, 0, 0, 0, 0], now is: + # was: eta = [0, 0, 0, 0, 0, 0], now is: eta = [0.5, 0.5, 0, 0, 0, 0] trajectory_segments.append(eta3_path_segment( start_pose=start_pose, end_pose=end_pose, eta=eta, kappa=kappa)) @@ -376,7 +402,8 @@ def test3(max_vel=2.0): start_pose=start_pose, end_pose=end_pose, eta=eta, kappa=kappa)) # 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 times = np.linspace(0, traj.total_time, 1001) @@ -392,8 +419,8 @@ def test3(max_vel=2.0): points = np.array([x, y]).T.reshape(-1, 1, 2) segs = np.concatenate([points[:-1], points[1:]], axis=1) lc = LineCollection(segs, cmap=plt.get_cmap('inferno')) - ax.set_xlim(np.min(x)-1, np.max(x)+1) - ax.set_ylim(np.min(y)-1, np.max(y)+1) + ax.set_xlim(np.min(x) - 1, np.max(x) + 1) + ax.set_ylim(np.min(y) - 1, np.max(y) + 1) lc.set_array(state[3, :]) lc.set_linewidth(3) ax.add_collection(lc) @@ -422,8 +449,8 @@ def main(): """ recreate path from reference (see Table 1) """ - #test1() - #test2() + # test1() + # test2() test3() diff --git a/PathPlanning/FrenetOptimalTrajectory/cubic_spline_planner.py b/PathPlanning/FrenetOptimalTrajectory/cubic_spline_planner.py index 81c6f839..8c33c213 100644 --- a/PathPlanning/FrenetOptimalTrajectory/cubic_spline_planner.py +++ b/PathPlanning/FrenetOptimalTrajectory/cubic_spline_planner.py @@ -40,7 +40,7 @@ class Spline: self.b.append(tb) def calc(self, t): - u""" + """ Calc position if t is outside of the input x, return None @@ -60,7 +60,7 @@ class Spline: return result def calcd(self, t): - u""" + """ Calc first derivative if t is outside of the input x, return None @@ -77,7 +77,7 @@ class Spline: return result def calcdd(self, t): - u""" + """ Calc second derivative """ @@ -92,13 +92,13 @@ class Spline: return result def __search_index(self, x): - u""" + """ search data segment index """ return bisect.bisect(self.x, x) - 1 def __calc_A(self, h): - u""" + """ calc matrix A for spline coefficient c """ A = np.zeros((self.nx, self.nx)) @@ -116,7 +116,7 @@ class Spline: return A def __calc_B(self, h): - u""" + """ calc matrix B for spline coefficient c """ B = np.zeros(self.nx) @@ -128,7 +128,7 @@ class Spline: class Spline2D: - u""" + """ 2D Cubic Spline class """ @@ -148,7 +148,7 @@ class Spline2D: return s def calc_position(self, s): - u""" + """ calc position """ x = self.sx.calc(s) @@ -157,7 +157,7 @@ class Spline2D: return x, y def calc_curvature(self, s): - u""" + """ calc curvature """ dx = self.sx.calcd(s) @@ -168,7 +168,7 @@ class Spline2D: return k def calc_yaw(self, s): - u""" + """ calc yaw """ dx = self.sx.calcd(s) diff --git a/PathPlanning/ModelPredictiveTrajectoryGenerator/model_predictive_trajectory_generator.py b/PathPlanning/ModelPredictiveTrajectoryGenerator/model_predictive_trajectory_generator.py index a682d455..e2e2d40f 100644 --- a/PathPlanning/ModelPredictiveTrajectoryGenerator/model_predictive_trajectory_generator.py +++ b/PathPlanning/ModelPredictiveTrajectoryGenerator/model_predictive_trajectory_generator.py @@ -20,7 +20,7 @@ show_animation = False def plot_arrow(x, y, yaw, length=1.0, width=0.5, fc="r", ec="k"): - u""" + """ Plot arrow """ plt.arrow(x, y, length * math.cos(yaw), length * math.sin(yaw), diff --git a/PathPlanning/ProbabilisticRoadMap/probabilistic_road_map.py b/PathPlanning/ProbabilisticRoadMap/probabilistic_road_map.py index aacade7d..d4e21467 100644 --- a/PathPlanning/ProbabilisticRoadMap/probabilistic_road_map.py +++ b/PathPlanning/ProbabilisticRoadMap/probabilistic_road_map.py @@ -45,7 +45,7 @@ class KDTree: self.tree = scipy.spatial.cKDTree(data) def search(self, inp, k=1): - u""" + """ Search NN inp: input data, single frame or multi frame @@ -62,12 +62,12 @@ class KDTree: dist.append(idist) return index, dist - else: - dist, index = self.tree.query(inp, k=k) - return index, dist + + dist, index = self.tree.query(inp, k=k) + return index, dist def search_in_distance(self, inp, r): - u""" + """ 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 while True: - if len(openset) == 0: + if not openset: print("Cannot find path") 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): - for i in range(len(road_map)): + for i, _ in enumerate(road_map): for ii in range(len(road_map[i])): ind = road_map[i][ii] @@ -307,7 +307,7 @@ def main(): 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: plt.plot(rx, ry, "-r") diff --git a/PathPlanning/RRTStarDubins/dubins_path_planning.py b/PathPlanning/RRTStarDubins/dubins_path_planning.py index 22bceda7..a6bf295f 100644 --- a/PathPlanning/RRTStarDubins/dubins_path_planning.py +++ b/PathPlanning/RRTStarDubins/dubins_path_planning.py @@ -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"): - u""" + """ Plot arrow """ diff --git a/PathPlanning/RRTStarDubins/rrt_star_dubins.py b/PathPlanning/RRTStarDubins/rrt_star_dubins.py index 8888c623..b600414d 100644 --- a/PathPlanning/RRTStarDubins/rrt_star_dubins.py +++ b/PathPlanning/RRTStarDubins/rrt_star_dubins.py @@ -203,7 +203,7 @@ class RRT(): self.nodeList[i] = tNode def DrawGraph(self, rnd=None): - u""" + """ Draw Graph """ plt.clf() diff --git a/PathPlanning/RRTstar/rrt_star.py b/PathPlanning/RRTstar/rrt_star.py index f006b794..b05da7d4 100644 --- a/PathPlanning/RRTstar/rrt_star.py +++ b/PathPlanning/RRTstar/rrt_star.py @@ -71,7 +71,7 @@ class RRT(): return path def choose_parent(self, newNode, nearinds): - if len(nearinds) == 0: + if not nearinds: return newNode dlist = [] @@ -130,9 +130,8 @@ class RRT(): disglist = [self.calc_dist_to_goal( node.x, node.y) for node in self.nodeList] goalinds = [disglist.index(i) for i in disglist if i <= self.expandDis] - # print(goalinds) - if len(goalinds) == 0: + if not goalinds: return None mincost = min([self.nodeList[i].cost for i in goalinds]) diff --git a/PathPlanning/ReedsSheppPath/reeds_shepp_path_planning.py b/PathPlanning/ReedsSheppPath/reeds_shepp_path_planning.py index 4d7e3d38..6049f793 100644 --- a/PathPlanning/ReedsSheppPath/reeds_shepp_path_planning.py +++ b/PathPlanning/ReedsSheppPath/reeds_shepp_path_planning.py @@ -357,14 +357,14 @@ def reeds_shepp_path_planning(sx, sy, syaw, paths = calc_paths(sx, sy, syaw, gx, gy, gyaw, maxc, step_size) - if len(paths) == 0: + if not paths: # print("No path") # print(sx, sy, syaw, gx, gy, gyaw) return None, None, None, None, None minL = float("Inf") best_path_index = -1 - for i in range(len(paths)): + for i, _ in enumerate(paths): if paths[i].L <= minL: minL = paths[i].L best_path_index = i diff --git a/PathPlanning/VoronoiRoadMap/voronoi_road_map.py b/PathPlanning/VoronoiRoadMap/voronoi_road_map.py index 3676c431..0327b723 100644 --- a/PathPlanning/VoronoiRoadMap/voronoi_road_map.py +++ b/PathPlanning/VoronoiRoadMap/voronoi_road_map.py @@ -43,7 +43,7 @@ class KDTree: self.tree = scipy.spatial.cKDTree(data) def search(self, inp, k=1): - u""" + """ Search NN inp: input data, single frame or multi frame @@ -60,12 +60,12 @@ class KDTree: dist.append(idist) return index, dist - else: - dist, index = self.tree.query(inp, k=k) - return index, dist + + dist, index = self.tree.query(inp, k=k) + return index, dist def search_in_distance(self, inp, r): - u""" + """ 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 while True: - if len(openset) == 0: + if not openset: print("Cannot find path") 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): - for i in range(len(road_map)): + for i, _ in enumerate(road_map): for ii in range(len(road_map[i])): ind = road_map[i][ii] @@ -296,7 +296,7 @@ def main(): 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: plt.plot(rx, ry, "-r") diff --git a/PathTracking/model_predictive_speed_and_steer_control/model_predictive_speed_and_steer_control.py b/PathTracking/model_predictive_speed_and_steer_control/model_predictive_speed_and_steer_control.py index db05b698..ef6577a5 100644 --- a/PathTracking/model_predictive_speed_and_steer_control/model_predictive_speed_and_steer_control.py +++ b/PathTracking/model_predictive_speed_and_steer_control/model_predictive_speed_and_steer_control.py @@ -5,14 +5,18 @@ Path tracking simulation with iterative linear model predictive control for spee author: Atsushi Sakai (@Atsushi_twi) """ +import matplotlib.pyplot as plt +import cvxpy +import math +import numpy as np import sys sys.path.append("../../PathPlanning/CubicSpline/") -import numpy as np -import math -import cvxpy -import matplotlib.pyplot as plt -import cubic_spline_planner +try: + import cubic_spline_planner +except: + raise + NX = 4 # x = x, y, v, yaw NU = 2 # a = [accel, steer] @@ -105,10 +109,10 @@ def get_linear_model_matrix(v, phi, delta): def plot_car(x, y, yaw, steer=0.0, cabcolor="-r", truckcolor="-k"): outline = np.array([[-BACKTOWHEEL, (LENGTH - BACKTOWHEEL), (LENGTH - BACKTOWHEEL), -BACKTOWHEEL, -BACKTOWHEEL], - [WIDTH / 2, WIDTH / 2, - WIDTH / 2, -WIDTH / 2, WIDTH / 2]]) + [WIDTH / 2, WIDTH / 2, - WIDTH / 2, -WIDTH / 2, WIDTH / 2]]) fr_wheel = np.array([[WHEEL_LEN, -WHEEL_LEN, -WHEEL_LEN, WHEEL_LEN, WHEEL_LEN], - [-WHEEL_WIDTH - TREAD, -WHEEL_WIDTH - TREAD, WHEEL_WIDTH - TREAD, WHEEL_WIDTH - TREAD, -WHEEL_WIDTH - TREAD]]) + [-WHEEL_WIDTH - TREAD, -WHEEL_WIDTH - TREAD, WHEEL_WIDTH - TREAD, WHEEL_WIDTH - TREAD, -WHEEL_WIDTH - TREAD]]) rr_wheel = np.copy(fr_wheel) @@ -118,9 +122,9 @@ def plot_car(x, y, yaw, steer=0.0, cabcolor="-r", truckcolor="-k"): rl_wheel[1, :] *= -1 Rot1 = np.array([[math.cos(yaw), math.sin(yaw)], - [-math.sin(yaw), math.cos(yaw)]]) + [-math.sin(yaw), math.cos(yaw)]]) Rot2 = np.array([[math.cos(steer), math.sin(steer)], - [-math.sin(steer), math.cos(steer)]]) + [-math.sin(steer), math.cos(steer)]]) fr_wheel = (fr_wheel.T.dot(Rot2)).T fl_wheel = (fl_wheel.T.dot(Rot2)).T @@ -208,7 +212,7 @@ def calc_nearest_index(state, cx, cy, cyaw, pind): def predict_motion(x0, oa, od, xref): xbar = xref * 0.0 - for i in range(len(x0)): + for i, _ in enumerate(x0): xbar[i, 0] = x0[i] 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] d = math.sqrt(dx ** 2 + dy ** 2) - if (d <= GOAL_DIS): - isgoal = True - else: - isgoal = False + isgoal = (d <= GOAL_DIS) if abs(tind - nind) >= 5: isgoal = False - if (abs(state.v) <= STOP_SPEED): - isstop = True - else: - isstop = False + isstop = (abs(state.v) <= STOP_SPEED) if isgoal and isstop: return True