In Chapter 14 Lesson 1 you saw the global–local–reactive layering; in Lesson 2 you learned costmaps and inflation as the key interface between perception and motion generation. The missing piece is the decision executive that answers: when do we replan? when do we keep tracking? what conditions must hold before issuing commands?

Two dominant formalisms are used in AMR navigation executives:

• Hierarchical Finite-State Machines (HFSM / Statecharts): explicit modes (PLAN, CONTROL, …) with guarded transitions. Common in embedded deployments and in MATLAB/Simulink via Stateflow.
• Behavior Trees (BTs): a compositional tree of conditions/actions evaluated by periodic “ticks,” widely used in ROS2 navigation stacks due to modularity and readable recovery/fallback composition.

flowchart TD
  A["Inputs: goal, localization, costmap, odom"] --> E["Decision Executive (HFSM / BT)"]
  E --> GP["Global Plan (low rate)"]
  E --> LC["Local Control (high rate)"]
  E --> MON["Monitors: progress, safety, validity"]
  GP --> LC
  MON --> E
  LC --> OUT["cmd_vel / steering commands"]
        

The key engineering tradeoff is expressivity vs maintainability. Flat FSMs tend to grow brittle as the number of modes and exceptions increases. BTs mitigate “state explosion” by composing reusable subtrees, but require careful semantics for \( \text{RUNNING} \) propagation and tick scheduling.

Let the navigation stack expose a set of observable signals (from localization, costmaps, controllers): \( \mathbf{o}_k \in \mathbb{R}^m \), sampled every tick \( k \). The executive maintains an internal mode and memory.

A deterministic HFSM can be modeled as:

\[ \mathcal{M} = (\mathcal{S}, \mathcal{X}, \Sigma, \delta, \lambda), \quad s_k \in \mathcal{S},\; \mathbf{x}_k \in \mathcal{X} \]

where \( \mathcal{S} \) is a finite set of modes (PLAN, CONTROL, …), \( \mathbf{x}_k \) is “extended state” (timers, counters, last plan time), \( \Sigma \) is an event alphabet, and \( \delta \), \( \lambda \) are transition/output maps. With guards expressed as predicates \( g_j(\mathbf{o}_k,\mathbf{x}_k) \in \{0,1\} \), a common deterministic semantics is:

\[ (s_{k+1}, \mathbf{x}_{k+1}) = \delta(s_k, \mathbf{o}_k, \mathbf{x}_k), \qquad \mathbf{u}_k = \lambda(s_k, \mathbf{o}_k, \mathbf{x}_k) \]

Determinism condition. For each mode \( s \in \mathcal{S} \), suppose outgoing transitions have guards \( g_1,\dots,g_r \) evaluated at tick \( k \). If at most one guard is true at a time: \( \sum_{j=1}^r g_j(\mathbf{o}_k,\mathbf{x}_k) \le 1 \), then the next mode is unique.

Proof sketch: If all guards are false, the “default self-loop” (or “do nothing”) transition applies. If exactly one is true, it selects a unique edge. The inequality excludes the ambiguous case with two simultaneously enabled transitions.

A BT is a rooted tree with leaf nodes (conditions and actions) and internal nodes (control flow, decorators). Each tick maps a blackboard (memory) to a status: SUCCESS, FAILURE, or RUNNING.

Let \( \mathcal{B} \) be the blackboard space and \( \Sigma = \{\text{S},\text{F},\text{R}\} \) the status set. Each node \( n \) defines a (possibly stateful) tick map:

\[ T_n:\mathcal{B} \rightarrow \Sigma \times \mathcal{B}, \qquad T_n(\mathbf{b}) = (\sigma, \mathbf{b}') \]

A condition is typically memoryless: \( T_c(\mathbf{b}) = (\text{S},\mathbf{b}) \) if predicate true, else \( (\text{F},\mathbf{b}) \). An action may update the blackboard and return RUNNING for multiple ticks.

Two essential control-flow nodes:

• Sequence (AND-like): fails fast, succeeds only if all children succeed.
• Fallback (OR-like selector): succeeds fast, fails only if all children fail.

\[ \text{Sequence}(n_1,\dots,n_M):\; \begin{cases} \text{F} & \text{if some child returns F before all succeed} \\ \text{R} & \text{if some child returns R and no earlier child fails} \\ \text{S} & \text{if all children return S} \end{cases} \]

\[ \text{Fallback}(n_1,\dots,n_M):\; \begin{cases} \text{S} & \text{if some child returns S before all fail} \\ \text{R} & \text{if some child returns R and no earlier child succeeds} \\ \text{F} & \text{if all children return F} \end{cases} \]

Many robotics BTs use memory: if child \( n_i \) is RUNNING, the next tick resumes at \( i \) rather than re-ticking from \( 1 \). This approximates a compact HFSM over “currently active” leaves.

flowchart TD
  T["Tick root"] --> SQ["Sequence node"]
  SQ --> C1["Child 1 returns S/F/R"]
  C1 -->|S| C2["Child 2 tick"]
  C1 -->|F| F["Return FAILURE (reset index)"]
  C1 -->|R| R["Return RUNNING \n(remember index)"]
  C2 -->|S ... all S| S["Return SUCCESS (reset index)"]
        

Navigation executives are judged by two classes of properties: safety (“never command unsafe motion”) and progress (“eventually reach goal if feasible and sensors remain valid”).

Safety as an invariant. Let \( \mathcal{U} \) be an unsafe set of outputs (e.g., too large velocity near obstacles, or motion while localization uncertainty is high). Define a predicate Safe on blackboard and outputs: \( \text{Safe}(\mathbf{b},\mathbf{u}) \). If the root is a sequence that checks safety before commanding motion: \( \text{Seq}(\text{Safe?}, \text{CommandMotion}) \), then any tick that issues motion must have passed Safe?.

\[ \Big(\sigma_k = \text{S from Safe?}\Big) \;\Rightarrow\; \text{Safe}(\mathbf{b}_k,\mathbf{u}_k) \]

Invariant proof pattern (tick induction). Suppose: (i) Safe? returns SUCCESS only when \( \text{Safe}(\mathbf{b},\mathbf{u}) \) holds; (ii) CommandMotion is implemented so that it never violates Safe when called under Safe; then by induction over ticks, the system never emits unsafe commands.

Progress with a Lyapunov-like “distance” certificate. Let \( d_k \) be distance-to-goal exposed by the tracking module (or a proxy like path index). A simple progress condition is: while RUNNING in control, \( d_{k+1} \le d_k - \epsilon \) whenever no blocking obstacle is present.

\[ \neg \text{Blocked}_k \;\Rightarrow\; d_{k+1} \le d_k - \epsilon, \quad \epsilon > 0 \]

If \( d_k \ge 0 \) and decreases by at least \( \epsilon \) on unblocked ticks, then after at most \( \left\lceil d_0/\epsilon \right\rceil \) such ticks, the goal tolerance is reached. This is the discrete analogue of finite-time convergence under a descent condition.

Tick-rate constraint (real-time safety margin). If a monitor detects hazards with a maximum detection delay of one tick, then for a required reaction time \( T_{\text{react}} \), the tick period must satisfy:

\[ \Delta t_{\text{tick}} \le T_{\text{react}} \]

In practice: local control runs fast (e.g., 20–50 Hz), while global planning is rate-limited (e.g., 1 Hz), and monitors (progress, validity) run at intermediate rates.

To reason about “how often do we replan?” and “what is the expected time-to-goal?” we can model the executive as a Markov chain over abstract phases. Consider phases \( \{P, C, D\} \) (Plan, Control, Done). Let:

• \( p \): probability that global planning succeeds when called
• \( q \): probability that a control tick makes progress (not blocked)
• \( r \): probability that a control tick reaches goal given progress (simplified)

A minimal expected-ticks model (coarse but useful) can be written with linear equations:

\[ \begin{aligned} E_D &= 0 \\ E_P &= 1 + p\,E_C + (1-p)\,E_P \\ E_C &= 1 + (q r)\,E_D + (q(1-r))\,E_C + (1-q)\,E_P \end{aligned} \]

Solving: \( E_P = \frac{1}{p} + E_C \) and \( E_C = \frac{1 + (1-q)\,E_P}{1-q(1-r)} \). This type of analysis lets you compare architectural choices (e.g., replan-on-failure vs replan-on-timer) without changing the motion algorithms.

For richer executives with many modes, write the transient-state matrix \( \mathbf{Q} \) and use the fundamental matrix:

\[ \mathbf{N} = (\mathbf{I} - \mathbf{Q})^{-1}, \qquad \mathbb{E}[\text{ticks to absorption}] = \mathbf{N}\mathbf{1} \]

This is particularly helpful when comparing HFSM vs BT versions of the same policy: their state graphs may differ, but you can measure expected replans, recoveries, and total time by mapping them to the same abstract chain.

The executive should orchestrate (not re-implement) the navigation pipeline:

• Preconditions (guards): localization quality, goal validity, costmap availability.
• Scheduling: rate-limit expensive modules (global planning), keep control responsive.
• Monitoring: progress (distance decreases), validity (path not blocked), safety constraints.
• Fallbacks: minimal and modular (detailed recovery is next lesson).

A typical navigation BT skeleton (conceptually) is:

\[ \text{Seq}( \text{HaveGoal?}, \text{LocalizationOK?}, \text{RateLimit}(\text{GlobalPlan}), \\ \text{Fallback}(\text{DriveToGoal}, \text{MinimalFallback}) ) \]

In an HFSM, the same policy becomes explicit modes: \( \text{IDLE} \rightarrow \text{PLAN} \rightarrow \text{CONTROL} \rightarrow \text{DONE} \), with a guarded transition to a fallback mode if progress stalls. BTs typically keep this compact and compositional.

Robotics libraries (industry practice):

• Python: py_trees, py_trees_ros; ROS2 integration via rclpy.
• C++: BehaviorTree.CPP (widely used), ROS2 Nav2 uses BT-based task execution.
• Java: robotics stacks often use explicit FSMs; integration options include ROSJava and custom BT frameworks.
• MATLAB/Simulink: Stateflow for HFSM/Statecharts; Simulink blocks for interfacing with controllers/filters.
• Wolfram Mathematica: rapid prototyping of policy logic and symbolic analysis (e.g., expected-time equations).

This implementation builds a minimal BT (Sequence, Fallback, RateLimiter) with a blackboard, plus a comparable HFSM. The “global plan” and “local control” are mocked to focus on executive logic.

Chapter14_Lesson3.py

"""
Chapter14_Lesson3.py
Autonomous Mobile Robots (AMR) — Chapter 14 Lesson 3
Behavior Trees / State Machines for Navigation

This file provides:
- A minimal Behavior Tree (BT) framework (from scratch)
- A navigation-oriented BT example using a shared blackboard
- A comparable hierarchical finite-state machine (HFSM) example

No external dependencies required (standard library only).
"""

from __future__ import annotations
from dataclasses import dataclass, field
from enum import Enum, auto
from typing import Callable, Dict, List, Optional, Any
import time


# -----------------------------
# Behavior Tree core
# -----------------------------

class Status(Enum):
    SUCCESS = auto()
    FAILURE = auto()
    RUNNING = auto()


Blackboard = Dict[str, Any]


class Node:
    """Abstract BT node."""
    def __init__(self, name: str):
        self.name = name

    def tick(self, bb: Blackboard) -> Status:
        raise NotImplementedError


class Condition(Node):
    def __init__(self, name: str, fn: Callable[[Blackboard], bool]):
        super().__init__(name)
        self.fn = fn

    def tick(self, bb: Blackboard) -> Status:
        return Status.SUCCESS if self.fn(bb) else Status.FAILURE


class Action(Node):
    """
    Action node with optional internal state.
    The action function returns one of: 'success', 'failure', 'running'.
    """
    def __init__(self, name: str, fn: Callable[[Blackboard], str]):
        super().__init__(name)
        self.fn = fn

    def tick(self, bb: Blackboard) -> Status:
        out = self.fn(bb).lower().strip()
        if out == "success":
            return Status.SUCCESS
        if out == "failure":
            return Status.FAILURE
        return Status.RUNNING


class Sequence(Node):
    """Ticks children left-to-right. Fails fast; succeeds only if all succeed."""
    def __init__(self, name: str, children: List[Node]):
        super().__init__(name)
        self.children = children
        self._i = 0  # memory for RUNNING semantics (Sequence with memory)

    def tick(self, bb: Blackboard) -> Status:
        while self._i < len(self.children):
            s = self.children[self._i].tick(bb)
            if s == Status.SUCCESS:
                self._i += 1
                continue
            if s == Status.FAILURE:
                self._i = 0
                return Status.FAILURE
            return Status.RUNNING
        self._i = 0
        return Status.SUCCESS


class Fallback(Node):
    """Ticks children left-to-right. Succeeds fast; fails only if all fail."""
    def __init__(self, name: str, children: List[Node]):
        super().__init__(name)
        self.children = children
        self._i = 0  # memory for RUNNING semantics (Fallback with memory)

    def tick(self, bb: Blackboard) -> Status:
        while self._i < len(self.children):
            s = self.children[self._i].tick(bb)
            if s == Status.FAILURE:
                self._i += 1
                continue
            if s == Status.SUCCESS:
                self._i = 0
                return Status.SUCCESS
            return Status.RUNNING
        self._i = 0
        return Status.FAILURE


class RateLimiter(Node):
    """Ticks child at most once every dt seconds (simple 'decorator' node)."""
    def __init__(self, name: str, child: Node, dt: float):
        super().__init__(name)
        self.child = child
        self.dt = float(dt)
        self._t_last = 0.0
        self._last_status = Status.FAILURE

    def tick(self, bb: Blackboard) -> Status:
        now = time.time()
        if now - self._t_last >= self.dt:
            self._t_last = now
            self._last_status = self.child.tick(bb)
        return self._last_status


# -----------------------------
# Navigation-oriented primitives
# -----------------------------

def cond_have_goal(bb: Blackboard) -> bool:
    return bb.get("goal") is not None

def cond_goal_reached(bb: Blackboard) -> bool:
    # Goal is reached if distance <= tol
    return bb.get("dist_to_goal", float("inf")) <= bb.get("goal_tol", 0.2)

def cond_localization_ok(bb: Blackboard) -> bool:
    # "Localization OK" if covariance trace below a threshold
    P = bb.get("P_trace", 1e9)
    return P <= bb.get("P_trace_max", 2.0)

def cond_path_valid(bb: Blackboard) -> bool:
    # A coarse validity bit (e.g., global path not blocked)
    return bool(bb.get("path_valid", False))


def act_global_plan(bb: Blackboard) -> str:
    """
    Mock global planning:
    - Requires localization OK and a goal.
    - Produces a 'path' and marks path_valid.
    """
    if bb.get("goal") is None:
        return "failure"
    if not cond_localization_ok(bb):
        return "failure"
    # pretend planning succeeds with high probability
    bb["path"] = ["(x0,y0)", "(x1,y1)", "(xg,yg)"]
    bb["path_valid"] = True
    bb.setdefault("planner_calls", 0)
    bb["planner_calls"] += 1
    return "success"


def act_local_control(bb: Blackboard) -> str:
    """
    Mock local control:
    - If no valid path, fail.
    - Otherwise, reduce distance-to-goal.
    - Can 'stall' if obstacle flag is set.
    """
    if not bb.get("path_valid", False):
        return "failure"
    if bb.get("obstacle_blocking", False):
        # local controller running but not making progress
        bb["dist_to_goal"] = bb.get("dist_to_goal", 5.0)
        return "running"

    # progress
    d = float(bb.get("dist_to_goal", 5.0))
    v = float(bb.get("progress_per_tick", 0.15))
    bb["dist_to_goal"] = max(0.0, d - v)
    return "running" if not cond_goal_reached(bb) else "success"


def act_recover_clear_costmap(bb: Blackboard) -> str:
    """
    Mock recovery: clears obstacle and invalidates path (forces re-plan).
    """
    bb["obstacle_blocking"] = False
    bb["path_valid"] = False
    bb.setdefault("recoveries", 0)
    bb["recoveries"] += 1
    return "success"


def build_navigation_bt() -> Node:
    """
    A typical pattern:
    - Preconditions: have goal, localization ok
    - Plan at limited rate
    - Control until goal reached
    - If control stalls (obstacle), run recovery then re-plan
    """
    return Sequence("NavigateToGoal", [
        Condition("HaveGoal?", cond_have_goal),
        Condition("LocalizationOK?", cond_localization_ok),
        RateLimiter("Replan@1Hz", Action("GlobalPlan", act_global_plan), dt=1.0),
        Fallback("DriveOrRecover", [
            Sequence("DriveToGoal", [
                Condition("PathValid?", cond_path_valid),
                Action("LocalControl", act_local_control),
                Condition("GoalReached?", cond_goal_reached),
            ]),
            Action("RecoveryClearCostmap", act_recover_clear_costmap),
        ])
    ])


# -----------------------------
# Comparable Hierarchical FSM (HFSM)
# -----------------------------

class NavState(Enum):
    IDLE = auto()
    PLAN = auto()
    CONTROL = auto()
    RECOVERY = auto()
    DONE = auto()
    FAIL = auto()


@dataclass
class HFSM:
    state: NavState = NavState.IDLE
    stall_ticks: int = 0
    stall_max: int = 10

    def step(self, bb: Blackboard) -> NavState:
        if self.state == NavState.IDLE:
            if cond_have_goal(bb) and cond_localization_ok(bb):
                self.state = NavState.PLAN
            else:
                self.state = NavState.FAIL
            return self.state

        if self.state == NavState.PLAN:
            if act_global_plan(bb) == "success":
                self.state = NavState.CONTROL
            else:
                self.state = NavState.FAIL
            return self.state

        if self.state == NavState.CONTROL:
            if cond_goal_reached(bb):
                self.state = NavState.DONE
                return self.state
            out = act_local_control(bb)
            if out == "failure":
                self.state = NavState.PLAN
                return self.state
            # detect stall (no distance decrease) by obstacle flag
            if bb.get("obstacle_blocking", False):
                self.stall_ticks += 1
                if self.stall_ticks >= self.stall_max:
                    self.state = NavState.RECOVERY
                    self.stall_ticks = 0
            else:
                self.stall_ticks = 0
            return self.state

        if self.state == NavState.RECOVERY:
            act_recover_clear_costmap(bb)
            self.state = NavState.PLAN
            return self.state

        return self.state


# -----------------------------
# Demo
# -----------------------------

def run_demo():
    bb: Blackboard = {
        "goal": (5.0, 0.0),
        "dist_to_goal": 5.0,
        "goal_tol": 0.2,
        "P_trace": 0.8,
        "P_trace_max": 2.0,
        "progress_per_tick": 0.2,
        "obstacle_blocking": False,
        "path_valid": False,
    }

    bt = build_navigation_bt()
    fsm = HFSM()

    print("=== Behavior Tree demo ===")
    for t in range(60):
        # inject an obstacle episode
        if t == 12:
            bb["obstacle_blocking"] = True
        if t == 25:
            bb["obstacle_blocking"] = False

        s = bt.tick(bb)
        print(f"t={t:02d} status={s.name:7s} dist={bb['dist_to_goal']:.2f} "
              f"path_valid={bb['path_valid']} rec={bb.get('recoveries',0)}")
        if s == Status.SUCCESS:
            break
        time.sleep(0.02)

    # reset
    bb["dist_to_goal"] = 5.0
    bb["path_valid"] = False
    bb["obstacle_blocking"] = False
    bb["recoveries"] = 0

    print("\n=== HFSM demo ===")
    for t in range(60):
        if t == 12:
            bb["obstacle_blocking"] = True
        if t == 25:
            bb["obstacle_blocking"] = False

        st = fsm.step(bb)
        print(f"t={t:02d} state={st.name:9s} dist={bb['dist_to_goal']:.2f} "
              f"path_valid={bb['path_valid']} rec={bb.get('recoveries',0)}")
        if st == NavState.DONE:
            break
        time.sleep(0.02)


if __name__ == "__main__":
    run_demo()

In C++ robotics, BTs are commonly implemented with BehaviorTree.CPP. Here we provide a minimal from-scratch implementation to make the semantics explicit.

Chapter14_Lesson3.cpp

// Chapter14_Lesson3.cpp
// Autonomous Mobile Robots (AMR) — Chapter 14 Lesson 3
// Behavior Trees / State Machines for Navigation
//
// This is a self-contained, from-scratch minimal BT implementation (no external libs).
// It also contains a simple hierarchical finite-state machine (HFSM) for comparison.
//
// Build (example):
//   g++ -std=c++17 -O2 Chapter14_Lesson3.cpp -o Chapter14_Lesson3
// Run:
//   ./Chapter14_Lesson3

#include <iostream>
#include <string>
#include <vector>
#include <functional>
#include <unordered_map>
#include <variant>
#include <chrono>
#include <thread>
#include <cmath>

enum class Status { SUCCESS, FAILURE, RUNNING };

static const char* to_cstr(Status s) {
  switch (s) {
    case Status::SUCCESS: return "SUCCESS";
    case Status::FAILURE: return "FAILURE";
    default: return "RUNNING";
  }
}

using Value = std::variant<bool, int, double, std::string>;
using Blackboard = std::unordered_map<std::string, Value>;

template <typename T>
T bb_get(const Blackboard& bb, const std::string& key, const T& defval) {
  auto it = bb.find(key);
  if (it == bb.end()) return defval;
  if (auto p = std::get_if<T>(&it->second)) return *p;
  return defval;
}

template <typename T>
void bb_set(Blackboard& bb, const std::string& key, const T& v) {
  bb[key] = v;
}

// -----------------------------
// BT core
// -----------------------------
struct Node {
  std::string name;
  explicit Node(std::string n) : name(std::move(n)) {}
  virtual ~Node() = default;
  virtual Status tick(Blackboard& bb) = 0;
};

struct Condition : Node {
  std::function<bool(const Blackboard&)> fn;
  Condition(std::string n, std::function<bool(const Blackboard&)> f)
    : Node(std::move(n)), fn(std::move(f)) {}
  Status tick(Blackboard& bb) override {
    (void)bb;
    return fn(bb) ? Status::SUCCESS : Status::FAILURE;
  }
};

struct Action : Node {
  std::function<std::string(Blackboard&)> fn;
  Action(std::string n, std::function<std::string(Blackboard&)> f)
    : Node(std::move(n)), fn(std::move(f)) {}
  Status tick(Blackboard& bb) override {
    std::string out = fn(bb);
    for (auto& c : out) c = static_cast<char>(std::tolower(c));
    if (out == "success") return Status::SUCCESS;
    if (out == "failure") return Status::FAILURE;
    return Status::RUNNING;
  }
};

struct Sequence : Node {
  std::vector<Node*> children;
  size_t i = 0; // memory
  Sequence(std::string n, std::vector<Node*> ch)
    : Node(std::move(n)), children(std::move(ch)) {}
  Status tick(Blackboard& bb) override {
    while (i < children.size()) {
      Status s = children[i]->tick(bb);
      if (s == Status::SUCCESS) { ++i; continue; }
      if (s == Status::FAILURE) { i = 0; return Status::FAILURE; }
      return Status::RUNNING;
    }
    i = 0;
    return Status::SUCCESS;
  }
};

struct Fallback : Node {
  std::vector<Node*> children;
  size_t i = 0; // memory
  Fallback(std::string n, std::vector<Node*> ch)
    : Node(std::move(n)), children(std::move(ch)) {}
  Status tick(Blackboard& bb) override {
    while (i < children.size()) {
      Status s = children[i]->tick(bb);
      if (s == Status::FAILURE) { ++i; continue; }
      if (s == Status::SUCCESS) { i = 0; return Status::SUCCESS; }
      return Status::RUNNING;
    }
    i = 0;
    return Status::FAILURE;
  }
};

struct RateLimiter : Node {
  Node* child;
  double dt;
  std::chrono::steady_clock::time_point last;
  Status last_status = Status::FAILURE;

  RateLimiter(std::string n, Node* c, double dt_sec)
    : Node(std::move(n)), child(c), dt(dt_sec), last(std::chrono::steady_clock::now()) {
    last -= std::chrono::milliseconds(static_cast<int>(dt * 1000)); // force first tick
  }

  Status tick(Blackboard& bb) override {
    auto now = std::chrono::steady_clock::now();
    double elapsed = std::chrono::duration<double>(now - last).count();
    if (elapsed >= dt) {
      last = now;
      last_status = child->tick(bb);
    }
    return last_status;
  }
};

// -----------------------------
// Navigation primitives
// -----------------------------
static bool have_goal(const Blackboard& bb) {
  return bb_get<bool>(bb, "have_goal", false);
}
static bool localization_ok(const Blackboard& bb) {
  double P_trace = bb_get<double>(bb, "P_trace", 1e9);
  double P_max   = bb_get<double>(bb, "P_trace_max", 2.0);
  return P_trace <= P_max;
}
static bool path_valid(const Blackboard& bb) {
  return bb_get<bool>(bb, "path_valid", false);
}
static bool goal_reached(const Blackboard& bb) {
  double d = bb_get<double>(bb, "dist_to_goal", 1e9);
  double tol = bb_get<double>(bb, "goal_tol", 0.2);
  return d <= tol;
}

static std::string global_plan(Blackboard& bb) {
  if (!have_goal(bb)) return "failure";
  if (!localization_ok(bb)) return "failure";
  bb_set(bb, "path_valid", true);
  int calls = bb_get<int>(bb, "planner_calls", 0);
  bb_set(bb, "planner_calls", calls + 1);
  return "success";
}

static std::string local_control(Blackboard& bb) {
  if (!path_valid(bb)) return "failure";
  bool obs = bb_get<bool>(bb, "obstacle_blocking", false);
  if (obs) return "running";

  double d = bb_get<double>(bb, "dist_to_goal", 5.0);
  double step = bb_get<double>(bb, "progress_per_tick", 0.2);
  d = std::max(0.0, d - step);
  bb_set(bb, "dist_to_goal", d);
  return goal_reached(bb) ? "success" : "running";
}

static std::string recover_clear_costmap(Blackboard& bb) {
  bb_set(bb, "obstacle_blocking", false);
  bb_set(bb, "path_valid", false);
  int rec = bb_get<int>(bb, "recoveries", 0);
  bb_set(bb, "recoveries", rec + 1);
  return "success";
}

// -----------------------------
// HFSM
// -----------------------------
enum class NavState { IDLE, PLAN, CONTROL, RECOVERY, DONE, FAIL };

static const char* to_cstr(NavState s) {
  switch (s) {
    case NavState::IDLE: return "IDLE";
    case NavState::PLAN: return "PLAN";
    case NavState::CONTROL: return "CONTROL";
    case NavState::RECOVERY: return "RECOVERY";
    case NavState::DONE: return "DONE";
    default: return "FAIL";
  }
}

struct HFSM {
  NavState st = NavState::IDLE;
  int stall = 0;
  int stall_max = 10;

  NavState step(Blackboard& bb) {
    if (st == NavState::IDLE) {
      st = (have_goal(bb) && localization_ok(bb)) ? NavState::PLAN : NavState::FAIL;
      return st;
    }
    if (st == NavState::PLAN) {
      st = (global_plan(bb) == "success") ? NavState::CONTROL : NavState::FAIL;
      return st;
    }
    if (st == NavState::CONTROL) {
      if (goal_reached(bb)) { st = NavState::DONE; return st; }
      std::string out = local_control(bb);
      if (out == "failure") { st = NavState::PLAN; return st; }
      bool obs = bb_get<bool>(bb, "obstacle_blocking", false);
      if (obs) {
        ++stall;
        if (stall >= stall_max) { st = NavState::RECOVERY; stall = 0; }
      } else stall = 0;
      return st;
    }
    if (st == NavState::RECOVERY) {
      recover_clear_costmap(bb);
      st = NavState::PLAN;
      return st;
    }
    return st;
  }
};

// -----------------------------
// Build BT
// -----------------------------
Node* build_navigation_bt(std::vector<std::unique_ptr<Node>>& arena) {
  auto make = [&](auto ptr) -> Node* {
    arena.emplace_back(std::move(ptr));
    return arena.back().get();
  };

  Node* haveGoal = make(std::make_unique<Condition>("HaveGoal?", [](const Blackboard& bb){ return have_goal(bb); }));
  Node* locOK    = make(std::make_unique<Condition>("LocalizationOK?", [](const Blackboard& bb){ return localization_ok(bb); }));
  Node* pathOK   = make(std::make_unique<Condition>("PathValid?", [](const Blackboard& bb){ return path_valid(bb); }));
  Node* reached  = make(std::make_unique<Condition>("GoalReached?", [](const Blackboard& bb){ return goal_reached(bb); }));

  Node* planAct  = make(std::make_unique<Action>("GlobalPlan", [](Blackboard& bb){ return global_plan(bb); }));
  Node* planRate = make(std::make_unique<RateLimiter>("Replan@1Hz", planAct, 1.0));

  Node* ctrlAct  = make(std::make_unique<Action>("LocalControl", [](Blackboard& bb){ return local_control(bb); }));
  Node* recAct   = make(std::make_unique<Action>("RecoveryClearCostmap", [](Blackboard& bb){ return recover_clear_costmap(bb); }));

  Node* driveSeq = make(std::make_unique<Sequence>("DriveToGoal", std::vector<Node*>{ pathOK, ctrlAct, reached }));
  Node* driveOrRecover = make(std::make_unique<Fallback>("DriveOrRecover", std::vector<Node*>{ driveSeq, recAct }));

  Node* root = make(std::make_unique<Sequence>("NavigateToGoal",
    std::vector<Node*>{ haveGoal, locOK, planRate, driveOrRecover }));

  return root;
}

int main() {
  Blackboard bb;
  bb_set(bb, "have_goal", true);
  bb_set(bb, "dist_to_goal", 5.0);
  bb_set(bb, "goal_tol", 0.2);
  bb_set(bb, "P_trace", 0.8);
  bb_set(bb, "P_trace_max", 2.0);
  bb_set(bb, "progress_per_tick", 0.2);
  bb_set(bb, "obstacle_blocking", false);
  bb_set(bb, "path_valid", false);
  bb_set(bb, "recoveries", 0);
  bb_set(bb, "planner_calls", 0);

  std::cout << "=== Behavior Tree demo ===\n";
  std::vector<std::unique_ptr<Node>> arena;
  Node* bt = build_navigation_bt(arena);

  for (int t = 0; t < 60; ++t) {
    if (t == 12) bb_set(bb, "obstacle_blocking", true);
    if (t == 25) bb_set(bb, "obstacle_blocking", false);

    Status s = bt->tick(bb);
    std::cout << "t=" << (t<10?"0":"") << t
              << " status=" << to_cstr(s)
              << " dist=" << bb_get<double>(bb, "dist_to_goal", 0.0)
              << " path_valid=" << (bb_get<bool>(bb, "path_valid", false) ? "true":"false")
              << " rec=" << bb_get<int>(bb, "recoveries", 0)
              << "\n";
    if (s == Status::SUCCESS) break;
    std::this_thread::sleep_for(std::chrono::milliseconds(20));
  }

  // Reset and run HFSM
  bb_set(bb, "dist_to_goal", 5.0);
  bb_set(bb, "path_valid", false);
  bb_set(bb, "obstacle_blocking", false);
  bb_set(bb, "recoveries", 0);
  bb_set(bb, "planner_calls", 0);

  std::cout << "\n=== HFSM demo ===\n";
  HFSM fsm;
  for (int t = 0; t < 60; ++t) {
    if (t == 12) bb_set(bb, "obstacle_blocking", true);
    if (t == 25) bb_set(bb, "obstacle_blocking", false);

    NavState st = fsm.step(bb);
    std::cout << "t=" << (t<10?"0":"") << t
              << " state=" << to_cstr(st)
              << " dist=" << bb_get<double>(bb, "dist_to_goal", 0.0)
              << " path_valid=" << (bb_get<bool>(bb, "path_valid", false) ? "true":"false")
              << " rec=" << bb_get<int>(bb, "recoveries", 0)
              << "\n";
    if (st == NavState::DONE) break;
    std::this_thread::sleep_for(std::chrono::milliseconds(20));
  }

  return 0;
}

Java deployments often use explicit HFSMs; nevertheless, BTs can be implemented cleanly and tested with unit tests. This single-file example keeps all classes as static inner classes for portability.

Chapter14_Lesson3.java

// Chapter14_Lesson3.java
// Autonomous Mobile Robots (AMR) — Chapter 14 Lesson 3
// Behavior Trees / State Machines for Navigation
//
// A single-file Java implementation (all classes as static inner classes).
//
// Compile:
//   javac Chapter14_Lesson3.java
// Run:
//   java Chapter14_Lesson3

import java.util.*;

public class Chapter14_Lesson3 {

  // -----------------------------
  // Behavior Tree core
  // -----------------------------
  enum Status { SUCCESS, FAILURE, RUNNING }

  interface Node {
    Status tick(Map<String, Object> bb);
    String name();
  }

  static final class Condition implements Node {
    private final String name;
    private final java.util.function.Predicate<Map<String,Object>> fn;
    Condition(String name, java.util.function.Predicate<Map<String,Object>> fn) {
      this.name = name; this.fn = fn;
    }
    public Status tick(Map<String,Object> bb) { return fn.test(bb) ? Status.SUCCESS : Status.FAILURE; }
    public String name() { return name; }
  }

  static final class Action implements Node {
    private final String name;
    private final java.util.function.Function<Map<String,Object>, String> fn;
    Action(String name, java.util.function.Function<Map<String,Object>, String> fn) {
      this.name = name; this.fn = fn;
    }
    public Status tick(Map<String,Object> bb) {
      String out = fn.apply(bb).toLowerCase(Locale.ROOT).trim();
      if (out.equals("success")) return Status.SUCCESS;
      if (out.equals("failure")) return Status.FAILURE;
      return Status.RUNNING;
    }
    public String name() { return name; }
  }

  static final class Sequence implements Node {
    private final String name;
    private final List<Node> children;
    private int i = 0; // memory
    Sequence(String name, List<Node> children) { this.name = name; this.children = children; }
    public Status tick(Map<String,Object> bb) {
      while (i < children.size()) {
        Status s = children.get(i).tick(bb);
        if (s == Status.SUCCESS) { i++; continue; }
        if (s == Status.FAILURE) { i = 0; return Status.FAILURE; }
        return Status.RUNNING;
      }
      i = 0;
      return Status.SUCCESS;
    }
    public String name() { return name; }
  }

  static final class Fallback implements Node {
    private final String name;
    private final List<Node> children;
    private int i = 0; // memory
    Fallback(String name, List<Node> children) { this.name = name; this.children = children; }
    public Status tick(Map<String,Object> bb) {
      while (i < children.size()) {
        Status s = children.get(i).tick(bb);
        if (s == Status.FAILURE) { i++; continue; }
        if (s == Status.SUCCESS) { i = 0; return Status.SUCCESS; }
        return Status.RUNNING;
      }
      i = 0;
      return Status.FAILURE;
    }
    public String name() { return name; }
  }

  static final class RateLimiter implements Node {
    private final String name;
    private final Node child;
    private final long dtMillis;
    private long last = 0L;
    private Status lastStatus = Status.FAILURE;
    RateLimiter(String name, Node child, double dtSeconds) {
      this.name = name; this.child = child; this.dtMillis = (long)(dtSeconds * 1000.0);
      this.last = 0L;
    }
    public Status tick(Map<String,Object> bb) {
      long now = System.currentTimeMillis();
      if (now - last >= dtMillis) {
        last = now;
        lastStatus = child.tick(bb);
      }
      return lastStatus;
    }
    public String name() { return name; }
  }

  // -----------------------------
  // Navigation primitives
  // -----------------------------
  static boolean haveGoal(Map<String,Object> bb) { return (Boolean)bb.getOrDefault("have_goal", false); }

  static boolean localizationOk(Map<String,Object> bb) {
    double P = (Double)bb.getOrDefault("P_trace", 1e9);
    double Pmax = (Double)bb.getOrDefault("P_trace_max", 2.0);
    return P <= Pmax;
  }

  static boolean pathValid(Map<String,Object> bb) { return (Boolean)bb.getOrDefault("path_valid", false); }

  static boolean goalReached(Map<String,Object> bb) {
    double d = (Double)bb.getOrDefault("dist_to_goal", 1e9);
    double tol = (Double)bb.getOrDefault("goal_tol", 0.2);
    return d <= tol;
  }

  static String globalPlan(Map<String,Object> bb) {
    if (!haveGoal(bb)) return "failure";
    if (!localizationOk(bb)) return "failure";
    bb.put("path_valid", true);
    int calls = (Integer)bb.getOrDefault("planner_calls", 0);
    bb.put("planner_calls", calls + 1);
    return "success";
  }

  static String localControl(Map<String,Object> bb) {
    if (!pathValid(bb)) return "failure";
    boolean obs = (Boolean)bb.getOrDefault("obstacle_blocking", false);
    if (obs) return "running";

    double d = (Double)bb.getOrDefault("dist_to_goal", 5.0);
    double step = (Double)bb.getOrDefault("progress_per_tick", 0.2);
    d = Math.max(0.0, d - step);
    bb.put("dist_to_goal", d);
    return goalReached(bb) ? "success" : "running";
  }

  static String recoverClearCostmap(Map<String,Object> bb) {
    bb.put("obstacle_blocking", false);
    bb.put("path_valid", false);
    int rec = (Integer)bb.getOrDefault("recoveries", 0);
    bb.put("recoveries", rec + 1);
    return "success";
  }

  static Node buildNavigationBT() {
    Node haveGoal = new Condition("HaveGoal?", Chapter14_Lesson3::haveGoal);
    Node locOK = new Condition("LocalizationOK?", Chapter14_Lesson3::localizationOk);
    Node pathOK = new Condition("PathValid?", Chapter14_Lesson3::pathValid);
    Node reached = new Condition("GoalReached?", Chapter14_Lesson3::goalReached);

    Node planAct = new Action("GlobalPlan", Chapter14_Lesson3::globalPlan);
    Node planRate = new RateLimiter("Replan@1Hz", planAct, 1.0);

    Node ctrlAct = new Action("LocalControl", Chapter14_Lesson3::localControl);
    Node recAct  = new Action("RecoveryClearCostmap", Chapter14_Lesson3::recoverClearCostmap);

    Node driveSeq = new Sequence("DriveToGoal", Arrays.asList(pathOK, ctrlAct, reached));
    Node driveOrRecover = new Fallback("DriveOrRecover", Arrays.asList(driveSeq, recAct));

    return new Sequence("NavigateToGoal", Arrays.asList(haveGoal, locOK, planRate, driveOrRecover));
  }

  // -----------------------------
  // HFSM
  // -----------------------------
  enum NavState { IDLE, PLAN, CONTROL, RECOVERY, DONE, FAIL }

  static final class HFSM {
    NavState st = NavState.IDLE;
    int stall = 0;
    int stallMax = 10;

    NavState step(Map<String,Object> bb) {
      switch (st) {
        case IDLE:
          st = (haveGoal(bb) && localizationOk(bb)) ? NavState.PLAN : NavState.FAIL;
          return st;
        case PLAN:
          st = globalPlan(bb).equals("success") ? NavState.CONTROL : NavState.FAIL;
          return st;
        case CONTROL:
          if (goalReached(bb)) { st = NavState.DONE; return st; }
          String out = localControl(bb);
          if (out.equals("failure")) { st = NavState.PLAN; return st; }
          boolean obs = (Boolean)bb.getOrDefault("obstacle_blocking", false);
          if (obs) {
            stall++;
            if (stall >= stallMax) { stall = 0; st = NavState.RECOVERY; }
          } else stall = 0;
          return st;
        case RECOVERY:
          recoverClearCostmap(bb);
          st = NavState.PLAN;
          return st;
        default:
          return st;
      }
    }
  }

  // -----------------------------
  // Demo
  // -----------------------------
  public static void main(String[] args) throws Exception {
    Map<String,Object> bb = new HashMap<>();
    bb.put("have_goal", true);
    bb.put("dist_to_goal", 5.0);
    bb.put("goal_tol", 0.2);
    bb.put("P_trace", 0.8);
    bb.put("P_trace_max", 2.0);
    bb.put("progress_per_tick", 0.2);
    bb.put("obstacle_blocking", false);
    bb.put("path_valid", false);
    bb.put("recoveries", 0);
    bb.put("planner_calls", 0);

    Node bt = buildNavigationBT();
    System.out.println("=== Behavior Tree demo ===");
    for (int t = 0; t < 60; t++) {
      if (t == 12) bb.put("obstacle_blocking", true);
      if (t == 25) bb.put("obstacle_blocking", false);

      Status s = bt.tick(bb);
      System.out.printf(Locale.ROOT,
        "t=%02d status=%-7s dist=%.2f path_valid=%s rec=%d%n",
        t, s.name(),
        (Double)bb.get("dist_to_goal"),
        bb.get("path_valid"),
        (Integer)bb.get("recoveries")
      );
      if (s == Status.SUCCESS) break;
      Thread.sleep(20);
    }

    // reset and run HFSM
    bb.put("dist_to_goal", 5.0);
    bb.put("path_valid", false);
    bb.put("obstacle_blocking", false);
    bb.put("recoveries", 0);
    bb.put("planner_calls", 0);

    HFSM fsm = new HFSM();
    System.out.println("\n=== HFSM demo ===");
    for (int t = 0; t < 60; t++) {
      if (t == 12) bb.put("obstacle_blocking", true);
      if (t == 25) bb.put("obstacle_blocking", false);

      NavState st = fsm.step(bb);
      System.out.printf(Locale.ROOT,
        "t=%02d state=%-9s dist=%.2f path_valid=%s rec=%d%n",
        t, st.name(),
        (Double)bb.get("dist_to_goal"),
        bb.get("path_valid"),
        (Integer)bb.get("recoveries")
      );
      if (st == NavState.DONE) break;
      Thread.sleep(20);
    }
  }
}

In Simulink-based AMR prototyping, executives are frequently encoded as HFSMs using Stateflow. The MATLAB file below (i) implements an HFSM in pure MATLAB and (ii) optionally builds a Simulink model shell and a placeholder Stateflow chart if available.

Chapter14_Lesson3.m

% Chapter14_Lesson3.m
% Autonomous Mobile Robots (AMR) — Chapter 14 Lesson 3
% Behavior Trees / State Machines for Navigation
%
% This file provides:
% 1) A hierarchical finite-state machine (HFSM) navigation controller in MATLAB
% 2) (Optional) Programmatic creation of a simple Simulink model shell and
%    a placeholder Stateflow chart (if Stateflow is available on your installation).
%
% Note: The BT implementation is demonstrated in other languages; MATLAB here
% focuses on HFSM/Stateflow patterns that are common for embedded navigation logic.

function Chapter14_Lesson3()
    % Blackboard-like struct
    bb.have_goal = true;
    bb.dist_to_goal = 5.0;
    bb.goal_tol = 0.2;
    bb.P_trace = 0.8;
    bb.P_trace_max = 2.0;
    bb.progress_per_tick = 0.2;
    bb.obstacle_blocking = false;
    bb.path_valid = false;
    bb.recoveries = 0;
    bb.planner_calls = 0;

    fsm.state = NavState.IDLE;
    fsm.stall = 0;
    fsm.stall_max = 10;

    fprintf('=== HFSM demo (MATLAB) ===\n');
    for t = 0:59
        if t == 12
            bb.obstacle_blocking = true;
        end
        if t == 25
            bb.obstacle_blocking = false;
        end

        [fsm, bb] = fsm_step(fsm, bb);

        fprintf('t=%02d state=%-9s dist=%.2f path_valid=%d rec=%d\n', ...
            t, char(fsm.state), bb.dist_to_goal, bb.path_valid, bb.recoveries);

        if fsm.state == NavState.DONE
            break;
        end
        pause(0.02);
    end

    % Optional: generate a Simulink/Stateflow skeleton
    % Uncomment the line below if you want to auto-create a model.
    % build_simulink_stateflow_skeleton();
