Chapter 14: Navigation Architecture for AMR
Lesson 4: Recovery Behaviors and Fault Handling
This lesson formalizes how a navigation stack detects “something is going wrong” and then executes structured recovery behaviors while preserving safety. We treat recovery as a supervisory layer over global/local navigation: it monitors residuals (progress, oscillation, uncertainty, proximity) and triggers a bounded escalation policy (clear costmaps, rotate, back up, re-localize, safe stop). We emphasize quantitative decision rules, hybrid-mode modeling, and a safety invariant based on braking distance with latency.
1. Conceptual Overview
In Chapter 14 so far, we built a layered navigation architecture and introduced a decision logic (state machines / behavior trees) to coordinate global planning, local control, and reactive avoidance. A recovery layer is the “last mile” that keeps the robot productive under real-world non-idealities: dynamic obstacles, partial map errors, localization dropouts, wheel slip, narrow passages, and sensor occlusions.
The key design idea is to separate: (i) monitoring (detect anomalies via residuals), (ii) diagnosis (classify likely cause), and (iii) mitigation (execute a recovery action sequence with escalation). Importantly, recovery is not “random motion”; it is structured action that reshapes the belief and the costmap so normal navigation can resume.
We will use a supervisor that transitions among modes \( q \in \{\text{NAV},\text{RECOVER},\text{STOP}\} \). The supervisor reads a vector of health signals \( \mathbf{z}_k \) and applies guards that decide when to switch modes.
2. Fault–Error–Failure and Hook Points in the Navigation Stack
We use the classical dependability chain: Fault → Error → Failure. A fault is an underlying cause (e.g., temporary LiDAR occlusion, wheel slip); an error is the internal deviation (e.g., pose estimate drift, stale obstacle layer); a failure is a violated service (e.g., cannot make progress to goal, collision risk).
In a layered navigation stack, monitoring “hooks” naturally appear at:
- Commanded motion \( \mathbf{u}_k = [v_k,\omega_k]^\top \) and its sign/variance patterns.
- Progress to goal measured by distance along the global plan or Euclidean distance in the map frame.
- Collision proximity extracted from costmap distance fields / inflation radius logic (Lesson 2).
- Localization uncertainty (e.g., covariance trace from a filter in Chapter 7).
- Planner/controller status (timeouts, infeasible trajectory count, repeated replans).
A principled recovery system does not require perfect diagnosis. It only needs enough information to pick a safe mitigation sequence. We will build decision statistics that are robust to noise and timing jitter.
3. Monitoring Residuals and Decision Statistics
Let the robot’s pose estimate at discrete time \( k \) be \( \hat{\mathbf{x}}_k = [\hat{x}_k,\hat{y}_k,\hat{\theta}_k]^\top \) (Chapter 6–7). Let the goal position be \( \mathbf{g}=[g_x,g_y]^\top \). A basic progress signal is the Euclidean distance-to-goal:
\[ d_k = \left\| \mathbf{g} - \hat{\mathbf{p}}_k \right\|_2, \quad \hat{\mathbf{p}}_k = [\hat{x}_k,\hat{y}_k]^\top. \]
Progress residual. Over a sliding window of length \( W \) seconds (or \( N_W \) samples), define:
\[ \Delta d_k = d_{k-N_W} - d_k. \]
A “no-progress” failure is triggered if \( \Delta d_k < \delta_d \), where \( \delta_d \) is a minimum expected progress (meters) over the window.
Oscillation statistic. Let the commanded velocity be \( \mathbf{u}_k=[v_k,\omega_k]^\top \). Define a sign function with deadband \( \epsilon_v \): \( s(v_k) \): equals \( +1 \) if \( v_k > \epsilon_v \), \( -1 \) if \( v_k < -\epsilon_v \), and \( 0 \) otherwise. Count sign flips over the last \( N_O \) samples:
\[ F_k = \sum_{i=k-N_O+1}^{k} \mathbb{I}\!\left( s(v_i)\neq 0 \land s(v_{i-1})\neq 0 \land s(v_i)\neq s(v_{i-1}) \right) \;+ \\ \sum_{i=k-N_O+1}^{k} \mathbb{I}\!\left( s(\omega_i)\neq 0 \land s(\omega_{i-1})\neq 0 \land s(\omega_i)\neq s(\omega_{i-1}) \right). \]
Trigger oscillation recovery if \( F_k \ge \delta_F \).
Uncertainty guard. If the localization module provides covariance \( \mathbf{P}_k \) (Chapter 7), use the trace as a scalar health indicator: \( \rho_k \): defined by \( \rho_k = \operatorname{tr}(\mathbf{P}_k) \). Declare localization degraded if \( \rho_k > \rho_\max \).
Composite health score. You can combine monitors into a single scalar:
\[ h_k = w_d \,\max(0,\,\delta_d - \Delta d_k) + w_F \,\max(0,\,F_k - \delta_F) + w_\rho \,\max(0,\,\rho_k - \rho_{\max}). \]
Then recover if \( h_k > 0 \). This “hinge-loss” form is robust and easy to tune.
4. Supervisor Model and Escalation Logic
Model the overall navigation system as a hybrid automaton with discrete mode \( q_k \) and continuous state \( \hat{\mathbf{x}}_k \). The supervisor selects recovery actions (macro-actions) while the local controller handles fast stabilizing control.
Mode update. Let \( \mathcal{G}_r \) be the recovery guard and \( \mathcal{G}_s \) be the safety-stop guard:
\[ q_{k+1} = \begin{cases} \text{STOP} & \text{if } \mathcal{G}_s(\mathbf{z}_k)=1 \\ \text{RECOVER} & \text{if } q_k=\text{NAV} \land \mathcal{G}_r(\mathbf{z}_k)=1 \\ \text{NAV} & \text{if } q_k=\text{RECOVER} \land \text{recovery_done}=1 \\ q_k & \text{otherwise}. \end{cases} \]
Escalation ladder. Recovery is a bounded sequence of increasingly intrusive actions. Denote the recovery stage by \( r \in \{0,1,2,3\} \) with limit \( r \le r_{\max} \).
flowchart TD
Z["Monitor signals z_k (progress, cmd, uncertainty, proximity)"] --> G1["Evaluate guards: no-progress? oscillation? uncertainty high?"]
G1 -->|no| NAV["Mode NAV: global plan + local controller"]
G1 -->|yes| REC["Enter RECOVER mode; stage r = r+1"]
REC --> A1["Action: clear costmaps"]
A1 --> A2["Action: spin in place"]
A2 --> A3["Action: backup short distance"]
A3 --> CHK["Re-check guards; progress restored?"]
CHK -->|yes| NAV
CHK -->|no and r < r_max| REC
CHK -->|no and r == r_max| STOP["Mode STOP: safe stop + request help"]
5. Safety Envelope and an Emergency-Stop Invariant
Let \( v \) be forward speed, \( a_{\max} > 0 \) maximum braking deceleration magnitude, and \( \tau \): total latency (detection + computation + actuation).
\[ d_{\text{stop}}(v) = v\, \tau + \frac{v^2}{2a_{\max}}. \]
With measured free distance \( d_{\text{free},k} \) and margin \( m > 0 \), define:
\[ \mathcal{G}_s(\mathbf{z}_k)=\mathbb{I}\!\left(d_{\text{free},k} < d_{\text{stop}}(v_k)+m\right). \]
Proposition. If at stop trigger time \( d_{\text{free},k} \ge d_{\text{stop}}(v_k)+m \) and the robot can brake with at least \( a_{\max} \), then collision is avoided with margin \( m \).
Proof. Worst-case traveled distance equals latency distance \( v_k \tau \) plus braking distance \( v_k^2/(2a_{\max}) \). Since free distance exceeds their sum by \( m \), remaining clearance is at least \( m \). ∎
6. Recovery Behaviors as Belief/Map Reshaping Operators
Clearing costmaps resets stale occupancy; spinning improves perceptual coverage and can reduce uncertainty; backing up escapes geometric dead-ends; waiting handles dynamic obstacles; bounded retries prevent infinite loops.
stateDiagram-v2
[*] --> NAV
NAV --> REC_CLEAR: "no-progress or oscillation"
NAV --> REC_SPIN: "uncertainty high"
NAV --> STOP: "d_free < d_stop + m"
REC_CLEAR --> REC_SPIN: "always"
REC_SPIN --> REC_BACKUP: "always"
REC_BACKUP --> NAV: "guards cleared"
REC_BACKUP --> REC_CLEAR: "guards not cleared and r < r_max"
REC_BACKUP --> STOP: "guards not cleared and r == r_max"
STOP --> [*]
This policy matches causes to actions: map inconsistency → clear; localization degradation → spin; local minima → backup.
7. Implementation Notes (ROS 2 / Nav2 Pattern)
Recovery can be embedded inside the navigation decision logic (e.g., a behavior tree), or implemented as an external supervisor node. The examples below show the external supervisor pattern: subscribe to odometry / localization / cmd signals, detect no-progress or oscillation, call costmap clear services, and publish short open-loop commands (spin/backup) as recovery macro-actions.
8. Multi-Language Implementations
8.1 Python (ROS 2) — External Recovery Supervisor
Chapter14_Lesson4.py
"""
Chapter14_Lesson4.py
Recovery Behaviors and Fault Handling — ROS 2 / Nav2-oriented supervisor (example)
Notes:
- This script is a *teaching* example. Topic/service names can differ by robot/nav2 config.
- It demonstrates: progress monitoring, oscillation detection, costmap clearing, simple spin/backup,
and escalation to safe-stop.
Dependencies (typical):
sudo apt install ros-<distro>-nav2-msgs ros-<distro>-geometry-msgs ros-<distro>-nav-msgs
pip install --user numpy
"""
import time
from dataclasses import dataclass
import numpy as np
import rclpy
from rclpy.node import Node
from rclpy.qos import QoSProfile, QoSReliabilityPolicy, QoSHistoryPolicy
from geometry_msgs.msg import Twist, PoseWithCovarianceStamped
from nav_msgs.msg import Odometry
# Nav2 message/service types (names may vary by distro)
from nav2_msgs.srv import ClearEntireCostmap
@dataclass
class MonitorParams:
# Progress monitor
window_s: float = 10.0 # seconds
min_progress_m: float = 0.25 # required decrease in distance-to-goal over window
# Oscillation monitor
osc_window_s: float = 6.0
osc_sign_flips: int = 6 # number of sign flips in cmd_vel over window
v_eps: float = 0.03 # treat |v| < v_eps as zero
# Localization uncertainty (AMCL covariance trace threshold)
cov_trace_max: float = 0.35
# Recovery actions
spin_time_s: float = 3.0
spin_wz: float = 0.8
backup_time_s: float = 2.0
backup_vx: float = -0.12
# Escalation
max_recovery_attempts: int = 3
# Topics/services (common defaults)
cmd_vel_topic: str = "/cmd_vel"
odom_topic: str = "/odom"
amcl_topic: str = "/amcl_pose"
local_clear_srv: str = "/local_costmap/clear_entirely_local_costmap"
global_clear_srv: str = "/global_costmap/clear_entirely_global_costmap"
class RecoverySupervisor(Node):
def __init__(self):
super().__init__("recovery_supervisor")
# Parameters (could be declared as ROS params; kept simple for teaching)
self.p = MonitorParams()
qos = QoSProfile(
reliability=QoSReliabilityPolicy.BEST_EFFORT,
history=QoSHistoryPolicy.KEEP_LAST,
depth=10
)
self.pub_cmd = self.create_publisher(Twist, self.p.cmd_vel_topic, 10)
self.sub_odom = self.create_subscription(Odometry, self.p.odom_topic, self.on_odom, qos)
self.sub_amcl = self.create_subscription(PoseWithCovarianceStamped, self.p.amcl_topic, self.on_amcl, 10)
self.sub_cmd = self.create_subscription(Twist, self.p.cmd_vel_topic, self.on_cmd, 10)
self.cli_local = self.create_client(ClearEntireCostmap, self.p.local_clear_srv)
self.cli_global = self.create_client(ClearEntireCostmap, self.p.global_clear_srv)
# State buffers
self.goal_xy = None # (gx, gy) set externally (e.g., from NavigateToPose goal); for demo we keep None
self.last_xy = None
self.last_cov_trace = None
self.progress_hist = [] # list of (t, dist_to_goal)
self.cmd_hist = [] # list of (t, vx, wz)
self.recovery_attempts = 0
self.in_recovery = False
# Periodic check loop
self.timer = self.create_timer(0.2, self.tick)
self.get_logger().info("RecoverySupervisor started. Set goal_xy in code or adapt to listen to Nav2 goals.")
# ------------------------- Callbacks -------------------------
def on_odom(self, msg: Odometry):
x = msg.pose.pose.position.x
y = msg.pose.pose.position.y
self.last_xy = (x, y)
if self.goal_xy is not None:
d = float(np.hypot(self.goal_xy[0] - x, self.goal_xy[1] - y))
t = self.now_s()
self.progress_hist.append((t, d))
self.progress_hist = self.trim_by_time(self.progress_hist, self.p.window_s)
def on_amcl(self, msg: PoseWithCovarianceStamped):
cov = np.array(msg.pose.covariance, dtype=float).reshape(6, 6)
self.last_cov_trace = float(np.trace(cov[:3, :3])) # x,y,yaw block approx
def on_cmd(self, msg: Twist):
t = self.now_s()
self.cmd_hist.append((t, float(msg.linear.x), float(msg.angular.z)))
self.cmd_hist = self.trim_by_time(self.cmd_hist, self.p.osc_window_s)
# ------------------------- Helpers -------------------------
def now_s(self) -> float:
return self.get_clock().now().nanoseconds * 1e-9
@staticmethod
def trim_by_time(hist, window_s):
if not hist:
return hist
t_latest = hist[-1][0]
t_min = t_latest - window_s
i = 0
while i < len(hist) and hist[i][0] < t_min:
i += 1
return hist[i:]
def progress_ok(self) -> bool:
"""Return True if sufficient progress toward goal occurred in the window."""
if self.goal_xy is None or len(self.progress_hist) < 2:
return True # not enough info; don't fault
d_old = self.progress_hist[0][1]
d_new = self.progress_hist[-1][1]
return (d_old - d_new) >= self.p.min_progress_m
def oscillating(self) -> bool:
"""Detect oscillation by counting sign flips of linear-x or angular-z commands."""
if len(self.cmd_hist) < 3:
return False
def sign_eps(v):
if abs(v) < self.p.v_eps:
return 0
return 1 if v > 0 else -1
sx = [sign_eps(vx) for _, vx, _ in self.cmd_hist]
sz = [sign_eps(wz) for _, _, wz in self.cmd_hist]
flips_x = sum(1 for i in range(1, len(sx)) if sx[i] != 0 and sx[i-1] != 0 and sx[i] != sx[i-1])
flips_z = sum(1 for i in range(1, len(sz)) if sz[i] != 0 and sz[i-1] != 0 and sz[i] != sz[i-1])
return (flips_x + flips_z) >= self.p.osc_sign_flips
def localization_bad(self) -> bool:
if self.last_cov_trace is None:
return False
return self.last_cov_trace > self.p.cov_trace_max
# ------------------------- Recovery actions -------------------------
def call_clear_costmaps(self):
"""Best-effort: clear local then global costmap."""
for cli, name in [(self.cli_local, "local"), (self.cli_global, "global")]:
if not cli.service_is_ready():
self.get_logger().warn(f"Clear {name} costmap service not ready: {cli.srv_name}")
continue
req = ClearEntireCostmap.Request()
fut = cli.call_async(req)
rclpy.spin_until_future_complete(self, fut, timeout_sec=2.0)
self.get_logger().info(f"Clear {name} costmap result: {fut.result() is not None}")
def publish_for(self, vx: float, wz: float, duration_s: float):
"""Open-loop velocity command for a limited time."""
t_end = time.time() + duration_s
msg = Twist()
while rclpy.ok() and time.time() < t_end:
msg.linear.x = float(vx)
msg.angular.z = float(wz)
self.pub_cmd.publish(msg)
time.sleep(0.05)
# stop
msg.linear.x = 0.0
msg.angular.z = 0.0
self.pub_cmd.publish(msg)
def do_recovery_cycle(self, reason: str):
self.in_recovery = True
self.recovery_attempts += 1
self.get_logger().warn(f"RECOVERY #{self.recovery_attempts}: reason={reason}")
# 1) Clear costmaps (handles transient obstacles / stale inflation)
self.call_clear_costmaps()
# 2) Spin in place (improves sensor coverage / localization)
self.publish_for(0.0, self.p.spin_wz, self.p.spin_time_s)
# 3) Backup (escape tight corners / local minima near obstacles)
self.publish_for(self.p.backup_vx, 0.0, self.p.backup_time_s)
self.in_recovery = False
def safe_stop(self, reason: str):
self.get_logger().error(f"SAFE STOP: {reason}")
msg = Twist()
msg.linear.x = 0.0
msg.angular.z = 0.0
# publish a few times
for _ in range(10):
self.pub_cmd.publish(msg)
time.sleep(0.05)
# ------------------------- Main tick -------------------------
def tick(self):
if self.in_recovery:
return
if self.recovery_attempts >= self.p.max_recovery_attempts:
self.safe_stop("exceeded max recovery attempts")
rclpy.shutdown()
return
# Fault conditions (simple; extend with more residuals)
if self.localization_bad():
self.do_recovery_cycle("localization covariance too high")
return
if self.goal_xy is not None:
if (not self.progress_ok()) or self.oscillating():
why = "no progress" if not self.progress_ok() else "oscillation"
self.do_recovery_cycle(why)
return
def main():
rclpy.init()
node = RecoverySupervisor()
# DEMO: set a fixed goal (replace with nav2 action goal listener)
node.goal_xy = (5.0, 0.0)
try:
rclpy.spin(node)
except KeyboardInterrupt:
pass
finally:
node.destroy_node()
rclpy.shutdown()
if __name__ == "__main__":
main()
8.2 Python Exercise — Standalone Detectors
Chapter14_Lesson4_Ex1.py
"""
Chapter14_Lesson4_Ex1.py
Exercise: pure-Python progress + oscillation detector (no ROS).
Given:
- distance-to-goal samples d(t)
- cmd samples (v_x(t), w_z(t))
Compute:
- progress statistic over a sliding window
- oscillation score via sign flips
"""
from dataclasses import dataclass
from typing import List, Tuple
@dataclass
class DetParams:
window_s: float = 10.0
min_progress_m: float = 0.25
osc_window_s: float = 6.0
osc_sign_flips: int = 6
v_eps: float = 0.03
def _trim(hist: List[Tuple[float, ...]], window_s: float) -> List[Tuple[float, ...]]:
if not hist:
return hist
t_latest = hist[-1][0]
t_min = t_latest - window_s
i = 0
while i < len(hist) and hist[i][0] < t_min:
i += 1
return hist[i:]
def progress_ok(dist_hist: List[Tuple[float, float]], p: DetParams) -> bool:
dist_hist = _trim(dist_hist, p.window_s)
if len(dist_hist) < 2:
return True
d0 = dist_hist[0][1]
d1 = dist_hist[-1][1]
return (d0 - d1) >= p.min_progress_m
def oscillating(cmd_hist: List[Tuple[float, float, float]], p: DetParams) -> bool:
cmd_hist = _trim(cmd_hist, p.osc_window_s)
if len(cmd_hist) < 3:
return False
def sgn(v: float) -> int:
if abs(v) < p.v_eps:
return 0
return 1 if v > 0 else -1
sx = [sgn(vx) for _, vx, _ in cmd_hist]
sz = [sgn(wz) for _, _, wz in cmd_hist]
flips_x = sum(1 for i in range(1, len(sx)) if sx[i] != 0 and sx[i-1] != 0 and sx[i] != sx[i-1])
flips_z = sum(1 for i in range(1, len(sz)) if sz[i] != 0 and sz[i-1] != 0 and sz[i] != sz[i-1])
return (flips_x + flips_z) >= p.osc_sign_flips
if __name__ == "__main__":
p = DetParams()
# Toy example: d(t) stagnates, commands flip
dist = [(0.0, 5.0), (5.0, 4.95), (10.0, 4.93)]
cmd = [
(0.0, 0.10, 0.0),
(1.0, -0.10, 0.0),
(2.0, 0.10, 0.0),
(3.0, -0.10, 0.0),
(4.0, 0.10, 0.0),
(5.0, -0.10, 0.0),
]
print("progress_ok:", progress_ok(dist, p))
print("oscillating:", oscillating(cmd, p))
8.3 C++ (ROS 2) — External Recovery Supervisor
Chapter14_Lesson4.cpp
/*
Chapter14_Lesson4.cpp
Recovery Behaviors and Fault Handling — ROS 2 / Nav2-oriented supervisor (example)
Build example (adapt your ROS 2 distro):
add_executable(recovery_supervisor src/Chapter14_Lesson4.cpp)
ament_target_dependencies(recovery_supervisor rclcpp geometry_msgs nav_msgs nav2_msgs)
*/
#include <chrono>
#include <cmath>
#include <deque>
#include <string>
#include <utility>
#include "rclcpp/rclcpp.hpp"
#include "geometry_msgs/msg/twist.hpp"
#include "nav_msgs/msg/odometry.hpp"
#include "geometry_msgs/msg/pose_with_covariance_stamped.hpp"
#include "nav2_msgs/srv/clear_entire_costmap.hpp"
using namespace std::chrono_literals;
struct MonitorParams {
double window_s = 10.0;
double min_progress_m = 0.25;
double osc_window_s = 6.0;
int osc_sign_flips = 6;
double v_eps = 0.03;
double cov_trace_max = 0.35;
double spin_time_s = 3.0;
double spin_wz = 0.8;
double backup_time_s = 2.0;
double backup_vx = -0.12;
int max_recovery_attempts = 3;
std::string cmd_vel_topic = "/cmd_vel";
std::string odom_topic = "/odom";
std::string amcl_topic = "/amcl_pose";
std::string local_clear_srv = "/local_costmap/clear_entirely_local_costmap";
std::string global_clear_srv = "/global_costmap/clear_entirely_global_costmap";
};
class RecoverySupervisor : public rclcpp::Node {
public:
RecoverySupervisor() : Node("recovery_supervisor_cpp") {
p_ = MonitorParams{};
pub_cmd_ = this->create_publisher<geometry_msgs::msg::Twist>(p_.cmd_vel_topic, 10);
sub_odom_ = this->create_subscription<nav_msgs::msg::Odometry>(
p_.odom_topic, rclcpp::QoS(10).best_effort(),
std::bind(&RecoverySupervisor::on_odom, this, std::placeholders::_1));
sub_amcl_ = this->create_subscription<geometry_msgs::msg::PoseWithCovarianceStamped>(
p_.amcl_topic, 10,
std::bind(&RecoverySupervisor::on_amcl, this, std::placeholders::_1));
sub_cmd_ = this->create_subscription<geometry_msgs::msg::Twist>(
p_.cmd_vel_topic, 10,
std::bind(&RecoverySupervisor::on_cmd, this, std::placeholders::_1));
cli_local_ = this->create_client<nav2_msgs::srv::ClearEntireCostmap>(p_.local_clear_srv);
cli_global_ = this->create_client<nav2_msgs::srv::ClearEntireCostmap>(p_.global_clear_srv);
goal_set_ = true;
goal_x_ = 5.0;
goal_y_ = 0.0;
timer_ = this->create_wall_timer(200ms, std::bind(&RecoverySupervisor::tick, this));
RCLCPP_INFO(this->get_logger(), "RecoverySupervisor C++ started (demo goal set to (5,0)).");
}
private:
// history entries
using DistEntry = std::pair<double, double>; // (t, dist)
struct CmdEntry { double t; double vx; double wz; };
void on_odom(const nav_msgs::msg::Odometry::SharedPtr msg) {
last_x_ = msg->pose.pose.position.x;
last_y_ = msg->pose.pose.position.y;
have_pose_ = true;
if (goal_set_) {
double d = std::hypot(goal_x_ - last_x_, goal_y_ - last_y_);
double t = now_s();
progress_hist_.push_back({t, d});
trim_progress_();
}
}
void on_amcl(const geometry_msgs::msg::PoseWithCovarianceStamped::SharedPtr msg) {
// covariance is 6x6 row-major in array[36]
const auto & c = msg->pose.covariance;
// trace of (x,y,yaw) approx indices: (0,0), (1,1), (5,5)
last_cov_trace_ = c[0] + c[7] + c[35];
have_cov_ = true;
}
void on_cmd(const geometry_msgs::msg::Twist::SharedPtr msg) {
double t = now_s();
cmd_hist_.push_back({t, msg->linear.x, msg->angular.z});
trim_cmd_();
}
double now_s() const {
return this->get_clock()->now().nanoseconds() * 1e-9;
}
void trim_progress_() {
if (progress_hist_.empty()) return;
double t_latest = progress_hist_.back().first;
double t_min = t_latest - p_.window_s;
while (!progress_hist_.empty() && progress_hist_.front().first < t_min) {
progress_hist_.pop_front();
}
}
void trim_cmd_() {
if (cmd_hist_.empty()) return;
double t_latest = cmd_hist_.back().t;
double t_min = t_latest - p_.osc_window_s;
while (!cmd_hist_.empty() && cmd_hist_.front().t < t_min) {
cmd_hist_.pop_front();
}
}
bool progress_ok_() const {
if (!goal_set_ || progress_hist_.size() < 2) return true;
double d0 = progress_hist_.front().second;
double d1 = progress_hist_.back().second;
return (d0 - d1) >= p_.min_progress_m;
}
int sgn_eps_(double v) const {
if (std::fabs(v) < p_.v_eps) return 0;
return (v > 0.0) ? 1 : -1;
}
bool oscillating_() const {
if (cmd_hist_.size() < 3) return false;
int flips = 0;
for (size_t i = 1; i < cmd_hist_.size(); ++i) {
int s_prev = sgn_eps_(cmd_hist_[i-1].vx);
int s_now = sgn_eps_(cmd_hist_[i].vx);
if (s_prev != 0 && s_now != 0 && s_prev != s_now) flips++;
}
for (size_t i = 1; i < cmd_hist_.size(); ++i) {
int s_prev = sgn_eps_(cmd_hist_[i-1].wz);
int s_now = sgn_eps_(cmd_hist_[i].wz);
if (s_prev != 0 && s_now != 0 && s_prev != s_now) flips++;
}
return flips >= p_.osc_sign_flips;
}
bool localization_bad_() const {
if (!have_cov_) return false;
return last_cov_trace_ > p_.cov_trace_max;
}
void publish_for_(double vx, double wz, double duration_s) {
auto t_end = this->get_clock()->now() + rclcpp::Duration::from_seconds(duration_s);
rclcpp::Rate rate(20.0);
geometry_msgs::msg::Twist cmd;
while (rclcpp::ok() && this->get_clock()->now() < t_end) {
cmd.linear.x = vx;
cmd.angular.z = wz;
pub_cmd_->publish(cmd);
rate.sleep();
}
cmd.linear.x = 0.0; cmd.angular.z = 0.0;
pub_cmd_->publish(cmd);
}
void call_clear_costmaps_() {
for (auto & cli : {cli_local_, cli_global_}) {
if (!cli->service_is_ready()) {
RCLCPP_WARN(this->get_logger(), "Costmap clear service not ready: %s", cli->get_service_name());
continue;
}
auto req = std::make_shared<nav2_msgs::srv::ClearEntireCostmap::Request>();
auto fut = cli->async_send_request(req);
auto ret = rclcpp::spin_until_future_complete(this->get_node_base_interface(), fut, 2s);
RCLCPP_INFO(this->get_logger(), "Clear costmap %s", (ret == rclcpp::FutureReturnCode::SUCCESS) ? "OK" : "TIMEOUT/FAIL");
}
}
void do_recovery_(const std::string & reason) {
in_recovery_ = true;
recovery_attempts_++;
RCLCPP_WARN(this->get_logger(), "RECOVERY #%d reason=%s", recovery_attempts_, reason.c_str());
call_clear_costmaps_();
publish_for_(0.0, p_.spin_wz, p_.spin_time_s);
publish_for_(p_.backup_vx, 0.0, p_.backup_time_s);
in_recovery_ = false;
}
void safe_stop_(const std::string & reason) {
RCLCPP_ERROR(this->get_logger(), "SAFE STOP: %s", reason.c_str());
geometry_msgs::msg::Twist cmd;
cmd.linear.x = 0.0;
cmd.angular.z = 0.0;
for (int i = 0; i < 10; ++i) {
pub_cmd_->publish(cmd);
rclcpp::sleep_for(50ms);
}
}
void tick() {
if (in_recovery_) return;
if (recovery_attempts_ >= p_.max_recovery_attempts) {
safe_stop_("exceeded max recovery attempts");
rclcpp::shutdown();
return;
}
if (localization_bad_()) {
do_recovery_("localization covariance too high");
return;
}
if (goal_set_) {
if (!progress_ok_() || oscillating_()) {
do_recovery_(!progress_ok_() ? "no progress" : "oscillation");
return;
}
}
}
private:
MonitorParams p_;
rclcpp::Publisher<geometry_msgs::msg::Twist>::SharedPtr pub_cmd_;
rclcpp::Subscription<nav_msgs::msg::Odometry>::SharedPtr sub_odom_;
rclcpp::Subscription<geometry_msgs::msg::PoseWithCovarianceStamped>::SharedPtr sub_amcl_;
rclcpp::Subscription<geometry_msgs::msg::Twist>::SharedPtr sub_cmd_;
rclcpp::Client<nav2_msgs::srv::ClearEntireCostmap>::SharedPtr cli_local_;
rclcpp::Client<nav2_msgs::srv::ClearEntireCostmap>::SharedPtr cli_global_;
rclcpp::TimerBase::SharedPtr timer_;
bool have_pose_ = false;
double last_x_ = 0.0, last_y_ = 0.0;
bool have_cov_ = false;
double last_cov_trace_ = 0.0;
bool goal_set_ = false;
double goal_x_ = 0.0, goal_y_ = 0.0;
std::deque<DistEntry> progress_hist_;
std::deque<CmdEntry> cmd_hist_;
int recovery_attempts_ = 0;
bool in_recovery_ = false;
};
int main(int argc, char ** argv) {
rclcpp::init(argc, argv);
auto node = std::make_shared<RecoverySupervisor>();
rclcpp::spin(node);
rclcpp::shutdown();
return 0;
}
8.4 Java — Supervisor Core (Template)
Chapter14_Lesson4.java
/*
Chapter14_Lesson4.java
Recovery Behaviors and Fault Handling — ROS 2 Java (rcljava) oriented supervisor (teaching skeleton)
Notes:
- rcljava APIs evolve across ROS 2 distros. Treat this as a structured template.
- Core logic: progress window + oscillation score + recovery escalation.
*/
import java.util.ArrayDeque;
import java.util.Deque;
public class Chapter14_Lesson4 {
static class DetParams {
double windowS = 10.0;
double minProgressM = 0.25;
double oscWindowS = 6.0;
int oscSignFlips = 6;
double vEps = 0.03;
int maxRecoveryAttempts = 3;
double spinTimeS = 3.0;
double spinWz = 0.8;
double backupTimeS = 2.0;
double backupVx = -0.12;
}
static class DistEntry {
double t, d;
DistEntry(double t, double d) { this.t = t; this.d = d; }
}
static class CmdEntry {
double t, vx, wz;
CmdEntry(double t, double vx, double wz) { this.t = t; this.vx = vx; this.wz = wz; }
}
static class SupervisorCore {
DetParams p = new DetParams();
Deque<DistEntry> progress = new ArrayDeque<>();
Deque<CmdEntry> cmd = new ArrayDeque<>();
int recoveryAttempts = 0;
void addDistanceSample(double t, double distToGoal) {
progress.addLast(new DistEntry(t, distToGoal));
trim(progress, p.windowS);
}
void addCmdSample(double t, double vx, double wz) {
cmd.addLast(new CmdEntry(t, vx, wz));
trim(cmd, p.oscWindowS);
}
boolean progressOk() {
if (progress.size() < 2) return true;
double d0 = progress.peekFirst().d;
double d1 = progress.peekLast().d;
return (d0 - d1) >= p.minProgressM;
}
boolean oscillating() {
if (cmd.size() < 3) return false;
int flips = 0;
CmdEntry prev = null;
for (CmdEntry e : cmd) {
if (prev != null) {
int sPrev = sgnEps(prev.vx);
int sNow = sgnEps(e.vx);
if (sPrev != 0 && sNow != 0 && sPrev != sNow) flips++;
sPrev = sgnEps(prev.wz);
sNow = sgnEps(e.wz);
if (sPrev != 0 && sNow != 0 && sPrev != sNow) flips++;
}
prev = e;
}
return flips >= p.oscSignFlips;
}
int sgnEps(double v) {
if (Math.abs(v) < p.vEps) return 0;
return (v > 0.0) ? 1 : -1;
}
void trim(Deque<?> q, double windowS) {
if (q.isEmpty()) return;
double tLatest;
Object last = q.peekLast();
if (last instanceof DistEntry) tLatest = ((DistEntry) last).t;
else tLatest = ((CmdEntry) last).t;
double tMin = tLatest - windowS;
while (!q.isEmpty()) {
Object first = q.peekFirst();
double tFirst = (first instanceof DistEntry) ? ((DistEntry) first).t : ((CmdEntry) first).t;
if (tFirst >= tMin) break;
q.removeFirst();
}
}
boolean shouldRecover() {
return (!progressOk()) || oscillating();
}
boolean canContinue() {
return recoveryAttempts < p.maxRecoveryAttempts;
}
void onRecoveryDone() {
recoveryAttempts++;
}
}
public static void main(String[] args) {
// In a real robot, wrap SupervisorCore with ROS2 subscriptions/publishers:
// - subscribe /odom (distance-to-goal computed from pose + goal)
// - subscribe /cmd_vel (oscillation)
// - call costmap clear services (Nav2) and publish temporary cmd_vel for spin/backup
SupervisorCore core = new SupervisorCore();
// Toy stream (stagnation + oscillation)
core.addDistanceSample(0.0, 5.0);
core.addDistanceSample(5.0, 4.95);
core.addDistanceSample(10.0, 4.93);
core.addCmdSample(0.0, 0.10, 0.0);
core.addCmdSample(1.0, -0.10, 0.0);
core.addCmdSample(2.0, 0.10, 0.0);
core.addCmdSample(3.0, -0.10, 0.0);
core.addCmdSample(4.0, 0.10, 0.0);
core.addCmdSample(5.0, -0.10, 0.0);
System.out.println("progressOk=" + core.progressOk());
System.out.println("oscillating=" + core.oscillating());
System.out.println("shouldRecover=" + core.shouldRecover());
}
}
8.5 MATLAB / Simulink — Discrete Supervisor + Auto-Model Stub
Chapter14_Lesson4.m
% Chapter14_Lesson4.m
% Recovery Behaviors and Fault Handling — MATLAB + Simulink-friendly supervisor (teaching example)
%
% This script:
% 1) Simulates a navigation "progress" signal d(t) and cmd signals v(t), w(t)
% 2) Implements a discrete-time fault monitor (no-progress + oscillation)
% 3) Demonstrates a recovery escalation policy
% 4) (Optional) Creates a simple Simulink model programmatically with a MATLAB Function block
%
% NOTE: For full architecture, you typically implement the supervisor in Stateflow.
clear; clc;
%% Parameters
dt = 0.1; % sample time [s]
T = 40.0; % total time [s]
N = round(T/dt);
window_s = 10.0;
min_progress_m = 0.25;
osc_window_s = 6.0;
osc_sign_flips = 6;
v_eps = 0.03;
max_attempts = 3;
spin_time_s = 3.0;
backup_time_s = 2.0;
%% Toy signals (stagnation + oscillatory commands)
t = (0:N-1)*dt;
% distance-to-goal: decreases then stalls
d = 5.0 - 0.08*t;
stall_idx = t > 15;
d(stall_idx) = d(find(~stall_idx,1,'last')) - 0.005*(t(stall_idx)-t(find(~stall_idx,1,'last')));
% cmd signals: start OK, then oscillate after 15s
v = 0.2*ones(size(t));
w = zeros(size(t));
osc_idx = t > 15;
v(osc_idx) = 0.12 * sign(sin(2*pi*0.8*(t(osc_idx)-15))); % flip sign ~1.6 Hz
w(osc_idx) = 0.0;
%% Supervisor state
attempts = 0;
state = "NAVIGATE"; % NAVIGATE, RECOVER_SPIN, RECOVER_BACKUP, SAFE_STOP
state_timer = 0.0;
hist_d = []; % columns: [t, d]
hist_cmd = []; % columns: [t, v, w]
log_state = strings(N,1);
%% Helpers
trim_hist = @(H, win) H(H(:,1) >= (H(end,1)-win), :);
sign_eps = @(x) (abs(x) < v_eps).*0 + (x >= v_eps).*1 + (x <= -v_eps).*(-1);
progress_ok = @(Hd) (size(Hd,1) < 2) || ((Hd(1,2) - Hd(end,2)) >= min_progress_m);
oscillating = @(Hc) ( ...
size(Hc,1) >= 3 && ...
( ...
sum( (sign_eps(Hc(2:end,2)) ~= 0) & (sign_eps(Hc(1:end-1,2)) ~= 0) & ...
(sign_eps(Hc(2:end,2)) ~= sign_eps(Hc(1:end-1,2))) ) + ...
sum( (sign_eps(Hc(2:end,3)) ~= 0) & (sign_eps(Hc(1:end-1,3)) ~= 0) & ...
(sign_eps(Hc(2:end,3)) ~= sign_eps(Hc(1:end-1,3))) ) ...
) >= osc_sign_flips ...
);
%% Main loop
for k = 1:N
% update histories
hist_d = [hist_d; t(k), d(k)];
hist_cmd = [hist_cmd; t(k), v(k), w(k)];
hist_d = trim_hist(hist_d, window_s);
hist_cmd = trim_hist(hist_cmd, osc_window_s);
no_progress = ~progress_ok(hist_d);
osc = oscillating(hist_cmd);
% State machine
switch state
case "NAVIGATE"
if attempts >= max_attempts
state = "SAFE_STOP";
state_timer = 0.0;
elseif no_progress || osc
state = "RECOVER_SPIN";
state_timer = 0.0;
attempts = attempts + 1;
end
case "RECOVER_SPIN"
state_timer = state_timer + dt;
if state_timer >= spin_time_s
state = "RECOVER_BACKUP";
state_timer = 0.0;
end
case "RECOVER_BACKUP"
state_timer = state_timer + dt;
if state_timer >= backup_time_s
state = "NAVIGATE";
state_timer = 0.0;
% after recovery, clear histories to avoid immediate re-trigger
hist_d = hist_d(end,:);
hist_cmd = hist_cmd(end,:);
end
case "SAFE_STOP"
% do nothing; remain stopped
state_timer = state_timer + dt;
end
log_state(k) = state;
end
%% Plot
figure;
subplot(3,1,1); plot(t, d); grid on; ylabel('d(t) [m]'); title('Progress signal');
subplot(3,1,2); plot(t, v); grid on; ylabel('v(t) [m/s]');
subplot(3,1,3); plot(t, double(categorical(log_state))); grid on; ylabel('state'); xlabel('t [s]');
disp("Final state: " + log_state(end) + ", recovery attempts=" + attempts);
%% Optional: create a minimal Simulink model with a MATLAB Function block
% This is intentionally minimal; for production use, use Stateflow.
try
mdl = "Chapter14_Lesson4_Sim";
if bdIsLoaded(mdl), close_system(mdl, 0); end
new_system(mdl); open_system(mdl);
add_block("simulink/Sources/From Workspace", mdl + "/d_in", "VariableName", "d");
add_block("simulink/Sources/From Workspace", mdl + "/v_in", "VariableName", "v");
add_block("simulink/Sources/From Workspace", mdl + "/w_in", "VariableName", "w");
add_block("simulink/User-Defined Functions/MATLAB Function", mdl + "/Supervisor");
add_block("simulink/Sinks/To Workspace", mdl + "/state_out", "VariableName", "state_out");
set_param(mdl + "/Supervisor", "Script", [
"function st = fcn(d,v,w)\n" ...
"% Simple placeholder: output 0=NAV,1=RECOVER,2=STOP\n" ...
"persistent cnt\n" ...
"if isempty(cnt), cnt=0; end\n" ...
"if abs(v) < 0.01\n" ...
" cnt = cnt + 1;\n" ...
"else\n" ...
" cnt = 0;\n" ...
"end\n" ...
"if cnt > 30\n" ...
" st = 1;\n" ...
"else\n" ...
" st = 0;\n" ...
"end\n" ...
"end\n"
]);
add_line(mdl, "d_in/1", "Supervisor/1");
add_line(mdl, "v_in/1", "Supervisor/2");
add_line(mdl, "w_in/1", "Supervisor/3");
add_line(mdl, "Supervisor/1", "state_out/1");
set_param(mdl, "StopTime", num2str(T));
save_system(mdl);
disp("Created Simulink model: " + mdl + " (saved).");
catch ME
disp("Simulink model creation skipped (Simulink may be unavailable):");
disp(ME.message);
end
8.6 Wolfram Mathematica — Safety Invariant Sandbox
Chapter14_Lesson4.nb
(* Chapter14_Lesson4.nb
Recovery Behaviors and Fault Handling — Wolfram Language notebook-style script (text)
Focus: safety invariant for emergency stop under bounded deceleration and latency.
*)
ClearAll[StoppingDistance, SafeStopCondition, MonteCarloCheck];
(* v: current speed (m/s), a: maximum available deceleration (m/s^2), tau: latency (s) *)
StoppingDistance[v_, a_, tau_] := v^2/(2 a) + v tau;
(* If the measured free distance d_free is >= stopping distance + margin, then collision is avoided *)
SafeStopCondition[dFree_, v_, a_, tau_, margin_] := dFree >= StoppingDistance[v, a, tau] + margin;
(* Proof sketch (computational): worst-case distance traveled equals v*tau + v^2/(2a) *)
(* Monte Carlo sanity check *)
MonteCarloCheck[n_Integer : 5000] := Module[
{a = 1.5, tau = 0.2, margin = 0.05, ok, v, dFree},
ok = Table[
v = RandomReal[{0.0, 1.5}];
dFree = RandomReal[{0.0, 3.0}];
{v, dFree, SafeStopCondition[dFree, v, a, tau, margin]},
{n}
];
{Count[ok[[All, 3]], True], Count[ok[[All, 3]], False], ok[[1 ;; 5]]}
];
(* Example values *)
aMax = 1.8; latency = 0.25; margin = 0.10;
v0 = 1.2;
Print["StoppingDistance(v0) = ", N@StoppingDistance[v0, aMax, latency], " m"];
dFree = 1.5;
Print["Safe? ", SafeStopCondition[dFree, v0, aMax, latency, margin]];
Print["MonteCarloCheck: ", MonteCarloCheck[2000]];
(* Optional: visualize stopping distance curve *)
Plot[StoppingDistance[v, aMax, latency], {v, 0, 2.0},
AxesLabel -> {"v (m/s)", "d_stop (m)"},
PlotLabel -> "d_stop(v) = v^2/(2 a_max) + v tau"
]
9. Problems and Solutions
Problem 1 (Progress threshold): With nominal speed \( v_0 \) and window length \( W \), propose a conservative \( \delta_d \) for the guard \( \Delta d_k < \delta_d \).
Solution: Choose \( \delta_d=\eta v_0 W \) with \( \eta\in(0,1) \) accounting for turning/slowdowns (e.g., \( \eta=0.2 \)–\( 0.4 \)).
Problem 2 (Oscillation flips): If \( s(v_k) \) alternates sign each sample for \( N_O \) samples (nonzero), compute \( F_k \). Suggest \( \delta_F \) to ignore short transients.
Solution: There is a flip at each transition, so \( F_k=N_O-1 \). Choose \( \delta_F \approx \lceil 0.6(N_O-1)\rceil \) so only sustained flip patterns trigger recovery.
Problem 3 (Stopping distance derivation): Derive \( d_{\text{stop}}(v)=v\tau + v^2/(2a_{\max}) \) from constant deceleration braking.
Solution: Latency distance is \( v\tau \). Braking distance uses \( t_s=v/a_{\max} \) and \( d_b=\int_0^{t_s}(v-a_{\max}t)dt=v^2/(2a_{\max}) \).
Problem 4 (No-progress false alarm under Gaussian noise): If \( d_k = d_k^{\text{true}}+\nu_k \), \( \nu_k\sim\mathcal{N}(0,\sigma_d^2) \), and true progress over the window is \( \mu>0 \), approximate \( \mathbb{P}(\Delta d_k < \delta_d) \).
Solution: \( \Delta d_k=\mu+(\nu_{k-N_W}-\nu_k) \) with variance \( 2\sigma_d^2 \), so:
\[ \mathbb{P}(\Delta d_k < \delta_d)=\Phi\!\left(\frac{\delta_d-\mu}{\sqrt{2}\,\sigma_d}\right). \]
Problem 5 (Expected recovery time): Each recovery cycle takes \( T_R \), succeeds with probability \( p \), max attempts \( r_{\max} \). Compute expected recovery time before success or stop.
Solution:
\[ \mathbb{E}[T] = T_R \sum_{k=1}^{r_{\max}} k (1-p)^{k-1}p \;+\; T_R\, r_{\max}(1-p)^{r_{\max}}. \]
10. Summary
We designed recovery as a supervisor over navigation: define residuals (progress, oscillation, uncertainty), implement guards, execute a bounded escalation ladder, and enforce a safety-stop invariant via conservative braking distance. Next, in the lab lesson, you will integrate these concepts into a full navigation stack and evaluate robustness.
11. References
- Thrun, S., Burgard, W., & Fox, D. (2005). Probabilistic Robotics. MIT Press.
- Isermann, R. (2005). Model-based fault-detection and diagnosis—status and applications. Annual Reviews in Control, 29(1), 71–85.
- Blanke, M., Kinnaert, M., Lunze, J., & Staroswiecki, M. (2016). Diagnosis and Fault-Tolerant Control (3rd ed.). Springer.
- Koenig, S., & Simmons, R.G. (1998). Xavier: A robot navigation architecture based on POMDP models. Artificial Intelligence, 101(1–2), 1–28.
- Quigley, M., et al. (2009). ROS: an open-source Robot Operating System. ICRA Workshop on Open Source Software.
