In the capstone setting, deployment means moving from an individually tested set of modules (odometry, localization, mapping, planners, controllers) to a coordinated, fault-tolerant runtime stack. The deployed system must satisfy simultaneously: \( \text{safety} \), \( \text{real-time responsiveness} \), \( \text{mission completion} \), and \( \text{recoverability} \).

Let the navigation stack state be \( \mathbf{x}_N(t) \), including robot pose belief, map/costmap state, planner outputs, controller state, and behavior-tree/state-machine status. A compact deployment abstraction is

\[ \mathbf{x}_N(k+1) = \mathbf{F}\!\left(\mathbf{x}_N(k), \mathbf{u}(k), \mathbf{z}(k), \boldsymbol{\pi}, \mathbf{m}\right) \]

where \( \mathbf{u}(k) \) is the command sent to actuators, \( \mathbf{z}(k) \) are sensor observations, \( \boldsymbol{\pi} \) are stack parameters, and \( \mathbf{m} \) denotes mode variables (normal, degraded, recovery, halted). Deployment is successful when admissible parameter and mode sets yield bounded error and bounded latency under expected disturbances.

flowchart TD
  A["Bringup Inputs: sensors, TF, map, robot model"] --> B["Lifecycle Configure"]
  B --> C["Lifecycle Activate"]
  C --> D["Localization + Costmaps Healthy?"]
  D -->|yes| E["Global Planner + \nLocal Planner + \nController"]
  D -->|no| R["Recovery / Reinitialize / Safe Stop"]
  E --> F["Behavior Logic Tick"]
  F --> G["Command Velocity Output"]
  G --> H["Runtime Monitoring and Logs"]
  H --> D
        

By Chapter 20, students already know the constituent algorithms from Chapters 5–19. The deployment problem is now interface alignment and scheduling. We define a practical layered architecture:

• Perception/Localization layer: wheel/IMU fusion, particle/EKF localization, optional map server.
• Map and costmap layer: static map, local obstacle layer, inflation, dynamic updates.
• Global planning layer: computes a path in global coordinates.
• Local planning and control layer: transforms path into feasible velocity commands.
• Behavior and recovery layer: goal execution policy, timeout handling, replanning, clearing costmaps.
• Runtime supervision layer: node lifecycle, watchdogs, logging, diagnostics, and shutdown.

Interface compatibility is a hidden failure source. If localization publishes pose at frequency \( f_{\text{loc} } \) and the local planner consumes pose at \( f_{\text{plan} } \), then stale-state risk increases when \( f_{\text{loc} } < f_{\text{plan} } \) and timestamp jitter is large. A simple freshness condition is

\[ t_k^{\text{plan} } - t_{\text{pose,last} } \le \Delta t_{\max}, \quad \Delta t_{\max} = \eta / f_{\text{ctrl} }, \; 0 < \eta \le 1 \]

with \( f_{\text{ctrl} } \) the control output frequency and \( \eta \) a design factor. This ensures the controller does not use pose estimates older than an acceptable fraction of a control cycle.

A navigation stack is often limited not by algorithm correctness, but by deadline misses. Let the major periodic tasks be indexed by \( i=1,\dots,n \) with worst-case computation time \( C_i \) and period \( T_i \). On a single core, a necessary utilization condition is

\[ U = \sum_{i=1}^n \frac{C_i}{T_i} \le 1. \]

For fixed-priority scheduling, a stronger sufficient condition (Liu–Layland) is

\[ U \le n\left(2^{1/n} - 1\right). \]

In practice, we also reserve slack for communication and OS jitter. A deployment engineering rule is

\[ U_{\text{measured} } \le U_{\text{target} }, \quad U_{\text{target} } \in [0.6, 0.8], \]

so that planner spikes and recovery behaviors do not destabilize loop timing. End-to-end control cycle latency can be decomposed as

\[ T_{\text{cycle} } = T_{\text{sense} } + T_{\text{loc} } + T_{\text{costmap} } + T_{\text{plan,g} } + T_{\text{plan,l} } + T_{\text{ctrl} } + T_{\text{comm} }, \]

and we require \( T_{\text{cycle} } \le T_{\text{deadline} } \). The deployment scripts in this lesson explicitly compute and log these budgets.

Safety deployment is the coupling of geometry, latency, and braking capability. If the robot moves at speed \( v \) and the stack reacts after delay \( T_r \), the minimum stopping distance is

\[ d_{\text{stop} } = vT_r + \frac{v^2}{2a_{\text{br} } }, \quad a_{\text{br} } > 0. \]

A conservative deployment policy requires the inflated obstacle margin to exceed this stopping distance plus a geometric uncertainty term:

\[ d_{\text{infl} } \ge d_{\text{stop} } + r_{\text{footprint} } + \gamma \sigma_{xy}, \quad \gamma \in [2,3]. \]

Here \( \sigma_{xy} \) is the localization standard deviation in the horizontal plane. In occupancy/costmap-based navigation, inflation costs are commonly modeled by an exponential decay:

\[ C(d) = \begin{cases} C_{\text{lethal} }, & d \le r_{\text{inscribed} } \\ C_{\text{inscribed} } \exp\!\left(-\kappa(d-r_{\text{inscribed} })\right), & r_{\text{inscribed} } < d < r_{\text{infl} } \\ 0, & d \ge r_{\text{infl} } \end{cases} \]

with \( \kappa \) tuning obstacle aversion. Increasing \( \kappa \) sharpens decay and can reduce planner conservatism, but too large a value may produce brittle near-obstacle behavior.

Deployment of a local planner (DWA-style or trajectory rollout style) requires tuning a weighted objective over candidate controls. A standard score can be written as

