feat: Add reverse mode in pure pursuit (#1194)

* feat: Add vehicle model rendering function and pause simulation function

* feat: Add support for reverse mode and related function adjustments

* feat: Add reverse mode test case

* feat: Limit the maximum steering angle to improve path tracking control

* feat: Add a newline for improved readability in plot_arrow function

* fix: Replace max/min with np.clip for steering angle limitation in pure pursuit control

Author: yangtzech

PathTracking/pure_pursuit/pure_pursuit.py

@@ -10,33 +10,49 @@
 import math
 import matplotlib.pyplot as plt
 
+import sys
+import pathlib
+sys.path.append(str(pathlib.Path(__file__).parent.parent.parent))
+from utils.angle import angle_mod
+
 # Parameters
 k = 0.1  # look forward gain
 Lfc = 2.0  # [m] look-ahead distance
 Kp = 1.0  # speed proportional gain
 dt = 0.1  # [s] time tick
 WB = 2.9  # [m] wheel base of vehicle
 
-show_animation = True
 
+# Vehicle parameters
+LENGTH = WB + 1.0  # Vehicle length
+WIDTH = 2.0  # Vehicle width
+WHEEL_LEN = 0.6  # Wheel length
+WHEEL_WIDTH = 0.2  # Wheel width
+MAX_STEER = math.pi / 4  # Maximum steering angle [rad]
 
-class State:
 
-    def __init__(self, x=0.0, y=0.0, yaw=0.0, v=0.0):
+show_animation = True
+pause_simulation = False  # Flag for pause simulation
+is_reverse_mode = False   # Flag for reverse driving mode
+
+class State:
+    def __init__(self, x=0.0, y=0.0, yaw=0.0, v=0.0, is_reverse=False):
         self.x = x
         self.y = y
         self.yaw = yaw
         self.v = v
-        self.rear_x = self.x - ((WB / 2) * math.cos(self.yaw))
-        self.rear_y = self.y - ((WB / 2) * math.sin(self.yaw))
+        self.direction = -1 if is_reverse else 1  # Direction based on reverse flag
+        self.rear_x = self.x - self.direction * ((WB / 2) * math.cos(self.yaw))
+        self.rear_y = self.y - self.direction * ((WB / 2) * math.sin(self.yaw))
 
     def update(self, a, delta):
         self.x += self.v * math.cos(self.yaw) * dt
         self.y += self.v * math.sin(self.yaw) * dt
-        self.yaw += self.v / WB * math.tan(delta) * dt
+        self.yaw += self.direction * self.v / WB * math.tan(delta) * dt
+        self.yaw = angle_mod(self.yaw)
         self.v += a * dt
-        self.rear_x = self.x - ((WB / 2) * math.cos(self.yaw))
-        self.rear_y = self.y - ((WB / 2) * math.sin(self.yaw))
+        self.rear_x = self.x - self.direction * ((WB / 2) * math.cos(self.yaw))
+        self.rear_y = self.y - self.direction * ((WB / 2) * math.sin(self.yaw))
 
     def calc_distance(self, point_x, point_y):
         dx = self.rear_x - point_x
@@ -51,13 +67,15 @@ def __init__(self):
         self.y = []
         self.yaw = []
         self.v = []
+        self.direction = []
         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.direction.append(state.direction)
         self.t.append(t)
 
 
@@ -117,14 +135,18 @@ def pure_pursuit_steer_control(state, trajectory, pind):
     if ind < len(trajectory.cx):
         tx = trajectory.cx[ind]
         ty = trajectory.cy[ind]
-    else:  # toward goal
+    else:
         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
 
-    delta = math.atan2(2.0 * WB * math.sin(alpha) / Lf, 1.0)
+    # Reverse steering angle when reversing
+    delta = state.direction * math.atan2(2.0 * WB * math.sin(alpha) / Lf, 1.0)
+
+    # Limit steering angle to max value
+    delta = np.clip(delta, -MAX_STEER, MAX_STEER)
 
     return delta, ind
 
@@ -142,18 +164,119 @@ def plot_arrow(x, y, yaw, length=1.0, width=0.5, fc="r", ec="k"):
                   fc=fc, ec=ec, head_width=width, head_length=width)
         plt.plot(x, y)
 
