# FastRacing

**Repository Path**: npuerchen/fast-racing

## Basic Information

- **Project Name**: FastRacing
- **Description**: No description available
- **Primary Language**: Unknown
- **License**: Not specified
- **Default Branch**: master
- **Homepage**: None
- **GVP Project**: No

## Statistics

- **Stars**: 0
- **Forks**: 0
- **Created**: 2026-10-13
- **Last Updated**: 2026-10-25

## Categories & Tags

**Categories**: Uncategorized

**Tags**: None

## README

# FastRacing

A **ROS Noetic + Gazebo + XTDrone** workspace for PX4 software-in-the-loop (SITL)
drone racing research. Current task: **square-gate traversal** — given the gate center
and traversal direction, the drone plans a minimum-snap corridor-constrained
trajectory online and tracks it in a differentially-flat fashion (position + yaw
setpoints handled by the PX4 position loop).

![gate traversal demo](img/10月13日.gif)

The planning algorithm is a reimplementation of:

> D. Mellinger and V. Kumar, "Minimum snap trajectory generation and control for
> quadrotors," ICRA 2011.

## Features

- **Minimum-snap gate-traversal planner** (C++, `gate_traverse` package)
  - Handles gates at arbitrary poses: input = gate center + traversal direction
  - 4 decoupled QPs in the corridor frame: x'/ψ channels solved analytically (KKT),
    y'/z' channels with corridor inequality constraints
  - QP solver: NLopt (LD_SLSQP) with variable scaling, objective normalization,
    row-normalized constraints, and **warm-starting** across time-allocation iterations
  - Time allocation per Eq. (14) of the paper: numerical directional derivatives
    + backtracking line search
  - Built-in sanity report after planning (interpolation error / continuity /
    hover endpoints / corridor margin)
- **Trajectory tracking**: differential flatness — desired x/y/z + yaw sent to the
  `px4ctrl` relay, which forwards position setpoints to PX4 (OFFBOARD)
- **Dynamic gate spawning**: place the gate model in Gazebo from the command line by
  specifying the opening center (x, y) and model yaw; ground height and traversal
  direction are computed automatically from the mesh
- **RViz visualization**: planned trajectory (`/gate_traverse/planned_path`) and the
  gate model itself (`/gate_traverse/gate_markers`, same DAE meshes as Gazebo)
- **AprilTag pose-estimation experiment** (`apriltag_ros`, from XTDrone): a Gazebo
  monocular camera detects an AprilTag board and solves its 6-DoF pose; keyboard
  teleop to move the camera / tag board; RViz overlay of detected vs. ground-truth
  pose for accuracy evaluation

## Repository Layout

```
FastRacing/
├── img/                     # Demo GIFs / screenshots
├── models/
│   ├── drone_gate/          # Racing gate Gazebo model (1.4 m outer / 1.0 m opening)
│   ├── monocular_camera/    # Monocular camera model (from PX4 sitl_gazebo)
│   └── apriltag_board/      # Board with an AprilTag (tag36h11, black block 0.4 m)
├── scripts/                 # Python reference implementations & px4ctrl relay
│   ├── min_snap_corridor_3d.py   # Python reference planner (blueprint of the C++ port)
│   ├── px4ctrl.py                # Command relay layer (XTDrone)
│   ├── se3_tracker.py            # Half-SE3 geometric controller reference (currently unused)
│   └── keyboard_teleop_gazebo.py # Keyboard teleop for Gazebo models (camera / board)
└── src/
    ├── gate_traverse/       # Gate traversal package (C++)
    │   ├── msg/Gate.msg              # Gate pose message (center + normal)
    │   ├── src/min_snap_planner.cpp  # Planner
    │   ├── src/gate_traverse_node.cpp# Mission node (state machine: IDLE -> PLAN -> TRACK)
    │   ├── src/planner_selftest.cpp  # Offline planner self-test
    │   ├── scripts/spawn_gate.py     # Dynamic gate spawner for Gazebo
    │   └── launch/gate_traverse.launch
    └── apriltag_ros/        # AprilTag detection package (from XTDrone)
        ├── cfg/tags.yaml           # Tag description (id / size / frame)
        ├── launch/apriltag_cam_test.launch  # Camera + board + detector + viz
        └── scripts/apriltag_viz.py # Detected-vs-ground-truth RViz visualization
```

## Dependencies

