Skip to content

Autonomous: Check STEER_TRACKING against the measured steering angle - #238

Merged
GrayM8 merged 5 commits into
mainfrom
gray/steer-tracking
Nov 8, 2026
Merged

GrayM8 merged 5 commits into
mainfrom
gray/steer-tracking

Conversation

@GrayM8

@GrayM8 GrayM8 commented Nov 8, 2026 •

Copy link
Copy Markdown
Member

STEER_TRACKING now compares the measured steering angle with the command, instead of inferring the angle from yaw rate. On the car the angle comes from the steering motor's encoder. In Gazebo it comes from the steering joints' actual angles. The old rule missed steer_freeze at the 8 m/s tier on 11/1 and took 3.0 to 4.5 s at 3 and 5 m/s. It now catches 6 of 6, 0.25 to 0.35 s after the freeze becomes visible.

Why the old rule missed (the 11/1 bags)

  • 8 m/s, run 1 (missed): steering froze mid-corner at 0.125 rad. Pursuit asked for up to 0.31 rad while the car kept turning at the frozen rate. The band is half the expected yaw rate, so it widened as the request grew, and the mismatch only cleared it 1.64 s in. At about 2.0 s the car ran wide and hit something (true speed 6.2 to 2.3 m/s in 0.25 s). Below 2 m/s the check is off, so the 1 s window never filled.
  • 8 m/s, run 2 (wrong check): the same freeze and crash. The impact threw the EKF speed to -64 m/s, so EKF_DIVERGED tripped first.
  • 5 m/s: the freeze was on a gentle section, so the yaw-rate gap only cleared the 0.15 rad/s floor at 1.9 s, plus the 1 s window.

Tightening the band and window enough for 1.0 s would false-trip on tire slip, which the FMU plant will add.

