diff --git a/PathTracking/pure_pursuit/pure_pursuit.py b/PathTracking/pure_pursuit/pure_pursuit.py index 7d45f393..35d0abae 100644 --- a/PathTracking/pure_pursuit/pure_pursuit.py +++ b/PathTracking/pure_pursuit/pure_pursuit.py @@ -14,6 +14,8 @@ import unicycle_model Kp = 1.0 # speed propotional gain Lf = 1.0 # look-ahead distance +# animation = True +animation = False def PIDControl(target, current): @@ -29,16 +31,23 @@ def pure_pursuit_control(state, cx, cy, pind): if pind >= ind: ind = pind - tx = cx[ind] - ty = cy[ind] + # print(pind, ind) + if ind < len(cx): + tx = cx[ind] + ty = cy[ind] + else: + tx = cx[-1] + ty = cy[-1] + ind = len(cx) - 1 alpha = math.atan2(ty - state.y, tx - state.x) - state.yaw if state.v < 0: # back - if alpha > 0: - alpha = math.pi - alpha - else: - alpha = math.pi + alpha + alpha = math.pi - alpha + # if alpha > 0: + # alpha = math.pi - alpha + # else: + # alpha = math.pi + alpha delta = math.atan2(2.0 * unicycle_model.L * math.sin(alpha) / Lf, 1.0) @@ -64,13 +73,15 @@ def calc_target_index(state, cx, cy): return ind -def closed_loop_prediction(cx, cy, cyaw, speed_profile): +def closed_loop_prediction(cx, cy, cyaw, speed_profile, goal): - T = 100.0 # max simulation time + T = 500.0 # max simulation time + goal_dis = 0.3 + stop_speed = 0.05 state = unicycle_model.State(x=-0.0, y=-0.0, yaw=0.0, v=0.0) - lastIndex = len(cx) - 1 + # lastIndex = len(cx) - 1 time = 0.0 x = [state.x] y = [state.y] @@ -78,34 +89,40 @@ def closed_loop_prediction(cx, cy, cyaw, speed_profile): v = [state.v] t = [0.0] target_ind = calc_target_index(state, cx, cy) - # print(target_ind) - while T >= time and lastIndex > target_ind: + while T >= time: di, target_ind = pure_pursuit_control(state, cx, cy, target_ind) ai = PIDControl(speed_profile[target_ind], state.v) state = unicycle_model.update(state, ai, di) - if abs(state.v) <= 0.05: + if abs(state.v) <= stop_speed: target_ind += 1 time = time + unicycle_model.dt + # check goal + dx = state.x - goal[0] + dy = state.y - goal[1] + if math.sqrt(dx ** 2 + dy ** 2) <= goal_dis: + print("Goal") + break + x.append(state.x) y.append(state.y) yaw.append(state.yaw) v.append(state.v) t.append(time) - plt.cla() - plt.plot(cx, cy, "-r", label="course") - plt.plot(x, y, "ob", label="trajectory") - plt.plot(cx[target_ind], cy[target_ind], "xg", label="target") - plt.axis("equal") - plt.grid(True) - plt.title("speed:" + str(round(state.v, 2)) + - "tind:" + str(target_ind)) - plt.pause(0.0001) - # input() + if target_ind % 20 == 0 and animation: + plt.cla() + plt.plot(cx, cy, "-r", label="course") + plt.plot(x, y, "ob", label="trajectory") + plt.plot(cx[target_ind], cy[target_ind], "xg", label="target") + plt.axis("equal") + plt.grid(True) + plt.title("speed:" + str(round(state.v, 2)) + + "tind:" + str(target_ind)) + plt.pause(0.0001) return t, x, y, yaw, v @@ -185,45 +202,31 @@ def calc_speed_profile(cx, cy, cyaw, target_speed, a): speed_profile[-i - 1] = tspeed # flg, ax = plt.subplots(1) - plt.plot(speed_profile, "-r") - # plt.plot(cx, cy, "-r") - plt.show() + # plt.plot(speed_profile, "-r") + # plt.show() return speed_profile +def extend_path(cx, cy, cyaw): + + dl = 0.1 + dl_list = [dl] * (int(Lf / dl) + 0) + + move_direction = math.atan2(cy[-1] - cy[-2], cx[-1] - cx[-2]) + is_back = abs(move_direction - cyaw[-1]) >= math.pi / 2.0 + + for idl in dl_list: + if is_back: + idl *= -1 + cx = np.append(cx, cx[-1] + idl * math.cos(cyaw[-1])) + cy = np.append(cy, cy[-1] + idl * math.sin(cyaw[-1])) + cyaw = np.append(cyaw, cyaw[-1]) + + return cx, cy, cyaw + + def main(): - import pandas as pd - data = pd.read_csv("rrt_course.csv") - cx = np.array(data["x"]) - cy = np.array(data["y"]) - cyaw = np.array(data["yaw"]) - - target_speed = 10.0 / 3.6 - a = 0.1 - - speed_profile = calc_speed_profile(cx, cy, cyaw, target_speed, a) - - t, x, y, yaw, v = closed_loop_prediction(cx, cy, cyaw, speed_profile) - - flg, ax = plt.subplots(1) - plt.plot(cx, cy, ".r", label="course") - plt.plot(x, y, "-b", label="trajectory") - plt.legend() - plt.xlabel("x[m]") - plt.ylabel("y[m]") - plt.axis("equal") - plt.grid(True) - - flg, ax = plt.subplots(1) - plt.plot(t, [iv * 3.6 for iv in v], "-r") - plt.xlabel("Time[s]") - plt.ylabel("Speed[km/h]") - plt.grid(True) - plt.show() - - -def main2(): # target course import numpy as np cx = np.arange(0, 50, 0.1) @@ -233,8 +236,8 @@ def main2(): T = 15.0 # max simulation time - # state = unicycle_model.State(x=-0.0, y=-0.0, yaw=0.0, v=0.0) - state = unicycle_model.State(x=-1.0, y=-5.0, yaw=0.0, v=-30.0 / 3.6) + state = unicycle_model.State(x=-0.0, y=-3.0, yaw=0.0, v=0.0) + # state = unicycle_model.State(x=-1.0, y=-5.0, yaw=0.0, v=-30.0 / 3.6) # state = unicycle_model.State(x=10.0, y=5.0, yaw=0.0, v=-30.0 / 3.6) # state = unicycle_model.State( # x=3.0, y=5.0, yaw=math.radians(-40.0), v=-10.0 / 3.6) @@ -289,6 +292,43 @@ def main2(): plt.show() +def main2(): + import pandas as pd + data = pd.read_csv("rrt_course.csv") + cx = np.array(data["x"]) + cy = np.array(data["y"]) + cyaw = np.array(data["yaw"]) + + target_speed = 10.0 / 3.6 + a = 0.1 + + goal = [cx[-1], cy[-1]] + + cx, cy, cyaw = extend_path(cx, cy, cyaw) + + speed_profile = calc_speed_profile(cx, cy, cyaw, target_speed, a) + + t, x, y, yaw, v = closed_loop_prediction(cx, cy, cyaw, speed_profile, goal) + + flg, ax = plt.subplots(1) + plt.plot(cx, cy, ".r", label="course") + plt.plot(x, y, "-b", label="trajectory") + plt.plot(goal[0], goal[1], "xg", label="goal") + plt.legend() + plt.xlabel("x[m]") + plt.ylabel("y[m]") + plt.axis("equal") + plt.grid(True) + + flg, ax = plt.subplots(1) + plt.plot(t, [iv * 3.6 for iv in v], "-r") + plt.xlabel("Time[s]") + plt.ylabel("Speed[km/h]") + plt.grid(True) + plt.show() + + if __name__ == '__main__': print("Pure pursuit path tracking simulation start") - main() + # main() + main2()