def hybrid_astar_planning(sx, sy, syaw, gx, gy, gyaw, ox, oy, xyreso, yawreso):
sxr, syr = round(sx / xyreso), round(sy / xyreso)
gxr, gyr = round(gx / xyreso), round(gy / xyreso)
syawr = round(rs.pi_2_pi(syaw) / yawreso)
gyawr = round(rs.pi_2_pi(gyaw) / yawreso)
nstart = Node(sxr, syr, syawr, 1, [sx], [sy], [syaw], [1], 0.0, 0.0, -1)
ngoal = Node(gxr, gyr, gyawr, 1, [gx], [gy], [gyaw], [1], 0.0, 0.0, -1)
kdtree = kd.KDTree([[x, y] for x, y in zip(ox, oy)])
P = calc_parameters(ox, oy, xyreso, yawreso, kdtree)
hmap = astar.calc_holonomic_heuristic_with_obstacle(
ngoal, P.ox, P.oy, P.xyreso, 1.0)
steer_set, direc_set = calc_motion_set()
open_set, closed_set = {calc_index(nstart, P): nstart}, {}
qp = QueuePrior()
qp.put(calc_index(nstart, P), calc_hybrid_cost(nstart, hmap, P))
while True:
if not open_set:
return None
ind = qp.get()
n_curr = open_set[ind]
closed_set[ind] = n_curr
open_set.pop(ind)
update, fpath = update_node_with_analystic_expantion(n_curr, ngoal, P)
if update:
fnode = fpath
break
for i in range(len(steer_set)):
node = calc_next_node(n_curr, ind, steer_set[i], direc_set[i], P)
if not node:
continue
node_ind = calc_index(node, P)
if node_ind in closed_set:
continue
if node_ind not in open_set:
open_set[node_ind] = node
qp.put(node_ind, calc_hybrid_cost(node, hmap, P))
else:
if open_set[node_ind].cost > node.cost:
open_set[node_ind] = node
qp.put(node_ind, calc_hybrid_cost(node, hmap, P))
return extract_path(closed_set, fnode, nstart)
def calc_next_node(n, ind, u, d, P):
step = C.XY_RESO * 2.0
nlist = math.ceil(step / C.MOVE_STEP)
xlist = [n.x[-1] + d * C.MOVE_STEP * math.cos(n.yaw[-1])]
ylist = [n.y[-1] + d * C.MOVE_STEP * math.sin(n.yaw[-1])]
yawlist = [rs.pi_2_pi(n.yaw[-1] + d * C.MOVE_STEP / C.WB * math.tan(u))]
yawtlist = [
rs.pi_2_pi(n.yawt[-1] +
d * C.MOVE_STEP / C.RTR * math.sin(n.yaw[-1] - n.yawt[-1]))
]
for i in range(nlist - 1):
xlist.append(xlist[i] + d * C.MOVE_STEP * math.cos(yawlist[i]))
ylist.append(ylist[i] + d * C.MOVE_STEP * math.sin(yawlist[i]))
yawlist.append(
rs.pi_2_pi(yawlist[i] + d * C.MOVE_STEP / C.WB * math.tan(u)))
yawtlist.append(
rs.pi_2_pi(yawtlist[i] + d * C.MOVE_STEP / C.RTR *
math.sin(yawlist[i] - yawtlist[i])))
xind = round(xlist[-1] / P.xyreso)
yind = round(ylist[-1] / P.xyreso)
yawind = round(yawlist[-1] / P.yawreso)
cost = 0.0
if d > 0:
direction = 1.0
cost += abs(step)
else:
direction = -1.0
cost += abs(step) * C.BACKWARD_COST
if direction != n.direction: # switch back penalty
cost += C.GEAR_COST
cost += C.STEER_ANGLE_COST * abs(u) # steer penalyty
cost += C.STEER_CHANGE_COST * abs(n.steer - u) # steer change penalty
cost += C.SCISSORS_COST * sum(
[abs(rs.pi_2_pi(x - y))
for x, y in zip(yawlist, yawtlist)]) # jacknif cost
cost = n.cost + cost
directions = [direction for _ in range(len(xlist))]
node = Node(xind, yind, yawind, direction, xlist, ylist, yawlist, yawtlist,
directions, u, cost, ind)
return node
def calc_rs_path_cost(rspath, yawt):
cost = 0.0
for lr in rspath.lengths:
if lr >= 0:
cost += 1
else:
cost += abs(lr) * C.BACKWARD_COST
for i in range(len(rspath.lengths) - 1):
if rspath.lengths[i] * rspath.lengths[i + 1] < 0.0:
cost += C.GEAR_COST
for ctype in rspath.ctypes:
if ctype != "S":
cost += C.STEER_ANGLE_COST * abs(C.MAX_STEER)
nctypes = len(rspath.ctypes)
ulist = [0.0 for _ in range(nctypes)]
for i in range(nctypes):
if rspath.ctypes[i] == "R":
ulist[i] = -C.MAX_STEER
elif rspath.ctypes[i] == "L":
ulist[i] = C.MAX_STEER
for i in range(nctypes - 1):
cost += C.STEER_CHANGE_COST * abs(ulist[i + 1] - ulist[i])
cost += C.SCISSORS_COST * sum(
[abs(rs.pi_2_pi(x - y)) for x, y in zip(rspath.yaw, yawt)])
return cost
def update_node_with_analystic_expantion(n_curr, ngoal, gyawt, P):
path = analystic_expantion(n_curr, ngoal, P) # rs path: n -> ngoal
if not path:
return False, None
steps = [C.MOVE_STEP * d for d in path.directions]
yawt = calc_trailer_yaw(path.yaw, n_curr.yawt[-1], steps)
if abs(rs.pi_2_pi(yawt[-1] - gyawt)) >= C.GOAL_YAW_ERROR:
return False, None
fx = path.x[1:-1]
fy = path.y[1:-1]
fyaw = path.yaw[1:-1]
fd = []
for d in path.directions[1:-1]:
if d >= 0:
fd.append(1.0)
else:
fd.append(-1.0)
# fd = path.directions[1:-1]
fcost = n_curr.cost + calc_rs_path_cost(path, yawt)
fpind = calc_index(n_curr, P)
fyawt = yawt[1:-1]
fsteer = 0.0
fpath = Node(n_curr.xind, n_curr.yind, n_curr.yawind, n_curr.direction, fx,
fy, fyaw, fyawt, fd, fsteer, fcost, fpind)
return True, fpath
def calc_next_node(n_curr, c_id, u, d, P):
step = C.XY_RESO * 2
nlist = math.ceil(step / C.MOVE_STEP)
xlist = [n_curr.x[-1] + d * C.MOVE_STEP * math.cos(n_curr.yaw[-1])]
ylist = [n_curr.y[-1] + d * C.MOVE_STEP * math.sin(n_curr.yaw[-1])]
yawlist = [
rs.pi_2_pi(n_curr.yaw[-1] + d * C.MOVE_STEP / C.WB * math.tan(u))
]
for i in range(nlist - 1):
xlist.append(xlist[i] + d * C.MOVE_STEP * math.cos(yawlist[i]))
ylist.append(ylist[i] + d * C.MOVE_STEP * math.sin(yawlist[i]))
yawlist.append(
rs.pi_2_pi(yawlist[i] + d * C.MOVE_STEP / C.WB * math.tan(u)))
xind = round(xlist[-1] / P.xyreso)
yind = round(ylist[-1] / P.xyreso)
yawind = round(yawlist[-1] / P.yawreso)
if not is_index_ok(xind, yind, xlist, ylist, yawlist, P):
return None
cost = 0.0
if d > 0:
direction = 1
cost += abs(step)
else:
direction = -1
cost += abs(step) * C.BACKWARD_COST
if direction != n_curr.direction: # switch back penalty
cost += C.GEAR_COST
cost += C.STEER_ANGLE_COST * abs(u) # steer angle penalyty
cost += C.STEER_CHANGE_COST * abs(n_curr.steer - u) # steer change penalty
cost = n_curr.cost + cost
directions = [direction for _ in range(len(xlist))]
node = Node(xind, yind, yawind, direction, xlist, ylist, yawlist,
directions, u, cost, c_id)
return node
def main():
print("start!")
sx, sy = 18.0, 34.0 # [m]
syaw0 = np.deg2rad(180.0)
syawt = np.deg2rad(180.0)
gx, gy = 0.0, 12.0 # [m]
gyaw0 = np.deg2rad(90.0)
gyawt = np.deg2rad(90.0)
ox, oy = design_obstacles()
plt.plot(ox, oy, 'sk')
draw_model(sx, sy, syaw0, syawt, 0.0)
draw_model(gx, gy, gyaw0, gyawt, 0.0)
# test(sx, sy, syaw0, syawt, 3.5, 32)
# plt.axis("equal")
# plt.show()
oox, ooy = ox[:], oy[:]
t0 = time.time()
path = hybrid_astar_planning(sx, sy, syaw0, syawt, gx, gy, gyaw0, gyawt,
oox, ooy, C.XY_RESO, C.YAW_RESO)
t1 = time.time()
print("running T: ", t1 - t0)
x = path.x
y = path.y
yaw = path.yaw
yawt = path.yawt
direction = path.direction
plt.pause(10)
for k in range(len(x)):
plt.cla()
plt.plot(ox, oy, "sk")
plt.plot(x, y, linewidth=1.5, color='r')
if k < len(x) - 2:
dy = (yaw[k + 1] - yaw[k]) / C.MOVE_STEP
steer = rs.pi_2_pi(math.atan(C.WB * dy / direction[k]))
else:
steer = 0.0
draw_model(gx, gy, gyaw0, gyawt, 0.0, 'gray')
draw_model(x[k], y[k], yaw[k], yawt[k], steer)
plt.axis("equal")
plt.pause(0.0001)
plt.show()
print("Done")
def main():
# generate path
ax = np.arange(0, 50, 0.5)
ay = [math.sin(ix / 5.0) * ix / 2.0 for ix in ax]
cx, cy, cyaw, ck, _ = cs.calc_spline_course(ax, ay, ds=C.dt)
t = 0.0
maxTime = 100.0
yaw_old = 0.0
x0, y0, yaw0 = cx[0], cy[0], cyaw[0]
xrec, yrec, yawrec = [], [], []
node = Node(x=x0, y=y0, yaw=yaw0, v=0.0)
ref_trajectory = Trajectory(cx, cy, cyaw, ck)
e, e_th = 0.0, 0.0
while t < maxTime:
speed_ref = 25.0 / 3.6
sp = calc_speed_profile(cyaw, speed_ref)
dl, target_ind, e, e_th, ai = ref_trajectory.lqr_control(
node, e, e_th, sp, C.lqr_Q, C.lqr_R)
dist = math.hypot(node.x - cx[-1], node.y - cy[-1])
node.update(ai, dl, 1.0)
t += C.dt
if dist <= C.dref:
break
dy = (node.yaw - yaw_old) / (node.v * C.dt)
steer = rs.pi_2_pi(-math.atan(C.WB * dy))
yaw_old = node.yaw
xrec.append(node.x)
yrec.append(node.y)
yawrec.append(node.yaw)
plt.cla()
plt.plot(cx, cy, color='gray', linewidth=2.0)
plt.plot(xrec, yrec, linewidth=2.0, color='darkviolet')
plt.plot(cx[target_ind], cy[target_ind], '.r')
draw.draw_car(node.x, node.y, node.yaw, steer, C)
plt.axis("equal")
plt.title("FrontWheelFeedback: v=" + str(node.v * 3.6)[:4] + "km/h")
plt.gcf().canvas.mpl_connect(
'key_release_event',
lambda event: [exit(0) if event.key == 'escape' else None])
plt.pause(0.001)
plt.show()
def main():
print("start!")
x, y = 51, 31
sx, sy, syaw0 = 10.0, 7.0, np.deg2rad(120.0)
gx, gy, gyaw0 = 45.0, 20.0, np.deg2rad(90.0)
ox, oy = design_obstacles(x, y)
t0 = time.time()
path = hybrid_astar_planning(sx, sy, syaw0, gx, gy, gyaw0, ox, oy,
C.XY_RESO, C.YAW_RESO)
t1 = time.time()
print("running T: ", t1 - t0)
if not path:
print("Searching failed!")
return
x = path.x
y = path.y
yaw = path.yaw
direction = path.direction
for k in range(len(x)):
plt.cla()
plt.plot(ox, oy, "sk")
plt.plot(x, y, linewidth=1.5, color='r')
if k < len(x) - 2:
dy = (yaw[k + 1] - yaw[k]) / C.MOVE_STEP
steer = rs.pi_2_pi(math.atan(-C.WB * dy / direction[k]))
else:
steer = 0.0
draw_car(gx, gy, gyaw0, 0.0, 'dimgray')
draw_car(x[k], y[k], yaw[k], steer)
plt.title("Hybrid A*")
plt.axis("equal")
plt.pause(0.0001)
plt.show()
print("Done!")
def hybrid_astar_planning(sx, sy, syaw, syawt, gx, gy, gyaw, gyawt, ox, oy,
xyreso, yawreso):
"""
planning hybrid A* path.
:param sx: starting node x position [m]
:param sy: starting node y position [m]
:param syaw: starting node yaw angle [rad]
:param syawt: starting node trailer yaw angle [rad]
:param gx: goal node x position [m]
:param gy: goal node y position [m]
:param gyaw: goal node yaw angle [rad]
:param gyawt: goal node trailer yaw angle [rad]
:param ox: obstacle x positions [m]
:param oy: obstacle y positions [m]
:param xyreso: grid resolution [m]
:param yawreso: yaw resolution [m]
:return: hybrid A* path
"""
sxr, syr = round(sx / xyreso), round(sy / xyreso)
gxr, gyr = round(gx / xyreso), round(gy / xyreso)
syawr = round(rs.pi_2_pi(syaw) / yawreso)
gyawr = round(rs.pi_2_pi(gyaw) / yawreso)
nstart = Node(sxr, syr, syawr, 1, [sx], [sy], [syaw], [syawt], [1], 0.0,
0.0, -1)
ngoal = Node(gxr, gyr, gyawr, 1, [gx], [gy], [gyaw], [gyawt], [1], 0.0,
0.0, -1)
kdtree = kd.KDTree([[x, y] for x, y in zip(ox, oy)])
P = calc_parameters(ox, oy, xyreso, yawreso, kdtree)
hmap = astar.calc_holonomic_heuristic_with_obstacle(
ngoal, P.ox, P.oy, P.xyreso, 1.0)
steer_set, direc_set = calc_motion_set()
open_set, closed_set = {calc_index(nstart, P): nstart}, {}
qp = QueuePrior()
qp.put(calc_index(nstart, P), calc_hybrid_cost(nstart, hmap, P))
while True:
if not open_set:
return None
ind = qp.get()
n_curr = open_set[ind]
closed_set[ind] = n_curr
open_set.pop(ind)
update, fpath = update_node_with_analystic_expantion(
n_curr, ngoal, gyawt, P)
if update:
fnode = fpath
break
yawt0 = n_curr.yawt[0]
for i in range(len(steer_set)):
node = calc_next_node(n_curr, ind, steer_set[i], direc_set[i], P)
if not is_index_ok(node, yawt0, P):
continue
node_ind = calc_index(node, P)
if node_ind in closed_set:
continue
if node_ind not in open_set:
open_set[node_ind] = node
qp.put(node_ind, calc_hybrid_cost(node, hmap, P))
else:
if open_set[node_ind].cost > node.cost:
open_set[node_ind] = node
print("final expand node: ", len(open_set) + len(closed_set))
return extract_path(closed_set, fnode, nstart)
def main():
ax = [0.0, 15.0, 30.0, 50.0, 60.0]
ay = [0.0, 40.0, 15.0, 30.0, 0.0]
cx, cy, cyaw, ck, s = cs.calc_spline_course(ax, ay, ds=P.d_dist)
sp = calc_speed_profile(cx, cy, cyaw, P.target_speed)
ref_path = PATH(cx, cy, cyaw, ck)
node = Node(x=cx[0], y=cy[0], yaw=cyaw[0], v=0.0)
time = 0.0
x = [node.x]
y = [node.y]
yaw = [node.yaw]
v = [node.v]
t = [0.0]
d = [0.0]
a = [0.0]
delta_opt, a_opt = None, None
a_exc, delta_exc = 0.0, 0.0
while time < P.time_max:
z_ref, target_ind = \
calc_ref_trajectory_in_T_step(node, ref_path, sp)
z0 = [node.x, node.y, node.v, node.yaw]
a_opt, delta_opt, x_opt, y_opt, yaw_opt, v_opt = \
linear_mpc_control(z_ref, z0, a_opt, delta_opt)
if delta_opt is not None:
delta_exc, a_exc = delta_opt[0], a_opt[0]
node.update(a_exc, delta_exc, 1.0)
time += P.dt
x.append(node.x)
y.append(node.y)
yaw.append(node.yaw)
v.append(node.v)
t.append(time)
d.append(delta_exc)
a.append(a_exc)
dist = math.hypot(node.x - cx[-1], node.y - cy[-1])
if dist < P.dist_stop and \
abs(node.v) < P.speed_stop:
break
dy = (node.yaw - yaw[-2]) / (node.v * P.dt)
steer = rs.pi_2_pi(-math.atan(P.WB * dy))
plt.cla()
draw.draw_car(node.x, node.y, node.yaw, steer, P)
plt.gcf().canvas.mpl_connect(
'key_release_event',
lambda event: [exit(0) if event.key == 'escape' else None])
if x_opt is not None:
plt.plot(x_opt, y_opt, color='darkviolet', marker='*')
plt.plot(cx, cy, color='gray')
plt.plot(x, y, '-b')
plt.plot(cx[target_ind], cy[target_ind])
plt.axis("equal")
plt.title("Linear MPC, " + "v = " + str(round(node.v * 3.6, 2)))
plt.pause(0.001)
plt.show()
def main():
# generate path
states = [(0, 0, 0), (20, 15, 0), (35, 20, 90), (40, 0, 180), (20, 0, 120),
(5, -10, 180), (15, 5, 30)]
#
# states = [(-3, 3, 120), (10, -7, 30), (10, 13, 30), (20, 5, -25),
# (35, 10, 180), (30, -10, 160), (5, -12, 90)]
x_ref, y_ref, yaw_ref, direct, curv, x_all, y_all = generate_path(states)
maxTime = 100.0
yaw_old = 0.0
x0, y0, yaw0, direct0 = \
x_ref[0][0], y_ref[0][0], yaw_ref[0][0], direct[0][0]
x_rec, y_rec, yaw_rec, direct_rec = [], [], [], []
for cx, cy, cyaw, cdirect, ccurv in zip(x_ref, y_ref, yaw_ref, direct,
curv):
t = 0.0
er, theta_e = 0.0, 0.0
node = Node(x=x0, y=y0, yaw=yaw0, v=0.0, direct=cdirect[0])
ref_path = PATH(cx, cy, cyaw, ccurv)
while t < maxTime:
if cdirect[0] > 0:
speed_ref = 30.0 / 3.6
else:
speed_ref = 20.0 / 3.6
delta, er, theta_e, ind = lqr_lateral_control(
node, er, theta_e, ref_path)
dist = math.hypot(node.x - cx[-1], node.y - cy[-1])
acceleration = pid_longitudinal_control(speed_ref, node.v, dist,
node.direct)
node.update(acceleration, delta, node.direct)
t += C.dt
if dist <= C.dist_stop:
break
x_rec.append(node.x)
y_rec.append(node.y)
yaw_rec.append(node.yaw)
direct_rec.append(node.direct)
dy = (node.yaw - yaw_old) / (node.v * C.dt)
steer = rs.pi_2_pi(-math.atan(C.WB * dy))
yaw_old = node.yaw
x0 = x_rec[-1]
y0 = y_rec[-1]
yaw0 = yaw_rec[-1]
plt.cla()
plt.plot(x_all, y_all, color='gray', linewidth=2.0)
plt.plot(x_rec, y_rec, linewidth=2.0, color='darkviolet')
plt.plot(cx[ind], cy[ind], '.r')
draw.draw_car(node.x, node.y, node.yaw, steer, C)
plt.axis("equal")
plt.title("LQR & PID: v=" + str(node.v * 3.6)[:4] + "km/h")
plt.gcf().canvas.mpl_connect(
'key_release_event',
lambda event: [exit(0) if event.key == 'escape' else None])
plt.pause(0.001)
plt.show()
def main():
# generate path: [x, y, yaw]
states = [(0, 0, 0), (20, 15, 0), (35, 20, 90), (40, 0, 180),
(20, 0, 120), (5, -10, 180), (15, 5, 30)]
# states = [(-3, 3, 120), (10, -7, 30), (10, 13, 30), (20, 5, -25),
# (35, 10, 180), (30, -10, 160), (5, -12, 90)]
x, y, yaw, direct, path_x, path_y = generate_path(states)
# simulation
maxTime = 100.0
yaw_old = 0.0
x0, y0, yaw0, direct0 = x[0][0], y[0][0], yaw[0][0], direct[0][0]
x_rec, y_rec = [], []
for cx, cy, cyaw, cdirect in zip(x, y, yaw, direct):
t = 0.0
node = Node(x=x0, y=y0, yaw=yaw0, v=0.0, direct=direct0)
nodes = Nodes()
nodes.add(t, node)
ref_trajectory = PATH(cx, cy)
target_ind, _ = ref_trajectory.target_index(node)
while t <= maxTime:
if cdirect[0] > 0:
target_speed = 30.0 / 3.6
C.Ld = 4.0
C.dist_stop = 1.5
C.dc = -1.1
else:
target_speed = 20.0 / 3.6
C.Ld = 2.5
C.dist_stop = 0.2
C.dc = 0.2
xt = node.x + C.dc * math.cos(node.yaw)
yt = node.y + C.dc * math.sin(node.yaw)
dist = math.hypot(xt - cx[-1], yt - cy[-1])
if dist < C.dist_stop:
break
acceleration = pid_control(target_speed, node.v, dist, cdirect[0])
delta, target_ind = pure_pursuit(node, ref_trajectory, target_ind)
t += C.dt
node.update(acceleration, delta, cdirect[0])
nodes.add(t, node)
x_rec.append(node.x)
y_rec.append(node.y)
dy = (node.yaw - yaw_old) / (node.v * C.dt)
steer = rs.pi_2_pi(-math.atan(C.WB * dy))
yaw_old = node.yaw
x0 = nodes.x[-1]
y0 = nodes.y[-1]
yaw0 = nodes.yaw[-1]
direct0 = nodes.direct[-1]
# animation
plt.cla()
plt.plot(node.x, node.y, marker='.', color='k')
plt.plot(path_x, path_y, color='gray', linewidth=2)
plt.plot(x_rec, y_rec, color='darkviolet', linewidth=2)
plt.plot(cx[target_ind], cy[target_ind], ".r")
draw.draw_car(node.x, node.y, yaw_old, steer, C)
# for m in range(len(states)):
# draw.Arrow(states[m][0], states[m][1], np.deg2rad(states[m][2]), 2, 'blue')
plt.axis("equal")
plt.title("PurePursuit: v=" + str(node.v * 3.6)[:4] + "km/h")
plt.gcf().canvas.mpl_connect('key_release_event',
lambda event:
[exit(0) if event.key == 'escape' else None])
plt.pause(0.001)
plt.show()
def main():
# generate path
states = [(0, 0, 0), (20, 15, 0), (35, 20, 90), (40, 0, 180), (20, 0, 120),
(5, -10, 180), (15, 5, 30)]
#
# states = [(-3, 3, 120), (10, -7, 30), (10, 13, 30), (20, 5, -25),
# (35, 10, 180), (30, -10, 160), (5, -12, 90)]
x_ref, y_ref, yaw_ref, direct, curv, x_all, y_all = generate_path(states)
maxTime = 100.0
yaw_old = 0.0
x0, y0, yaw0, direct0 = \
x_ref[0][0], y_ref[0][0], yaw_ref[0][0], direct[0][0]
x_rec, y_rec, yaw_rec, direct_rec = [], [], [], []
for cx, cy, cyaw, cdirect, ccurv in zip(x_ref, y_ref, yaw_ref, direct,
curv):
t = 0.0
node = Node(x=x0, y=y0, yaw=yaw0, v=0.0, direct=cdirect[0])
ref_trajectory = Trajectory(cx, cy, cyaw, ccurv)
speed_ref = 30 / 3.6
while t < maxTime:
if cdirect[0] > 0:
speed_ref = 30.0 / 3.6
C.Ld = 3.5
else:
speed_ref = 20.0 / 3.6
C.Ld = 2.5
e, k, yawref, s0 = ref_trajectory.calc_track_error(node)
di = rear_wheel_feedback_control(node, e, k, yawref)
dist = math.hypot(node.x - cx[-1], node.y - cy[-1])
ai = pid_control(speed_ref, node.v, dist, node.direct)
node.update(ai, di, node.direct)
t += C.dt
if dist <= C.dref:
break
x_rec.append(node.x)
y_rec.append(node.y)
yaw_rec.append(node.yaw)
direct_rec.append(node.direct)
dy = (node.yaw - yaw_old) / (node.v * C.dt)
steer = rs.pi_2_pi(-math.atan(C.WB * dy))
yaw_old = node.yaw
x0 = x_rec[-1]
y0 = y_rec[-1]
yaw0 = yaw_rec[-1]
plt.cla()
plt.plot(x_all, y_all, color='gray', linewidth=2.0)
plt.plot(x_rec, y_rec, linewidth=2.0, color='darkviolet')
plt.plot(cx[s0], cy[s0], '.r')
draw.draw_car(node.x, node.y, node.yaw, steer, C)
plt.axis("equal")
plt.title("RearWheelFeedback: v=" + str(node.v * 3.6)[:4] + "km/h")
plt.gcf().canvas.mpl_connect(
'key_release_event',
lambda event: [exit(0) if event.key == 'escape' else None])
plt.pause(0.001)
plt.show()
