Drive intent

/drive_intent carries what a driving node is trying to do and why, once per decision. The dashboard draws it as a curved arrow ahead of the car and a panel naming the limit currently holding the speed down. Measured speed and heading show what the car did. Intent shows the plan before the car drives it, so a wrong plan can be seen before it reaches a wall.

gap_follow_node, pure_pursuit_node and neural_racer_node publish it. The schema and the prediction code are in src/drive_intent/GitHub, which imports no ROS, plus a single C++ header for the team repositories.

The topic

JSON in a string was chosen over a custom message type so that racerbot_a and racerbot_b, which build in their own workspaces, can publish it with no build dependency beyond std_msgs. The cost is no type checking at compile time. schema.validate() enforces the format, and every consumer treats incoming messages as untrusted: the dashboard drops a malformed one with a warning.

Safety rules for publishers

Intent is a diagnostic inside nodes that steer the car. Three rules keep it from affecting the drive command:

1. Publish after the command. The intent call is the last statement of each tick, after the drive command has gone out.
2. Wrap it in one try/except. An error in the intent code must never stop the node. After repeated failures, FailureLatch in throttle.py turns intent off, logs once, and the node keeps driving.
3. Read only, write nothing. Intent uses only values the control path already computed, and the control path reads nothing intent produces.

Both nodes have tests that break intent generation on purpose and assert that the car still drives, for example test_gap_follow_intent.pyGitHub.

The message

{
  "v": 1,
  "stamp": 1786062657.489,
  "node": "gap_follow_node",
  "frame": "base_link",
  "state": "corner_fallback",
  "severity": "caution",
  "horizon_s": 1.5,
  "desired_steering": 0.2130,
  "commanded_steering": 0.1400,
  "desired_speed": 2.400,
  "commanded_speed": 1.900,
  "path":           [{"x": 0.0, "y": 0.0, "v": 2.4}],
  "commanded_path": [{"x": 0.0, "y": 0.0, "v": 1.9}],
  "factors": [
    {"name": "curve cap",     "value": 2.410, "unit": "m/s", "binding": false},
    {"name": "corner cap",    "value": 1.900, "unit": "m/s", "binding": true}
  ],
  "targets": [{"kind": "gap_target", "x": 1.761, "y": -0.385}]
}

validate() limits each path to 512 points, factors and targets to 32 entries each, and reason to 2000 characters.

How the path is predicted

path answers one question: if the algorithm got exactly what it asked for, where would the car be over the next horizon_s seconds? It is not where the car is going, which odometry already shows. predict.pyGitHub rolls the bicycle model forward in samples steps of $\Delta t$ = horizon_s / (samples − 1).

Each step holds the speed $v$ and steering $\delta$ constant, so the car moves along an arc of curvature $\kappa =\tan \delta /L$. Over a step of length $s=v\Delta t$ the heading turns by $\kappa s$, and integrating the heading along the arc gives the end point exactly, the same formula Stanley’s delay compensation derives:

def arc_step(x, y, yaw, speed, steering, wheelbase, dt):
    kappa = curvature_from_steering(steering, wheelbase)
    ds = speed * dt
    if abs(kappa) < STRAIGHT_CURVATURE_EPS:
        return x + ds * math.cos(yaw), y + ds * math.sin(yaw), yaw
    dyaw = kappa * ds
    radius = 1.0 / kappa
    nx = x + radius * (math.sin(yaw + dyaw) - math.sin(yaw))
    ny = y - radius * (math.cos(yaw + dyaw) - math.cos(yaw))
    return nx, ny, yaw + dyaw

The exact arc is used instead of a simple Euler step because over a 1.5 s horizon at full lock the two differ by several centimetres, the scale at which someone reads the arrow to decide whether the car will clip a cone. Below a curvature of $10^{-9}$ m⁻¹ the step is a straight line, because $1/\kappa$ would overflow.

The steering at each step depends on the node:

Why factors matter most

The speed a node commands is the minimum of several limits, and a single number hides which one won. factors lists all of them, and bind_min() marks the smallest as binding, or every one of them on a tie. Reading the binding factor answers “why is the car this slow here” directly.

Add intent to a new node

1. Import drive_intent and build the message from values the tick already computed.
2. Call it after the drive command, inside one try/except, behind a FailureLatch.
3. Throttle it to intent_rate_hz.
4. Add a test that breaks intent and asserts the node still publishes its command.

The porting guide for the team repositories, including the C++ header, is in docs/drive-intent.mdGitHub.