A “full navigation stack” is not a single algorithm; it is a set of contracts between modules that operate at different rates and levels of abstraction. In this lab we assemble and validate the following contracts:

• Frame contract: a consistent transform chain among \( \{\text{map}, \text{odom}, \text{base\_link}, \text{sensors}\} \) such that commands and observations refer to the same geometry.
• Belief contract: localization produces a pose estimate and covariance consistent with motion and sensor updates (students implemented Bayes filters earlier).
• Geometry contract: layered costmaps provide inflated obstacle costs for safe motion with a tunable safety margin.
• Planning/control contract: the global plan is a geometric reference, the local controller is a tracking/avoidance policy with bounded latency and bounded command outputs.
• Orchestration contract: the behavior tree / state machine selects tasks and recovery modes to preserve liveness (the robot “keeps making progress”).

Formally, we model the closed-loop system as a hybrid dynamical system with discrete mode \( q(t) \in \mathcal{Q} \) (behavior tree node state / recovery mode) and continuous state \( \mathbf{x}(t) = [x(t), y(t), \theta(t), \mathbf{v}(t)]^\top \). The module interfaces constrain how information flows:

\[ \underbrace{\dot{\mathbf{x}}(t) = f_{q(t)}\!\left(\mathbf{x}(t), \mathbf{u}(t)\right)}_{\text{robot dynamics}} \quad,\quad \underbrace{\mathbf{u}(t) = \pi_{q(t)}\!\left(\hat{\mathbf{x}}(t), \mathcal{C}(t), \mathcal{P}(t)\right)}_{\text{local control}} \]

Here \( \hat{\mathbf{x}}(t) \) is the estimated pose/belief, \( \mathcal{C}(t) \) is the (local) costmap, and \( \mathcal{P}(t) \) is the global plan (path). This lab’s success criteria are:

• Correctness: transforms are consistent, localization is stable, plans are in the correct frame.
• Safety: command outputs respect speed/acceleration limits given latency and braking.
• Liveness: recovery transitions do not deadlock; goals either succeed or fail with diagnosable reasons.
• Observability: every failure mode produces measurable symptoms on topics/TF/state.

flowchart TD
  S["Sensors (scan, imu, wheel)"] --> E["Estimator (localization)"]
  E --> TF["TF tree: map-odom-base"]
  TF --> CM["Costmaps (global/local)"]
  CM --> GP["Global planner plugin (black-box)"]
  GP --> P["Global plan (Path)"]
  P --> LC["Local controller plugin"]
  LC --> U["cmd_vel"]
  U --> R["Robot base"]
  R --> S
  BT["Task logic (behavior tree)"] --> GP
  BT --> LC
  BT --> RC["Recovery modes"]
  RC --> BT
        

The navigation stack becomes ill-posed if frames are inconsistent. Let \( {}^{a}\mathbf{T}_{b} \in SE(2) \) be the planar transform mapping coordinates from frame \( b \) to frame \( a \). The core chain is: \( \text{map} \leftarrow \text{odom} \leftarrow \text{base\_link} \). Any point \( {}^{\text{base}}\mathbf{p} \) transforms to the map frame by:

\[ {}^{\text{map}}\mathbf{p} = {}^{\text{map}}\mathbf{T}_{\text{odom}}\; {}^{\text{odom}}\mathbf{T}_{\text{base}}\; {}^{\text{base}}\mathbf{p}. \]

Localization typically updates \( {}^{\text{map}}\mathbf{T}_{\text{odom}} \) (global drift correction), while wheel odometry integrates \( {}^{\text{odom}}\mathbf{T}_{\text{base}} \) (local smooth motion). A minimal consistency invariant is: \( {}^{\text{map}}\mathbf{T}_{\text{base}} = {}^{\text{map}}\mathbf{T}_{\text{odom}}\;{}^{\text{odom}}\mathbf{T}_{\text{base}} \) must be continuous (no implausible jumps) except when re-localization explicitly resets belief.

