From 57d0ae1ca686315ef7f8b6f2c144b28256622f47 Mon Sep 17 00:00:00 2001 From: yangtzech Date: Sat, 5 Apr 2025 17:33:25 +0800 Subject: [PATCH 1/4] feat: Add vehicle model rendering function and pause simulation function --- PathTracking/pure_pursuit/pure_pursuit.py | 110 ++++++++++++++++++++-- 1 file changed, 104 insertions(+), 6 deletions(-) diff --git a/PathTracking/pure_pursuit/pure_pursuit.py b/PathTracking/pure_pursuit/pure_pursuit.py index ff995033a9..ec57350c53 100644 --- a/PathTracking/pure_pursuit/pure_pursuit.py +++ b/PathTracking/pure_pursuit/pure_pursuit.py @@ -18,7 +18,7 @@ WB = 2.9 # [m] wheel base of vehicle show_animation = True - +pause_simulation = False # 新增暂停标志 class State: @@ -141,7 +141,94 @@ 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'): + """ + 绘制带四个车轮的车辆模型 + Args: + x, y: 车辆中心位置 + yaw: 车辆航向角 + steer: 转向角 + color: 车辆颜色 + """ + # 车辆参数 + LENGTH = WB + 1.0 # 车长 + WIDTH = 2.0 # 车宽 + WHEEL_LEN = 0.6 # 车轮长度 + WHEEL_WIDTH = 0.2 # 车轮宽度 + + def plot_wheel(x, y, yaw, steer=0.0): + """绘制单个车轮""" + 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] + ]) + + # 如果有转向角，先旋转车轮 + if steer != 0: + c, s = np.cos(steer), np.sin(steer) + rot_steer = np.array([[c, -s], [s, c]]) + wheel = wheel @ rot_steer.T + + # 考虑车辆朝向 + c, s = np.cos(yaw), np.sin(yaw) + rot_yaw = np.array([[c, -s], [s, c]]) + wheel = wheel @ rot_yaw.T + + # 平移到指定位置 + wheel[:, 0] += x + wheel[:, 1] += y + + plt.plot(wheel[:, 0], wheel[:, 1], color=color) + + # 计算车身四个角点 + corners = np.array([ + [-LENGTH/2, WIDTH/2], + [LENGTH/2, WIDTH/2], + [LENGTH/2, -WIDTH/2], + [-LENGTH/2, -WIDTH/2], + [-LENGTH/2, WIDTH/2] + ]) + + # 旋转矩阵 + c, s = np.cos(yaw), np.sin(yaw) + Rot = np.array([[c, -s], [s, c]]) + + # 旋转并平移车身 + rotated = corners @ Rot.T + rotated[:, 0] += x + rotated[:, 1] += y + + # 绘制车身 + plt.plot(rotated[:, 0], rotated[:, 1], color=color) + + # 绘制四个车轮 + # 前轮（左） + plot_wheel(x + LENGTH/4 * c - WIDTH/2 * s, + y + LENGTH/4 * s + WIDTH/2 * c, + yaw, steer) + # 前轮（右） + plot_wheel(x + LENGTH/4 * c + WIDTH/2 * s, + y + LENGTH/4 * s - WIDTH/2 * c, + yaw, steer) + # 后轮（左） + plot_wheel(x - LENGTH/4 * c - WIDTH/2 * s, + y - LENGTH/4 * s + WIDTH/2 * c, + yaw) + # 后轮（右） + plot_wheel(x - LENGTH/4 * c + WIDTH/2 * s, + y - LENGTH/4 * s - WIDTH/2 * c, + yaw) + +# 添加键盘事件处理函数 +def on_key(event): + global pause_simulation + if event.key == ' ': # 空格键 + pause_simulation = not pause_simulation + elif event.key == 'escape': + exit(0) def main(): # target course @@ -177,18 +264,29 @@ def main(): 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) + plot_vehicle(state.x, state.y, state.yaw, di) # di 是转向角 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() # 添加这一行显示图例 + + # 添加暂停状态显示 + 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) + # 暂停处理 + while pause_simulation: + plt.pause(0.1) # 降低 CPU 占用 + if not plt.get_fignums(): # 检查窗口是否被关闭 + exit(0) + # Test assert lastIndex >= target_ind, "Cannot goal" From 69d7712085996aa963866ed1ba54e45fe12fba61 Mon Sep 17 00:00:00 2001 From: yangtzech Date: Sat, 5 Apr 2025 22:36:01 +0800 Subject: [PATCH 2/4] feat: Add support for reverse mode and related function adjustments --- PathTracking/pure_pursuit/pure_pursuit.py | 140 ++++++++++++++-------- 1 file changed, 89 insertions(+), 51 deletions(-) diff --git a/PathTracking/pure_pursuit/pure_pursuit.py b/PathTracking/pure_pursuit/pure_pursuit.py index ec57350c53..5eacfc8790 100644 --- a/PathTracking/pure_pursuit/pure_pursuit.py +++ b/PathTracking/pure_pursuit/pure_pursuit.py @@ -10,6 +10,11 @@ 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 @@ -17,26 +22,36 @@ 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,6 +66,7 @@ def __init__(self): self.y = [] self.yaw = [] self.v = [] + self.direction = [] self.t = [] def append(self, t, state): @@ -58,6 +74,7 @@ def append(self, t, state): 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,47 +218,59 @@ 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] @@ -240,7 +278,7 @@ def main(): 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 From f03f1384e41406c167a8eef8b363802d98b77043 Mon Sep 17 00:00:00 2001 From: yangtzech Date: Sat, 5 Apr 2025 22:46:42 +0800 Subject: [PATCH 3/4] feat: Add reverse mode test case --- tests/test_pure_pursuit.py | 4 ++++ 1 file changed, 4 insertions(+) diff --git a/tests/test_pure_pursuit.py b/tests/test_pure_pursuit.py index 0e0b83bf6c..89c1829514 100644 --- a/tests/test_pure_pursuit.py +++ b/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__) From f6be29f56c9fd69584935a52933b7cd03fca0533 Mon Sep 17 00:00:00 2001 From: yangtzech Date: Sat, 5 Apr 2025 22:55:53 +0800 Subject: [PATCH 4/4] feat: Limit the maximum steering angle to improve path tracking control --- PathTracking/pure_pursuit/pure_pursuit.py | 9 +++------ 1 file changed, 3 insertions(+), 6 deletions(-) diff --git a/PathTracking/pure_pursuit/pure_pursuit.py b/PathTracking/pure_pursuit/pure_pursuit.py index 5eacfc8790..de1c7d5ddb 100644 --- a/PathTracking/pure_pursuit/pure_pursuit.py +++ b/PathTracking/pure_pursuit/pure_pursuit.py @@ -28,6 +28,7 @@ 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] show_animation = True @@ -144,12 +145,8 @@ def pure_pursuit_steer_control(state, trajectory, pind): # 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 + # Limit steering angle to max value + delta = max(min(delta, MAX_STEER), -MAX_STEER) return delta, ind