Running an Pure Pursuit Agent in the Racing Scenario¶

This notebook introduces how to train an agent within the developed traffic scenario envs/racing.py. This environment can generate a racing track with a centerline.

Install dependencies¶

This notebook is using pygame to render the environment. You may need to download the notebook and run it locally.

1. Download this notebook.

2. Run the commands in the next cell to install the dependencies.

In [1]:

%%capture
# %pip install tactics2d

%%capture # %pip install tactics2d

In [2]:

# Setup environment variables
import sys
sys.path.append("../..")
sys.path.append("../../tactics2d")

# Import dependencies 
import matplotlib as mpl
import matplotlib.pyplot as plt
from matplotlib.patches import Polygon
from matplotlib.lines import Line2D
from matplotlib.animation import FuncAnimation
import numpy as np
from shapely.geometry import LineString, Point
from shapely.affinity import affine_transform

from tactics2d.participant.trajectory.state import State
from tactics2d.envs import RacingEnv
from tactics2d.controller import PurePursuitController

# Setup environment variables import sys sys.path.append("../..") sys.path.append("../../tactics2d") # Import dependencies import matplotlib as mpl import matplotlib.pyplot as plt from matplotlib.patches import Polygon from matplotlib.lines import Line2D from matplotlib.animation import FuncAnimation import numpy as np from shapely.geometry import LineString, Point from shapely.affinity import affine_transform from tactics2d.participant.trajectory.state import State from tactics2d.envs import RacingEnv from tactics2d.controller import PurePursuitController

In [3]:

# Setting up parameters for matplotlib
mpl.rcParams.update(
    {
        "figure.dpi": 200, # 300 for high quality
        "figure.frameon": False,
        "font.family": "Dejavu Serif",
        "font.size": 10,
        "font.stretch": "semi-expanded",
        "animation.html": "jshtml",
        "animation.embed_limit": 5000,
    }
)

# Setting up parameters for matplotlib mpl.rcParams.update( { "figure.dpi": 200, # 300 for high quality "figure.frameon": False, "font.family": "Dejavu Serif", "font.size": 10, "font.stretch": "semi-expanded", "animation.html": "jshtml", "animation.embed_limit": 5000, } )

About RacingEnv¶

RacingEnv is an environment that randomly generates a racing track by a circle with radius around 800 m. The time needed for a vehicle to make a full turn in this circle is around 3 minutes if it drives at a speed of 100 km/h. The width of the track is varying around 5 meters.

In [4]:

# Setting up environment
render_mode = ["rgb_array", "human"][0]
env = RacingEnv(
    render_mode=render_mode,
    render_fps=60,
    max_step=int(1e5),
    continuous=True
)
_ = env.reset()

# Setting up environment render_mode = ["rgb_array", "human"][0] env = RacingEnv( render_mode=render_mode, render_fps=60, max_step=int(1e5), continuous=True ) _ = env.reset()

In [5]:

%matplotlib inline
def visualize_map(map_):
    fig, ax = plt.subplots()
    for lane in map_.lanes.values():
        # print(lane)
        ax.add_patch(
            Polygon(
                xy=lane.shape,
                closed=True,
                facecolor="#2f3542",
                edgecolor=None,
            )
        )
    
    ax.set_xlim(-800, 800)
    ax.set_ylim(-800, 800)
    ax.set_aspect("equal")
    plt.show()

%matplotlib inline def visualize_map(map_): fig, ax = plt.subplots() for lane in map_.lanes.values(): # print(lane) ax.add_patch( Polygon( xy=lane.shape, closed=True, facecolor="#2f3542", edgecolor=None, ) ) ax.set_xlim(-800, 800) ax.set_ylim(-800, 800) ax.set_aspect("equal") plt.show()

A randomly generated racing track looks like the following:

In [6]:

map_ = env.unwrapped.scenario_manager.map_
visualize_map(map_)

map_ = env.unwrapped.scenario_manager.map_ visualize_map(map_)

In [7]:

center_line = map_.roadlines["center_line"]
ego_state = env.get_ego_state()
print(ego_state)

center_line = map_.roadlines["center_line"] ego_state = env.get_ego_state() print(ego_state)

State(frame=0, x=-319.53987060033836, y=127.8266919740173, heading=-0.09142824304308907, vx=0.0, vy=0.0, speed=0.0, ax=None, ay=None, accel=0.0)

About the Pure Pursuit Control¶

The Pure Pursuit method is a geometric control algorithm commonly used in autonomous vehicle path tracking. It steers the vehicle by looking ahead at a target point along the path—referred to as the "pre-aiming point"—and adjusting the steering angle to guide the vehicle toward that point. By continuously updating the pre-aiming point and steering accordingly, the vehicle follows the intended trajectory.

How Pure Pursuit Works¶

