AMZ FSAE Driverless Learning Repo

An autonomy learning repo built around AMZ racing's public driverless rosbag (2017) using C++ and ROS2 Jazzy.

The repo does the following:

• LiDAR preprocessing and cone detection
• Vehicle state estimation
• Coordinate frames and timestamped transforms
• Persistent landmark tracking and data association
• Batch and incremental pose-landmark GraphSLAM
• Local centerline generation

Design Principles

• C++ for live and replayed nodes
• Python is used for initial batch GraphSLAM prototype
• ROS-independent core algorithms with light ROS adapters for the nodes (core is isolated from ROS)
• Explicit frames, units, timestamps, and failure handling
• Unit tests for core behavior and a couple more involved ROS conversions
• Foxglove for visualization and debugging
• use_sim_time:=true during rosbag/mcap replay

Proposed Driverless Architecture

flowchart TD
    OSS[Optical speed sensor] --> SE[State estimator]
    IMU[IMU] --> SE
    GNSS[GNSS] --> SE
    VIO[VIO] --> SE

    SE -->|/state/odometry| FOX[Foxglove]
    SE -->|/state/odometry_path| FOX
    SE -->|T_odom_base| TF[TF tree]

    LIDAR[LiDAR] --> LP[LiDAR processor]
    TF -->|T_odom_base at packet time| LP

    LP -->|Ground and non-ground clouds| FOX
    LP -->|Candidate and rejection markers| FOX
    LP -->|/perception/cones| FE[SLAM frontend]

    TF -->|T_odom_base at observation time| FE

    FE -->|/frontend/associated_graph_frame| SLAM[Incremental GraphSLAM]
    SLAM -->|/slam/incremental/map_state| FE
    SLAM -->|T_map_odom| TF
    SLAM -->|Optimized path and landmark markers| FOX

    FE -->|/mapping/planner_landmarks| PLANNER[Centerline planner]
    FE -->|Association and tracking markers| FOX

    TF -->|T_map_base| PLANNER

    PLANNER -->|/planning/path| FOX
    PLANNER -->|Pair and rejection markers| FOX

Loading

Mapping Ownership

In the closed-loop SLAM pipeline, fsd_slam_frontend is the owner of:

• Detection-to-landmark association
• Tentative and pending landmark tracks
• Canonical landmark IDs
• Landmark confirmation
• Observation counts and freshness metadata
• Association diagnostics

fsd_graph_slam owns:

• Incremental factor-graph updates
• Optimized vehicle poses
• Optimized landmark positions
• The map -> odom correction

The frontend combines optimized backend geometry with frontend tracking metadata and publishes a persistent planner-facing landmark map.

fsd_local_mapping was developed before any SLAM implementation, which is why it uses topics in odom. It should not run as a second authoritative mapper when closed-loop GraphSLAM is enabled.

fsd_local_mapping remains in the repository as a baseline, fallback, and comparison implementation.

Current AMZ Replay Pipeline

AMZ ROS 2 MCAP
    |
    +--> IMU + optical speed sensor
    |       |
    |       v
    |   state estimation
    |       |
    |       +--> odometry
    |       +--> odom -> base_link
    |
    +--> partial Velodyne point clouds
            |
            v
        accumulation and deskew
            |
            v
        ground removal
            |
            v
        Euclidean clustering
            |
            v
        cone filtering
            |
            v
        cone observations
            |
            v
        SLAM frontend
            |
            v
        incremental GTSAM / iSAM2

AMZ's dataset does not publish LiDAR points one full scan at a time, hence the need for accumulation. I do not expect this to be a problem with the Outser LiDAR.

Coordinate Frames in this Repo

map
  |
  +-- odom
        |
        +-- base_link
              |
              +-- velodyne

• map: globally corrected SLAM frame
• odom: locally continuous dead-reckoning frame
• base_link: vehicle body frame, with x forward, y left, and z up (FLU-axis-system)
• velodyne: LiDAR sensor frame

Transform ownership:

• State estimator publishes odom -> base_link
• GraphSLAM publishes map -> odom
• Sensor extrinsics publish base_link -> sensor
• Consumers request transforms at the measurement timestamp

Planned Planner Interface Change

The current fsd_centerline_planner was originally designed around:

/mapping/local_landmarks in odom
+
T_odom_base

The proposed SLAM-aware interface is:

/mapping/planner_landmarks in map
+
T_map_base

The planner should not consume /frontend/associated_graph_frame directly. That message represents one frame of graph measurements, not a persistent map. It is intended for the graph SLAM backend.

The frontend will publish a planner-facing landmark snapshot containing:

• Canonical landmark ID
• Best current landmark position in map
• Observation count
• Last-seen timestamp
• Confirmation or pending state
• Optional confidence and association diagnostics
• Optional cone color when available (probably from the RGB LiDAR and/or cameras)

The planner will use optimized landmark positions from the SLAM backend together with tracking metadata owned by the frontend.

The ROS wrapper will ensure that the landmark positions and vehicle pose are expressed in the same reference frame before calling the core algorithm. So, core should expect everything to be in the same frame already.

Planned Planner Data Flow

flowchart LR
    SLAM[Incremental GraphSLAM] -->|Optimized landmarks| FE[SLAM frontend]
    FE -->|Tracking metadata| MAP[/mapping/planner_landmarks/]
    MAP --> PLANNER[Centerline planner]
    TF[TF tree] -->|T_map_base| PLANNER
    PLANNER -->|/planning/path| FOX[Foxglove]

Loading

Current Packages

Package	Responsibility
fsd_msgs	Custom messages shared across the pipeline
fsd_bringup	Rosbag replay, node launch, parameters, static transforms, and Foxglove bridge
fsd_lidar_cone_detection	Point-cloud accumulation, deskewing, filtering, ground removal, clustering, and cone candidate extraction
fsd_state_estimation	Dead reckoning and EKF-based state estimation
fsd_local_mapping	Baseline local landmark tracker and mapper
fsd_slam_frontend	Closed-loop association, local tracking, confirmation, and canonical landmark IDs
fsd_graph_slam	Batch and incremental GTSAM pose-landmark GraphSLAM
fsd_centerline_planner	Local cone pairing and centerline generation
offline/graph_slam	Python dataset extraction, diagnostics, plotting, and offline GraphSLAM experiments

Building

I built this project using Ubuntu, ROS 2 Jazzy, and a zsh shell (bash should work mostly the same, idk about fish)

source /opt/ros/jazzy/setup.zsh
cd ros2_ws

rosdep install   --from-paths src   --ignore-src   --rosdistro jazzy   -y

colcon build --symlink-install
source install/setup.zsh

To generate compile commands for C++ tooling:

colcon build   --symlink-install   --cmake-args -DCMAKE_EXPORT_COMPILE_COMMANDS=ON

Testing

Run all ROS 2 tests:

cd ros2_ws

source /opt/ros/jazzy/setup.zsh
source install/setup.zsh

colcon test
colcon test-result --verbose

Though there might be failed tests because of the ROS formatting checker for the bringup package.

Run a specific package:

colcon test --packages-select fsd_slam_frontend
colcon test-result --verbose

These should all pass.

Run the offline Python tests:

cd offline/graph_slam
python -m pytest

Replay

cd ros2_ws

source /opt/ros/jazzy/setup.zsh
source install/setup.zsh

ros2 launch fsd_bringup amz_replay_pipeline.launch.py   bag_path:=/absolute/path/to/recording.mcap   storage_id:=mcap   rate:=1.0   use_sim_time:=true

A convenience script is also available:

./scripts/launch_replay_pipeline.sh

The bag files themselves are not committed to the repository. To run this repo, you will need to download AMZ's rosbag from their repo

There is a conversion script available (you will need to modify the paths yourself):

./scripts/convert_ros1_to_ros2.sh

I recommend you also trim the bag to remove all the down time.

Check the log metadata first:

Assuming you have a converted and recovered mcap file called: amz_driverless_2017_rec.mcap

mcap info amz_driverless_2017_rec.mcap

Should produce something like:

library:     mcap-cli/0.2.0 mcap-rust/0.25.0
profile:     ros2
messages:    204302
duration:    2m35.541897649s
start:       2017-05-10T11:14:17.109509971Z (1494414857.109509971)
end:         2017-05-10T11:16:52.65140762Z (1494415012.651407620)
compression:
        zstd: [742/742 chunks] [780.55 MB/251.69 MB (67.75%)] [1.62 MB/s]
chunks:
        max uncompressed size: 1.06 MB
        max compressed size: 348.31 kB
        overlaps: no
channels:
        (1) /imu                         31110 msgs (200.01Hz)   : sensor_msgs/msg/Imu [ros2msg]
        (2) /wheel_rpm                   15555 msgs (100.01Hz)   : geometry_msgs/msg/QuaternionStamped [ros2msg]
        (3) /velodyne_points            117197 msgs (753.48Hz)   : sensor_msgs/msg/PointCloud2 [ros2msg]
        (4) /optical_speed_sensor        38885 msgs (250.00Hz)   : geometry_msgs/msg/TwistStamped [ros2msg]
        (5) /gps                          1555 msgs (10.00Hz)    : sensor_msgs/msg/NavSatFix [ros2msg]
channels:    5
attachments: 0
metadata:    0

You can then trim the mcap with the following (check your start and end times):

mcap filter \
  --start 1494414877109509971 \
  --end 1494414957109509971 \
  amz_driverless_2017_rec.mcap > amz_driverless_2017_rec_trimmed.mcap

Results

Dead-Reckoning Odometry

Dead-reckoning odometry produced by fusing IMU yaw rate with the optical speed sensor through a constant-velocity EKF motion model.

Incremental GraphSLAM

Optimized trajectory and cone positions produced by incremental pose-landmark GraphSLAM.

Current Capabilities

• Replay of converted AMZ data with simulated time
• LiDAR packet accumulation
• Motion deskew using timestamped vehicle poses
• ROI and angular filtering
• Adaptive ground removal
• XY Euclidean clustering
• Range-dependent cone filtering
• EKF dead reckoning
• odom -> base_link TF publication
• Baseline persistent local cone mapping
• Geometry-based centerline generation
• Offline Python pose-landmark GraphSLAM
• C++ GTSAM batch GraphSLAM
• C++ GTSAM incremental GraphSLAM
• Closed-loop SLAM frontend architecture
• Unit tests for core algorithms and ROS conversions
• Foxglove debug output and markers

Known Limitations

• The AMZ LiDAR data arrives as high-rate partial clouds and requires dataset-specific accumulation.
• Cone classification is currently geometry-based and does not infer cone color.
• Perception and association thresholds are tuned for the AMZ recording.
• New physical sensors will require new calibration, timing validation, and parameter tuning.
• The planner-facing SLAM map interface is still a planned change.
• Closed-loop vehicle control cannot be validated through rosbag replay.
• Robust losses, landmark covariance, and more conservative loop-closure validation remain future work.

References

• AMZ Racing FSD Resources
• AMZ Driverless: The Full Autonomous Racing System
• Accurate Mapping and Planning for Autonomous Racing
• ROS 2 Jazzy Documentation
• GTSAM
• Graph-Based SLAM Tutorial — Grisetti et al.

Project Status

The AMZ replay milestone has largely achieved its learning objective for me. The repository now serves as implementation reference. Next phase will be a migration to HyTech's software infrastructure.