\[ J(\xi) = w_p d_{\text{path} }(\xi) + w_g d_{\text{goal} }(\xi) + w_o \,\bar{C}_{\text{obs} }(\xi) + w_v |v(\xi)-v^*| + w_\omega |\omega(\xi)|, \]

where \( \xi \) is a candidate trajectory rollout. The weights \( w_p,w_g,w_o,w_v,w_\omega \) are not independent: if \( w_o \) is increased without adjusting inflation, the planner may become overly conservative and fail to progress in narrow passages. Conversely, reducing \( w_o \) while increasing the inflation radius can preserve safety while allowing smoother motion.

A useful deployment normalization is to scale each term by an empirical standard deviation, \( \hat{\sigma}_j \), collected during logs:

\[ \tilde{J}(\xi) = \sum_j \tilde{w}_j \frac{\phi_j(\xi)}{\hat{\sigma}_j + \epsilon}, \quad \epsilon > 0. \]

This makes weight magnitudes interpretable and reduces trial-and-error.

Consider the common lateral-heading error subsystem for a path-tracking controller (small-angle regime), with lateral error \( e_y \) and heading error \( e_\psi \):

\[ \dot{e}_y = v e_\psi, \qquad \dot{e}_\psi = -k_y e_y - k_\psi e_\psi, \]

where \( v > 0 \), \( k_y > 0 \), and \( k_\psi > 0 \). Choose the Lyapunov candidate

\[ V(e_y,e_\psi) = \frac{1}{2}k_y e_y^2 + \frac{1}{2}v e_\psi^2. \]

Then along trajectories,

\[ \dot{V} = k_y e_y \dot{e}_y + v e_\psi \dot{e}_\psi = k_y e_y (v e_\psi) + v e_\psi(-k_y e_y - k_\psi e_\psi) = -v k_\psi e_\psi^2 \le 0. \]

Therefore, the equilibrium is Lyapunov stable, and by LaSalle-type reasoning in the linear system setting, the origin is asymptotically stable because the only invariant set satisfying \( \dot{V}=0 \) is \( e_\psi=0 \), which implies \( \dot{e}_\psi = -k_y e_y = 0 \Rightarrow e_y=0 \). This proof is directly relevant to deployment because it shows why controller gains must remain positive and why saturation limits should preserve the local linear regime near the path.

Runtime supervision must detect when the stack leaves its designed operating region. A simple localization confidence gate uses the horizontal covariance submatrix \( \mathbf{P}_{xy} \in \mathbb{R}^{2\times 2} \). Define

\[ s_{\text{loc} } = \operatorname{tr}(\mathbf{P}_{xy}), \]

and require \( s_{\text{loc} } \le s_{\max} \). If violated, the behavior layer switches to a degraded mode (slow-down, relocalization, or stop). For planning deadlines, let \( T_{\text{plan,l} }^{(k)} \) be the runtime of the \( k \)-th local planning cycle. A watchdog trigger can be defined by

