Add comments + clean up

Author: araffin

PathTracking/stanley_controller/stanley_controller.py

@@ -5,99 +5,144 @@
 author: Atsushi Sakai (@Atsushi_twi)
 
 """
+from __future__ import division, print_function
+
 import sys
+
 sys.path.append("../../PathPlanning/CubicSpline/")
 
-import math
 import matplotlib.pyplot as plt
-import cubic_spline_planner
+import numpy as np
 
+import cubic_spline_planner
 
 k = 0.5  # control gain
 Kp = 1.0  # speed propotional gain
 dt = 0.1  # [s] time difference
 L = 2.9  # [m] Wheel base of vehicle
-max_steer = math.radians(30.0)  # [rad] max steering angle
+max_steer = np.radians(30.0)  # [rad] max steering angle
 
 show_animation = True
 
 
-class State:
+class State(object):
+    """
+    Class representing the state of a vehicle.
+
+    :param x: (float) x-coordinate
+    :param y: (float) y-coordinate
+    :param yaw: (float) yaw angle
+    :param v: (float) speed
+    """
 
     def __init__(self, x=0.0, y=0.0, yaw=0.0, v=0.0):
+        """Instantiate the object."""
+        super(State, self).__init__()
         self.x = x
         self.y = y
         self.yaw = yaw
         self.v = v
 
+    def update(self, acceleration, delta):
+        """
+        Update the state of the vehicle.
 
-def update(state, a, delta):
+        Stanley Control uses bicycle model.
 
-    if delta >= max_steer:
-        delta = max_steer
-    elif delta <= -max_steer:
-        delta = -max_steer
+        :param acceleration: (float) Acceleration
+        :param delta: (float) Steering
+        """
+        delta = np.clip(delta, -max_steer, 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 / L * math.tan(delta) * dt
-    state.yaw = pi_2_pi(state.yaw)
-    state.v = state.v + a * dt
+        self.x += self.v * np.cos(self.yaw) * dt
+        self.y += self.v * np.sin(self.yaw) * dt
+        self.yaw += self.v / L * np.tan(delta) * dt
+        self.yaw = normalize_angle(self.yaw)
+        self.v += acceleration * dt
 
-    return state
 
+def pid_control(target, current):
+    """
+    Proportional control for the speed.
 
-def PIDControl(target, current):
-    a = Kp * (target - current)
+    :param target: (float)
+    :param current: (float)
+    :return: (float)
+    """
+    return Kp * (target - current)
 
-    return a
 
+def stanley_control(state, cx, cy, cyaw, last_target_idx):
+    """
+    Stanley steering control.
 
-def stanley_control(state, cx, cy, cyaw, pind):
+    :param state: (State object)
+    :param cx: ([float])
+    :param cy: ([float])
+    :param cyaw: ([float])
+    :param last_target_idx: (int)
+    :return: (float, int)
+    """
+    current_target_idx, error_front_axle = calc_target_index(state, cx, cy)
 
-    ind, efa = calc_target_index(state, cx, cy)
+    if last_target_idx >= current_target_idx:
+        current_target_idx = last_target_idx
 
-    if pind >= ind:
-        ind = pind
-
-    theta_e = pi_2_pi(cyaw[ind] - state.yaw)
-    theta_d = math.atan2(k * efa, state.v)
+    # theta_e corrects the heading error
+    theta_e = normalize_angle(cyaw[current_target_idx] - state.yaw)
+    # theta_d corrects the cross track error
+    theta_d = np.arctan2(k * error_front_axle, state.v)
+    # Steering control
     delta = theta_e + theta_d
 
-    return delta, ind
+    return delta, current_target_idx
+
 
+def normalize_angle(angle):
+    """
+    Normalize an angle to [-pi, pi].
 
-def pi_2_pi(angle):
-    while (angle > math.pi):
-        angle = angle - 2.0 * math.pi
+    :param angle: (float)
+    :return: (float) Angle in radian in [-pi, pi]
+    """
+    while angle > np.pi:
+        angle -= 2.0 * np.pi
 
-    while (angle < -math.pi):
-        angle = angle + 2.0 * math.pi
+    while angle < -np.pi:
+        angle += 2.0 * np.pi
 
     return angle
 
 
 def calc_target_index(state, cx, cy):
-
-    # calc frant axle position
-    fx = state.x + L * math.cos(state.yaw)
-    fy = state.y + L * math.sin(state.yaw)
-
-    # search nearest point index
+    """
+    Compute index in the trajectory list of the target.
+
+    :param state: (State object)
+    :param cx: [float]
+    :param cy: [float]
+    :return: (int, float)
+    """
+    # Calc front axle position
+    fx = state.x + L * np.cos(state.yaw)
+    fy = state.y + L * np.sin(state.yaw)
+
+    # Search nearest point index
     dx = [fx - icx for icx in cx]
     dy = [fy - icy for icy in cy]
-    d = [math.sqrt(idx ** 2 + idy ** 2) for (idx, idy) in zip(dx, dy)]
-    mind = min(d)
-    ind = d.index(mind)
+    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)
 
-    tyaw = pi_2_pi(math.atan2(fy - cy[ind], fx - cx[ind]) - state.yaw)
-    if tyaw > 0.0:
-        mind = - mind
+    target_yaw = normalize_angle(np.arctan2(fy - cy[target_idx], fx - cx[target_idx]) - state.yaw)
+    if target_yaw > 0.0:
+        error_front_axle = - error_front_axle
 
-    return ind, mind
+    return target_idx, error_front_axle
 
 
 def main():
+    """Plot an example of Stanley steering control on a cubic spline."""
     #  target course
     ax = [0.0, 100.0, 100.0, 50.0, 60.0]
     ay = [0.0, 0.0, -30.0, -20.0, 0.0]
@@ -107,26 +152,26 @@ def main():
 
     target_speed = 30.0 / 3.6  # [m/s]
 
-    T = 100.0  # max simulation time
+    max_simulation_time = 100.0
 
-    # initial state
-    state = State(x=-0.0, y=5.0, yaw=math.radians(20.0), v=0.0)
+    # Initial state
+    state = State(x=-0.0, y=5.0, yaw=np.radians(20.0), v=0.0)
 
-    lastIndex = len(cx) - 1
+    last_idx = len(cx) - 1
     time = 0.0
     x = [state.x]
     y = [state.y]
     yaw = [state.yaw]
     v = [state.v]
     t = [0.0]
-    target_ind, mind = calc_target_index(state, cx, cy)
+    target_idx, _ = calc_target_index(state, cx, cy)
 
-    while T >= time and lastIndex > target_ind:
-        ai = PIDControl(target_speed, state.v)
-        di, target_ind = stanley_control(state, cx, cy, cyaw, target_ind)
-        state = update(state, ai, di)
+    while max_simulation_time >= time and last_idx > target_idx:
+        ai = pid_control(target_speed, state.v)
+        di, target_idx = stanley_control(state, cx, cy, cyaw, target_idx)
+        state.update(ai, di)
 
-        time = time + dt
+        time += dt
 
         x.append(state.x)
         y.append(state.y)
@@ -138,14 +183,14 @@ def main():
             plt.cla()
             plt.plot(cx, cy, ".r", label="course")
             plt.plot(x, y, "-b", label="trajectory")
-            plt.plot(cx[target_ind], cy[target_ind], "xg", label="target")
+            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 * 3.6)[:4])
             plt.pause(0.001)
 
     # Test
-    assert lastIndex >= target_ind, "Cannot goal"
+    assert last_idx >= target_idx, "Cannot reach goal"
 
     if show_animation:
         plt.plot(cx, cy, ".r", label="course")