Planning

Cubs2 uses Dubins path planning for autonomous navigation of fixed-wing aircraft.

Dubins Paths

What are Dubins Paths?

Dubins paths are the shortest curves connecting two poses (position + heading) with bounded curvature. They consist of three segments:

C - Circular arc (left or right turn)
S - Straight line

Path Types

Six canonical path types exist:

CSC Paths (turn-straight-turn):

RSR - Right, Straight, Right
RSL - Right, Straight, Left
LSR - Left, Straight, Right
LSL - Left, Straight, Left

CCC Paths (turn-turn-turn):

RLR - Right, Left, Right
LRL - Left, Right, Left

The planner selects the shortest feasible path.

Algorithm

Based on: Shkel, A. M. and Lumelsky, V. (2001). “Classification of the Dubins set”

Implementation handles:

All 6 path types
Singularities (collinear start/goal)
Path length optimization
Parameter extraction for trajectory generation

Usage

Python API

from cubs2_planning.dubins import dubins_path
import numpy as np

# Define start and goal poses
start = (0, 0, 0)  # (x, y, heading)
goal = (100, 50, np.pi/4)
turning_radius = 50.0

# Compute path
path_length, path_type, params = dubins_path(start, goal, turning_radius)

print(f"Path type: {path_type}")
print(f"Length: {path_length:.2f} m")

ROS2 Node

# Launch planner
ros2 run cubs2_planning planner_dubins

# With custom parameters
ros2 run cubs2_planning planner_dubins \
  --ros-args -p turning_radius:=75.0 -p cruise_altitude:=20.0

Racecourse Navigation

Gate Sequencing

The planner navigates through a sequence of gates:

ros2 launch cubs2_bringup sim.xml gate_sequence:="[0,1,2,3,4,5]"

Gate Definition:

Gates are defined in racecourse_description/config/racecourse.yaml:

gates:
  - id: 0
    position: [0, 0, -15]
    orientation: [1, 0, 0, 0]
  - id: 1
    position: [100, 50, -15]
    orientation: [0.707, 0, 0, 0.707]

Visualization

In RViz, the planner shows:

Green path - Planned trajectory
Colored spheres - Gate positions
Yellow highlight - Current target gate

Parameters

Turning Radius

Minimum turn radius based on aircraft performance:

\[\begin{split}R_{min} = \\frac{V^2}{g \\tan(\\phi_{max})}\end{split}\]

Where:

\(V\) - Airspeed
\(g\) - Gravity (9.81 m/s²)
\(\\phi_{max}\) - Maximum bank angle

Example: For V=20 m/s, φ_max=45°:

\[\begin{split}R_{min} = \\frac{20^2}{9.81 \\times \\tan(45°)} \\approx 40.8 \\text{ m}\end{split}\]

Set via parameter:

ros2 param set /planner_dubins turning_radius 50.0

Cruise Altitude

Target flight altitude:

ros2 param set /planner_dubins cruise_altitude 20.0

Path Following

Published Topics

/planned_path (nav_msgs/Path) - Waypoints along trajectory
/waypoint_markers (visualization_msgs/MarkerArray) - Gate visualization

Subscribed Topics

/sportcub/pose (geometry_msgs/PoseStamped) - Aircraft position

Control Integration

The path planner publishes waypoints. A separate guidance controller (not yet implemented) would:

Track the path using cross-track error
Compute desired heading
Command roll/pitch to follow

Advanced Usage

Custom Path Generation

from cubs2_planning.dubins import DubinsPath

# Create path object
path = DubinsPath(start, goal, turning_radius)

# Sample along path
num_points = 100
points = [path.sample(i / num_points) for i in range(num_points)]

# Get tangent at point
tangent = path.tangent(0.5)  # At midpoint

Obstacle Avoidance

For obstacle avoidance, combine Dubins paths with:

RRT (Rapidly-exploring Random Trees)
A* search on Dubins graph
Potential fields