If pose covariance is available (e.g., AMCL / EKF localization), the transform uncertainty impacts safe margins. For planar pose \( \hat{\boldsymbol{\xi}} = [\hat{x}, \hat{y}, \hat{\theta}]^\top \) and covariance \( \mathbf{\Sigma}_{\xi} \in \mathbb{R}^{3\times 3} \), a conservative position uncertainty radius is:

\[ r_{\text{unc}}(\alpha) = \sqrt{\lambda_{\max}\!\left(\mathbf{\Sigma}_{xy}\right)}\; \sqrt{\chi^2_{2,\,\alpha}}, \quad \mathbf{\Sigma}_{xy} = \mathbf{\Sigma}_{\xi}[1\!:\!2,\,1\!:\!2]. \]

This suggests a principled inflation adjustment: \( r_{\text{infl,eff}} = r_{\text{infl}} + r_{\text{unc}}(\alpha) \) when localization is uncertain (e.g., kidnapped robot, sparse features, glass walls).

Navigation safety is limited by how fast the stack can sense, update costs, decide, and command. Let \( L \) be the end-to-end latency from sensing to applied command (including scheduling), and let \( a_{\text{brake}} > 0 \) be achievable deceleration magnitude. A conservative 1D stopping-distance model is:

\[ d_{\text{stop}}(v) = v\,L + \frac{v^2}{2a_{\text{brake}}}. \]

If the nearest obstacle free distance along the commanded direction is \( d_{\text{free}} \), a sufficient condition for collision avoidance under worst-case delay is:

\[ d_{\text{stop}}(v) \le d_{\text{free}} \quad \Longrightarrow \quad v \le v_{\max} = -a_{\text{brake}}L + \sqrt{a_{\text{brake}}^2 L^2 + 2a_{\text{brake}} d_{\text{free}} }. \]

Proof sketch: During latency \( L \), the robot continues approximately at speed \( v \), covering distance \( vL \). After a braking command is applied, constant deceleration \( -a_{\text{brake}} \) yields braking distance \( v^2/(2a_{\text{brake}}) \). Summing yields \( d_{\text{stop}}(v) \). Enforcing \( d_{\text{stop}}(v) \le d_{\text{free}} \) and solving the quadratic inequality in \( v \) gives the bound.

In practice, \( L \) depends on: sensor rate, costmap update rate, global replanning rate, controller frequency, and executor scheduling. Your lab report should state measured rates and the resulting conservative \( v_{\max} \) for your platform.

In Chapter 14 Lesson 2 you introduced costmaps conceptually; here we calibrate them for an integrated stack. Let the obstacle distance field be \( d(\mathbf{s}) \) for grid cell center \( \mathbf{s} \), and let \( r_{\text{ins}} \) be the inscribed radius (robot footprint) and \( r_{\text{infl}} \) the inflation radius. A standard continuous inflation model is:

\[ c(\mathbf{s}) = \begin{cases} c_{\text{lethal}} & d(\mathbf{s}) = 0 \\ c_{\text{inscribed}} & 0 < d(\mathbf{s}) \le r_{\text{ins}} \\ (c_{\text{inscribed}}-1)\exp\!\big(-k(d(\mathbf{s})-r_{\text{ins}})\big) + 1 & r_{\text{ins}} < d(\mathbf{s}) \le r_{\text{infl}} \\ 0 & d(\mathbf{s}) > r_{\text{infl}} \end{cases} \]

A practical calibration uses corridor clearance. If the minimum corridor half-width is \( w_{\min} \), a conservative geometric constraint is: \( r_{\text{ins}} + r_{\text{unc}}(\alpha) \le w_{\min} \) and choose \( r_{\text{infl}} \approx w_{\min} \) so that the controller sees meaningful gradients without blocking feasible corridors.

Lab check: if your robot refuses to pass through a corridor that is physically traversable, your effective margin is too large (inflate too much, uncertainty too large, footprint too big, or map too conservative). If it scrapes walls, margin is too small or latency bound is violated (Section 3).

The exact commands differ by your course setup (simulation vs real robot), but the integration logic is invariant:

1. TF + robot description: publish robot footprint frames and sensor frames (static or via robot_state_publisher). Validate that \( \text{base\_link} \) to sensors is correct.
2. Odometry: publish /odom and odom ← base_link TF at high rate.
3. Localization: provide /amcl_pose (or EKF output) and map ← odom TF.
4. Costmaps: configure global and local costmaps (obstacle + inflation + optional voxel layers).
5. Planner interface: ensure global planner publishes a plan in the correct global frame (map).
6. Controller interface: ensure controller consumes plan + costmap and outputs bounded /cmd_vel.
7. Task logic: behavior tree activates planning, following, and recovery nodes; confirm lifecycle states are ACTIVE.

Minimal acceptance tests:

• TF acceptance: lookup transforms among \( \{\text{map},\text{odom},\text{base\_link}\} \) at runtime with no extrapolation errors.
• Frame acceptance: a commanded goal in map leads to motion in the correct direction in the real world.
• Costmap acceptance: a nearby obstacle increases local cost and triggers avoidance or stop.
• Recovery acceptance: forced failure (block path) triggers recovery, then resumes navigation or fails cleanly.

Treat debugging as identifying which contract is violated, using measurable signals. Common symptom-to-cause patterns:

• Robot does not move, no errors: lifecycle node not ACTIVE; behavior tree stuck waiting for transform; action server unreachable.
• Robot moves in wrong direction: frame mismatch (goal in map but controller interprets in another frame), or base_link orientation is inconsistent.
• Robot oscillates near obstacles: inflation too sharp (large \( k \)), controller rate too low, or latency too high.
• Robot “teleports” on map: localization resets; map-odom jump; inconsistent timestamps.

A useful quantitative check is to compare commanded speed with the conservative safe bound \( v \le v_{\max} \) from Section 3 for your measured latency \( L \).

This client sends a navigation goal and logs time-to-goal and odometry path length. It is deliberately “architecture-first”: we test the stack boundary (action goal + feedback), not planner internals.

File: Chapter14_Lesson5.py

#!/usr/bin/env python3
"""
Chapter 14 - Lesson 5: Lab: Assemble a Full Navigation Stack (ROS 2 Nav2)

This script demonstrates end-to-end integration testing of a navigation stack:
- Wait for Nav2 lifecycle bringup
- Set an initial pose (AMCL)
- Send a NavigateToPose goal (action)
- Log simple metrics (time-to-goal, path length from odom)
- Optionally subscribe to a global plan topic (nav_msgs/Path) if available
"""

from __future__ import annotations

import math
import time
from dataclasses import dataclass
from typing import Optional, Tuple

import rclpy
from rclpy.node import Node

from geometry_msgs.msg import PoseStamped, PoseWithCovarianceStamped
from nav_msgs.msg import Odometry, Path

try:
    from nav2_simple_commander.robot_navigator import BasicNavigator, TaskResult
except Exception:
    BasicNavigator = None
    TaskResult = None


@dataclass
class Metrics:
    start_time: float = 0.0
    end_time: float = 0.0
    odom_path_length_m: float = 0.0
    min_goal_dist_m: float = float("inf")
    goal_reached: bool = False