\[ \#\left\{i \in [k-N_{\text{miss} }+1,\,k] : T_{\text{plan,l} }^{(i)} > T_{\text{dl} } \right\} = N_{\text{miss} } \;\Rightarrow\; \text{recovery}. \]

In words: if the current window contains \( N_{\text{miss} } \) deadline violations, invoke recovery.

flowchart TD
  A["RUN state"] --> B["Check localization covariance"]
  B -->|too large| C["Degraded mode: slow + relocalize"]
  B -->|ok| D["Check planner deadline misses"]
  D -->|threshold reached| E["Recovery: clear costmap, stop, replan"]
  D -->|ok| F["Continue normal navigation"]
  C --> F
  E --> F
        

A robust capstone deployment procedure should be incremental. The recommended sequence is:

1. Validate static transforms, robot footprint, and sensor timestamp consistency.
2. Bring up localization only; verify pose covariance and drift under stationary and moving tests.
3. Bring up costmaps; validate inflation and obstacle layers independently.
4. Bring up global planner and verify path feasibility (no impossible curvature segments).
5. Bring up local planner and controller at low speed; tune objective weights and acceleration limits.
6. Enable behavior logic and recovery behaviors; inject faults deliberately.
7. Run stress scenarios and record logs (deadline misses, covariance spikes, oscillations, stop distance margins).

A practical mission acceptance criterion can be written as a conjunction:

\[ \mathcal{A} = \left(T_{\text{mission} } \le T_{\max}\right) \wedge \left(N_{\text{coll} } = 0\right) \wedge \left(\max_k s_{\text{loc} }(k) \le s_{\max}\right) \wedge \left(\rho_{\text{dl-miss} } \le \rho_{\max}\right), \]

where \( \rho_{\text{dl-miss} } \) is the observed deadline-miss ratio.

The following Python implementation provides an educational deployment supervisor that models lifecycle startup, timing-budget verification, costmap inflation, local planner scoring, and runtime health monitoring.

Code: Chapter20_Lesson4.py

"""
Chapter20_Lesson4.py
Navigation Stack Deployment (Capstone AMR)
Educational deployment supervisor + timing monitor + local scoring example.
"""
from __future__ import annotations
from dataclasses import dataclass, field
from enum import Enum, auto
from typing import Dict, List, Tuple
import math
import time
import random


class NodeState(Enum):
    UNCONFIGURED = auto()
    INACTIVE = auto()
    ACTIVE = auto()
    ERROR = auto()


@dataclass
class ManagedNode:
    name: str
    startup_ms: float
    state: NodeState = NodeState.UNCONFIGURED

    def configure(self) -> bool:
        time.sleep(self.startup_ms / 1000.0)
        self.state = NodeState.INACTIVE
        return True

    def activate(self) -> bool:
        if self.state != NodeState.INACTIVE:
            self.state = NodeState.ERROR
            return False
        self.state = NodeState.ACTIVE
        return True

    def deactivate(self) -> bool:
        if self.state == NodeState.ACTIVE:
            self.state = NodeState.INACTIVE
        return True

    def reset(self) -> bool:
        self.state = NodeState.UNCONFIGURED
        return True


@dataclass
class TimingBudget:
    period_ms: float
    wcet_ms: Dict[str, float]

    def utilization(self) -> float:
        return sum(v / self.period_ms for v in self.wcet_ms.values())

    def is_schedulable(self) -> bool:
        # Simplified single-core utilization check
        return self.utilization() <= 1.0

    def report(self) -> str:
        total = sum(self.wcet_ms.values())
        return (
            f"Period={self.period_ms:.1f} ms | Total WCET={total:.1f} ms | "
            f"Utilization={self.utilization():.3f} | "
            f"Schedulable={self.is_schedulable()}"
        )


@dataclass
class CostmapParams:
    inscribed_radius: float = 0.22
    inflation_radius: float = 0.70
    cost_lethal: int = 254
    cost_inscribed: int = 220
    kappa: float = 8.0  # exponential decay rate

    def inflated_cost(self, d: float) -> int:
        """Distance d from obstacle center to robot center."""
        if d <= self.inscribed_radius:
            return self.cost_lethal
        if d >= self.inflation_radius:
            return 0
        val = self.cost_inscribed * math.exp(-self.kappa * (d - self.inscribed_radius))
        return int(max(1, min(self.cost_inscribed, round(val))))


@dataclass
class LocalPlannerWeights:
    w_path: float = 1.4
    w_goal: float = 1.0
    w_obstacle: float = 2.6
    w_velocity: float = 0.4
    w_spin: float = 0.2


def score_trajectory(
    dist_to_path: float,
    dist_to_goal: float,
    max_obstacle_cost: int,
    v: float,
    omega: float,
    v_ref: float,
    weights: LocalPlannerWeights
) -> float:
    """Lower score is better."""
    obstacle_term = max_obstacle_cost / 254.0
    return (
        weights.w_path * dist_to_path
        + weights.w_goal * dist_to_goal
        + weights.w_obstacle * obstacle_term
        + weights.w_velocity * abs(v - v_ref)
        + weights.w_spin * abs(omega)
    )


@dataclass
class DeploymentSupervisor:
    nodes: List[ManagedNode]
    timing: TimingBudget
    localization_cov_threshold: float = 0.20 ** 2   # m^2
    max_consecutive_planner_misses: int = 3
    planner_deadline_ms: float = 80.0

    planner_miss_count: int = 0
    active: bool = False
    mission_log: List[str] = field(default_factory=list)

    def startup(self) -> bool:
        self.mission_log.append("Startup sequence begin")
        if not self.timing.is_schedulable():
            self.mission_log.append("Timing budget failed")
            return False

        for node in self.nodes:
            if not node.configure():
                self.mission_log.append(f"Configure failed: {node.name}")
                return False

        for node in self.nodes:
            if not node.activate():
                self.mission_log.append(f"Activate failed: {node.name}")
                return False

        self.active = True
        self.mission_log.append("Startup sequence complete")
        return True

    def check_localization_health(self, cov_xx: float, cov_yy: float) -> bool:
        trace_xy = cov_xx + cov_yy
        ok = trace_xy <= self.localization_cov_threshold
        self.mission_log.append(
            f"Localization trace={trace_xy:.4f} m^2 -> {'OK' if ok else 'RECOVERY'}"
        )
        return ok

    def monitor_planner_deadline(self, planner_runtime_ms: float) -> bool:
        if planner_runtime_ms > self.planner_deadline_ms:
            self.planner_miss_count += 1
            self.mission_log.append(
                f"Planner deadline miss ({planner_runtime_ms:.1f} ms), count={self.planner_miss_count}"
            )
        else:
            self.planner_miss_count = 0
            self.mission_log.append(
                f"Planner runtime OK ({planner_runtime_ms:.1f} ms)"
            )

        if self.planner_miss_count >= self.max_consecutive_planner_misses:
            self.mission_log.append("Trigger recovery: clear local costmap + stop")
            self.planner_miss_count = 0
            return False
        return True

    def graceful_shutdown(self) -> None:
        self.mission_log.append("Shutdown begin")
        for node in reversed(self.nodes):
            node.deactivate()
            node.reset()
        self.active = False
        self.mission_log.append("Shutdown complete")


def simulate_deployment():
    nodes = [
        ManagedNode("map_server", 20),
        ManagedNode("amcl_or_ekf", 25),
        ManagedNode("global_planner", 15),
        ManagedNode("local_planner", 12),
        ManagedNode("controller_server", 10),
        ManagedNode("bt_navigator", 18),
    ]

    timing = TimingBudget(
        period_ms=100.0,
        wcet_ms={
            "sensor_preprocess": 8.0,
            "localization": 14.0,
            "costmap_update": 18.0,
            "global_planner": 12.0,
            "local_planner": 22.0,
            "controller": 5.0,
            "bt_tick": 3.0,
        },
    )

    sup = DeploymentSupervisor(nodes=nodes, timing=timing)
    print(timing.report())

    if not sup.startup():
        print("Deployment startup failed")
        print("\n".join(sup.mission_log))
        return

    # Example costmap inflation profile
    infl = CostmapParams()
    distances = [0.18, 0.25, 0.35, 0.50, 0.80]
    print("Inflation costs:", {d: infl.inflated_cost(d) for d in distances})

    # Example trajectory scoring
    w = LocalPlannerWeights()
    candidates = [
        (0.05, 1.20, 140, 0.40, 0.10),
        (0.18, 0.95,  80, 0.55, 0.35),
        (0.10, 1.00, 220, 0.50, 0.05),
    ]
    scored: List[Tuple[int, float]] = []
    for idx, c in enumerate(candidates):
        s = score_trajectory(*c, v_ref=0.50, weights=w)
        scored.append((idx, s))
    print("Candidate scores:", scored, "best=", min(scored, key=lambda x: x[1]))

    # Simulated runtime monitoring
    random.seed(4)
    for step in range(12):
        # Fake localization covariance (grows under degraded sensing)
        cov_xx = 0.006 + 0.002 * math.sin(step)
        cov_yy = 0.005 + 0.002 * math.cos(step)
        if step in (8, 9):
            cov_xx += 0.03
            cov_yy += 0.03
        if not sup.check_localization_health(cov_xx, cov_yy):
            sup.mission_log.append("Recovery action: relocalize / slow down")

        runtime_ms = 55 + 40 * random.random()
        sup.monitor_planner_deadline(runtime_ms)

    sup.graceful_shutdown()
    print("\n--- Mission Log ---")
    print("\n".join(sup.mission_log))


if __name__ == "__main__":
    simulate_deployment()

      

The C++ implementation mirrors the Python deployment logic using strongly typed structs and a simple supervisor. It is suitable as a foundation for integrating with production AMR middleware.

Code: Chapter20_Lesson4.cpp

// Chapter20_Lesson4.cpp
// Navigation Stack Deployment (Capstone AMR)
// C++17 educational deployment manager + watchdog + inflation utility.
#include <algorithm>
#include <chrono>
#include <cmath>
#include <cstdint>
#include <iomanip>
#include <iostream>
#include <numeric>
#include <string>
#include <thread>
#include <unordered_map>
#include <vector>

enum class NodeState { Unconfigured, Inactive, Active, Error };

struct ManagedNode {
    std::string name;
    double startup_ms;
    NodeState state{NodeState::Unconfigured};

    bool configure() {
        std::this_thread::sleep_for(std::chrono::milliseconds(static_cast<int>(startup_ms)));
        state = NodeState::Inactive;
        return true;
    }
    bool activate() {
        if (state != NodeState::Inactive) {
            state = NodeState::Error;
            return false;
        }
        state = NodeState::Active;
        return true;
    }
    void deactivate() {
        if (state == NodeState::Active) state = NodeState::Inactive;
    }
    void reset() { state = NodeState::Unconfigured; }
};

struct TimingBudget {
    double period_ms{100.0};
    std::unordered_map<std::string, double> wcet_ms;

    double utilization() const {
        double sum = 0.0;
        for (const auto& kv : wcet_ms) sum += kv.second / period_ms;
        return sum;
    }

    bool schedulable() const { return utilization() <= 1.0; }

    void printReport() const {
        double total = 0.0;
        for (const auto& kv : wcet_ms) total += kv.second;
        std::cout << "Period=" << period_ms << " ms, Total WCET=" << total
                  << " ms, U=" << std::fixed << std::setprecision(3)
                  << utilization() << ", schedulable=" << std::boolalpha
                  << schedulable() << "\n";
    }
};

struct CostmapInflation {
    double r_ins{0.22};
    double r_inf{0.70};
    int c_lethal{254};
    int c_ins{220};
    double kappa{8.0};

    int cost(double d) const {
        if (d <= r_ins) return c_lethal;
        if (d >= r_inf) return 0;
        double v = c_ins * std::exp(-kappa * (d - r_ins));
        v = std::clamp(v, 1.0, static_cast<double>(c_ins));
        return static_cast<int>(std::round(v));
    }
};

struct TrajectoryScoreInput {
    double dist_to_path;
    double dist_to_goal;
    int obstacle_cost;
    double v;
    double omega;
};

struct PlannerWeights {
    double w_path{1.4};
    double w_goal{1.0};
    double w_obs{2.6};
    double w_vel{0.4};
    double w_spin{0.2};
};

double scoreTrajectory(const TrajectoryScoreInput& x, double v_ref, const PlannerWeights& w) {
    double obs_term = static_cast<double>(x.obstacle_cost) / 254.0;
    return w.w_path * x.dist_to_path +
           w.w_goal * x.dist_to_goal +
           w.w_obs * obs_term +
           w.w_vel * std::abs(x.v - v_ref) +
           w.w_spin * std::abs(x.omega);
}

class DeploymentSupervisor {
public:
    DeploymentSupervisor(std::vector<ManagedNode> nodes, TimingBudget timing)
        : nodes_(std::move(nodes)), timing_(std::move(timing)) {}

    bool startup() {
        if (!timing_.schedulable()) {
            std::cout << "[startup] timing budget invalid\n";
            return false;
        }
        for (auto& n : nodes_) {
            if (!n.configure()) return false;
        }
        for (auto& n : nodes_) {
            if (!n.activate()) return false;
        }
        active_ = true;
        std::cout << "[startup] all nodes active\n";
        return true;
    }

    bool localizationHealthy(double cov_xx, double cov_yy) const {
        double trace_xy = cov_xx + cov_yy;
        return trace_xy <= cov_trace_limit_;
    }

    bool plannerDeadlineCheck(double runtime_ms) {
        if (runtime_ms > planner_deadline_ms_) {
            planner_miss_count_++;
        } else {
            planner_miss_count_ = 0;
        }
        if (planner_miss_count_ >= max_misses_) {
            std::cout << "[recovery] planner misses threshold reached\n";
            planner_miss_count_ = 0;
            return false;
        }
        return true;
    }

    void shutdown() {
        for (auto it = nodes_.rbegin(); it != nodes_.rend(); ++it) {
            it->deactivate();
            it->reset();
        }
        active_ = false;
        std::cout << "[shutdown] nodes reset\n";
    }

private:
    std::vector<ManagedNode> nodes_;
    TimingBudget timing_;
    bool active_{false};
    double cov_trace_limit_{0.20 * 0.20};
    int planner_miss_count_{0};
    int max_misses_{3};
    double planner_deadline_ms_{80.0};
};

int main() {
    TimingBudget budget;
    budget.period_ms = 100.0;
    budget.wcet_ms = {
        {"sensor_preprocess", 8.0},
        {"localization", 14.0},
        {"costmap_update", 18.0},
        {"global_planner", 12.0},
        {"local_planner", 22.0},
        {"controller", 5.0},
        {"bt_tick", 3.0}
    };
    budget.printReport();

    std::vector<ManagedNode> nodes = {
        {"map_server", 20.0},
        {"localization", 25.0},
        {"global_planner", 15.0},
        {"local_planner", 12.0},
        {"controller_server", 10.0},
        {"bt_navigator", 18.0}
    };

    DeploymentSupervisor sup(nodes, budget);
    if (!sup.startup()) return 1;

    CostmapInflation infl;
    std::cout << "Inflation profile:\n";
    for (double d : {0.18, 0.25, 0.35, 0.50, 0.80}) {
        std::cout << "d=" << d << " -> " << infl.cost(d) << "\n";
    }

    PlannerWeights w;
    std::vector<TrajectoryScoreInput> candidates = {
        {0.05, 1.20, 140, 0.40, 0.10},
        {0.18, 0.95,  80, 0.55, 0.35},
        {0.10, 1.00, 220, 0.50, 0.05}
    };
    int best_idx = -1;
    double best_score = 1e9;
    for (int i = 0; i < static_cast<int>(candidates.size()); ++i) {
        double s = scoreTrajectory(candidates[i], 0.50, w);
        std::cout << "Candidate " << i << " score=" << s << "\n";
        if (s < best_score) { best_score = s; best_idx = i; }
    }
    std::cout << "Best candidate=" << best_idx << "\n";

    std::vector<double> planner_runtimes = {60, 72, 91, 88, 95, 65, 77, 83, 90};
    std::vector<std::pair<double,double>> covariances = {
        {0.006,0.007}, {0.007,0.006}, {0.008,0.005},
        {0.010,0.009}, {0.012,0.011}, {0.035,0.034}, // degraded localization
        {0.009,0.007}, {0.008,0.008}, {0.007,0.006}
    };

    for (std::size_t k = 0; k < planner_runtimes.size(); ++k) {
        bool loc_ok = sup.localizationHealthy(covariances[k].first, covariances[k].second);
        if (!loc_ok) {
            std::cout << "[recovery] localization covariance too large, slow/relocalize\n";
        }
        (void) sup.plannerDeadlineCheck(planner_runtimes[k]);
    }

    sup.shutdown();
    return 0;
}

      

The Java version is useful for students deploying AMR logic in JVM-based industrial software stacks or simulation backends. It preserves the same timing and recovery logic.

Code: Chapter20_Lesson4.java

// Chapter20_Lesson4.java
// Navigation Stack Deployment (Capstone AMR)
// Java educational deployment manager and monitoring logic.
import java.util.*;

public class Chapter20_Lesson4 {

    enum NodeState { UNCONFIGURED, INACTIVE, ACTIVE, ERROR }

    static class ManagedNode {
        String name;
        double startupMs;
        NodeState state = NodeState.UNCONFIGURED;

        ManagedNode(String name, double startupMs) {
            this.name = name;
            this.startupMs = startupMs;
        }

        boolean configure() {
            state = NodeState.INACTIVE;
            return true;
        }

        boolean activate() {
            if (state != NodeState.INACTIVE) {
                state = NodeState.ERROR;
                return false;
            }
            state = NodeState.ACTIVE;
            return true;
        }

        void deactivate() {
            if (state == NodeState.ACTIVE) state = NodeState.INACTIVE;
        }

        void reset() { state = NodeState.UNCONFIGURED; }
    }

    static class TimingBudget {
        double periodMs;
        Map<String, Double> wcetMs = new LinkedHashMap<>();

        TimingBudget(double periodMs) { this.periodMs = periodMs; }

        double utilization() {
            double u = 0.0;
            for (double c : wcetMs.values()) u += c / periodMs;
            return u;
        }

        boolean isSchedulable() { return utilization() <= 1.0; }
    }

    static class DeploymentSupervisor {
        List<ManagedNode> nodes;
        TimingBudget timing;
        double covTraceLimit = 0.20 * 0.20;
        int plannerMissCount = 0;
        int maxConsecutiveMisses = 3;
        double plannerDeadlineMs = 80.0;

        DeploymentSupervisor(List<ManagedNode> nodes, TimingBudget timing) {
            this.nodes = nodes;
            this.timing = timing;
        }

        boolean startup() {
            if (!timing.isSchedulable()) {
                System.out.println("Timing budget invalid");
                return false;
            }
            for (ManagedNode n : nodes) if (!n.configure()) return false;
            for (ManagedNode n : nodes) if (!n.activate()) return false;
            System.out.println("Startup complete");
            return true;
        }

        boolean checkLocalizationHealth(double covXX, double covYY) {
            double traceXY = covXX + covYY;
            boolean ok = traceXY <= covTraceLimit;
            System.out.printf(Locale.US, "Localization trace=%.4f => %s%n", traceXY, ok ? "OK" : "RECOVERY");
            return ok;
        }

        boolean monitorPlannerDeadline(double runtimeMs) {
            if (runtimeMs > plannerDeadlineMs) plannerMissCount++;
            else plannerMissCount = 0;

            if (plannerMissCount >= maxConsecutiveMisses) {
                System.out.println("Recovery trigger: clear costmap and stop");
                plannerMissCount = 0;
                return false;
            }
            return true;
        }

        void shutdown() {
            ListIterator<ManagedNode> it = nodes.listIterator(nodes.size());
            while (it.hasPrevious()) {
                ManagedNode n = it.previous();
                n.deactivate();
                n.reset();
            }
            System.out.println("Shutdown complete");
        }
    }

    static int inflatedCost(double d, double rIns, double rInf, int cLethal, int cIns, double kappa) {
        if (d <= rIns) return cLethal;
        if (d >= rInf) return 0;
        double v = cIns * Math.exp(-kappa * (d - rIns));
        v = Math.max(1.0, Math.min(cIns, v));
        return (int) Math.round(v);
    }

    static double scoreTrajectory(
        double distPath, double distGoal, int obsCost, double v, double omega, double vRef,
        double wPath, double wGoal, double wObs, double wVel, double wSpin
    ) {
        double obsTerm = ((double) obsCost) / 254.0;
        return wPath * distPath + wGoal * distGoal + wObs * obsTerm + wVel * Math.abs(v - vRef) + wSpin * Math.abs(omega);
    }

    public static void main(String[] args) {
        TimingBudget budget = new TimingBudget(100.0);
        budget.wcetMs.put("sensor_preprocess", 8.0);
        budget.wcetMs.put("localization", 14.0);
        budget.wcetMs.put("costmap_update", 18.0);
        budget.wcetMs.put("global_planner", 12.0);
        budget.wcetMs.put("local_planner", 22.0);
        budget.wcetMs.put("controller", 5.0);
        budget.wcetMs.put("bt_tick", 3.0);

        System.out.printf(Locale.US, "Utilization = %.3f, schedulable=%s%n", budget.utilization(), budget.isSchedulable());

        List<ManagedNode> nodes = Arrays.asList(
            new ManagedNode("map_server", 20),
            new ManagedNode("localization", 25),
            new ManagedNode("global_planner", 15),
            new ManagedNode("local_planner", 12),
            new ManagedNode("controller_server", 10),
            new ManagedNode("bt_navigator", 18)
        );

        DeploymentSupervisor sup = new DeploymentSupervisor(nodes, budget);
        if (!sup.startup()) return;

        double[] dSamples = {0.18, 0.25, 0.35, 0.50, 0.80};
        for (double d : dSamples) {
            System.out.println("inflation cost(" + d + ") = " + inflatedCost(d, 0.22, 0.70, 254, 220, 8.0));
        }

        double[][] candidates = {
            {0.05, 1.20, 140, 0.40, 0.10},
            {0.18, 0.95,  80, 0.55, 0.35},
            {0.10, 1.00, 220, 0.50, 0.05}
        };
        int bestIdx = -1;
        double bestScore = Double.POSITIVE_INFINITY;
        for (int i = 0; i < candidates.length; i++) {
            double s = scoreTrajectory(
                candidates[i][0], candidates[i][1], (int) candidates[i][2], candidates[i][3], candidates[i][4], 0.50,
                1.4, 1.0, 2.6, 0.4, 0.2
            );
            System.out.printf(Locale.US, "Candidate %d score = %.4f%n", i, s);
            if (s < bestScore) { bestScore = s; bestIdx = i; }
        }
        System.out.println("Best candidate = " + bestIdx);

        double[][] covs = {
            {0.006, 0.007}, {0.007, 0.006}, {0.008, 0.005},
            {0.010, 0.009}, {0.012, 0.011}, {0.035, 0.034},
            {0.009, 0.007}, {0.008, 0.008}, {0.007, 0.006}
        };
        double[] runtimes = {60, 72, 91, 88, 95, 65, 77, 83, 90};

        for (int k = 0; k < runtimes.length; k++) {
            if (!sup.checkLocalizationHealth(covs[k][0], covs[k][1])) {
                System.out.println("Action: reduce speed and relocalize");
            }
            sup.monitorPlannerDeadline(runtimes[k]);
        }

        sup.shutdown();
    }
}

      

The MATLAB script computes a deployment timing budget, plots the inflation profile, and simulates a small-angle tracking subsystem. It is directly compatible with a Simulink/Stateflow deployment workflow.

Code: Chapter20_Lesson4.m

% Chapter20_Lesson4.m
% Navigation Stack Deployment (Capstone AMR)
% MATLAB/Simulink-oriented deployment analysis: timing budget, inflation, and tracking stability.
clear; clc;

%% 1) Timing budget (single-core utilization check)
period_ms = 100;
tasks = struct( ...
    'sensor_preprocess', 8, ...
    'localization', 14, ...
    'costmap_update', 18, ...
    'global_planner', 12, ...
    'local_planner', 22, ...
    'controller', 5, ...
    'bt_tick', 3);

