AtHeartEngineer/PythonRobotics
1
1
Fork 0
mirror of https://github.com/AtsushiSakai/PythonRobotics.git synced 2026-01-13 21:58:46 -05:00
Code Issues Packages Projects Releases Wiki Activity

bug fix

Browse Source
This commit is contained in:
AtsushiSakai
2017-06-13 17:46:31 -07:00
parent 300aa25a12
commit ed4436ea2e
1 changed files with 99 additions and 59 deletions
Download Patch File Download Diff File Expand all files Collapse all files

158
PathTracking/pure_pursuit/pure_pursuit.py
View File

@@ -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()