Convoy-Following Robots on a Degraded Mesh

Rover R2 is a deliberate copy of R1: the same metal 4WD chassis with wheel encoders, a Pi 5 8 GB with an active cooler running Ubuntu 24.04, and two BTS7960 full-bridge modules (one per side). The drive stack is ROS 2 Jazzy with the same differential-drive bringup I already tuned on the GPS-denied rover: encoder ticks feed wheel odometry, and a BNO085 IMU feeds the heading estimate. Cloning first means any convoy weirdness I see later is a convoy problem, not a "why is this rover different" problem.

Provision R2 from the R1 workspace and verify odometry

# On the Pi 5 (Ubuntu 24.04 Server), ROS 2 Jazzy installed per the R1 notes:
sudo apt install ros-jazzy-ros-base python3-serial python3-gpiozero
mkdir -p ~/convoy_ws/src && cd ~/convoy_ws/src
git clone https://github.com/TYFSADIK/rover-drive-stack.git rover_drive
cd ~/convoy_ws && colcon build --symlink-install
source install/setup.bash
ros2 launch rover_drive bringup.launch.py rover_id:=r2
# In a second shell: encoders ticking, odometry sane
ros2 topic echo /odom --once
ros2 topic hz /odom        # expect a steady publish rate

How I verified: I drove a taped 2 m square on the floor under teleop and checked that /odom returned close to the origin. This is exactly how I caught a swapped encoder A/B pair on R2, the odometry reported a slow rotation while the rover drove straight, and swapping the pair in the config fixed it in minutes.

Each rover carries an ESP32 LoRa board (Heltec V3 / LilyGO T-Beam class, from the Phase 3 mesh stock) flashed with Arduino-IDE firmware built on the RadioLib SX1262 driver. The pin map is board-specific: the constructor in the listing is for the Heltec WiFi LoRa 32 V3, and a T-Beam needs its own NSS/DIO1/RST/BUSY values from its schematic, so one pin table does not cover both. The link deliberately runs at conservative LoRa settings, spreading factor 9, 125 kHz bandwidth, because range and robustness matter here, not throughput: a convoy update is a few dozen bytes. The same firmware image runs on both boards; a compile-time role flag decides who transmits.

firmware/lora_link.ino, radio init (915 MHz, SF9, BW 125 kHz, CR 4/5)

#include <RadioLib.h>
// Heltec WiFi LoRa 32 V3 only: NSS 8, DIO1 14, RST 12, BUSY 13.
// LilyGO T-Beam variants use different pins, check the board schematic.
SX1262 radio = new Module(8, 14, 12, 13);

void setup() {
  Serial.begin(115200);   // UART to the Pi
  int state = radio.begin(915.0, 125.0, 9, 5, 18, 10, 8, 0);
  if (state != RADIOLIB_ERR_NONE) {
    Serial.printf("radio init failed: %d\n", state);
    while (true) { delay(1000); }   // fail loud, never half-init
  }
  radio.setCurrentLimit(60.0);      // 60 mA over-current protection on the PA supply,
                                  // not a licence-exempt power limit; the TX power
                                  // figure in begin() is the regulatory-relevant knob
}

What can go wrong: radio.begin returns an error code instead of hanging silently, which is how I discovered one board in the Phase 3 stock was the 868 MHz European variant, it init'd but never linked at 915 MHz. The boards are labelled now.

Airtime budget: at SF9 / 125 kHz a 28-byte packet needs roughly 226 ms of airtime, so the LoRa path physically cannot carry the full 5 Hz stream (200 ms spacing). The 5 Hz cadence is what the Pi pushes over UART and what the Wi-Fi fallback carries; on the LoRa control path the firmware transmits at a sustainable lower rate. Bench target, measured with a timed transmit loop, not yet measured.

The packet is the contract between lead and follower, so I fixed it before writing any controller code. It is a small packed binary struct, no JSON, no protobuf, because LoRa airtime is precious and a fixed layout is trivial to decode identically on the ESP32 (C struct) and on the Pi (Python struct). The two fields that matter most for degraded-link work are the sequence number (loss shows up as gaps) and the send timestamp (staleness is measurable, not guessed).

convoy_packet.h, identical layout on ESP32 and Pi

struct __attribute__((packed)) ConvoyPacket {
  uint8_t  magic;      // 0xC0 - cheap frame sync on the wire
  uint8_t  rover_id;   // 1 = R1 lead
  uint32_t seq;        // increments every packet: loss = gaps in seq
  uint32_t t_ms;       // sender millis() at transmit time
  float    x_m, y_m;   // lead pose in its odometry frame
  float    heading;    // radians
  float    speed;      // m/s
  uint16_t crc16;      // CRC-16/CCITT over all preceding bytes
};                     // 28 bytes on the wire (26 payload + 2 CRC)

How I verified: with a clean bench link, receiver-side sequence gaps were exactly zero over long runs; when I later induced drops, the gap count matched the induced drop count one-for-one. If those numbers ever disagree, the radio path itself is dropping packets and the test data is invalid.