class NavMetricsNode(Node):
    def __init__(self):
        super().__init__("chapter14_lesson5_nav_metrics")

        self.metrics = Metrics()
        self._last_odom_xy: Optional[Tuple[float, float]] = None
        self._goal_xy: Optional[Tuple[float, float]] = None
        self._latest_plan: Optional[Path] = None

        self.create_subscription(Odometry, "/odom", self._on_odom, 50)
        self.create_subscription(PoseWithCovarianceStamped, "/amcl_pose", self._on_amcl, 20)
        self.create_subscription(Path, "/plan", self._on_plan, 10)
        self.create_subscription(Path, "/global_plan", self._on_plan, 10)

    def set_goal_xy(self, x: float, y: float) -> None:
        self._goal_xy = (x, y)

    def _on_odom(self, msg: Odometry) -> None:
        x = msg.pose.pose.position.x
        y = msg.pose.pose.position.y
        if self._last_odom_xy is not None:
            dx = x - self._last_odom_xy[0]
            dy = y - self._last_odom_xy[1]
            self.metrics.odom_path_length_m += math.hypot(dx, dy)
        self._last_odom_xy = (x, y)

        if self._goal_xy is not None:
            d = math.hypot(x - self._goal_xy[0], y - self._goal_xy[1])
            self.metrics.min_goal_dist_m = min(self.metrics.min_goal_dist_m, d)

    def _on_amcl(self, msg: PoseWithCovarianceStamped) -> None:
        pass

    def _on_plan(self, msg: Path) -> None:
        self._latest_plan = msg

    def report(self) -> str:
        dt = max(0.0, self.metrics.end_time - self.metrics.start_time)
        return (
            f"Goal reached: {self.metrics.goal_reached}\n"
            f"Time-to-goal (s): {dt:.3f}\n"
            f"Odom path length (m): {self.metrics.odom_path_length_m:.3f}\n"
            f"Closest distance to goal (m): {self.metrics.min_goal_dist_m:.3f}\n"
            f"Plan received: {self._latest_plan is not None}\n"
        )


def make_pose_stamped(node: Node, x: float, y: float, yaw_rad: float, frame_id: str = "map") -> PoseStamped:
    pose = PoseStamped()
    pose.header.frame_id = frame_id
    pose.header.stamp = node.get_clock().now().to_msg()
    pose.pose.position.x = float(x)
    pose.pose.position.y = float(y)
    pose.pose.position.z = 0.0
    pose.pose.orientation.x = 0.0
    pose.pose.orientation.y = 0.0
    pose.pose.orientation.z = math.sin(yaw_rad / 2.0)
    pose.pose.orientation.w = math.cos(yaw_rad / 2.0)
    return pose


def main():
    if BasicNavigator is None:
        raise RuntimeError("nav2_simple_commander not available. Install Nav2, then source ROS 2.")

    rclpy.init()
    metrics_node = NavMetricsNode()
    navigator = BasicNavigator()

    navigator.waitUntilNav2Active(localizer="amcl")

    init_pose = make_pose_stamped(metrics_node, x=0.0, y=0.0, yaw_rad=0.0, frame_id="map")
    navigator.setInitialPose(init_pose)

    goal_x, goal_y, goal_yaw = 2.0, 0.5, 0.0
    goal_pose = make_pose_stamped(metrics_node, goal_x, goal_y, goal_yaw, frame_id="map")
    metrics_node.set_goal_xy(goal_x, goal_y)

    metrics_node.metrics.start_time = time.time()
    navigator.goToPose(goal_pose)

    while not navigator.isTaskComplete():
        rclpy.spin_once(metrics_node, timeout_sec=0.1)
        fb = navigator.getFeedback()
        if fb is not None and getattr(fb, "distance_remaining", None) is not None:
            metrics_node.get_logger().info(f"Distance remaining: {fb.distance_remaining:.3f} m")
        time.sleep(0.05)

    metrics_node.metrics.end_time = time.time()
    result = navigator.getResult()
    metrics_node.metrics.goal_reached = (result == TaskResult.SUCCEEDED)

    for _ in range(10):
        rclpy.spin_once(metrics_node, timeout_sec=0.1)

    print("\n=== Chapter14 Lesson5 Metrics Report ===")
    print(metrics_node.report())

    metrics_node.destroy_node()
    rclpy.shutdown()


if __name__ == "__main__":
    main()
      

Exercise file: a TF/topic health-check helps localize failures to the correct contract.

File: Chapter14_Lesson5_Ex1.py

#!/usr/bin/env python3
"""
Chapter 14 - Lesson 5 (Exercise 1): Health-check checklist for a Nav stack
"""

from __future__ import annotations
import time
import rclpy
from rclpy.node import Node

try:
    from tf2_ros import Buffer, TransformListener
except Exception:
    Buffer = None
    TransformListener = None

REQUIRED_TOPICS = ["/tf", "/tf_static", "/odom", "/scan", "/amcl_pose", "/cmd_vel"]
REQUIRED_FRAMES = [("map", "odom"), ("odom", "base_link"), ("base_link", "laser")]