wcet_values = struct2array(tasks);
U = sum(wcet_values / period_ms);
fprintf('Period = %.1f ms, Utilization = %.3f, schedulable = %d\n', period_ms, U, U <= 1.0);

%% 2) Costmap inflation profile
r_ins = 0.22;
r_inf = 0.70;
c_lethal = 254;
c_ins = 220;
kappa = 8.0;

d = linspace(0, 1.0, 200);
C = zeros(size(d));
for i = 1:numel(d)
    if d(i) <= r_ins
        C(i) = c_lethal;
    elseif d(i) >= r_inf
        C(i) = 0;
    else
        C(i) = c_ins * exp(-kappa * (d(i) - r_ins));
    end
end

figure('Name', 'Chapter20_Lesson4 Inflation Profile');
plot(d, C, 'LineWidth', 2); grid on;
xlabel('Distance to obstacle center (m)');
ylabel('Inflation cost');
title('Inflation Decay for Local Costmap');

%% 3) Simple path-tracking error dynamics (small-angle approximation)
% e_y_dot = v * e_psi
% e_psi_dot = -k_y * e_y - k_psi * e_psi
v = 0.6; 
k_y = 1.8; 
k_psi = 2.4;

A = [0, v; -k_y, -k_psi];
eigA = eig(A);
disp('Closed-loop eigenvalues for lateral-heading subsystem:');
disp(eigA);