On R1, a small ROS 2 Python node subscribes to /odom, packs the latest pose into a ConvoyPacket, and pushes it over UART to the ESP32 at a fixed 5 Hz; the ESP32 simply transmits whatever framed bytes arrive. Keeping the radio firmware dumb and the packing logic on the Pi means I can iterate on the packet or the rate without reflashing, and the identical packet also goes out over UDP/Wi-Fi so the fallback path carries byte-identical data.

lead_broadcaster.py, /odom to UART at 5 Hz

import struct, serial, rclpy
from nav_msgs.msg import Odometry

PORT, BAUD, RATE_HZ = "/dev/serial0", 115200, 5.0
FMT = "<BBIIffffH"   # magic, id, seq, t_ms, x, y, heading, speed, crc16

def crc16_ccitt(data):
    # CRC-16/CCITT (poly 0x1021, init 0xFFFF): the exact algorithm the
    # ESP32 firmware checks, so the Pi and the radio always agree
    crc = 0xFFFF
    for byte in data:
        crc ^= byte << 8
        for _ in range(8):
            crc = ((crc << 1) ^ 0x1021) & 0xFFFF if crc & 0x8000 else (crc << 1) & 0xFFFF
    return crc

def pack(seq, odom):
    body = struct.pack("<BBIIffff", 0xC0, 1, seq, now_ms(),
                       odom.pose.pose.position.x,
                       odom.pose.pose.position.y,
                       yaw_from(odom), speed_from(odom))
    crc = crc16_ccitt(body)   # must match the firmware's CRC-16/CCITT check
    return body + struct.pack("<H", crc)

uart = serial.Serial(PORT, BAUD, timeout=0)
# rclpy timer at RATE_HZ: seq += 1; uart.write(pack(seq, latest_odom))

What can go wrong: a mistyped format string produces garbage that still passes a lazy CRC check if you compute the CRC over the garbage. I verify the first hundred packets of every session by decoding them on the laptop and eyeballing that x, y, and heading are plausible.

The follower computes a goal point at a set following distance and lateral offset behind the lead's reported pose, then tracks it with a pure-pursuit-style controller: curvature to the goal point comes from the follower's own odometry/IMU estimate, and linear speed is scaled by gap error so the follower can never drive up the lead's back on a hard brake. Because the lead's pose arrives over a lossy link, the controller consumes the newest packet together with its age, not a presumed-perfect stream, stale packets are extrapolated briefly, then distrusted (step 06 owns that policy).

follow_controller.py, goal point and curvature

import math

FOLLOW_DIST, OFFSET = 1.5, 0.4   # metres behind, metres to the right
V_MAX, K_GAP        = 0.6, 0.8   # m/s cap, proportional gap gain

# Goal point: FOLLOW_DIST behind the lead's pose, OFFSET to the side
gx = lead.x - FOLLOW_DIST * math.cos(lead.heading) \
           - OFFSET * math.sin(lead.heading)
gy = lead.y - FOLLOW_DIST * math.sin(lead.heading) \
           + OFFSET * math.cos(lead.heading)

# Transform goal into the follower frame, then pure-pursuit curvature
local_x, local_y = to_rover_frame(gx, gy, my_odom)
lookahead = math.hypot(local_x, local_y)
curvature = 2.0 * local_y / (lookahead ** 2)

# Never rear-end the lead: speed scales with gap error, capped at V_MAX
gap_error = dist_to_lead - FOLLOW_DIST
v_cmd = min(V_MAX, max(0.0, K_GAP * gap_error))
w_cmd = v_cmd * curvature

How I verified: with a clean link I walked the lead through a slalom by hand (teleop) and watched R2 trace the same path offset to the side. Oscillation around the goal point meant K_GAP too hot; a lazy, drifting gap meant too cold. Tuning happened on the clean link first, you cannot tune a controller and a link at the same time.

The watchdog is the heart of the build, the controller from step 05 only ever sees goal points the watchdog approves. It implements a four-state policy on packet age: fresh packets mean follow normally; a timeout starts a short grace window in which the follower tracks the extrapolated formation vector while decelerating; grace expired means a controlled ramp to a safe-stop, holding position; the first fresh packet after a stop moves it to REJOIN, a limited-speed close before normal follow resumes. On reconnect it re-acquires the lead's track and rejoins at limited speed from where it actually is, instead of snapping to the lead's current position.

link_watchdog.py, the four-state policy

FRESH_S, GRACE_S = 1.0, 4.0   # packet-age thresholds (seconds)

age = time.monotonic() - newest_pkt.t_recv
if age < FRESH_S and state in ("FOLLOW", "HOLD"):
    state = "FOLLOW"       # link healthy, controller consumes packets directly
elif age < FRESH_S and state == "SAFE_STOP":
    state = "REJOIN"       # first fresh packet after a stop: re-acquire track,
                           # plan rejoin from current pose, cap speed at V_MAX/2
elif state == "REJOIN" and age < FRESH_S and gap_closed():
    state = "FOLLOW"       # gap closed at half speed, resume normal follow
