Merge pull request AtsushiSakai#269 from Gjacquenot/patch-5

🔨 Refactored pure_pursuit.py

Author: AtsushiSakai

PathTracking/pure_pursuit/pure_pursuit.py

@@ -3,6 +3,7 @@
 Path tracking simulation with pure pursuit steering control and PID speed control.
 
 author: Atsushi Sakai (@Atsushi_twi)
+        Guillaume Jacquenot (@Gjacquenot)
 
 """
 import numpy as np
@@ -17,7 +18,6 @@
 L = 2.9  # [m] wheel base of vehicle
 
 
-old_nearest_point_index = None
 show_animation = True
 
 
@@ -31,89 +31,104 @@ def __init__(self, x=0.0, y=0.0, yaw=0.0, v=0.0):
         self.rear_x = self.x - ((L / 2) * math.cos(self.yaw))
         self.rear_y = self.y - ((L / 2) * math.sin(self.yaw))
 
+    def update(self, a, delta):
 
-def update(state, a, delta):
+        self.x += self.v * math.cos(self.yaw) * dt
+        self.y += self.v * math.sin(self.yaw) * dt
+        self.yaw += self.v / L * math.tan(delta) * dt
+        self.v += a * dt
+        self.rear_x = self.x - ((L / 2) * math.cos(self.yaw))
+        self.rear_y = self.y - ((L / 2) * math.sin(self.yaw))
 
-    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.v = state.v + a * dt
-    state.rear_x = state.x - ((L / 2) * math.cos(state.yaw))
-    state.rear_y = state.y - ((L / 2) * math.sin(state.yaw))
+    def calc_distance(self, point_x, point_y):
 
-    return state
+        dx = self.rear_x - point_x
+        dy = self.rear_y - point_y
+        return math.hypot(dx, dy)
 
 
-def PIDControl(target, current):
-    a = Kp * (target - current)
+class States:
 
-    return a
+    def __init__(self):
+        self.x = []
+        self.y = []
+        self.yaw = []
+        self.v = []
+        self.t = []
 
+    def append(self, t , state):
+        self.x.append(state.x)
+        self.y.append(state.y)
+        self.yaw.append(state.yaw)
+        self.v.append(state.v)
+        self.t.append(t)
 
-def pure_pursuit_control(state, cx, cy, pind):
 
-    ind = calc_target_index(state, cx, cy)
+def PIDControl(target, current):
+    a = Kp * (target - current)
 
-    if pind >= ind:
-        ind = pind
+    return a
 
-    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.rear_y, tx - state.rear_x) - state.yaw
+class Trajectory:
+    def __init__(self, cx, cy):
+        self.cx = cx
+        self.cy = cy
+        self.old_nearest_point_index = None
 
-    Lf = k * state.v + Lfc
+    def search_target_index(self, state):
+        if self.old_nearest_point_index is None:
+            # search nearest point index
+            dx = [state.rear_x - icx for icx in self.cx]
+            dy = [state.rear_y - icy for icy in self.cy]
+            d = np.hypot(dx, dy)
+            ind = np.argmin(d)
+            self.old_nearest_point_index = ind
+        else:
+            ind = self.old_nearest_point_index
+            distance_this_index = state.calc_distance(self.cx[ind], self.cy[ind])
+            while True:
+                ind = ind + 1 if (ind + 1) < len(self.cx) else ind
+                distance_next_index = state.calc_distance(self.cx[ind], self.cy[ind])
+                if distance_this_index < distance_next_index:
+                    break
+                distance_this_index = distance_next_index
+            self.old_nearest_point_index = ind
 
-    delta = math.atan2(2.0 * L * math.sin(alpha) / Lf, 1.0)
+        L = 0.0
 
-    return delta, ind
+        Lf = k * state.v + Lfc
 
-def calc_distance(state, point_x, point_y):
+        # search look ahead target point index
+        while Lf > L and (ind + 1) < len(self.cx):
+            L = state.calc_distance(self.cx[ind], self.cy[ind])
+            ind += 1
 
-    dx = state.rear_x - point_x
-    dy = state.rear_y - point_y
-    return math.hypot(dx, dy)
+        return ind
 
 
-def calc_target_index(state, cx, cy):
+def pure_pursuit_control(state, trajectory, pind):
 
-    global old_nearest_point_index
+    ind = trajectory.search_target_index(state)
+
+    if pind >= ind:
+        ind = pind
 
-    if old_nearest_point_index is None:
-        # search nearest point index
-        dx = [state.rear_x - icx for icx in cx]
-        dy = [state.rear_y - icy for icy in cy]
-        d = [idx ** 2 + idy ** 2 for (idx, idy) in zip(dx, dy)]
-        ind = d.index(min(d))
-        old_nearest_point_index = ind
+    if ind < len(trajectory.cx):
+        tx = trajectory.cx[ind]
+        ty = trajectory.cy[ind]
     else:
-        ind = old_nearest_point_index
-        distance_this_index = calc_distance(state, cx[ind], cy[ind])
-        while True:
-            ind = ind + 1 if (ind + 1) < len(cx) else ind
-            distance_next_index = calc_distance(state, cx[ind], cy[ind])
-            if distance_this_index < distance_next_index:
-                break
-            distance_this_index = distance_next_index
-        old_nearest_point_index = ind
-
-    L = 0.0
+        tx = trajectory.cx[-1]
+        ty = trajectory.cy[-1]
+        ind = len(trajectory.cx) - 1
+
+    alpha = math.atan2(ty - state.rear_y, tx - state.rear_x) - state.yaw
 
     Lf = k * state.v + Lfc
 
-    # search look ahead target point index
-    while Lf > L and (ind + 1) < len(cx):
-        dx = cx[ind] - state.rear_x
-        dy = cy[ind] - state.rear_y
-        L = math.hypot(dx, dy)
-        ind += 1
+    delta = math.atan2(2.0 * L * math.sin(alpha) / Lf, 1.0)
 
-    return ind
+    return delta, ind
 
 
 def plot_arrow(x, y, yaw, length=1.0, width=0.5, fc="r", ec="k"):
@@ -122,7 +137,7 @@ def plot_arrow(x, y, yaw, length=1.0, width=0.5, fc="r", ec="k"):
     """
 
     if not isinstance(x, float):