What changed

  • lhr_safety: STEER_TRACKING trips when the measured angle is more than steer_angle_tol_rad (0.035 rad, about 2°) off the command for steer_angle_window_s (0.2 s).
    • The command is first clamped to vehicle.yaml's steering lock and rate-limited at its max_rate_rad_s, so a saturated actuator or a step it is still following is not a fault.
    • A measured angle that stops arriving, or never arrives, for steer_angle_timeout_s (0.25 s) trips too. With require_steer_measurement on (the default), a missing measurement is stale from startup like every other stream, so a wrong topic or an encoder that is dead at boot keeps the car from arming. It never quietly hands the check back to the yaw-rate rule.
    • The old yaw-rate rule runs only with require_steer_measurement off, for a plant that has no steering measurement.
    • New parameters: steering_measured_topic, require_steer_measurement and the three steer_angle_* thresholds. The thresholds are exposed on the Gazebo launch as safety_steer_angle_*; the topic and the requirement deliberately are not, and lhr_safety's README says why.
  • lhr_gazebo:
    • The vehicle template adds Gazebo's joint state publisher for both steering joints at 50 Hz, and both generated models are regenerated.
    • The bridge maps it to /lhr/vehicle/steering_joint_states.
    • joint_cmd_adapter inverts the Ackermann split exactly (tan of the center angle is the harmonic mean of the wheels' tangents) and publishes /lhr/vehicle/steering_measured (sensor_msgs/JointState, joint steering).
    • This touches the template Autonomous: drive Gazebo sensors from the FMU in lockstep #221 generates its FMU model from, so expect a small conflict there.
  • Fault tools:
    • A frozen actuator is invisible while the command still asks for the frozen angle. So analyze_run.py adds visible_after_s and latency_from_visible_s for steer_freeze, measured from when the command first moves the tolerance off the frozen angle, and the summary prints both.
    • run_faults.sh records the measured steering topic.
  • Pass criteria:
  • Docs: lhr_safety, lhr_gazebo, ros2 and the fault tool READMEs.

Testing

  • Unit tests: 9 new lhr_safety monitor tests: tracking, a rate-limited step, an actuator slower than its limit, a freeze, a lost measurement, NaN, a command past the steering lock, a measurement that never arrives, and the explicit yaw-rate fallback. 10 new lhr_gazebo tests cover the Ackermann round trip from -0.55 to 0.55 rad, name-based joint lookup and a missing joint. On red-mesa, colcon test for lhr_safety and lhr_gazebo ran 336 tests with 0 failures.
  • steer_freeze on red-mesa, two runs per tier:
Tier Caught Visible after the freeze Trip after visible Trip from injection Within PDR formula
3 m/s 2 of 2 2.51 to 2.53 s 0.25 to 0.35 s 2.78 to 2.86 s 2 of 2
5 m/s 2 of 2 1.66 to 1.68 s 0.25 s 1.91 to 1.93 s 2 of 2
8 m/s (5.3 at the fault) 2 of 2 0.63 s 0.25 s 0.88 s 2 of 2
  • False trips: 0 on 9 clean laps: MVP autocross seeds 1 to 5, rules-compliant seeds 2 and 3, and Autonomous: Add fixed extreme autocross scenarios #218's hairpin and slalom-chicane worlds (copied in, uncommitted). Over all of them:
    • The measured angle arrived at 50 Hz in sim time.
    • The median error was 0.000 rad and the 99th percentile at most 0.014 rad.
    • Peaks were 0.015 to 0.040 rad, and the error went over 0.035 rad on at most 2 isolated single samples per lap, where tripping needs 10 in a row.
  • After Ultron's two fixes (the lock clamp, and requiring the measurement): lhr_safety ran 339 tests with 0 failures on red-mesa. Under the new default a gate lap on autocross seed 1 passes, and steer_freeze at 5 m/s is caught 0.25 s after it becomes visible.
  • flake8 and pep257 are clean, mkdocs build --strict passes, and no nodes were left over between runs.

Closes #208

@GrayM8
GrayM8 requested a review from a team as a code owner November 8, 2026 20:35
@coderabbitai

coderabbitai Bot commented Nov 8, 2026 •

Copy link
Copy Markdown

Important

Review skipped

Ignore keyword(s) in the title.

⛔ Ignored keywords: 1

  • Autonomous:

Please check the settings in the CodeRabbit UI or the .coderabbit.yaml file in this repository. To trigger a single review, invoke the @coderabbitai review command.

Run configuration

You can disable this status message by setting the reviews.review_status to false in the CodeRabbit configuration file.

Use the checkbox below for a quick retry:

  • Trigger review
  • Autofix · Keep fixing CodeRabbit findings and required CI, and resolving merge conflicts

Thanks for using CodeRabbit! It's free for OSS, and your support helps us grow. If you like it, consider giving us a shout-out.

Share

@lhre-ultron

lhre-ultron Bot commented Nov 8, 2026 •

Copy link
Copy Markdown
Contributor

Safe to merge: I found no bugs in the diff, and the earlier comments are fixed, so I'm dropping them. I couldn't build ROS or Gazebo here, so none of the PR's tests were run.

What I checked

Checked:

  • I read all of SafetyMonitor.on_steering, _check_steering and check:
    • Reference rate limiting and lock clamping.
    • The dt == 0 guard.
    • Resync on first sample and on a clock jump.
    • A NaN measurement.
    • The stale-from-startup path with require_steer_measurement on.
    • The fallback when it is off.
  • Every parameter, topic and launch argument is wired consistently through safety_node.py, the launch file, test_safety_launch.py and the READMEs.
  • vehicle.max_steer_rate_rad_s and max_steer_rad exist in lhr_vehicle and come from the steering.max_rate_rad_s and max_angle_rad entries in vehicle.yaml.
  • bicycle_angle inverts the Ackermann split.
  • The Gazebo JointStatePublisher honors <update_rate> and the <topic> element (read from the gz-sim8 source).
  • The analyze_run.py freeze-visibility logic, and the summarize.py handling of never and NaN values.
  • The new unit tests agree with the monitor's logic.

Didn't check:

  • Behavior in a real Gazebo run:
    • whether the joint controller follows a step within 0.035 rad inside 0.2 s, given that max_rate_rad_s is marked ASSUMED at 10 rad/s;
    • the gz.msgs.Model to JointState bridge mapping.
  • The regenerated models were not diffed against the template output.

Assumed:

  • Receive-time stamping in safety_node matches how the other streams are timed.
  • The default steer_angle_tol_rad is what analyze_run.py uses to decide when a freeze becomes visible.

Ultron reviewed 287d201 · it re-reviews on new pushes

@lhre-ultron lhre-ultron Bot left a comment

Copy link
Copy Markdown
Contributor

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Ultron's review of e3818b2, line by line.

# No time passed, no movement, even at an unlimited rate.
dt = now - previous
step = cfg.max_steer_rate_rad_s * dt if dt > 0.0 else 0.0
self._steer_ref += max(-step, min(step, self._steer_cmd - self._steer_ref))

Copy link
Copy Markdown
Contributor

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

🟠 Major | Correctness

Clamp the reference to the steering lock so a command past lock is not a fault

When the command exceeds max_angle_rad (0.55 rad), joint_cmd_adapter clamps it before it reaches the wheels (joint_cmd_adapter.py:62). The measured angle then saturates at ±0.55 rad. The monitor's reference chases the unclamped command, so the gap grows past steer_angle_tol_rad. After 0.2 s the check latches STEER_TRACKING and stops a healthy car. For example, a 0.60 rad command gives 0.05 rad error, which trips. SafetyConfig has no steering-lock limit, so the monitor cannot know about the clamp.

Add max_steer_rad to SafetyConfig, fill it from vehicle.max_steer_rad in safety_node, and clamp the target before the rate step.

💚 Proposed fix
--- a/autonomy/ros2/src/lhr_safety/lhr_safety/monitors.py
+++ b/autonomy/ros2/src/lhr_safety/lhr_safety/monitors.py
@@
-                self._steer_ref += max(-step, min(step, self._steer_cmd - self._steer_ref))
+                target = max(-cfg.max_steer_rad, min(cfg.max_steer_rad, self._steer_cmd))
+                self._steer_ref += max(-step, min(step, target - self._steer_ref))

Also add max_steer_rad: float = math.inf to SafetyConfig, and pass max_steer_rad=vehicle.max_steer_rad in safety_node.py.

🤖 Prompt for AI agents
In autonomy/ros2/src/lhr_safety/lhr_safety/monitors.py around lines 159-171:
The measured-angle reference follows the raw steering command, but joint_cmd_adapter clamps commands to vehicle max_angle_rad. Add a max_steer_rad field to SafetyConfig (default math.inf), set it from vehicle.max_steer_rad in safety_node.py, and clamp the command to +/-max_steer_rad before the rate-limited reference update in on_steering. Add a test where the command is 0.7 rad and the measured angle holds at 0.55 rad; it must not trip.

@lhre-ultron lhre-ultron Bot left a comment

Copy link
Copy Markdown
Contributor

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Ultron's review of 9d5b055, line by line.

Comment on lines 235 to 240
if self._last_steer_measured is not None:
if (self._angle_fault or self._age(self._last_steer_measured, now)
> cfg.steer_angle_timeout_s):
return Fault.STEER_TRACKING
elif self._steer_fault:
return Fault.STEER_TRACKING

Copy link
Copy Markdown
Contributor

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

🟠 Major | Safety

Don't fall back to the yaw-rate rule silently when the steering measurement never arrives

If the encoder, bridge or adapter is dead from the start, _last_steer_measured stays None. check() then uses the old yaw-rate rule, which the PR shows missing steer_freeze at 8 m/s and taking 3.0 to 4.5 s at 3 and 5 m/s. The loss timeout only applies after a first sample, so a wrong steering_measured_topic, a missing joint-state plugin, or an encoder that is down at boot goes undetected. The timeout would have tripped on that same failure mid-run. The fallback was meant for the kinematic demo only, so it should be an explicit opt-in. Otherwise a missing measurement should trip once the car is armed.

💚 Proposed fix
--- a/autonomy/ros2/src/lhr_safety/lhr_safety/monitors.py
+++ b/autonomy/ros2/src/lhr_safety/lhr_safety/monitors.py
+    # In SafetyConfig:
+    require_steer_measurement: bool = False  # True on the car and in Gazebo runs
@@ check()
-        if self._last_steer_measured is not None:
+        if self._last_steer_measured is None and cfg.require_steer_measurement:
+            if self._age(self._started, now) > cfg.steer_angle_timeout_s:
+                return Fault.STEER_TRACKING
+        elif self._last_steer_measured is not None:

The require_steer_measurement flag has to come from a node parameter, set true in the Gazebo launch whenever the dynamic plant is used. self._started stands for the construction time the monitor already receives as now; store it in __init__ if no such field exists.

🤖 Prompt for AI agents
In autonomy/ros2/src/lhr_safety/lhr_safety/monitors.py around lines 235-240:
check() uses the yaw-rate fallback whenever no measured steering angle has ever arrived, so a dead encoder, bridge or wrong topic at boot goes unflagged. Add a SafetyConfig flag (exposed as a safety_node parameter and a Gazebo launch argument, on for the dynamic plant) that makes a measurement that never arrives within steer_angle_timeout_s of start return Fault.STEER_TRACKING. Keep the fallback only when the flag is off, and add a unit test for it.

@lhre-ultron lhre-ultron Bot left a comment

Copy link
Copy Markdown
Contributor

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Ultron's review of 7c35af1, line by line.

and abs(a - frozen) > STEER_TOL_RAD), None)
if visible is not None:
row['visible_after_s'] = f'{visible - fault_t:.3f}'
row['latency_from_visible_s'] = f'{trip[0] - visible:.3f}'