class HealthCheck(Node):
    def __init__(self):
        super().__init__("chapter14_lesson5_healthcheck")
        self.buffer = Buffer() if Buffer is not None else None
        self.listener = TransformListener(self.buffer, self) if self.buffer is not None else None

    def topic_ok(self, name: str) -> bool:
        topics = [t[0] for t in self.get_topic_names_and_types()]
        return name in topics

    def tf_ok(self, parent: str, child: str, timeout_s: float = 1.0) -> bool:
        if self.buffer is None:
            return False
        t0 = time.time()
        while time.time() - t0 < timeout_s:
            try:
                self.buffer.lookup_transform(parent, child, rclpy.time.Time())
                return True
            except Exception:
                rclpy.spin_once(self, timeout_sec=0.05)
        return False

def main():
    rclpy.init()
    node = HealthCheck()

    print("\n=== Topic checks ===")
    for t in REQUIRED_TOPICS:
        print(f"{t:20s}: {'OK' if node.topic_ok(t) else 'MISSING'}")

    print("\n=== TF checks ===")
    for parent, child in REQUIRED_FRAMES:
        print(f"{parent:10s} -> {child:10s}: {'OK' if node.tf_ok(parent, child, 1.5) else 'FAIL'}")

    node.destroy_node()
    rclpy.shutdown()

if __name__ == "__main__":
    main()
      

The C++ client is the same architectural test: it validates that the action server is reachable, feedback arrives, and the stack can execute a goal. This is a high-signal diagnostic tool when Python tooling is not available.

File: Chapter14_Lesson5.cpp

/*
Chapter 14 - Lesson 5: Lab: Assemble a Full Navigation Stack
*/

#include <chrono>
#include <cmath>
#include <memory>
#include <string>

#include "rclcpp/rclcpp.hpp"
#include "rclcpp_action/rclcpp_action.hpp"

#include "geometry_msgs/msg/pose_stamped.hpp"
#include "nav2_msgs/action/navigate_to_pose.hpp"

using NavigateToPose = nav2_msgs::action::NavigateToPose;
using GoalHandleNavigateToPose = rclcpp_action::ClientGoalHandle<NavigateToPose>;

static geometry_msgs::msg::PoseStamped make_goal_pose(
    rclcpp::Node* node, double x, double y, double yaw_rad, const std::string& frame_id = "map")
{
  geometry_msgs::msg::PoseStamped pose;
  pose.header.frame_id = frame_id;
  pose.header.stamp = node->get_clock()->now();

  pose.pose.position.x = x;
  pose.pose.position.y = y;
  pose.pose.position.z = 0.0;

  pose.pose.orientation.z = std::sin(yaw_rad / 2.0);
  pose.pose.orientation.w = std::cos(yaw_rad / 2.0);
  return pose;
}

class Chapter14Lesson5NavClient : public rclcpp::Node
{
public:
  Chapter14Lesson5NavClient() : Node("chapter14_lesson5_nav_client")
  {
    client_ = rclcpp_action::create_client<NavigateToPose>(this, "navigate_to_pose");
    timer_ = this->create_wall_timer(std::chrono::milliseconds(200),
      std::bind(&Chapter14Lesson5NavClient::tick, this));
  }

private:
  void tick()
  {
    if (sent_) return;

    if (!client_->wait_for_action_server(std::chrono::seconds(1))) {
      RCLCPP_INFO(this->get_logger(), "Waiting for /navigate_to_pose action server...");
      return;
    }

    NavigateToPose::Goal goal;
    goal.pose = make_goal_pose(this, 2.0, 0.5, 0.0, "map");

    auto opts = rclcpp_action::Client<NavigateToPose>::SendGoalOptions();
    opts.feedback_callback =
      [this](GoalHandleNavigateToPose::SharedPtr,
             const std::shared_ptr<const NavigateToPose::Feedback> feedback) {
        if (feedback && std::isfinite(feedback->distance_remaining)) {
          RCLCPP_INFO(this->get_logger(), "Distance remaining: %.3f m", feedback->distance_remaining);
        }
      };
    opts.result_callback =
      [this](const GoalHandleNavigateToPose::WrappedResult & result) {
        if (result.code == rclcpp_action::ResultCode::SUCCEEDED) {
          RCLCPP_INFO(this->get_logger(), "Goal reached (SUCCEEDED).");
        } else {
          RCLCPP_WARN(this->get_logger(), "Navigation finished with code: %d", (int)result.code);
        }
        rclcpp::shutdown();
      };

    RCLCPP_INFO(this->get_logger(), "Sending NavigateToPose goal...");
    client_->async_send_goal(goal, opts);
    sent_ = true;
  }

