Repository navigation
Autonomous: Check STEER_TRACKING against the measured steering angle - #238
Conversation
|
Important Review skippedIgnore keyword(s) in the title. ⛔ Ignored keywords: 1
Please check the settings in the CodeRabbit UI or the
|
|
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 checkedChecked:
Didn't check:
Assumed:
Ultron reviewed 287d201 · it re-reviews on new pushes |
| # 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)) |
There was a problem hiding this comment.
🟠 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.
| 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 |
There was a problem hiding this comment.
🟠 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.
| 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}' |
There was a problem hiding this comment.
🟡 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.
| 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.
| m = SafetyMonitor(YAW_CFG, now=0.0) | ||
| last = _drive(m, 0.0, 3.0, steer=0.1) | ||
| assert m.check(last) == Fault.NONE |
There was a problem hiding this comment.
🟡 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.
| 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.
STEER_TRACKINGnow 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 missedsteer_freezeat 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)
EKF_DIVERGEDtripped first.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_TRACKINGtrips when the measured angle is more thansteer_angle_tol_rad(0.035 rad, about 2°) off the command forsteer_angle_window_s(0.2 s).vehicle.yaml's steering lock and rate-limited at itsmax_rate_rad_s, so a saturated actuator or a step it is still following is not a fault.steer_angle_timeout_s(0.25 s) trips too. Withrequire_steer_measurementon (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.require_steer_measurementoff, for a plant that has no steering measurement.steering_measured_topic,require_steer_measurementand the threesteer_angle_*thresholds. The thresholds are exposed on the Gazebo launch assafety_steer_angle_*; the topic and the requirement deliberately are not, andlhr_safety's README says why.lhr_gazebo:/lhr/vehicle/steering_joint_states.joint_cmd_adapterinverts 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, jointsteering).analyze_run.pyaddsvisible_after_sandlatency_from_visible_sforsteer_freeze, measured from when the command first moves the tolerance off the frozen angle, and the summary prints both.run_faults.shrecords the measured steering topic.STEER_TRACKING1.0 s budget now runs from when the freeze becomes visible.lhr_safety,lhr_gazebo,ros2and the fault tool READMEs.Testing
lhr_safetymonitor 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 newlhr_gazebotests cover the Ackermann round trip from -0.55 to 0.55 rad, name-based joint lookup and a missing joint. On red-mesa,colcon testforlhr_safetyandlhr_gazeboran 336 tests with 0 failures.steer_freezeon red-mesa, two runs per tier:lhr_safetyran 339 tests with 0 failures on red-mesa. Under the new default a gate lap on autocross seed 1 passes, andsteer_freezeat 5 m/s is caught 0.25 s after it becomes visible.mkdocs build --strictpasses, and no nodes were left over between runs.Closes #208