"""
Path tracking simulation with Stanley steering control and PID speed control.
https://www.ri.cmu.edu/pub_files/2009/2/Automatic_Steering_Methods_for_Autonomous_Automobile_Path_Tracking.pdf
Original Author: Atsushi Sakai(@Atsushi_twi)
From https://github.com/AtsushiSakai/PythonRobotics
"""
from __future__ import division, print_function
import argparse
import matplotlib.pyplot as plt
import numpy as np
from .bezier_curve import calcTrajectory, demo_cp
from constants import K_STANLEY_CONTROL, CAR_LENGTH, MAX_STEERING_ANGLE,\
MAX_SPEED_STRAIGHT_LINE, MIN_SPEED, MIN_RADIUS, MAX_RADIUS
Kp_speed = 0.1 # speed propotional gain
dt = 1 # [s] time difference
class State(object):
def __init__(self, x=0.0, y=0.0, yaw=0.0, v=0.0):
super(State, self).__init__()
self.x = x
self.y = y
self.yaw = yaw
self.v = v
def update(self, acceleration, delta):
"""
:param acceleration: (float) Speed control
:param delta: (float) Steering control
"""
delta = np.clip(delta, -MAX_STEERING_ANGLE, MAX_STEERING_ANGLE)
self.x += self.v * np.cos(self.yaw) * dt
self.y += self.v * np.sin(self.yaw) * dt
self.yaw += self.v / CAR_LENGTH * np.tan(delta) * dt
self.yaw = normalizeAngle(self.yaw)
self.v += acceleration * dt
def stanleyControl(state, cx, cy, cyaw, last_target_idx):
"""
:param state: (State object)
:param cx: ([float])
:param cy: ([float])
:param cyaw: ([float])
:param last_target_idx: (int)
:return: (float, int, float)
"""
# Cross track error
current_target_idx, error_front_axle = calcTargetIndex(state, cx, cy)
if last_target_idx >= current_target_idx:
current_target_idx = last_target_idx
# theta_e corrects the heading error
theta_e = normalizeAngle(cyaw[current_target_idx] - state.yaw)
# theta_d corrects the cross track error
theta_d = np.arctan2(K_STANLEY_CONTROL * error_front_axle, state.v)
# Steering control
delta = theta_e + theta_d
return delta, current_target_idx, error_front_axle
def normalizeAngle(angle):
"""
:param angle: (float)
:return: (float) Angle in radian in [-pi, pi]
"""
while angle > np.pi:
angle -= 2.0 * np.pi
while angle < -np.pi:
angle += 2.0 * np.pi
return angle
def calcTargetIndex(state, cx, cy):
"""
:param state: (State object)
:param cx: [float]
:param cy: [float]
:return: (int, float)
"""
# Calc front axle position
fx = state.x + CAR_LENGTH * np.cos(state.yaw)
fy = state.y + CAR_LENGTH * np.sin(state.yaw)
# Search nearest point index
dx = [fx - icx for icx in cx]
dy = [fy - icy for icy in cy]
d = [np.sqrt(idx ** 2 + idy ** 2) for (idx, idy) in zip(dx, dy)]
error_front_axle = min(d)
target_idx = d.index(error_front_axle)
target_yaw = normalizeAngle(np.arctan2(fy - cy[target_idx], fx - cx[target_idx]) - state.yaw)
if target_yaw > 0.0:
error_front_axle = - error_front_axle
return target_idx, error_front_axle
def main(show_animation):
cp = demo_cp * 100
cx, cy, cyaw, ck = calcTrajectory(cp, n_points=200)
target_speed = MAX_SPEED_STRAIGHT_LINE
max_simulation_time = 100.0
# initial state
state = State(x=100, y=50, yaw=np.radians(-180.0), v=0)
last_idx = len(cx) - 1
current_t = 0.0
x = [state.x]
y = [state.y]
yaw = [state.yaw]
v = [state.v]
t = [0.0]
cross_track_errors = []
curvatures = []
target_idx, _ = calcTargetIndex(state, cx, cy)
max_speed = target_speed
max_radius = 40
min_radius = 5
while max_simulation_time >= current_t and last_idx > target_idx:
# Compute Acceleration
acceleration = Kp_speed * (target_speed - state.v)
delta, target_idx, cross_track_error = stanleyControl(state, cx, cy, cyaw, target_idx)
state.update(acceleration, delta)
cross_track_errors.append(cross_track_error)
current_radius = 1 / ck[target_idx]
h = 1 - (np.clip(current_radius, MIN_RADIUS, MAX_RADIUS) - MIN_RADIUS) / (MAX_RADIUS - MIN_RADIUS)
target_speed = h * MIN_SPEED + (1 - h) * MAX_SPEED_STRAIGHT_LINE
curvatures.append(current_radius)
current_t += dt
x.append(state.x)
y.append(state.y)
yaw.append(state.yaw)
v.append(state.v)
t.append(current_t)
if show_animation: # pragma: no cover
plt.cla()
plt.plot(cx, cy, ".r", label="course")
plt.plot(x, y, "-b", label="trajectory")
plt.arrow(state.x, state.y, np.cos(state.yaw), np.sin(state.yaw),
fc="r", ec="k", head_width=0.5, head_length=0.5)
plt.plot(cx[target_idx], cy[target_idx], "xg", label="target")
plt.axis("equal")
plt.grid(True)
plt.title("Speed[km/h]:" + str(state.v)[:4])
plt.pause(0.001)
if show_animation: # pragma: no cover
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)
_, ax1 = plt.subplots(1)
plt.plot(t, [iv for iv in v], "-r")
plt.xlabel("Time[s]")
plt.ylabel("Speed[km/h]")
plt.grid(True)
fig, ax2 = plt.subplots(1)
plt.plot(t[1:], cross_track_errors, "-r", label="cross_track_error")
plt.plot(t[1:], curvatures, "-b", label="curvature radius")
plt.xlabel("Time[s]")
plt.grid(True)
plt.legend()
plt.show()
if __name__ == '__main__':
parser = argparse.ArgumentParser(description='Test a line detector')
parser.add_argument('--no-display', action='store_true', default=False, help='Do not display plots (for tests)')
args = parser.parse_args()
main(not args.no_display)