1. Identify the Pre-aiming Point: The controller selects a target point ahead of the vehicle. This point is typically located at a fixed distance, known as the "pre-aiming distance," along the path.
2. Calculate the Steering Angle: Using the vehicle’s current position relative to the pre-aiming point, the controller computes the steering angle needed to guide the vehicle toward the target point.
3. Adjust the Vehicle’s Heading: The vehicle continuously updates its heading, making real-time adjustments to maintain the desired trajectory.

Key Parameters¶

• Pre-aiming Distance: This distance defines how far ahead along the path the target point is selected.

  • Short Pre-aiming Distance: The vehicle closely follows the path and quickly reacts to changes, but this can reduce overall stability, leading to potentially more erratic movement.
  • Long Pre-aiming Distance: The vehicle’s response becomes smoother and more stable, but it may react more slowly to sharp turns or sudden changes in the path.

• Target Speed: The desired speed at which the vehicle follows the path.

  • High Target Speed: Encourages faster driving, often with more aggressive steering as the controller tries to keep up with the target point.
  • Low Target Speed: Results in a slower, more deliberate motion, allowing the vehicle to handle curves more gracefully and maintain smoother control.

In [8]:

def truncate_waypoint(ego_state: State, waypoints: LineString):
    ego_location = Point(ego_state.location)
    waypoint_coords = list(waypoints.coords)
    min_distance = np.inf
    closest_idx = None
    for i, waypoint in enumerate(waypoint_coords):
        distance = Point(waypoint).distance(ego_location)
        if distance < min_distance:
            min_distance = distance
            closest_idx = i
    
    return LineString(waypoint_coords[closest_idx:])

def truncate_waypoint(ego_state: State, waypoints: LineString): ego_location = Point(ego_state.location) waypoint_coords = list(waypoints.coords) min_distance = np.inf closest_idx = None for i, waypoint in enumerate(waypoint_coords): distance = Point(waypoint).distance(ego_location) if distance < min_distance: min_distance = distance closest_idx = i return LineString(waypoint_coords[closest_idx:])

In [9]:

pure_pursuit_controller = PurePursuitController(
    min_pre_aiming_distance=10.0, target_speed=5.0
)
# pure_pursuit_controller.update_driving_style(0)

pure_pursuit_controller = PurePursuitController( min_pre_aiming_distance=10.0, target_speed=5.0 ) # pure_pursuit_controller.update_driving_style(0)

In [10]:

waypoints = map_.roadlines["center_line"].geometry
smooth_waypoint = []
for i in np.arange(0, waypoints.length, 1.0):
    smooth_waypoint.append(waypoints.interpolate(i))
waypoints = LineString(smooth_waypoint)

history_waypoints = dict()
history_command = dict()

for i in range(500):
    ego_state = env.get_ego_state()
    waypoints = truncate_waypoint(ego_state, waypoints)
    steering, accel = pure_pursuit_controller.step(ego_state, waypoints)
    history_waypoints[ego_state.frame] = waypoints
    history_command[ego_state.frame] = (steering, accel)
    env.step([steering, accel])

waypoints = map_.roadlines["center_line"].geometry smooth_waypoint = [] for i in np.arange(0, waypoints.length, 1.0): smooth_waypoint.append(waypoints.interpolate(i)) waypoints = LineString(smooth_waypoint) history_waypoints = dict() history_command = dict() for i in range(500): ego_state = env.get_ego_state() waypoints = truncate_waypoint(ego_state, waypoints) steering, accel = pure_pursuit_controller.step(ego_state, waypoints) history_waypoints[ego_state.frame] = waypoints history_command[ego_state.frame] = (steering, accel) env.step([steering, accel])

/home/rowena/miniconda3/envs/tactics2d/lib/python3.9/site-packages/gymnasium/spaces/box.py:240: UserWarning: WARN: Casting input x to numpy array.
  gym.logger.warn("Casting input x to numpy array.")

In [11]:

%matplotlib notebook

fig, ax = plt.subplots()
fig.set_size_inches((4,3))
fig.set_layout_engine("none")

