diff --git a/PathTracking/pure_pursuit/pure_pursuit.py b/PathTracking/pure_pursuit/pure_pursuit.py index ff995033a9..de1c7d5ddb 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,37 @@ 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,6 +67,7 @@ def __init__(self): self.y = [] self.yaw = [] self.v = [] + self.direction = [] self.t = [] def append(self, t, state): @@ -58,6 +75,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 +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 = max(min(delta, MAX_STEER), -MAX_STEER) return delta, ind @@ -141,11 +163,111 @@ 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', 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] @@ -153,7 +275,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 @@ -173,22 +295,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" 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__)