+def plot_vehicle(x, y, yaw, steer=0.0, color='blue', is_reverse=False):
+    """
+    Plot vehicle model with four wheels
+    Args:
+        x, y: Vehicle center position
+        yaw: Vehicle heading angle
+        steer: Steering angle
+        color: Vehicle color
+        is_reverse: Flag for reverse mode
+    """
+    # Adjust heading angle in reverse mode
+    if is_reverse:
+        yaw = angle_mod(yaw + math.pi)  # Rotate heading by 180 degrees
+        steer = -steer  # Reverse steering direction
+
+    def plot_wheel(x, y, yaw, steer=0.0, color=color):
+        """Plot single wheel"""
+        wheel = np.array([
+            [-WHEEL_LEN/2, WHEEL_WIDTH/2],
+            [WHEEL_LEN/2, WHEEL_WIDTH/2],
+            [WHEEL_LEN/2, -WHEEL_WIDTH/2],
+            [-WHEEL_LEN/2, -WHEEL_WIDTH/2],
+            [-WHEEL_LEN/2, WHEEL_WIDTH/2]
+        ])
+
+        # Rotate wheel if steering
+        if steer != 0:
+            c, s = np.cos(steer), np.sin(steer)
+            rot_steer = np.array([[c, -s], [s, c]])
+            wheel = wheel @ rot_steer.T
+
+        # Apply vehicle heading rotation
+        c, s = np.cos(yaw), np.sin(yaw)
+        rot_yaw = np.array([[c, -s], [s, c]])
+        wheel = wheel @ rot_yaw.T
+
+        # Translate to position
+        wheel[:, 0] += x
+        wheel[:, 1] += y
+
+        # Plot wheel with color
+        plt.plot(wheel[:, 0], wheel[:, 1], color=color)
+
+    # Calculate vehicle body corners
+    corners = np.array([
+        [-LENGTH/2, WIDTH/2],
+        [LENGTH/2, WIDTH/2],
+        [LENGTH/2, -WIDTH/2],
+        [-LENGTH/2, -WIDTH/2],
+        [-LENGTH/2, WIDTH/2]
+    ])
+
+    # Rotation matrix
+    c, s = np.cos(yaw), np.sin(yaw)
+    Rot = np.array([[c, -s], [s, c]])
+
+    # Rotate and translate vehicle body
+    rotated = corners @ Rot.T
+    rotated[:, 0] += x
+    rotated[:, 1] += y
+
+    # Plot vehicle body
+    plt.plot(rotated[:, 0], rotated[:, 1], color=color)
+
+    # Plot wheels (darker color for front wheels)
+    front_color = 'darkblue'
+    rear_color = color
+    
+    # Plot four wheels
+    # Front left
+    plot_wheel(x + LENGTH/4 * c - WIDTH/2 * s,
+              y + LENGTH/4 * s + WIDTH/2 * c,
+              yaw, steer, front_color)
+    # Front right  
+    plot_wheel(x + LENGTH/4 * c + WIDTH/2 * s,
+              y + LENGTH/4 * s - WIDTH/2 * c,
+              yaw, steer, front_color)
+    # Rear left
+    plot_wheel(x - LENGTH/4 * c - WIDTH/2 * s,
+              y - LENGTH/4 * s + WIDTH/2 * c,
+              yaw, color=rear_color)
+    # Rear right
+    plot_wheel(x - LENGTH/4 * c + WIDTH/2 * s,
+              y - LENGTH/4 * s - WIDTH/2 * c,
+              yaw, color=rear_color)
+
+    # Add direction arrow
+    arrow_length = LENGTH/3
+    plt.arrow(x, y,
+             -arrow_length * math.cos(yaw) if is_reverse else arrow_length * math.cos(yaw),
+             -arrow_length * math.sin(yaw) if is_reverse else arrow_length * math.sin(yaw),
+             head_width=WIDTH/4, head_length=WIDTH/4,
+             fc='r', ec='r', alpha=0.5)
+
+# Keyboard event handler
+def on_key(event):
+    global pause_simulation
+    if event.key == ' ':  # Space key
+        pause_simulation = not pause_simulation
+    elif event.key == 'escape':
+        exit(0)
 
 def main():
     #  target course
-    cx = np.arange(0, 50, 0.5)
+    cx = -1 * np.arange(0, 50, 0.5) if is_reverse_mode else np.arange(0, 50, 0.5)
     cy = [math.sin(ix / 5.0) * ix / 2.0 for ix in cx]
 
     target_speed = 10.0 / 3.6  # [m/s]
 
     T = 100.0  # max simulation time
 
     # initial state
-    state = State(x=-0.0, y=-3.0, yaw=0.0, v=0.0)
+    state = State(x=-0.0, y=-3.0, yaw=math.pi if is_reverse_mode else 0.0, v=0.0, is_reverse=is_reverse_mode)
 
     lastIndex = len(cx) - 1
     time = 0.0
@@ -173,22 +296,33 @@ def main():
 
         time += dt
         states.append(time, state)
-
         if show_animation:  # pragma: no cover
             plt.cla()
             # for stopping simulation with the esc key.
-            plt.gcf().canvas.mpl_connect(
-                'key_release_event',
-                lambda event: [exit(0) if event.key == 'escape' else None])
-            plot_arrow(state.x, state.y, state.yaw)
+            plt.gcf().canvas.mpl_connect('key_release_event', on_key)
+            # Pass is_reverse parameter
+            plot_vehicle(state.x, state.y, state.yaw, di, is_reverse=is_reverse_mode)
             plt.plot(cx, cy, "-r", label="course")
             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)
             plt.title("Speed[km/h]:" + str(state.v * 3.6)[:4])
+            plt.legend()  # Add legend display
+
+            # Add pause state display
+            if pause_simulation:
+                plt.text(0.02, 0.95, 'PAUSED', transform=plt.gca().transAxes,
+                        bbox=dict(facecolor='red', alpha=0.5))
+
             plt.pause(0.001)
 
+            # Handle pause state
+            while pause_simulation:
+                plt.pause(0.1)  # Reduce CPU usage
+                if not plt.get_fignums():  # Check if window is closed
+                    exit(0)
+
     # Test
     assert lastIndex >= target_ind, "Cannot goal"
 

tests/test_pure_pursuit.py

@@ -6,6 +6,10 @@ def test1():
     m.show_animation = False
     m.main()
 
+def test_backward():
+    m.show_animation = False
+    m.is_reverse_mode = True
+    m.main()
 
 if __name__ == '__main__':
     conftest.run_this_test(__file__)