Copy link
Copy Markdown
Contributor

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

🟡 Minor | Correctness

Only score latency_from_visible_s when the trip comes after the freeze is visible

If STEER_TRACKING trips before the command has moved STEER_TOL_RAD off the frozen angle (for example the measured-angle timeout fires first), trip[0] - visible is negative. A negative value passes the 1.0 s budget, and the summary reports it as a fast catch. When the command never leaves tolerance, visible is None and the fields stay empty, so that run is never checked against the budget either.

Suggested change
row['latency_from_visible_s'] = f'{trip[0] - visible:.3f}'
row['visible_after_s'] = f'{visible - fault_t:.3f}'
row['latency_from_visible_s'] = f'{max(trip[0] - visible, 0.0):.3f}' if trip[0] >= visible else 'early'
🤖 Prompt for AI agents
In autonomy/testing/tools/fault_injection/analyze_run.py around lines 152-156:
latency_from_visible_s can be negative when the trip precedes the freeze becoming visible. Emit a marker such as 'early' instead of a negative number, and make summarize.py skip or flag non-numeric values rather than counting them toward the budget.

@lhre-ultron lhre-ultron Bot left a comment

Copy link
Copy Markdown
Contributor

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Ultron's review of 350639a, line by line.

Comment on lines +365 to +367
m = SafetyMonitor(YAW_CFG, now=0.0)
last = _drive(m, 0.0, 3.0, steer=0.1)
assert m.check(last) == Fault.NONE