elif age < GRACE_S:
    state = "HOLD"         # track extrapolated vector, ramp speed down
else:
    state = "SAFE_STOP"    # controlled ramp to zero, hold position

What can go wrong: an early grace window of 10 s let the follower track a stale vector long enough to enter the lead's braking zone during a blackout test, nothing hit anything, but it proved the grace window is a safety parameter, not a comfort parameter. It is short now, and HOLD always decelerates.

On the Wi-Fi fallback path, controlled impairment is a solved problem: Linux traffic control's netem qdisc applies exact loss, delay, jitter, and rate limits, and replace/change let me move between profiles mid-run without dropping the interface. The LoRa path has no netem, so I degrade at the source: the lead firmware duty-cycles transmissions (drop every nth packet) or holds a configurable blackout window on a serial command. Either way, every impairment is scripted and repeatable, "link felt bad" is not a test condition.

Impairment profiles on the follower's Wi-Fi path

# Light degradation: 10% loss, 100 ms delay with 25 ms jitter
sudo tc qdisc replace dev wlan0 root netem loss 10% delay 100ms 25ms

# Severe: 30% loss plus a 256 kbit rate cap
sudo tc qdisc change dev wlan0 root netem loss 30% rate 256kbit

# Full blackout, then clean again
sudo tc qdisc replace dev wlan0 root netem loss 100%
sudo tc qdisc del dev wlan0 root

# Before EVERY run: confirm what is actually applied
tc -s qdisc show dev wlan0

How I verified: tc -s qdisc show prints the live rule and packet counters, and the follower's own sequence-gap log must agree with the configured loss rate. The one run where they disagreed, netem was on the wrong interface, see Troubleshooting.

Every run logs two ways. Rosbag captures the ROS-native topics (/odom, /cmd_vel, link stats) so I can replay a run against a modified controller offline. Plain CSV from the watchdog node carries the derived signals, sequence gaps, RSSI, packet age, gap error, watchdog state, because a CSV loads into pandas in one line and survives tooling changes. Redundant logging looks wasteful until the first run where one recorder dies mid-lap.

Recording a blackout run on the follower

mkdir -p ~/runs
ros2 bag record -o ~/runs/r2_blackout30 \
  /odom /cmd_vel /convoy/link_stats /convoy/watchdog_state

# CSV (written by the watchdog node itself), one row per control cycle:
# t, seq_gaps, rssi_dbm, pkt_age_s, gap_err_m, state
#  1789670401.2, 0, -87, 0.21, 0.12, FOLLOW
#  1789670401.4, 3, -91, 1.30, 0.18, HOLD
#  1789670406.1, 24, 0, 6.02, 0.00, SAFE_STOP

How I verified: before any test series I record a 30-second dummy run and load both the bag and the CSV on the laptop. Missing columns or a dead bag at that point costs a minute; discovering it after a blackout series costs an afternoon.

With formation holding reliably on a clean link, the follower carries the dummy payload, a weighted box standing in for resupply cargo, strapped low over the chassis centreline so the extra mass changes the dynamics the way a real load would: longer braking distance, more lag in the gap controller. Test laps run in a clear area with scripted outages injected mid-lap, so every lap contains at least one full hold-stop-rejoin cycle under load. The outage script is deliberately boring: apply loss, wait, remove, timestamp the log.

blackout_run.sh, a scripted 30 s outage mid-lap (Wi-Fi path shown)

#!/bin/bash
# Runs ON the follower, triggered from the laptop over ssh
echo "$(date +%s) blackout start" >> ~/runs/run.log
sudo tc qdisc replace dev wlan0 root netem loss 100%
sleep 30
sudo tc qdisc del dev wlan0 root
echo "$(date +%s) link restored" >> ~/runs/run.log

What can go wrong: the payload shifted sideways on the first hard turn until I added a second strap, a moving centre of mass makes the follower waddle and invalidates every comparison against the no-payload baseline. The box is weighed and strapped the same way every run now.

Post-run, a small Python script (pandas + matplotlib) turns the CSV logs into the three headline numbers. Recovery time is the one I care about most, from the first good packet after a blackout to back within formation tolerance, because that is exactly what a resupply convoy commander would ask about. The analysis code is written and tested against the dummy-run logs from step 08; it is waiting on real test-series data, not the other way around.

analyze_run.py, recovery time after a blackout

import pandas as pd

df = pd.read_csv("runs/r2_blackout30.csv")

# End of the last SAFE_STOP period = moment the link came back
stopped = df[df["state"] == "SAFE_STOP"]
t_restored = stopped["t"].max()

# Rejoined = first row after that where gap error is back in tolerance
TOL_M = 0.3
rejoined = df[(df["t"] > t_restored) & (df["gap_err_m"].abs() < TOL_M)]
recovery_s = (rejoined["t"].min() - t_restored) if len(rejoined) else None
print(f"recovery time: {recovery_s}")

How I verified: the script's output on hand-crafted CSV fixtures (where I know the true recovery time) matches to the control-cycle resolution. Numbers from real laps get published only after the blackout series runs, no fixture data will ever be presented as a result.