end

% -----------------------------
% State definitions
% -----------------------------
classdef NavState
   enumeration
      IDLE, PLAN, CONTROL, RECOVERY, DONE, FAIL
   end
end

% -----------------------------
% Guard conditions
% -----------------------------
function ok = have_goal(bb)
    ok = isfield(bb,'have_goal') && bb.have_goal;
end

function ok = localization_ok(bb)
    ok = bb.P_trace <= bb.P_trace_max;
end

function ok = path_valid(bb)
    ok = bb.path_valid;
end

function ok = goal_reached(bb)
    ok = bb.dist_to_goal <= bb.goal_tol;
end

% -----------------------------
% Actions
% -----------------------------
function [bb, out] = act_global_plan(bb)
    if ~have_goal(bb) || ~localization_ok(bb)
        out = "failure"; return;
    end
    bb.path_valid = true;
    bb.planner_calls = bb.planner_calls + 1;
    out = "success";
end

function [bb, out] = act_local_control(bb)
    if ~path_valid(bb)
        out = "failure"; return;
    end
    if bb.obstacle_blocking
        out = "running"; return;
    end
    bb.dist_to_goal = max(0.0, bb.dist_to_goal - bb.progress_per_tick);
    out = "running";
    if goal_reached(bb), out = "success"; end
end

function [bb, out] = act_recovery_clear_costmap(bb)
    bb.obstacle_blocking = false;
    bb.path_valid = false;
    bb.recoveries = bb.recoveries + 1;
    out = "success";
end

% -----------------------------
% HFSM step
% -----------------------------
function [fsm, bb] = fsm_step(fsm, bb)
    switch fsm.state
        case NavState.IDLE
            if have_goal(bb) && localization_ok(bb)
                fsm.state = NavState.PLAN;
            else
                fsm.state = NavState.FAIL;
            end

        case NavState.PLAN
            [bb, out] = act_global_plan(bb);
            if out == "success"
                fsm.state = NavState.CONTROL;
            else
                fsm.state = NavState.FAIL;
            end

        case NavState.CONTROL
            if goal_reached(bb)
                fsm.state = NavState.DONE;
                return;
            end
            [bb, out] = act_local_control(bb);
            if out == "failure"
                fsm.state = NavState.PLAN;
                return;
            end
            if bb.obstacle_blocking
                fsm.stall = fsm.stall + 1;
                if fsm.stall >= fsm.stall_max
                    fsm.stall = 0;
                    fsm.state = NavState.RECOVERY;
                end
            else
                fsm.stall = 0;
            end

        case NavState.RECOVERY
            [bb, ~] = act_recovery_clear_costmap(bb);
            fsm.state = NavState.PLAN;

        otherwise
            % DONE or FAIL -> no-op
    end
end

% -----------------------------
% Optional Simulink/Stateflow skeleton builder
% -----------------------------
function build_simulink_stateflow_skeleton()
    % This creates a minimal Simulink model and attempts to add a Stateflow chart.
    % If Stateflow isn't licensed/installed, this will error; keep it optional.
    mdl = 'Chapter14_Lesson3_Simulink';
    if bdIsLoaded(mdl)
        close_system(mdl, 0);
    end
    if exist([mdl '.slx'],'file')
        delete([mdl '.slx']);
    end
    new_system(mdl);
    open_system(mdl);

    add_block('simulink/Sources/Constant', [mdl '/have_goal'], 'Value', '1');
    add_block('simulink/Sources/Constant', [mdl '/P_trace'], 'Value', '0.8');
    add_block('simulink/Sources/Constant', [mdl '/P_trace_max'], 'Value', '2.0');
    add_block('simulink/Sources/Constant', [mdl '/dist_to_goal'], 'Value', '5.0');
    add_block('simulink/Sources/Constant', [mdl '/goal_tol'], 'Value', '0.2');

    % MATLAB Function placeholder (for HFSM logic)
    blk = add_block('simulink/User-Defined Functions/MATLAB Function', [mdl '/HFSM_Controller']);
    set_param(blk, 'Position', [300 80 500 180]);

    % Try adding Stateflow chart if available
    try
        rt = sfroot;
        m = rt.find('-isa','Simulink.BlockDiagram','Name',mdl);
        ch = Stateflow.Chart(m);
        ch.Name = 'NavLogic';
        % A real implementation would add states/transitions here.
        disp('Stateflow chart created: NavLogic (populate states/transitions manually).');
    catch ME
        warning('Stateflow not available or error occurred: %s', ME.message);
    end

    save_system(mdl);
    disp(['Saved model: ' mdl '.slx']);
end

Mathematica is useful for quickly prototyping executive semantics and validating expected-time equations symbolically. The downloadable notebook encodes BT nodes as associations with tick functions and runs a small simulation.

Chapter14_Lesson3.nb

(* Chapter14_Lesson3.nb
   Autonomous Mobile Robots (AMR) — Chapter 14 Lesson 3
   Behavior Trees / State Machines for Navigation

   This is a plain-text Wolfram Notebook expression (Mathematica can open .nb).
*)

Notebook[{
  Cell["AMR — Chapter 14 Lesson 3: Behavior Trees / State Machines for Navigation", "Title"],

  Cell["A minimal Behavior Tree model in Wolfram Language", "Section"],

  Cell[BoxData @ ToBoxes @ HoldForm[
    (* Status values *)
    Status /: Format[Status[s_]] := s;
    statuses = {"SUCCESS", "FAILURE", "RUNNING"};

    (* Blackboard: Association *)
    bb0 = <|
      "haveGoal" -> True,
      "PTrace" -> 0.8,
      "PTraceMax" -> 2.0,
      "dist" -> 5.0,
      "tol" -> 0.2,
      "pathValid" -> False,
      "obstacle" -> False,
      "progress" -> 0.2,
      "recoveries" -> 0,
      "plannerCalls" -> 0
    |>;

    haveGoalQ[bb_] := TrueQ[bb["haveGoal"]];
    locOKQ[bb_] := bb["PTrace"] <= bb["PTraceMax"];
    goalReachedQ[bb_] := bb["dist"] <= bb["tol"];
    pathValidQ[bb_] := TrueQ[bb["pathValid"]];

    globalPlan[bb_] := Module[{bb2 = bb},
      If[!haveGoalQ[bb2] || !locOKQ[bb2], Return[{bb2, "FAILURE"}]];
      bb2["pathValid"] = True;
      bb2["plannerCalls"] = bb2["plannerCalls"] + 1;
      {bb2, "SUCCESS"}
    ];

    localControl[bb_] := Module[{bb2 = bb, d},
      If[!pathValidQ[bb2], Return[{bb2, "FAILURE"}]];
      If[TrueQ[bb2["obstacle"]], Return[{bb2, "RUNNING"}]];
      d = Max[0.0, bb2["dist"] - bb2["progress"]];
      bb2["dist"] = d;
      If[goalReachedQ[bb2], {bb2, "SUCCESS"}, {bb2, "RUNNING"}]
    ];

    recoverClear[bb_] := Module[{bb2 = bb},
      bb2["obstacle"] = False;
      bb2["pathValid"] = False;
      bb2["recoveries"] = bb2["recoveries"] + 1;
      {bb2, "SUCCESS"}
    ];

    (* BT node constructors: represent a node by an association with a tick function *)
    ConditionNode[name_, pred_] := <|
      "name" -> name,
      "tick" -> Function[{bb}, If[pred[bb], {bb, "SUCCESS"}, {bb, "FAILURE"}]]
    |>;

    ActionNode[name_, act_] := <|
      "name" -> name,
      "tick" -> Function[{bb}, act[bb]]
    |>;

    SequenceNode[name_, children_] := Module[{i = 1},
      <|
        "name" -> name,
        "tick" -> Function[{bb},
          Module[{bb2 = bb, s},
            While[i <= Length[children],
              {bb2, s} = children[[i]]["tick"][bb2];
              If[s === "SUCCESS", i++, If[s === "FAILURE", i = 1; Return[{bb2, "FAILURE"}], Return[{bb2, "RUNNING"}]]]
            ];
            i = 1; {bb2, "SUCCESS"}
          ]
        ]
      |>
    ];

    FallbackNode[name_, children_] := Module[{i = 1},
      <|
        "name" -> name,
        "tick" -> Function[{bb},
          Module[{bb2 = bb, s},
            While[i <= Length[children],
              {bb2, s} = children[[i]]["tick"][bb2];
              If[s === "FAILURE", i++, If[s === "SUCCESS", i = 1; Return[{bb2, "SUCCESS"}], Return[{bb2, "RUNNING"}]]]
            ];
            i = 1; {bb2, "FAILURE"}
          ]
        ]
      |>
    ];

    (* Build a navigation BT *)
    navBT = SequenceNode["NavigateToGoal", {
      ConditionNode["HaveGoal?", haveGoalQ],
      ConditionNode["LocalizationOK?", locOKQ],
      ActionNode["GlobalPlan", globalPlan],
      FallbackNode["DriveOrRecover", {
        SequenceNode["DriveToGoal", {
          ConditionNode["PathValid?", pathValidQ],
          ActionNode["LocalControl", localControl],
          ConditionNode["GoalReached?", goalReachedQ]
        }],
        ActionNode["RecoveryClear", recoverClear]
      }]
    }];

    (* Run a small simulation *)
    bb = bb0;
    Table[
      If[t == 12, bb["obstacle"] = True];
      If[t == 25, bb["obstacle"] = False];
      {bb, s} = navBT["tick"][bb];
      {t, s, bb["dist"], bb["pathValid"], bb["recoveries"]}
    , {t, 0, 40}]
  ], "Input"],

  Cell["The table shows (t, status, distance-to-goal, pathValid, recoveries).", "Text"]
},
WindowSize -> {1000, 700},
StyleDefinitions -> "Default.nb"
]

Problem 1 (Sequence Correctness): Consider a Sequence with children \( n_1,\dots,n_M \). Prove that if all children are memoryless and return either S or F (never R), then the Sequence returns S if and only if all children return S.

Solution: Under the assumption “never R,” the Sequence ticks \( n_1 \). If it returns F, the Sequence returns F immediately. Otherwise it proceeds to \( n_2 \), and so on. If any child returns F, the overall output is F. If no child returns F, all returned S and the Sequence returns S. Conversely, if all children return S, the Sequence reaches the end and returns S. This is a direct induction on \( M \): base \( M=1 \) is trivial; inductive step appends \( n_{M+1} \) and applies the same reasoning.

Problem 2 (Fallback Correctness): For a Fallback (selector) with children \( n_1,\dots,n_M \) and no RUNNING outcomes, prove that it returns F if and only if all children return F.

Solution: The Fallback returns S immediately when it encounters a child returning S. Therefore it can return F only if every child returned F. Conversely, if every child returns F, the selector exhausts all children and returns F. QED.

Problem 3 (Determinism of HFSM Transitions): Let a mode \( s \) have two outgoing transitions with guards \( g_1(\mathbf{o},\mathbf{x}) \) and \( g_2(\mathbf{o},\mathbf{x}) \). Show that if \( g_1(\mathbf{o},\mathbf{x}) + g_2(\mathbf{o},\mathbf{x}) \le 1 \) for all \( (\mathbf{o},\mathbf{x}) \), then the next state is well-defined (unique).

Solution: At any tick, either both are false (no transition taken or default transition), or exactly one is true, selecting a unique edge. The inequality rules out the case where both are simultaneously enabled.

Problem 4 (Expected Ticks to Completion): Using the model in Section 5, solve for \( E_P \) and \( E_C \) explicitly.

Solution: From \( E_P = 1 + pE_C + (1-p)E_P \), rearrange to \( pE_P = 1 + pE_C \Rightarrow E_P = \frac{1}{p} + E_C \). Substitute into \( E_C = 1 + q(1-r)E_C + (1-q)E_P \):

\[ E_C(1 - q(1-r)) = 1 + (1-q)\left(\frac{1}{p} + E_C\right) \]

Thus \( E_C(1 - q + qr) = 1 + \frac{1-q}{p} + (1-q)E_C \), so \( E_C(qr) = 1 + \frac{1-q}{p} \Rightarrow E_C = \frac{1}{qr} + \frac{1-q}{pqr} \). Finally, \( E_P = \frac{1}{p} + E_C \).

Problem 5 (Tick-Rate Requirement): Suppose a hazard can emerge at any time and must be reacted to within \( T_{\text{react}} \). If the executive only changes commands on tick boundaries and worst-case detection delay is one tick, derive a sufficient condition on tick period \( \Delta t_{\text{tick}} \).

Solution: Worst-case: hazard appears immediately after a tick. It will be detected at the next tick (delay \( \Delta t_{\text{tick}} \)) and reacted to in that tick update. To guarantee reaction within \( T_{\text{react}} \), require \( \Delta t_{\text{tick}} \le T_{\text{react}} \).

We built a university-level foundation for navigation executives as discrete decision systems. HFSMs were modeled as deterministic guarded transition systems; BTs were formalized as tick maps returning SUCCESS/FAILURE/RUNNING with compositional Sequence/Fallback semantics. We proved core correctness properties for these compositions, introduced safety invariants and a progress certificate based on descent of distance-to-goal, and showed how tick rate constraints connect directly to real-time safety margins. The provided implementations illustrate how the same navigation policy can be encoded as either a BT or an HFSM.

Next lesson (Lesson 4) will develop recovery behaviors and fault handling systematically and connect them to monitoring signals and costmap operations.