% Simulate error convergence
dt = 0.01;
T = 8;
t = 0:dt:T;
x = zeros(2, numel(t));
x(:,1) = [0.35; 0.25]; % [e_y; e_psi]

for k = 1:numel(t)-1
    xdot = A * x(:,k);
    x(:,k+1) = x(:,k) + dt * xdot;
end

V = 0.5 * (k_y * x(1,:).^2 + v * x(2,:).^2);

figure('Name', 'Chapter20_Lesson4 Tracking Error');
plot(t, x(1,:), 'LineWidth', 2); hold on;
plot(t, x(2,:), 'LineWidth', 2); grid on;
xlabel('Time (s)');
ylabel('Error');
legend('e_y', 'e_\psi');
title('Lateral and Heading Error Convergence');

figure('Name', 'Chapter20_Lesson4 Lyapunov');
plot(t, V, 'LineWidth', 2); grid on;
xlabel('Time (s)');
ylabel('V(e)');
title('Lyapunov Function Decrease');

%% 4) Recovery trigger logic (suitable for Stateflow/Simulink logic implementation)
planner_runtimes_ms = [60 72 91 88 95 65 77 83 90];
max_consecutive_miss = 3;
deadline_ms = 80;
miss_count = 0;

for k = 1:numel(planner_runtimes_ms)
    if planner_runtimes_ms(k) > deadline_ms
        miss_count = miss_count + 1;
    else
        miss_count = 0;
    end

    if miss_count >= max_consecutive_miss
        fprintf('Step %d: Recovery -> clear local costmap + stop + retry\n', k);
        miss_count = 0;
    else
        fprintf('Step %d: OK (runtime %.1f ms, miss_count=%d)\n', k, planner_runtimes_ms(k), miss_count);
    end