  rclcpp_action::Client<NavigateToPose>::SharedPtr client_;
  rclcpp::TimerBase::SharedPtr timer_;
  bool sent_{false};
};

int main(int argc, char** argv)
{
  rclcpp::init(argc, argv);
  auto node = std::make_shared<Chapter14Lesson5NavClient>();
  rclcpp::spin(node);
  rclcpp::shutdown();
  return 0;
}
      

Java integration is useful in enterprise stacks (fleet management, task services). The file below is a template: students adapt it to their installed ROS 2 Java toolchain. The architectural point is that the boundary is still an action goal + feedback + result, independent of language.

File: Chapter14_Lesson5.java

/*
Chapter 14 - Lesson 5: Lab: Assemble a Full Navigation Stack
*/

package amr.chapter14.lesson5;

import org.ros2.rcljava.RCLJava;
import org.ros2.rcljava.node.Node;

import geometry_msgs.msg.PoseStamped;
import nav2_msgs.action.NavigateToPose;

public class Chapter14Lesson5 {
  private static PoseStamped makeGoalPose(Node node, double x, double y, double yawRad, String frameId) {
    PoseStamped pose = new PoseStamped();
    pose.getHeader().setFrameId(frameId);
    pose.getHeader().setStamp(node.getClock().now().toMsg());

    pose.getPose().getPosition().setX(x);
    pose.getPose().getPosition().setY(y);
    pose.getPose().getPosition().setZ(0.0);

    pose.getPose().getOrientation().setZ(Math.sin(yawRad / 2.0));
    pose.getPose().getOrientation().setW(Math.cos(yawRad / 2.0));
    return pose;
  }

  public static void main(String[] args) throws Exception {
    RCLJava.rclJavaInit();
    Node node = RCLJava.createNode("chapter14_lesson5_nav_client_java");

    // PSEUDOCODE:
    //   ActionClient<NavigateToPose.Goal, NavigateToPose.Result, NavigateToPose.Feedback> client =
    //     new ActionClient<>(node, NavigateToPose.class, "navigate_to_pose");
    //   client.waitForServer(...);
    //   NavigateToPose.Goal goal = new NavigateToPose.Goal();
    //   goal.setPose(makeGoalPose(node, 2.0, 0.5, 0.0, "map"));
    //   client.sendGoal(goal, feedbackCb, resultCb);

    NavigateToPose.Goal goal = new NavigateToPose.Goal();
    goal.setPose(makeGoalPose(node, 2.0, 0.5, 0.0, "map"));

    node.getLogger().info("Template prepared. Implement ActionClient calls for your rcljava version.");

    for (int i = 0; i < 20; i++) {
      RCLJava.spinOnce(node, 100);
      Thread.sleep(100);
    }

    node.dispose();
    RCLJava.shutdown();
  }
}
      

MATLAB is useful for rapid experiment design, plotting, and parameter sweeps. This script sends a navigation goal and monitors localization. In Simulink, you can replace the monitoring loop with ROS 2 subscriber blocks and log signals.

File: Chapter14_Lesson5.m

% Chapter 14 - Lesson 5: Lab: Assemble a Full Navigation Stack

clear; clc;

node = ros2node("/chapter14_lesson5_matlab_client");
amclSub = ros2subscriber(node, "/amcl_pose", "geometry_msgs/PoseWithCovarianceStamped");

ac = ros2actionclient(node, "/navigate_to_pose", "nav2_msgs/NavigateToPose");
fprintf("Waiting for action server...\n");
waitForServer(ac);