def visualize_process(frame, ego_vehicle, map_, history_waypoints, history_command, ax):
    ax.clear()
    ax.set(aspect="equal", xlim=[-40, 40], ylim=[-30, 30])
    ax.set_axis_off()

    ego_state = ego_vehicle.trajectory.get_state(frame)
    ego_heading = ego_state.heading
    transform_matrix = np.array([
        np.cos(ego_heading), 
        np.sin(ego_heading), 
        -np.sin(ego_heading), 
        np.cos(ego_heading), 
        -ego_state.x * np.cos(ego_heading) - ego_state.y * np.sin(ego_heading),
        ego_state.x * np.sin(ego_heading) - ego_state.y * np.cos(ego_heading)
    ])

    ax.add_patch(
        Polygon(
            xy=ego_vehicle.geometry.coords,
            closed=True,
            facecolor="#2bcbba",
            edgecolor=None,
            zorder=5
        )
    )

    for lane in map_.lanes.values():
        transformed_lane_shape = affine_transform(lane.geometry, transform_matrix)
        if transformed_lane_shape.distance(ego_vehicle.geometry) > 50:
            continue
        ax.add_patch(
            Polygon(
                xy=transformed_lane_shape.coords,
                closed=True,
                facecolor="#2f3542",
                edgecolor=None,
                zorder=3
            )
        )
    
    waypoints = history_waypoints[frame]
    pre_aiming_distance = np.max([
        ego_state.speed * pure_pursuit_controller.interval, 
        pure_pursuit_controller.min_pre_aiming_distance
    ])
    splitted_waypoints = []
    for i in np.arange(0, pre_aiming_distance, 1.0):
        point = waypoints.interpolate(i)
        splitted_waypoints.append(point)

    splitted_waypoints = np.array(
        list(affine_transform(LineString(splitted_waypoints), transform_matrix).coords)
    )
    ax.add_line(
        Line2D(
            splitted_waypoints[:, 0],
            splitted_waypoints[:, 1],
            linewidth=1,
            color="#fa8231",
            zorder=7
        )
    )

    steering, accel = history_command[frame]
    ax.text(
        10, 
        20, 
        f" speed: {round(ego_state.speed, 3)}\n steering: {round(steering, 3)}\n acceleration: {round(accel, 3)}"
    )

%matplotlib notebook fig, ax = plt.subplots() fig.set_size_inches((4,3)) fig.set_layout_engine("none") def visualize_process(frame, ego_vehicle, map_, history_waypoints, history_command, ax): ax.clear() ax.set(aspect="equal", xlim=[-40, 40], ylim=[-30, 30]) ax.set_axis_off() ego_state = ego_vehicle.trajectory.get_state(frame) ego_heading = ego_state.heading transform_matrix = np.array([ np.cos(ego_heading), np.sin(ego_heading), -np.sin(ego_heading), np.cos(ego_heading), -ego_state.x * np.cos(ego_heading) - ego_state.y * np.sin(ego_heading), ego_state.x * np.sin(ego_heading) - ego_state.y * np.cos(ego_heading) ]) ax.add_patch( Polygon( xy=ego_vehicle.geometry.coords, closed=True, facecolor="#2bcbba", edgecolor=None, zorder=5 ) ) for lane in map_.lanes.values(): transformed_lane_shape = affine_transform(lane.geometry, transform_matrix) if transformed_lane_shape.distance(ego_vehicle.geometry) > 50: continue ax.add_patch( Polygon( xy=transformed_lane_shape.coords, closed=True, facecolor="#2f3542", edgecolor=None, zorder=3 ) ) waypoints = history_waypoints[frame] pre_aiming_distance = np.max([ ego_state.speed * pure_pursuit_controller.interval, pure_pursuit_controller.min_pre_aiming_distance ]) splitted_waypoints = [] for i in np.arange(0, pre_aiming_distance, 1.0): point = waypoints.interpolate(i) splitted_waypoints.append(point) splitted_waypoints = np.array( list(affine_transform(LineString(splitted_waypoints), transform_matrix).coords) ) ax.add_line( Line2D( splitted_waypoints[:, 0], splitted_waypoints[:, 1], linewidth=1, color="#fa8231", zorder=7 ) ) steering, accel = history_command[frame] ax.text( 10, 20, f" speed: {round(ego_state.speed, 3)}\n steering: {round(steering, 3)}\n acceleration: {round(accel, 3)}" )

In [12]:

# Visualize the running process
ego_vehicle = env.unwrapped.scenario_manager.agent
animation = FuncAnimation(
    fig,
    visualize_process,
    frames=ego_vehicle.trajectory.frames[:-1],
    fargs=(ego_vehicle, map_, history_waypoints, history_command, ax),
    interval=100
)
animation

# Visualize the running process ego_vehicle = env.unwrapped.scenario_manager.agent animation = FuncAnimation( fig, visualize_process, frames=ego_vehicle.trajectory.frames[:-1], fargs=(ego_vehicle, map_, history_waypoints, history_command, ax), interval=100 ) animation

Out[12]:

Once Loop Reflect

In [13]:

# animation.save("demo_pure_pursuit.mp4", writer="ffmpeg", fps=60, dpi=200)
animation.save("demo_pure_pursuit.gif", writer="pillow", fps=60, dpi=200)

# animation.save("demo_pure_pursuit.mp4", writer="ffmpeg", fps=60, dpi=200) animation.save("demo_pure_pursuit.gif", writer="pillow", fps=60, dpi=200)

In [ ]: