Autonomous Ackermann Robot Car: ROS 2 Distributed Stack, Gazebo Digital Twin and STM32 Velocity Control

Part of: SC2079 Multidisciplinary Project (NTU)

Overview

Ackermann-steered car (Wheeltec Mini-Ackermann, STM32F407VET6, Raspberry Pi 4B) that runs two tasks in a known arena:

• Task 1: visit up to 8 obstacles, recognise the symbol on each target face with YOLOv8, and report each ID with its photo to an Android tablet.
• Task 2: leave the carpark, read the arrow on obstacle 1 and pass on that side, read the arrow on obstacle 2, go round it and park. Obstacle 2 has an unknown length (≥ 30 cm) that the car measures during the run. Time limit 3 min. A wrong side is a DQ, and a carpark wall hit is a DNF.

My part: designed the system architecture, integrated and wired the whole stack (ROS 2 workspace, firmware, planner, simulation, Foxglove telemetry layer, rosbag tooling), and did most of the hardware debugging. ~378 of 486 commits (authors “Ray”, “Ray Shao”, “frieddeli”, “SC2079 Group 16”). Lam Nguyen did most of the Android UI and much of the Task 2 tuning.

Key Innovations

1. ROS 2 stack, distributed over Zenoh

First group to build the project on ROS. The course reference is a socket-based Pi program, which the repo keeps in raspberry-pi/ as a parallel track.

• ROS 2 Jazzy, 7 packages: mdp_bringup (mission FSMs, planner node, launch), mdp_hardware_bridge (UART, motion control), mdp_perception, mdp_camera_bringup, mdp_android_bridge, mdp_interfaces, mdp_simulation.
• Custom interfaces: ExecuteMoves.srv, FollowPath.srv, MotionProgress.msg, and the Task 1 run/observe/phase contract between the planner and perception.
• Single ownership of each device. serial_bridge_node alone owns /dev/ttyAMA0. android_bridge_node alone owns /dev/rfcomm0. motion_controller_node is the only /cmd_vel publisher, and it arbitrates teleop against service calls. A live teleop target makes a service call return BUSY_LOCAL. An active service makes it ignore teleop.
• rmw_zenoh with a router on the Pi and a client on the laptop, configured for both the LAN and a phone hotspot. YOLO and the planner can move to the laptop (task1-split + task1-compute, task2-rails + task2-rails-compute). The Pi keeps the hardware nodes. The Task 2 return planner runs on the PC, and the Pi takes over if no PC answer arrives.
• pixi with one lockfile and three environments: pi (linux-aarch64, NCNN), pc (Ultralytics, RViz, CUDA), sim (Gazebo). No system ROS install on any host.

2. Velocity-setpoint control with a Pi-side pure pursuit tracker

The course firmware runs distance primitives (FC030, FR090): the Pi sends a move, and the STM32 drives it and replies FIN. I replaced this with continuous velocity control.

• Wire format: 5-byte packets at 115200 8N1. V + int16 LE speed (mm/s) + int16 LE yaw rate (mrad/s) replaces the previous target. Q is a latched E-STOP in the ISR. R clears the latch and the old state. The STM32 streams TLM:x,y,heading,us,ir_l,ir_r,batt_mv,us_fresh at 40 Hz.
• Firmware (FreeRTOS): curvature κ = ω / v, clamped to 1 / 250 mm (measured full lock). Servo command = atan(L·κ) / atan(L / R_min), mapped onto asymmetric endpoints (101 / 143 / 206). Rear wheel targets v(1 ∓ κW/2), scaled together if either one exceeds 750 mm/s. Per-wheel PI with feedforward on the encoder speed, PWM capped at 1580 of 1600. A non-zero target older than 750 ms stops the car (STOP:WATCHDOG, no latch). ω is forced to 0 when v = 0, because an Ackermann car cannot turn in place.
• UART framer: one byte per interrupt. A partial frame older than 2 ms is dropped. An invalid frame resyncs on the next header byte. Before this, one lost byte or an STM32 reboot during a transmit shifted all later frames, and Q/R were unreadable until a power cycle.
• Tracker (algorithm/path_tracking.py, 20 Hz in motion_controller_node): regulated pure pursuit on the rear-axle pose. Lookahead goes from 0.22 m on straights to 0.18 m at full lock. Speed is regulated by curvature and slows over the last 0.15 m to a 0.08 m/s creep, with a 1 cm goal tolerance. Each gear change is a cusp with a stop. When the steering error is more than 25 % of full lock, the car slows to 0.03 m/s until the servo settles (0.4 s centre to lock). Cross-track error over 8 cm gives OFF_PATH. A curvature scale of 1.12 maps the physical 28 cm radius onto the firmware’s 25 cm command scale.
• Result: the old executor stopped for 0.64 to 0.96 s after every primitive (measured in the 2 Oct bags). The tracker drives a full Reeds-Shepp path without these stops. The firmware command ceiling went from 0.70 to 0.75 m/s on 15 Oct. The ExecuteMoves schema stayed the same, so Android, Task 1 and Task 2 callers did not change.

3. Sim-to-real: Gazebo runs the real stack

• sim_stm32_node opens a pseudo-terminal (os.openpty) and runs a Python emulator of the firmware behind it: the same V/Q/R packets, the same TLM/STOP lines, the same 25 cm clamp and watchdog. The real serial_bridge_node connects to the pty as if it were /dev/ttyAMA0. Everything above the UART runs unchanged in sim: bridge, controller, tracker, planner, perception and both FSMs.
• Gazebo Harmonic model from the Wheeltec CAD meshes, with steering joints, a Pi Camera v2.1 (640×480, 30 fps, 62.2° HFOV), an HC-SR04 15° cone (lidar proxy), 2× Sharp IR and an IMU. The camera adapter keeps Gazebo acquisition stamps, so a paused sim makes /camera/health fail and stops Task 1, as on the robot.
• Adjustable non-ideal dynamics: command lag (0.06 s Task 2 default), servo response (0.16 s), yaw gain, ultrasonic spike probability.
• Arenas: Task 1 with YOLO textures and bullseyes, Task 2 from the briefing dimensions with a seeded course generator, and an empty 2 × 2 m arena for calibration.
• Batch regression in sim (ros2_ws/scripts/sim_task2_sweep/). It runs 36 Task 2 courses headless: 3 × 3 gap pairs (60 / 105 / 150 cm) × 4 arrow pairs, with obstacle 2 widths 30 to 80 cm and side walls at the 50 cm minimum. Up to 3 Gazebo instances run in parallel, each with its own ROS_DOMAIN_ID, GZ_PARTITION and pty link (about 4 cores each). score.py reads each bag against the Gazebo ground-truth poses. It records completion, time, body contact with obstacles / walls / carpark, park check, side passed (WRONG_SIDE) and measured against true obstacle 2 width. batch.sh runs the same course set under several parameter sets and writes one summary table (clean / complete / mean time / OFF_PATH count).
• Limits: the sim uses CycloneDDS, not Zenoh. sim_robot.yaml is tuned apart from robot.yaml. The URDF wheelbase (0.143 m) differs from the firmware’s nominal 0.16 m. There is no paired sim/real measurement yet.

4. Live telemetry in Foxglove

• foxglove_bridge on :8765 in every robot and sim launch. The robot is reached over Tailscale or mDNS on the LAN, and the sim on localhost, with the same layout file (docs/foxglove/mdp_layout.json, in git).
• Run tab: arena markers with numbered vantage arrows, planned path, current leg path (replanned from the live pose) and driven path. Plots of commanded v, yaw rate and distance to checkpoint. Serial / E-STOP / Bluetooth lamps, camera, battery gauge, Task 1 state timeline, tablet line log, and START / RESET / E-STOP buttons that publish into the graph.
• Task 1 tab: /task1/observation_debug, the reason each frame did or did not vote, and the vote count per view.
• Task 2 tab: arrow camera, ultrasonic approach trace, sprint log.
• Debug tab: raw against Kalman-filtered ranges, tracker errors, parameters, battery voltage.
• The planner topics are latched, so a client that connects mid-run still gets the arena and plan.

5. Rosbag recording of every run

• mdp_run_recorder wraps ros2 bag record in every task1 / task2 / teleop launch. It writes MCAP in 5-minute chunks. On /task1/end or a Task 2 COMPLETE / ESTOP it records 3 s more, closes the bag and opens the next run directory.
• Each run directory holds run_info.yaml (mode, launch args, Pi git commit, dirty files, topic list, ended, end_reason), a copy of robot.yaml as it was at start, and bag/.
• Recorded by default: pose, ranges, /cmd_vel, E-STOP and reason, /android/*, /task1/*, /planner/{arena,path,leg_path,actual_path}, /task1/observation_debug, annotated images, IR edge events, /rosout, TF. Raw camera is opt-in (record_camera:=true, ~45 MB/min) because JPEG encoding loads the Pi. The full arena layout can be rebuilt from /android/cmd (ALG|...) and /planner/arena.
• bag pull N [--open] rsyncs the newest runs to the laptop and reindexes bags cut off by a kill. bag list / bag clean --keep N keep the newest 20 by default.
• bag-debug <run> reads the bag with rosbag2_py and the compiled mdp_interfaces types (no mcap packages). It prints a summary (duration, outcome, E-STOPs, proximity stops, confirmed/missed obstacles, link drops, battery, minimum ranges), one time-ordered timeline (tablet commands and status, E-STOP, FSM state changes, /task1/events, WARN+ logs) and the plan-against-actual drift per leg. Filters: --since, --until, --grep, --level, --json.
• ~125 runs recorded between 2 and 16 Oct 2026. Several robot.yaml values are set from specific bags: confirmation votes 3 → 2 (detections at 0.94 to 0.97 confidence, 7 Oct), view timeout 8 → 2 s (a good view confirms in about 1 s), stop tolerances widened after creep and rock after a hard stop reset the votes, and the per-move stop cost in the time model.

6. Task 2 measures the course during the run

• Obstacle 1 is placed from the median of 7 raw ultrasonic echoes. The car approaches at speed until 30 cm, then asks perception for arrow 38 or 39. It never guesses. If no read arrives, it reverses 5 cm every 3 s on an arc that turns the camera towards the arrow, up to 5 times before a fallback.
• Obstacle 2 length, first method: the camera finds the 6 cm bullseyes 2 cm in from each end. With one end visible, length = 2 × outer-edge offset + 4 cm.
• Second method: a side-IR edge sweep. The firmware samples the IR at 100 Hz and sends EDG<n>: when the range jumps by more than 15 cm from a face nearer than 60 cm (2 samples to confirm). The Pi does not wait for the 40 Hz stream and its confirm count, which saves 75 to 125 ms. A sweep with too few face readings, or with no reading past the end, is not trusted: the car backs off and scans again. The sweep ends on an ultrasonic miss only within 22° of square-on.
• The return path is planned from the measured obstacles into the 60 × 50 cm carpark, head-in. COMPLETE is sent only when inside_carpark passes on all four body corners and the heading. If it fails, the car replans up to park_retries times in three passes (full course model, measured obstacles only, straight aim at the goal). If it is still outside, it reports NOT parked and no time.
• “Rails” mode: lookup-table paths driven as map-fixed routes in the home frame, a drive-by read of arrow 2, and a cruise cap behind obstacle 2 (0.70 m/s), with turns at 0.35 m/s.

7. Development with coding agents under a written contract

• AGENTS.md (80 KB) is the system handbook for agents: architecture, wire formats, frames and kinematic conventions, code standards per language, test commands, and hard rules. For example, agents never start ROS launches, because parallel launches collide on /dev/ttyAMA0, /dev/rfcomm0 and the camera and exhaust the Pi. They hand the command to a human.
• MEMORY.md (~3,400 lines) is an engineering log of decisions, measured values and the bag that each came from. Agents read it before they change code.
• Docs follow ASD-STE100 Simplified Technical English, so humans and agents read them the same way.
• Tests: ~332 cases in 80 files across pytest (ROS packages, planner, tracker, time model, emulator, recorder), host-compiled C (velocity control, UART framer, IR edge) and Android JUnit.

Other engineering

• Planner cost is drive time. motion_time.py models the tracker: straight pieces at v_straight, full-lock arcs at v_arc, approach slow-down, cusp stops, start from rest (0.13 s, fitted to simulate_tracking) and 0.8 s per-leg overhead. An exact TSP orders the obstacles on this pairwise cost matrix. Reeds-Shepp candidates come first, then a Hybrid A* fallback. Collision checks use the car’s oriented body box against inflated obstacles (SAT), with a 12 cm look-ahead past the bumper on forward pieces and fallback margins (4 → 2 → 1 cm) when the start pose is inside the buffer. Legs plan in parallel on up to 8 cores.
• Perception: NCNN on the Pi, Ultralytics or ONNX on the PC, from one detector interface. The inference loop keeps only the newest pending frame. A detection must associate with the target obstacle’s projected face. Confirmation is 2 matching votes over distinct frames with consistent boxes (IoU). Frames can vote while moving if the car is near the view, on bearing and slow. One stationary miss can get a padded-crop retry (side = max(2.5 L, L + 70) px) at the normal 0.50 threshold.
• Evidence before result: the raw and annotated images are committed to disk before T1_RESULT is published. Photos go to the tablet in Bluetooth chunks with order, byte-count and CRC32 checks. The app sends T1_ACK only after it has drawn the ID on the map. Missing photos are requested again, and the session restores after a reconnect.
• Filtering: 1D Kalman per range sensor (q = 1e-3, r = 1e-2) with an innovation gate of 0.30 m. A new level is accepted only after 2 consecutive outliers, so single reflections do not trigger stops. A 3-DoF pose filter wraps yaw in (−π, π] and gates position jumps.
• Safety split: the firmware has no proximity logic, only the watchdog, Q latch and framer. The Pi owns the 10 cm forward-proximity inhibit and the stale-sensor stop. Both have switches, and the current robot.yaml disables both, together with Bull’s Eye orbit recovery.
• Provisioning: scripts for the Pi hotspot and the NTU network with backup/restore, a systemd RFCOMM listener, and a camera health contract with auto-restart. robot.yaml is the one config file for every node. STM32 flashing goes over UART with the board’s RTS/DTR auto-boot circuit (stm32flash, no ST-Link).

Stack

ROS 2 Jazzy, rmw_zenoh, CycloneDDS (sim), pixi, Python, C (FreeRTOS, STM32Cube HAL, PlatformIO), Ultralytics YOLOv8, NCNN, ONNX Runtime, OpenCV, Gazebo Harmonic, ros_gz_bridge, URDF/Xacro, RViz2, Foxglove, rosbag2 / MCAP, Java (Android, Bluetooth SPP), pytest, JUnit, Tailscale

Outcome

• Both tasks run end to end with no operator input after START: plan, drive, recognise, report, park.
• 0.75 m/s command ceiling for the Task 2 sprint (measure the real speed on the car).
• The sim runs the production bridge and controller, and the sweep harness scores 36 courses against ground truth.
• No competition result or physical success rate is recorded in the repo yet.

Skills Acquired

ROS 2, Robot Simulation, Controls & Estimation, Edge Inference, Python, embedded C / FreeRTOS, motion planning, path tracking, distributed ROS over Zenoh, rosbag tooling

Related

• Course: SC2079 - Multidisciplinary Project
• Degree Program: NTU Computer Engineering