feat: Add support for reverse mode and related function adjustments

Author: yangtzech

PathTracking/pure_pursuit/pure_pursuit.py

@@ -10,33 +10,48 @@
 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
 
+
+# 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
+
+
 show_animation = True
-pause_simulation = False  # 新增暂停标志
+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):
+    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 +66,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 +134,22 @@ 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)
+
+    if delta > math.pi / 4:
+        delta = math.pi / 4
+    elif delta < - math.pi / 4:
+        delta = - 1 * math.pi / 4
+    else:
+        delta = delta
 
     return delta, ind
 
@@ -141,23 +166,23 @@ def plot_arrow(x, y, yaw, length=1.0, width=0.5, fc="r", ec="k"):
         plt.arrow(x, y, length * math.cos(yaw), length * math.sin(yaw),
                   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'):
+def plot_vehicle(x, y, yaw, steer=0.0, color='blue', is_reverse=False):
     """
-    绘制带四个车轮的车辆模型
+    Plot vehicle model with four wheels
     Args:
-        x, y: 车辆中心位置
-        yaw: 车辆航向角
-        steer: 转向角
-        color: 车辆颜色
+        x, y: Vehicle center position
+        yaw: Vehicle heading angle
+        steer: Steering angle
+        color: Vehicle color
+        is_reverse: Flag for reverse mode
     """
-    # 车辆参数
-    LENGTH = WB + 1.0  # 车长
-    WIDTH = 2.0  # 车宽
-    WHEEL_LEN = 0.6  # 车轮长度
-    WHEEL_WIDTH = 0.2  # 车轮宽度
-
-    def plot_wheel(x, y, yaw, steer=0.0):
-        """绘制单个车轮"""
+    # 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],
@@ -166,24 +191,25 @@ def plot_wheel(x, y, yaw, steer=0.0):
             [-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],
@@ -192,55 +218,67 @@ def plot_wheel(x, y, yaw, steer=0.0):
         [-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)
-    # 前轮（右）
+              yaw, steer, front_color)
+    # Front right  
     plot_wheel(x + LENGTH/4 * c + WIDTH/2 * s,
               y + LENGTH/4 * s - WIDTH/2 * c,
-              yaw, steer)
-    # 后轮（左）
+              yaw, steer, front_color)
+    # Rear left
     plot_wheel(x - LENGTH/4 * c - WIDTH/2 * s,
               y - LENGTH/4 * s + WIDTH/2 * c,
-              yaw)
-    # 后轮（右）
+              yaw, color=rear_color)
+    # Rear right
     plot_wheel(x - LENGTH/4 * c + WIDTH/2 * s,
               y - LENGTH/4 * s - WIDTH/2 * c,
-              yaw)
+              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 == ' ':  # 空格键
+    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
@@ -260,31 +298,31 @@ 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', on_key)
-            plot_vehicle(state.x, state.y, state.yaw, di)  # di 是转向角
+            # 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()  # 添加这一行显示图例
+            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)  # 降低 CPU 占用
-                if not plt.get_fignums():  # 检查窗口是否被关闭
+                plt.pause(0.1)  # Reduce CPU usage
+                if not plt.get_fignums():  # Check if window is closed
                     exit(0)
 
     # Test