goalMsg = ros2message(ac);
goalMsg.pose.header.frame_id = "map";
goalMsg.pose.header.stamp = ros2time(node);

goalMsg.pose.pose.position.x = 2.0;
goalMsg.pose.pose.position.y = 0.5;

yaw = 0.0;
goalMsg.pose.pose.orientation.z = sin(yaw/2.0);
goalMsg.pose.pose.orientation.w = cos(yaw/2.0);

fprintf("Sending goal...\n");
gh = sendGoal(ac, goalMsg);

tStart = tic;
while ~isDone(gh)
    fb = getFeedback(gh);
    if ~isempty(fb)
        fprintf("Distance remaining: %.3f m\n", fb.distance_remaining);
    end

    if amclSub.NewMessageAvailable
        amcl = receive(amclSub, 0.01);
        x = amcl.pose.pose.position.x;
        y = amcl.pose.pose.position.y;
        fprintf("AMCL pose: (%.3f, %.3f)\n", x, y);
    end
    pause(0.1);
end

res = getResult(gh);
tGoal = toc(tStart);

fprintf("\n=== Chapter14 Lesson5 Report ===\n");
fprintf("Time-to-goal (s): %.3f\n", tGoal);
disp(res);

clear node;
      

A full navigation stack should be evaluated quantitatively. The notebook below analyzes a CSV log (exported from your run) and computes path length, speed statistics, curvature, and a conservative safe-speed bound based on measured latency and braking.

File: Chapter14_Lesson5.nb

(* Chapter 14 - Lesson 5: Offline analysis of navigation logs *)

ClearAll["Global`*"];

csvPath = "nav_run_log.csv";  (* edit to your exported log file *)
If[!FileExistsQ[csvPath],
  Print["CSV file not found. Export a CSV named nav_run_log.csv in the working directory."];
];

data = Import[csvPath, "CSV"];
data2 = If[VectorQ[data[[1]], StringQ], Rest[data], data] // N;

{t, x, y, yaw, v, w} = Transpose[data2];

xy = Transpose[{x, y}];
seg = Differences[xy];
segLen = Norm /@ seg;
pathLen = Total[segLen];

vStats = {Mean[v], Max[v]};
wStats = {Mean[Abs[w]], Max[Abs[w]]};