Copy link
Copy Markdown
Contributor

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

🟡 Minor | Correctness

Make the fallback test exercise the yaw-rate rule, so it can fail

_drive goes through _healthy, which now calls on_steering. That sets _last_steer_measured, so _check_steering returns early and the yaw-rate fallback never runs. The car also turns consistently with the command, so NONE is expected either way, and a broken fallback would still pass. Feed odom and command without any steering sample, and make the yaw rate disagree with the command so the fallback has to trip.

Suggested change
m = SafetyMonitor(YAW_CFG, now=0.0)
last = _drive(m, 0.0, 3.0, steer=0.1)
assert m.check(last) == Fault.NONE
m = SafetyMonitor(YAW_CFG, now=0.0)
t = 0.0
while t < 3.0:
m.on_lidar(t)
m.on_command(t, 0.3)
m.on_odom(t, 5.0 * t, 0.0, 5.0, 0.0, 0.0, 0.01, 0.01)
t += 0.05
assert m.check(t - 0.05) == Fault.STEER_TRACKING
🤖 Prompt for AI agents
In autonomy/ros2/src/lhr_safety/test/test_monitors.py around lines 365-367:
test_without_a_requirement_the_yaw_rate_rule_takes_over uses _drive, which feeds on_steering and so disables the yaw-rate fallback. Rewrite it to feed on_lidar, on_command (0.3 rad) and on_odom with zero yaw rate at 5 m/s, never calling on_steering, and assert the monitor reports Fault.STEER_TRACKING after steer_window_s.

@GrayM8
GrayM8 merged commit de53b75 into main Nov 8, 2026
12 checks passed
@GrayM8
GrayM8 deleted the gray/steer-tracking branch November 8, 2026 22:19
Sign up for free to join this conversation on GitHub. Already have an account? Sign in to comment

Labels

None yet

Projects

None yet

Development

Successfully merging this pull request may close these issues.

Autonomous: STEER_TRACKING is slow and can miss a steering freeze

1 participant