-        for (ix, iy, iyaw) in zip(x, y, yaw):
+        for ix, iy, iyaw in zip(x, y, yaw):
             plot_arrow(ix, iy, iyaw)
     else:
         plt.arrow(x, y, length * math.cos(yaw), length * math.sin(yaw),
@@ -144,25 +159,18 @@ def main():
 
     lastIndex = len(cx) - 1
     time = 0.0
-    x = [state.x]
-    y = [state.y]
-    yaw = [state.yaw]
-    v = [state.v]
-    t = [0.0]
-    target_ind = calc_target_index(state, cx, cy)
+    states = States()
+    states.append(time, state)
+    trajectory = Trajectory(cx, cy)
+    target_ind = trajectory.search_target_index(state)
 
     while T >= time and lastIndex > target_ind:
         ai = PIDControl(target_speed, state.v)
-        di, target_ind = pure_pursuit_control(state, cx, cy, target_ind)
-        state = update(state, ai, di)
-
-        time = time + dt
+        di, target_ind = pure_pursuit_control(state, trajectory, target_ind)
+        state.update(ai, di)
 
-        x.append(state.x)
-        y.append(state.y)
-        yaw.append(state.yaw)
-        v.append(state.v)
-        t.append(time)
+        time += dt
+        states.append(time, state)
 
         if show_animation:  # pragma: no cover
             plt.cla()
@@ -171,7 +179,7 @@ def main():
                     lambda event: [exit(0) if event.key == 'escape' else None])
             plot_arrow(state.x, state.y, state.yaw)
             plt.plot(cx, cy, "-r", label="course")
-            plt.plot(x, y, "-b", label="trajectory")
+            plt.plot(states.x, states.y, "-b", label="trajectory")
             plt.plot(cx[target_ind], cy[target_ind], "xg", label="target")
             plt.axis("equal")
             plt.grid(True)
@@ -184,15 +192,15 @@ def main():
     if show_animation:  # pragma: no cover
         plt.cla()
         plt.plot(cx, cy, ".r", label="course")
-        plt.plot(x, y, "-b", label="trajectory")
+        plt.plot(states.x, states.y, "-b", label="trajectory")
         plt.legend()
         plt.xlabel("x[m]")
         plt.ylabel("y[m]")
         plt.axis("equal")
         plt.grid(True)
 
         plt.subplots(1)
-        plt.plot(t, [iv * 3.6 for iv in v], "-r")
+        plt.plot(states.t, [iv * 3.6 for iv in states.v], "-r")
         plt.xlabel("Time[s]")
         plt.ylabel("Speed[km/h]")
         plt.grid(True)