eps = 1.*^-3;
kappa = w / Map[If[Abs[#] < eps, Sign[#]*eps + eps, #] &, v];
kStats = {Mean[Abs[kappa]], Max[Abs[kappa]]};

Print["Path length (m): ", pathLen];
Print["Mean v (m/s), Max v (m/s): ", vStats];
Print["Mean |w| (rad/s), Max |w| (rad/s): ", wStats];
Print["Mean |kappa| (1/m), Max |kappa| (1/m): ", kStats];

(* Safety bound: d_stop = v*L + v^2/(2 a_brake) <= d_free *)
L = 0.25;     (* seconds *)
aBrake = 1.5; (* m/s^2 *)
dFree = 0.8;  (* m *)
sol = Solve[vVar*L + vVar^2/(2 aBrake) <= dFree && vVar >= 0, vVar, Reals];
vMax = Max[0, vVar /. sol[[1]]];
Print["Conservative v_max (m/s): ", N[vMax]];

ListLinePlot[Transpose[{t, v}], Frame -> True, FrameLabel -> {"t (s)", "v (m/s)"}, PlotLabel -> "Linear speed"]
ListLinePlot[Transpose[{t, Abs[w]}], Frame -> True, FrameLabel -> {"t (s)", "|w| (rad/s)"}, PlotLabel -> "Angular rate"]
ListLinePlot[Transpose[{x, y}], Frame -> True, FrameLabel -> {"x (m)", "y (m)"}, PlotLabel -> "Trajectory (x,y)"]
      

Problem 1 (Transform Chain Validation): Suppose at time \( t \) you have transforms \( {}^{\text{map}}\mathbf{T}_{\text{odom}} \) and \( {}^{\text{odom}}\mathbf{T}_{\text{base}} \). Derive the expression for \( {}^{\text{map}}\mathbf{T}_{\text{base}} \) and state one measurable symptom if the chain is inconsistent.

Solution: The composition is:

\[ {}^{\text{map}}\mathbf{T}_{\text{base}} = {}^{\text{map}}\mathbf{T}_{\text{odom}}\;{}^{\text{odom}}\mathbf{T}_{\text{base}}. \]

If inconsistent, you commonly observe TF extrapolation errors or “goal moves backward” effects: a goal in map produces commands in a rotated/transposed base frame, visible as motion in the wrong direction relative to the map visualization.

Problem 2 (Latency-Limited Speed Bound): Given latency \( L \), braking capability \( a_{\text{brake}} \), and free distance \( d_{\text{free}} \), derive a bound on speed to satisfy \( d_{\text{stop}}(v) \le d_{\text{free}} \).

Solution: Use \( d_{\text{stop}}(v)=vL + \frac{v^2}{2a_{\text{brake}}} \). Solve the quadratic inequality:

\[ vL + \frac{v^2}{2a_{\text{brake}}} \le d_{\text{free}} \;\Longleftrightarrow\; v^2 + 2a_{\text{brake}}Lv - 2a_{\text{brake}}d_{\text{free}} \le 0. \]

The admissible nonnegative solution is:

\[ v \le -a_{\text{brake}}L + \sqrt{a_{\text{brake}}^2 L^2 + 2a_{\text{brake}} d_{\text{free}} }. \]

Problem 3 (Inflation Feasibility): A corridor has half-width \( w_{\min} \). The robot inscribed radius is \( r_{\text{ins}} \) and localization uncertainty radius is \( r_{\text{unc}} \). Give a sufficient condition for the corridor to remain feasible under conservative planning.

Solution: A sufficient feasibility condition is: \( r_{\text{ins}} + r_{\text{unc}} \le w_{\min} \). If violated, even perfect planning may reject the corridor because the conservative footprint does not fit. One remedy is reducing uncertainty (better localization) rather than shrinking safety margins blindly.

Problem 4 (Contract-Based Debugging): You send a goal and the action server responds, but the robot never moves. Provide a diagnostic decision procedure that isolates the failure to (i) TF contract, (ii) costmap contract, (iii) controller/output contract.

Solution (procedure):

flowchart TD
  A["Goal accepted, no motion"] --> B["Is cmd_vel published?"]
  B -->|no| C["Check controller active + lifecycle states"]
  C --> D["Check TF lookup map-odom-base"]
  D -->|fail| TFERR["Fix TF \ntimestamps/frames"]
  D -->|ok| E["Check controller \nconstraints (v,w limits)"]
  B -->|yes| F["Robot base consumes cmd_vel?"]
  F -->|no| BASE["Fix base driver / \nmux / safety stop"]
  F -->|yes| G["Is local costmap \nall lethal?"]
  G -->|yes| CMERR["Fix obstacle layer / \ninflation / footprint"]
  G -->|no| H["Check progress checker / \nrecovery loops"]
        

Problem 5 (Liveness vs Safety Trade-off): Explain why overly aggressive recovery (frequent spins/backups) can reduce liveness in cluttered environments, and propose one measurable criterion to tune recovery thresholds.

Solution: Aggressive recovery can cause repeated mode switching (hybrid “chattering”), consuming time without reducing costmap risk. A measurable tuning criterion is a progress metric such as: \( \Delta d = d_{\text{goal}}(t) - d_{\text{goal}}(t+T) \). Trigger recovery only if \( \Delta d \le \epsilon \) for a dwell time \( T \), while also enforcing safety constraints (Section 3). This reduces unnecessary recoveries when slow progress is expected.

You assembled a full navigation stack by enforcing architectural contracts: consistent TF chains, stable localization, calibrated layered costmaps, planner/controller boundary correctness, and behavior-tree orchestration with recoveries. You also derived and applied a latency-aware safety bound on speed and used contract-based observability to debug failures. This integration mindset is the foundation for Chapter 15, where local motion generation methods are studied in detail.