From fe39636d391db184075481924066809956f3b8bf Mon Sep 17 00:00:00 2001 From: Shunichi09 Date: Sun, 10 Feb 2019 22:52:24 +0900 Subject: [PATCH] add readme --- mpc/extend/README.md | 37 ++ mpc/{with_disturbance => extend}/animation.py | 0 .../coordinate_trans.py | 0 .../func_curvature.py | 0 .../iterative_MPC.py | 0 .../main_track.py | 0 .../mpc_func_with_cvxopt.py | 0 mpc/{with_disturbance => extend}/traj_func.py | 0 mpc/sample/pathplanner.py | 233 ------- mpc/sample/test.py | 614 ------------------ 10 files changed, 37 insertions(+), 847 deletions(-) create mode 100644 mpc/extend/README.md rename mpc/{with_disturbance => extend}/animation.py (100%) rename mpc/{with_disturbance => extend}/coordinate_trans.py (100%) rename mpc/{with_disturbance => extend}/func_curvature.py (100%) rename mpc/{with_disturbance => extend}/iterative_MPC.py (100%) rename mpc/{with_disturbance => extend}/main_track.py (100%) rename mpc/{with_disturbance => extend}/mpc_func_with_cvxopt.py (100%) rename mpc/{with_disturbance => extend}/traj_func.py (100%) delete mode 100644 mpc/sample/pathplanner.py delete mode 100644 mpc/sample/test.py diff --git a/mpc/extend/README.md b/mpc/extend/README.md new file mode 100644 index 0000000..2dec8ef --- /dev/null +++ b/mpc/extend/README.md @@ -0,0 +1,37 @@ +# Model Predictive Control for Vehicle model +This program is for controlling the vehicle model. +I implemented the steering control for vehicle by using Model Predictive Control. + +# Model +Usually, the vehicle model is expressed by extremely complicated nonlinear equation. +Acoording to reference 1, I used the simple model as shown in following equation. + + + +However, it is still a nonlinear equation. +Therefore, I assume that the car is tracking the reference trajectory. +If we get the assumption, the model can turn to linear model by using the path's curvatures. + + + +and \delta_r denoted + + + +R is the curvatures of the reference trajectory. + +Now we can get the linear state equation and can apply the MPC theory. + +However, you should care that this state euation could be changed during the predict horizon. +I implemented this, so if you know about the detail please go to "IteraticeMPC_func.py" + +# Expected Results + +# Usage + +``` +$ python main_track.py +``` + +# Reference +- 1. https://qiita.com/taka_horibe/items/47f86e02e2db83b0c570#%E8%BB%8A%E4%B8%A1%E3%81%AE%E8%BB%8C%E9%81%93%E8%BF%BD%E5%BE%93%E5%95%8F%E9%A1%8C%E9%9D%9E%E7%B7%9A%E5%BD%A2%E3%81%AB%E9%81%A9%E7%94%A8%E3%81%99%E3%82%8B (Japanese) diff --git a/mpc/with_disturbance/animation.py b/mpc/extend/animation.py similarity index 100% rename from mpc/with_disturbance/animation.py rename to mpc/extend/animation.py diff --git a/mpc/with_disturbance/coordinate_trans.py b/mpc/extend/coordinate_trans.py similarity index 100% rename from mpc/with_disturbance/coordinate_trans.py rename to mpc/extend/coordinate_trans.py diff --git a/mpc/with_disturbance/func_curvature.py b/mpc/extend/func_curvature.py similarity index 100% rename from mpc/with_disturbance/func_curvature.py rename to mpc/extend/func_curvature.py diff --git a/mpc/with_disturbance/iterative_MPC.py b/mpc/extend/iterative_MPC.py similarity index 100% rename from mpc/with_disturbance/iterative_MPC.py rename to mpc/extend/iterative_MPC.py diff --git a/mpc/with_disturbance/main_track.py b/mpc/extend/main_track.py similarity index 100% rename from mpc/with_disturbance/main_track.py rename to mpc/extend/main_track.py diff --git a/mpc/with_disturbance/mpc_func_with_cvxopt.py b/mpc/extend/mpc_func_with_cvxopt.py similarity index 100% rename from mpc/with_disturbance/mpc_func_with_cvxopt.py rename to mpc/extend/mpc_func_with_cvxopt.py diff --git a/mpc/with_disturbance/traj_func.py b/mpc/extend/traj_func.py similarity index 100% rename from mpc/with_disturbance/traj_func.py rename to mpc/extend/traj_func.py diff --git a/mpc/sample/pathplanner.py b/mpc/sample/pathplanner.py deleted file mode 100644 index 455ddb4..0000000 --- a/mpc/sample/pathplanner.py +++ /dev/null @@ -1,233 +0,0 @@ -""" -Cubic spline planner -Author: Atsushi Sakai(@Atsushi_twi) -""" -import math -import numpy as np -import bisect - - -class Spline: - """ - Cubic Spline class - """ - - def __init__(self, x, y): - self.b, self.c, self.d, self.w = [], [], [], [] - - self.x = x - self.y = y - - self.nx = len(x) # dimension of x - h = np.diff(x) - - # calc coefficient c - self.a = [iy for iy in y] - - # calc coefficient c - A = self.__calc_A(h) - B = self.__calc_B(h) - self.c = np.linalg.solve(A, B) - # print(self.c1) - - # calc spline coefficient b and d - for i in range(self.nx - 1): - self.d.append((self.c[i + 1] - self.c[i]) / (3.0 * h[i])) - tb = (self.a[i + 1] - self.a[i]) / h[i] - h[i] * \ - (self.c[i + 1] + 2.0 * self.c[i]) / 3.0 - self.b.append(tb) - - def calc(self, t): - """ - Calc position - if t is outside of the input x, return None - """ - - if t < self.x[0]: - return None - elif t > self.x[-1]: - return None - - i = self.__search_index(t) - dx = t - self.x[i] - result = self.a[i] + self.b[i] * dx + \ - self.c[i] * dx ** 2.0 + self.d[i] * dx ** 3.0 - - return result - - def calcd(self, t): - """ - Calc first derivative - if t is outside of the input x, return None - """ - - if t < self.x[0]: - return None - elif t > self.x[-1]: - return None - - i = self.__search_index(t) - dx = t - self.x[i] - result = self.b[i] + 2.0 * self.c[i] * dx + 3.0 * self.d[i] * dx ** 2.0 - return result - - def calcdd(self, t): - """ - Calc second derivative - """ - - if t < self.x[0]: - return None - elif t > self.x[-1]: - return None - - i = self.__search_index(t) - dx = t - self.x[i] - result = 2.0 * self.c[i] + 6.0 * self.d[i] * dx - return result - - def __search_index(self, x): - """ - search data segment index - """ - return bisect.bisect(self.x, x) - 1 - - def __calc_A(self, h): - """ - calc matrix A for spline coefficient c - """ - A = np.zeros((self.nx, self.nx)) - A[0, 0] = 1.0 - for i in range(self.nx - 1): - if i != (self.nx - 2): - A[i + 1, i + 1] = 2.0 * (h[i] + h[i + 1]) - A[i + 1, i] = h[i] - A[i, i + 1] = h[i] - - A[0, 1] = 0.0 - A[self.nx - 1, self.nx - 2] = 0.0 - A[self.nx - 1, self.nx - 1] = 1.0 - # print(A) - return A - - def __calc_B(self, h): - """ - calc matrix B for spline coefficient c - """ - B = np.zeros(self.nx) - for i in range(self.nx - 2): - B[i + 1] = 3.0 * (self.a[i + 2] - self.a[i + 1]) / \ - h[i + 1] - 3.0 * (self.a[i + 1] - self.a[i]) / h[i] - return B - - -class Spline2D: - """ - 2D Cubic Spline class - """ - - def __init__(self, x, y): - self.s = self.__calc_s(x, y) - self.sx = Spline(self.s, x) - self.sy = Spline(self.s, y) - - def __calc_s(self, x, y): - dx = np.diff(x) - dy = np.diff(y) - self.ds = [math.sqrt(idx ** 2 + idy ** 2) - for (idx, idy) in zip(dx, dy)] - s = [0] - s.extend(np.cumsum(self.ds)) - return s - - def calc_position(self, s): - """ - calc position - """ - x = self.sx.calc(s) - y = self.sy.calc(s) - - return x, y - - def calc_curvature(self, s): - """ - calc curvature - """ - dx = self.sx.calcd(s) - ddx = self.sx.calcdd(s) - dy = self.sy.calcd(s) - ddy = self.sy.calcdd(s) - k = (ddy * dx - ddx * dy) / ((dx ** 2 + dy ** 2)**(3 / 2)) - return k - - def calc_yaw(self, s): - """ - calc yaw - """ - dx = self.sx.calcd(s) - dy = self.sy.calcd(s) - yaw = math.atan2(dy, dx) - return yaw - - -def calc_spline_course(x, y, ds=0.1): - sp = Spline2D(x, y) - s = list(np.arange(0, sp.s[-1], ds)) - - rx, ry, ryaw, rk = [], [], [], [] - for i_s in s: - ix, iy = sp.calc_position(i_s) - rx.append(ix) - ry.append(iy) - ryaw.append(sp.calc_yaw(i_s)) - rk.append(sp.calc_curvature(i_s)) - - return rx, ry, ryaw, rk, s - - -def main(): - print("Spline 2D test") - import matplotlib.pyplot as plt - x = [-2.5, 0.0, 2.5, 5.0, 7.5, 3.0, -1.0] - y = [0.7, -6, 5, 6.5, 0.0, 5.0, -2.0] - ds = 0.1 # [m] distance of each intepolated points - - sp = Spline2D(x, y) - s = np.arange(0, sp.s[-1], ds) - - rx, ry, ryaw, rk = [], [], [], [] - for i_s in s: - ix, iy = sp.calc_position(i_s) - rx.append(ix) - ry.append(iy) - ryaw.append(sp.calc_yaw(i_s)) - rk.append(sp.calc_curvature(i_s)) - - plt.subplots(1) - plt.plot(x, y, "xb", label="input") - plt.plot(rx, ry, "-r", label="spline") - plt.grid(True) - plt.axis("equal") - plt.xlabel("x[m]") - plt.ylabel("y[m]") - plt.legend() - - plt.subplots(1) - plt.plot(s, [np.rad2deg(iyaw) for iyaw in ryaw], "-r", label="yaw") - plt.grid(True) - plt.legend() - plt.xlabel("line length[m]") - plt.ylabel("yaw angle[deg]") - - plt.subplots(1) - plt.plot(s, rk, "-r", label="curvature") - plt.grid(True) - plt.legend() - plt.xlabel("line length[m]") - plt.ylabel("curvature [1/m]") - - plt.show() - - -if __name__ == '__main__': - main() \ No newline at end of file diff --git a/mpc/sample/test.py b/mpc/sample/test.py deleted file mode 100644 index 5328002..0000000 --- a/mpc/sample/test.py +++ /dev/null @@ -1,614 +0,0 @@ -""" -Path tracking simulation with iterative linear model predictive control for speed and steer control -author: Atsushi Sakai (@Atsushi_twi) -""" -import matplotlib.pyplot as plt -import cvxpy -import math -import numpy as np -import sys - -try: - import pathplanner -except: - raise - - -NX = 4 # x = x, y, v, yaw -NU = 2 # a = [accel, steer] -T = 5 # horizon length - -# mpc parameters -R = np.diag([0.01, 0.01]) # input cost matrix -Rd = np.diag([0.01, 1.0]) # input difference cost matrix -Q = np.diag([1.0, 1.0, 0.5, 0.5]) # state cost matrix -Qf = Q # state final matrix -GOAL_DIS = 1.5 # goal distance -STOP_SPEED = 0.5 / 3.6 # stop speed -MAX_TIME = 500.0 # max simulation time - -# iterative paramter -MAX_ITER = 3 # Max iteration -DU_TH = 0.1 # iteration finish param - -TARGET_SPEED = 10.0 / 3.6 # [m/s] target speed -N_IND_SEARCH = 10 # Search index number - -DT = 0.2 # [s] time tick - -# Vehicle parameters -LENGTH = 4.5 # [m] -WIDTH = 2.0 # [m] -BACKTOWHEEL = 1.0 # [m] -WHEEL_LEN = 0.3 # [m] -WHEEL_WIDTH = 0.2 # [m] -TREAD = 0.7 # [m] -WB = 2.5 # [m] - -MAX_STEER = np.deg2rad(45.0) # maximum steering angle [rad] -MAX_DSTEER = np.deg2rad(30.0) # maximum steering speed [rad/s] -MAX_SPEED = 55.0 / 3.6 # maximum speed [m/s] -MIN_SPEED = -20.0 / 3.6 # minimum speed [m/s] -MAX_ACCEL = 1.0 # maximum accel [m/ss] - -show_animation = True - - -class State: - """ - vehicle state class - """ - - def __init__(self, x=0.0, y=0.0, yaw=0.0, v=0.0): - self.x = x - self.y = y - self.yaw = yaw - self.v = v - self.predelta = None - - -def pi_2_pi(angle): - while(angle > math.pi): - angle = angle - 2.0 * math.pi - - while(angle < -math.pi): - angle = angle + 2.0 * math.pi - - return angle - - -def get_linear_model_matrix(v, phi, delta): - - A = np.zeros((NX, NX)) - A[0, 0] = 1.0 - A[1, 1] = 1.0 - A[2, 2] = 1.0 - A[3, 3] = 1.0 - A[0, 2] = DT * math.cos(phi) - A[0, 3] = - DT * v * math.sin(phi) - A[1, 2] = DT * math.sin(phi) - A[1, 3] = DT * v * math.cos(phi) - A[3, 2] = DT * math.tan(delta) / WB - - B = np.zeros((NX, NU)) - B[2, 0] = DT - B[3, 1] = DT * v / (WB * math.cos(delta) ** 2) - - C = np.zeros(NX) - C[0] = DT * v * math.sin(phi) * phi - C[1] = - DT * v * math.cos(phi) * phi - C[3] = - v * delta / (WB * math.cos(delta) ** 2) - - return A, B, C - - -def plot_car(x, y, yaw, steer=0.0, cabcolor="-r", truckcolor="-k"): # pragma: no cover - - outline = np.array([[-BACKTOWHEEL, (LENGTH - BACKTOWHEEL), (LENGTH - BACKTOWHEEL), -BACKTOWHEEL, -BACKTOWHEEL], - [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]]) - - rr_wheel = np.copy(fr_wheel) - - fl_wheel = np.copy(fr_wheel) - fl_wheel[1, :] *= -1 - rl_wheel = np.copy(rr_wheel) - rl_wheel[1, :] *= -1 - - Rot1 = np.array([[math.cos(yaw), math.sin(yaw)], - [-math.sin(yaw), math.cos(yaw)]]) - Rot2 = np.array([[math.cos(steer), math.sin(steer)], - [-math.sin(steer), math.cos(steer)]]) - - fr_wheel = (fr_wheel.T.dot(Rot2)).T - fl_wheel = (fl_wheel.T.dot(Rot2)).T - fr_wheel[0, :] += WB - fl_wheel[0, :] += WB - - fr_wheel = (fr_wheel.T.dot(Rot1)).T - fl_wheel = (fl_wheel.T.dot(Rot1)).T - - outline = (outline.T.dot(Rot1)).T - rr_wheel = (rr_wheel.T.dot(Rot1)).T - rl_wheel = (rl_wheel.T.dot(Rot1)).T - - outline[0, :] += x - outline[1, :] += y - fr_wheel[0, :] += x - fr_wheel[1, :] += y - rr_wheel[0, :] += x - rr_wheel[1, :] += y - fl_wheel[0, :] += x - fl_wheel[1, :] += y - rl_wheel[0, :] += x - rl_wheel[1, :] += y - - plt.plot(np.array(outline[0, :]).flatten(), - np.array(outline[1, :]).flatten(), truckcolor) - plt.plot(np.array(fr_wheel[0, :]).flatten(), - np.array(fr_wheel[1, :]).flatten(), truckcolor) - plt.plot(np.array(rr_wheel[0, :]).flatten(), - np.array(rr_wheel[1, :]).flatten(), truckcolor) - plt.plot(np.array(fl_wheel[0, :]).flatten(), - np.array(fl_wheel[1, :]).flatten(), truckcolor) - plt.plot(np.array(rl_wheel[0, :]).flatten(), - np.array(rl_wheel[1, :]).flatten(), truckcolor) - plt.plot(x, y, "*") - - -def update_state(state, a, delta): - - # input check - if delta >= MAX_STEER: - delta = MAX_STEER - elif delta <= -MAX_STEER: - delta = -MAX_STEER - - state.x = state.x + state.v * math.cos(state.yaw) * DT - state.y = state.y + state.v * math.sin(state.yaw) * DT - state.yaw = state.yaw + state.v / WB * math.tan(delta) * DT - state.v = state.v + a * DT - - if state. v > MAX_SPEED: - state.v = MAX_SPEED - elif state. v < MIN_SPEED: - state.v = MIN_SPEED - - return state - - -def get_nparray_from_matrix(x): - return np.array(x).flatten() - - -def calc_nearest_index(state, cx, cy, cyaw, pind): - - dx = [state.x - icx for icx in cx[pind:(pind + N_IND_SEARCH)]] - dy = [state.y - icy for icy in cy[pind:(pind + N_IND_SEARCH)]] - - d = [idx ** 2 + idy ** 2 for (idx, idy) in zip(dx, dy)] - - mind = min(d) - - ind = d.index(mind) + pind - - mind = math.sqrt(mind) - - dxl = cx[ind] - state.x - dyl = cy[ind] - state.y - - angle = pi_2_pi(cyaw[ind] - math.atan2(dyl, dxl)) - if angle < 0: - mind *= -1 - - return ind, mind - - -def predict_motion(x0, oa, od, xref): - xbar = xref * 0.0 - for i, _ in enumerate(x0): - xbar[i, 0] = x0[i] - - state = State(x=x0[0], y=x0[1], yaw=x0[3], v=x0[2]) - for (ai, di, i) in zip(oa, od, range(1, T + 1)): - state = update_state(state, ai, di) - xbar[0, i] = state.x - xbar[1, i] = state.y - xbar[2, i] = state.v - xbar[3, i] = state.yaw - - return xbar - - -def iterative_linear_mpc_control(xref, x0, dref, oa, od): - """ - MPC contorl with updating operational point iteraitvely - """ - - if oa is None or od is None: - oa = [0.0] * T - od = [0.0] * T - - for i in range(MAX_ITER): - xbar = predict_motion(x0, oa, od, xref) - poa, pod = oa[:], od[:] - oa, od, ox, oy, oyaw, ov = linear_mpc_control(xref, xbar, x0, dref) - du = sum(abs(oa - poa)) + sum(abs(od - pod)) # calc u change value - if du <= DU_TH: - break - else: - print("Iterative is max iter") - - return oa, od, ox, oy, oyaw, ov - - -def linear_mpc_control(xref, xbar, x0, dref): - """ - linear mpc control - xref: reference point - xbar: operational point - x0: initial state - dref: reference steer angle - """ - - x = cvxpy.Variable((NX, T + 1)) - u = cvxpy.Variable((NU, T)) - - cost = 0.0 - constraints = [] - - for t in range(T): - cost += cvxpy.quad_form(u[:, t], R) - - if t != 0: - cost += cvxpy.quad_form(xref[:, t] - x[:, t], Q) - - A, B, C = get_linear_model_matrix( - xbar[2, t], xbar[3, t], dref[0, t]) - constraints += [x[:, t + 1] == A * x[:, t] + B * u[:, t] + C] - - if t < (T - 1): - cost += cvxpy.quad_form(u[:, t + 1] - u[:, t], Rd) - constraints += [cvxpy.abs(u[1, t + 1] - u[1, t]) <= - MAX_DSTEER * DT] - - cost += cvxpy.quad_form(xref[:, T] - x[:, T], Qf) - - constraints += [x[:, 0] == x0] - constraints += [x[2, :] <= MAX_SPEED] - constraints += [x[2, :] >= MIN_SPEED] - constraints += [cvxpy.abs(u[0, :]) <= MAX_ACCEL] - constraints += [cvxpy.abs(u[1, :]) <= MAX_STEER] - - prob = cvxpy.Problem(cvxpy.Minimize(cost), constraints) - prob.solve(solver=cvxpy.ECOS, verbose=False) - - if prob.status == cvxpy.OPTIMAL or prob.status == cvxpy.OPTIMAL_INACCURATE: - ox = get_nparray_from_matrix(x.value[0, :]) - oy = get_nparray_from_matrix(x.value[1, :]) - ov = get_nparray_from_matrix(x.value[2, :]) - oyaw = get_nparray_from_matrix(x.value[3, :]) - oa = get_nparray_from_matrix(u.value[0, :]) - odelta = get_nparray_from_matrix(u.value[1, :]) - - else: - print("Error: Cannot solve mpc..") - oa, odelta, ox, oy, oyaw, ov = None, None, None, None, None, None - - return oa, odelta, ox, oy, oyaw, ov - - -def calc_ref_trajectory(state, cx, cy, cyaw, ck, sp, dl, pind): - xref = np.zeros((NX, T + 1)) - dref = np.zeros((1, T + 1)) - ncourse = len(cx) - - ind, _ = calc_nearest_index(state, cx, cy, cyaw, pind) - - if pind >= ind: - ind = pind - - xref[0, 0] = cx[ind] - xref[1, 0] = cy[ind] - xref[2, 0] = sp[ind] - xref[3, 0] = cyaw[ind] - dref[0, 0] = 0.0 # steer operational point should be 0 - - travel = 0.0 - - for i in range(T + 1): - travel += abs(state.v) * DT - dind = int(round(travel / dl)) - - if (ind + dind) < ncourse: - xref[0, i] = cx[ind + dind] - xref[1, i] = cy[ind + dind] - xref[2, i] = sp[ind + dind] - xref[3, i] = cyaw[ind + dind] - dref[0, i] = 0.0 - else: - xref[0, i] = cx[ncourse - 1] - xref[1, i] = cy[ncourse - 1] - xref[2, i] = sp[ncourse - 1] - xref[3, i] = cyaw[ncourse - 1] - dref[0, i] = 0.0 - - return xref, ind, dref - - -def check_goal(state, goal, tind, nind): - - # check goal - dx = state.x - goal[0] - dy = state.y - goal[1] - d = math.sqrt(dx ** 2 + dy ** 2) - - isgoal = (d <= GOAL_DIS) - - if abs(tind - nind) >= 5: - isgoal = False - - isstop = (abs(state.v) <= STOP_SPEED) - - if isgoal and isstop: - return True - - return False - - -def do_simulation(cx, cy, cyaw, ck, sp, dl, initial_state): - """ - Simulation - cx: course x position list - cy: course y position list - cy: course yaw position list - ck: course curvature list - sp: speed profile - dl: course tick [m] - """ - - goal = [cx[-1], cy[-1]] - - state = initial_state - - # initial yaw compensation - if state.yaw - cyaw[0] >= math.pi: - state.yaw -= math.pi * 2.0 - elif state.yaw - cyaw[0] <= -math.pi: - state.yaw += math.pi * 2.0 - - time = 0.0 - x = [state.x] - y = [state.y] - yaw = [state.yaw] - v = [state.v] - t = [0.0] - d = [0.0] - a = [0.0] - target_ind, _ = calc_nearest_index(state, cx, cy, cyaw, 0) - - odelta, oa = None, None - - cyaw = smooth_yaw(cyaw) - - while MAX_TIME >= time: - xref, target_ind, dref = calc_ref_trajectory( - state, cx, cy, cyaw, ck, sp, dl, target_ind) - - x0 = [state.x, state.y, state.v, state.yaw] # current state - - oa, odelta, ox, oy, oyaw, ov = iterative_linear_mpc_control( - xref, x0, dref, oa, odelta) - - if odelta is not None: - di, ai = odelta[0], oa[0] - - state = update_state(state, ai, di) - time = time + DT - - x.append(state.x) - y.append(state.y) - yaw.append(state.yaw) - v.append(state.v) - t.append(time) - d.append(di) - a.append(ai) - - if check_goal(state, goal, target_ind, len(cx)): - print("Goal") - break - - if show_animation: # pragma: no cover - plt.cla() - if ox is not None: - plt.plot(ox, oy, "xr", label="MPC") - plt.plot(cx, cy, "-r", label="course") - plt.plot(x, y, "ob", label="trajectory") - plt.plot(xref[0, :], xref[1, :], "xk", label="xref") - plt.plot(cx[target_ind], cy[target_ind], "xg", label="target") - plot_car(state.x, state.y, state.yaw, steer=di) - plt.axis("equal") - plt.grid(True) - plt.title("Time[s]:" + str(round(time, 2)) - + ", speed[km/h]:" + str(round(state.v * 3.6, 2))) - plt.pause(0.0001) - - return t, x, y, yaw, v, d, a - - -def calc_speed_profile(cx, cy, cyaw, target_speed): - - speed_profile = [target_speed] * len(cx) - direction = 1.0 # forward - - # Set stop point - for i in range(len(cx) - 1): - dx = cx[i + 1] - cx[i] - dy = cy[i + 1] - cy[i] - - move_direction = math.atan2(dy, dx) - - if dx != 0.0 and dy != 0.0: - dangle = abs(pi_2_pi(move_direction - cyaw[i])) - if dangle >= math.pi / 4.0: - direction = -1.0 - else: - direction = 1.0 - - if direction != 1.0: - speed_profile[i] = - target_speed - else: - speed_profile[i] = target_speed - - speed_profile[-1] = 0.0 - - return speed_profile - - -def smooth_yaw(yaw): - - for i in range(len(yaw) - 1): - dyaw = yaw[i + 1] - yaw[i] - - while dyaw >= math.pi / 2.0: - yaw[i + 1] -= math.pi * 2.0 - dyaw = yaw[i + 1] - yaw[i] - - while dyaw <= -math.pi / 2.0: - yaw[i + 1] += math.pi * 2.0 - dyaw = yaw[i + 1] - yaw[i] - - return yaw - - -def get_straight_course(dl): - ax = [0.0, 5.0, 10.0, 20.0, 30.0, 40.0, 50.0] - ay = [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0] - cx, cy, cyaw, ck, s = pathplanner.calc_spline_course( - ax, ay, ds=dl) - - return cx, cy, cyaw, ck - - -def get_straight_course2(dl): - ax = [0.0, -10.0, -20.0, -40.0, -50.0, -60.0, -70.0] - ay = [0.0, -1.0, 1.0, 0.0, -1.0, 1.0, 0.0] - cx, cy, cyaw, ck, s = pathplanner.calc_spline_course( - ax, ay, ds=dl) - - return cx, cy, cyaw, ck - - -def get_straight_course3(dl): - ax = [0.0, -10.0, -20.0, -40.0, -50.0, -60.0, -70.0] - ay = [0.0, -1.0, 1.0, 0.0, -1.0, 1.0, 0.0] - cx, cy, cyaw, ck, s = pathplanner.calc_spline_course( - ax, ay, ds=dl) - - cyaw = [i - math.pi for i in cyaw] - - return cx, cy, cyaw, ck - - -def get_forward_course(dl): - ax = [0.0, 60.0, 125.0, 50.0, 75.0, 30.0, -10.0] - ay = [0.0, 0.0, 50.0, 65.0, 30.0, 50.0, -20.0] - cx, cy, cyaw, ck, s = pathplanner.calc_spline_course( - ax, ay, ds=dl) - - return cx, cy, cyaw, ck - - -def get_switch_back_course(dl): - ax = [0.0, 30.0, 6.0, 20.0, 35.0] - ay = [0.0, 0.0, 20.0, 35.0, 20.0] - cx, cy, cyaw, ck, s = pathplanner.calc_spline_course( - ax, ay, ds=dl) - ax = [35.0, 10.0, 0.0, 0.0] - ay = [20.0, 30.0, 5.0, 0.0] - cx2, cy2, cyaw2, ck2, s2 = pathplanner.calc_spline_course( - ax, ay, ds=dl) - cyaw2 = [i - math.pi for i in cyaw2] - cx.extend(cx2) - cy.extend(cy2) - cyaw.extend(cyaw2) - ck.extend(ck2) - - return cx, cy, cyaw, ck - - -def main(): - print(__file__ + " start!!") - - dl = 1.0 # course tick - # cx, cy, cyaw, ck = get_straight_course(dl) - # cx, cy, cyaw, ck = get_straight_course2(dl) - cx, cy, cyaw, ck = get_straight_course3(dl) - # cx, cy, cyaw, ck = get_forward_course(dl) - # CX, cy, cyaw, ck = get_switch_back_course(dl) - - sp = calc_speed_profile(cx, cy, cyaw, TARGET_SPEED) - - initial_state = State(x=cx[0], y=cy[0], yaw=cyaw[0], v=0.0) - - t, x, y, yaw, v, d, a = do_simulation( - cx, cy, cyaw, ck, sp, dl, initial_state) - - if show_animation: # pragma: no cover - plt.close("all") - plt.subplots() - plt.plot(cx, cy, "-r", label="spline") - plt.plot(x, y, "-g", label="tracking") - plt.grid(True) - plt.axis("equal") - plt.xlabel("x[m]") - plt.ylabel("y[m]") - plt.legend() - - plt.subplots() - plt.plot(t, v, "-r", label="speed") - plt.grid(True) - plt.xlabel("Time [s]") - plt.ylabel("Speed [kmh]") - - plt.show() - - -def main2(): - print(__file__ + " start!!") - - dl = 1.0 # course tick - cx, cy, cyaw, ck = get_straight_course3(dl) - - sp = calc_speed_profile(cx, cy, cyaw, TARGET_SPEED) - - initial_state = State(x=cx[0], y=cy[0], yaw=0.0, v=0.0) - - t, x, y, yaw, v, d, a = do_simulation( - cx, cy, cyaw, ck, sp, dl, initial_state) - - if show_animation: # pragma: no cover - plt.close("all") - plt.subplots() - plt.plot(cx, cy, "-r", label="spline") - plt.plot(x, y, "-g", label="tracking") - plt.grid(True) - plt.axis("equal") - plt.xlabel("x[m]") - plt.ylabel("y[m]") - plt.legend() - - plt.subplots() - plt.plot(t, v, "-r", label="speed") - plt.grid(True) - plt.xlabel("Time [s]") - plt.ylabel("Speed [kmh]") - - plt.show() - - -if __name__ == '__main__': - # main() - main2() \ No newline at end of file