end

%% 5) Simulink deployment note
% Recommended block partition:
% [Sensor Fusion] -> [Localization] -> [Global Planner] -> [Local Planner] -> [Controller]
% Add a Stateflow chart for lifecycle and recovery states:
%   INIT -> CONFIGURE -> ACTIVATE -> RUN -> RECOVERY -> RUN -> SHUTDOWN

      

The Wolfram Mathematica code evaluates timing utilization, inflation cost, local trajectory scores, and a linearized tracking stability example. The file is provided as a notebook-style source.

Code: Chapter20_Lesson4.nb

(* Chapter20_Lesson4.nb *)
(* Navigation Stack Deployment (Capstone AMR) *)
(* Wolfram Language notebook-style script for timing budget, inflation cost, and trajectory score. *)

ClearAll["Global`*"];

(* 1) Timing budget *)
periodMs = 100.0;
wcet = <|
  "sensor_preprocess" -> 8.0,
  "localization" -> 14.0,
  "costmap_update" -> 18.0,
  "global_planner" -> 12.0,
  "local_planner" -> 22.0,
  "controller" -> 5.0,
  "bt_tick" -> 3.0
|>;

utilization = Total[Values[wcet]]/periodMs;
Print["Utilization = ", NumberForm[utilization, {4, 3}], 
      ", schedulable = ", utilization <= 1.0];