- ROS Noetic (desktop-full, includes Gazebo)
- [XTDrone](https://github.com/robin-shaun/XTDrone) (PX4 SITL environment)
- PX4 Firmware (SITL)
- NLopt (`libnlopt`, in `/usr/local` or system packages)
- Eigen3
- `ros-noetic-apriltag` (for the AprilTag experiment)

## Build

```bash
cd ~/FastRacing
catkin_make
source devel/setup.bash
```

## Usage

### 1. Launch XTDrone / PX4 SITL + Gazebo

```bash
# Required so that Gazebo can resolve the drone_gate meshes/textures
export GAZEBO_MODEL_PATH=$HOME/FastRacing/models:$GAZEBO_MODEL_PATH
# Then start PX4 SITL via your usual XTDrone flow (it includes gazebo_ros plugins)
```

### 2. Start the px4ctrl relay

```bash
source ~/FastRacing/devel/setup.bash
python3 ~/FastRacing/scripts/px4ctrl.py iris 0
```

### 3. Spawn the gate in Gazebo

```bash
# Args: opening-center x y and model yaw (deg); z is computed from the mesh
# (opening center is 1.15 m above ground when the model stands on the ground)
rosrun gate_traverse spawn_gate.py 1.8 1.2 30
# The script prints the opening center and the gate_center / gate_yaw values
# for the launch file (traversal direction = model yaw + 90 deg)
```

### 4. Start the gate traversal node (with RViz)

```bash
roslaunch gate_traverse gate_traverse.launch \
    gate_center:="[1.8, 1.2, 1.15]" gate_yaw:=2.0944
```

- Copy `gate_center` / `gate_yaw` from the printout of step 3
- The launch file also starts RViz (config: `src/gate_traverse/rviz/vis.rviz`)

### 5. Takeoff and execution

Takeoff / arming / OFFBOARD switch follow the standard XTDrone workflow. Once the
drone is hovering, the node plans from the current position, executes the
trajectory, and returns to hover at the start point.

## Node Interface

| Name | Type | Description |
|---|---|---|
| `/iris_0/mavros/local_position/pose` (sub) | `geometry_msgs/PoseStamped` | Current drone pose (planning start point) |
| `/iris_0/gate` (sub, optional) | `gate_traverse/Gate` | Gate pose input (enabled with `use_gate_topic:=true`) |
| `/iris_0/px4ctrl/cmd_pose_enu` (pub) | `geometry_msgs/Pose` | Desired position + yaw (100 Hz) |
| `/gate_traverse/planned_path` (pub) | `nav_msgs/Path` | Planned trajectory (frame: map) |
| `/gate_traverse/gate_markers` (pub) | `visualization_msgs/MarkerArray` | Gate model markers (latched) |

### Main Parameters

| Parameter | Default | Description |
|---|---|---|
| `use_gate_topic` | `false` | `false`: use the launch-parameter gate; `true`: wait for `/iris_0/gate` |
| `gate_center` | `[1.8, 1.2, 1.15]` | Gate opening center (world frame) |
| `gate_yaw` | `0.0` | Traversal direction (rad); normal n = (cos, sin, 0) |
| `d_drone` | `0.25` | Drone size (sets the corridor width) |
| `v_cruise` | `1.5` | Cruise speed for the initial time allocation (m/s) |

## Offline Planner Self-Test

No roscore / Gazebo needed:

```bash
./devel/lib/gate_traverse/planner_selftest
```

Expected: interpolation / continuity / endpoint errors at the 1e-12 level,
corridor lateral deviation <= delta = 0.2 m, final verdict PASS.

## AprilTag Pose-Estimation Experiment

A standalone test of AprilTag detection and PnP pose solving, without any drone.
A monocular camera model looks at a board carrying a `tag36h11` tag; the
`apriltag_ros` detector estimates the tag pose, and RViz compares it against the
Gazebo ground truth.

![apriltag test](img/apriltag_test.png)

### 1. Launch Gazebo + camera + board + detector + visualization

```bash
export GAZEBO_MODEL_PATH=$HOME/FastRacing/models:$GAZEBO_MODEL_PATH
roslaunch apriltag_ros apriltag_cam_test.launch
```

The launch file starts an empty world, spawns the monocular camera
(default: `(0, -2, 1)`, looking at the board), spawns the tag board, then starts
the `apriltag_ros` continuous detector (image topic
`/monocular_camera/camera/image_raw`) and the `apriltag_viz.py` node.

### 2. Keyboard teleop for the camera / board

```bash
python3 ~/FastRacing/scripts/keyboard_teleop_gazebo.py
```

- `0` / `1`: select the monocular camera / the tag board
- `w a s d`: translate in the body frame (forward / left / backward / right)
- `z x`: down / up
- `q e`: yaw, `f g`: roll, `r t`: pitch
- `+ -`: adjust the step size

### 3. RViz visualization

| Marker / topic | Meaning |
|---|---|
| Green plane + `/apriltag_viz/tag_det_pose` | Detected tag pose |
| Blue plane + `/apriltag_viz/tag_gt_pose` | Ground-truth tag pose (from Gazebo) |
| White wireframe pyramid + `/apriltag_viz/camera_pose` | Camera ground truth |

The tag description lives in `src/apriltag_ros/cfg/tags.yaml`. Note: `size` is
the side length of the tag's **black block** (0.4 m on this board — the printed
patch is 0.5 m with the black block covering 80% of it); a wrong size scales the
estimated distance proportionally.

## References

- D. Mellinger, V. Kumar, "Minimum snap trajectory generation and control for
  quadrotors," ICRA 2011.