(* 2) Inflation cost model *)
rIns = 0.22; rInf = 0.70; cLethal = 254; cIns = 220; kappa = 8.0;

InflationCost[d_] := Piecewise[{
   {cLethal, d <= rIns},
   {0, d >= rInf}
  },
  Round[Clip[cIns Exp[-kappa (d - rIns)], {1, cIns}]]
];

inflationTable = Table[{d, InflationCost[d]}, {d, {0.18, 0.25, 0.35, 0.50, 0.80} }];
Print["Inflation samples: ", inflationTable];

Plot[Evaluate[Piecewise[{
  {cLethal, x <= rIns},
  {0, x >= rInf}
  },
  cIns Exp[-kappa (x - rIns)]
  ]], {x, 0, 1.0},
  PlotRange -> All,
  AxesLabel -> {"d (m)", "cost"},
  PlotLabel -> "Inflation Decay"];

(* 3) Local trajectory score *)
weights = <|"wPath" -> 1.4, "wGoal" -> 1.0, "wObs" -> 2.6, "wVel" -> 0.4, "wSpin" -> 0.2|>;

ScoreTrajectory[distPath_, distGoal_, obsCost_, v_, omega_, vRef_: 0.50] := Module[
  {obsTerm},
  obsTerm = N[obsCost]/254.0;
  weights["wPath"] distPath +
  weights["wGoal"] distGoal +
  weights["wObs"] obsTerm +
  weights["wVel"] Abs[v - vRef] +
  weights["wSpin"] Abs[omega]
];

candidates = {
  <|"distPath" -> 0.05, "distGoal" -> 1.20, "obsCost" -> 140, "v" -> 0.40, "omega" -> 0.10|>,
  <|"distPath" -> 0.18, "distGoal" -> 0.95, "obsCost" -> 80,  "v" -> 0.55, "omega" -> 0.35|>,
  <|"distPath" -> 0.10, "distGoal" -> 1.00, "obsCost" -> 220, "v" -> 0.50, "omega" -> 0.05|>
};

scored = Table[
  {i, ScoreTrajectory[candidates[[i, "distPath"]], candidates[[i, "distGoal"]], 
                      candidates[[i, "obsCost"]], candidates[[i, "v"]], candidates[[i, "omega"]]]},
  {i, Length[candidates]}
];

best = First @ MinimalBy[scored, Last];
Print["Scores = ", scored];
Print["Best candidate index = ", best[[1]]];

(* 4) Small-angle lateral-heading error dynamics and Lyapunov check *)
v = 0.6; ky = 1.8; kpsi = 2.4;
A = { {0, v}, {-ky, -kpsi} };
evals = Eigenvalues[A];
Print["Eigenvalues(A) = ", evals];

x0 = {0.35, 0.25};
sol = NDSolveValue[
  {
    x1'[t] == A[[1]].{x1[t], x2[t]},
    x2'[t] == A[[2]].{x1[t], x2[t]},
    x1[0] == x0[[1]],
    x2[0] == x0[[2]]
  },
  {x1, x2}, {t, 0, 8}
];

V[t_] := 1/2 (ky (sol[[1]][t])^2 + v (sol[[2]][t])^2);

Plot[{sol[[1]][t], sol[[2]][t]}, {t, 0, 8},
  PlotRange -> All,
  PlotLegends -> {"e_y", "e_psi"},
  AxesLabel -> {"t (s)", "error"},
  PlotLabel -> "Tracking Error Convergence"];

Plot[V[t], {t, 0, 8},
  PlotRange -> All,
  AxesLabel -> {"t (s)", "V"},
  PlotLabel -> "Lyapunov Decrease"];

      

Problem 1 (Timing Budget Feasibility): A stack has periodic tasks with \( (C_i, T_i) \) in milliseconds: \( (8,100),(14,100),(18,100),(12,100),(22,100),(5,100),(3,100) \). Compute the utilization and determine whether the simple single-core bound is satisfied.

Solution: The total computation time per 100 ms cycle is \( 8+14+18+12+22+5+3 = 82 \) ms. Therefore

\[ U = \frac{82}{100} = 0.82. \]

The necessary bound \( U \le 1 \) is satisfied, so the task set may be schedulable. However, it is too close to the limit for comfortable deployment with jitter, logging, and recovery actions.

Problem 2 (Inflation Radius from Stopping Constraint): Let \( v=0.7 \) m/s, \( T_r=0.18 \) s, \( a_{\text{br} }=1.2 \) m/s², \( r_{\text{footprint} }=0.24 \) m, \( \sigma_{xy}=0.05 \) m, and \( \gamma=3 \). Compute a conservative lower bound on \( d_{\text{infl} } \).

Solution: First compute stopping distance:

\[ d_{\text{stop} } = vT_r + \frac{v^2}{2a_{\text{br} } } = (0.7)(0.18) + \frac{0.49}{2(1.2)} = 0.126 + 0.2042 \approx 0.3302 \text{ m}. \]

Then add geometric and uncertainty margins:

\[ d_{\text{infl} } \ge 0.3302 + 0.24 + 3(0.05) = 0.7202 \text{ m}. \]

A practical deployment choice is \( d_{\text{infl} } \approx 0.75 \) m.

Problem 3 (Lyapunov Derivative): For the error dynamics \( \dot{e}_y = v e_\psi \), \( \dot{e}_\psi = -k_y e_y - k_\psi e_\psi \), prove that \( V = \frac{1}{2}k_y e_y^2 + \frac{1}{2}v e_\psi^2 \) is nonincreasing.

Solution: Differentiate:

\[ \dot{V} = k_y e_y \dot{e}_y + v e_\psi \dot{e}_\psi = k_y e_y (v e_\psi) + v e_\psi(-k_y e_y - k_\psi e_\psi) = -vk_\psi e_\psi^2 \le 0. \]

Since \( v > 0 \) and \( k_\psi > 0 \), the derivative is always nonpositive. Hence the energy-like function never increases.

Problem 4 (Deadline Watchdog Design): Suppose the local planner deadline is \( T_{\text{dl} }=80 \) ms. A runtime trace in ms is \( 61, 86, 90, 84, 70, 79, 95 \). If recovery is triggered after \( N_{\text{miss} }=3 \) consecutive misses, determine whether recovery occurs.

Solution: Compare each sample to 80 ms. Misses occur at \( 86, 90, 84 \), which are three consecutive misses. Therefore recovery is triggered immediately after the third miss (the sample \( 84 \) ms). The later values are handled after recovery.

Problem 5 (Normalized Planner Score): Two trajectories have term values \( \phi(\xi_1)=[0.10,1.0,0.6,0.1,0.1] \) and \( \phi(\xi_2)=[0.18,0.8,0.2,0.05,0.3] \), corresponding to path, goal, obstacle, velocity, and spin terms. Let \( \hat{\sigma}=[0.1,0.5,0.2,0.1,0.2] \), \( \tilde{w}=[1,1,2,0.5,0.2] \), and \( \epsilon=0 \). Compute \( \tilde{J} \) for each and choose the better trajectory.

Solution: For \( \xi_1 \),

\[ \tilde{J}(\xi_1) = 1\cdot\frac{0.10}{0.10} + 1\cdot\frac{1.0}{0.5} + 2\cdot\frac{0.6}{0.2} + 0.5\cdot\frac{0.1}{0.1} + 0.2\cdot\frac{0.1}{0.2} = 1 + 2 + 6 + 0.5 + 0.1 = 9.6. \]

For \( \xi_2 \),

\[ \tilde{J}(\xi_2) = 1\cdot\frac{0.18}{0.10} + 1\cdot\frac{0.8}{0.5} + 2\cdot\frac{0.2}{0.2} + 0.5\cdot\frac{0.05}{0.1} + 0.2\cdot\frac{0.3}{0.2} = \\ 1.8 + 1.6 + 2 + 0.25 + 0.3 = 5.95. \]

Thus \( \xi_2 \) is preferred under the normalized objective.

Navigation stack deployment is a systems-integration problem, not only an algorithm-selection problem. In this lesson we formalized timing budgets, inflation safety margins, planner score tuning, controller stability, and runtime recovery logic, and we implemented these ideas in Python, C++, Java, MATLAB/Simulink-oriented code, and Wolfram Mathematica. These artifacts directly support the final capstone demonstration and the research-style writeup in the next lesson.