Autonomous Navigation and the ROS Nav Stack

Autonomous navigation on a mobile robot is a closed loop, not a one-shot computation:

1. Perceive — laser scan or depth image update the costmap; wheel odometry and IMU feed a state estimator; AMCL fuses them to produce a pose estimate $\hat{q}(t)$.
2. Plan — given $\hat{q}$ and the goal $q_g$, the global planner computes a reference path through the static map; the local planner computes velocity commands $(v, \omega)$ to follow it while avoiding dynamic obstacles.
3. Act — velocity commands go to the motor controllers; encoders report back wheel speeds; the loop repeats at 10–50 Hz.

This loop never fully stops: the robot replans the local trajectory on every cycle, so a pedestrian stepping into the path triggers an immediate response without global replanning. The architectural split between a slow global planner ($\sim$1 Hz) and a fast local planner ($\sim$10–50 Hz) is the central design decision of the ROS navigation stack.

The navigation stack maintains two layered costmaps:

Global costmap: built from a static occupancy grid (SLAM output or a hand-drawn map) plus the robot's inflation radius. Each cell gets a cost $c \in [0, 254]$: 254 = lethal obstacle, 128 = inscribed (robot centre would collide), 1–127 = inflated zone (penalised but traversable), 0 = free. The global planner finds the lowest-cost path.

Local costmap: a rolling window (typically $4 \times 4$ m) around the robot, updated in real time from sensor data. Dynamic obstacles appear here. The local planner operates on this window and ignores anything outside it.

Cell costs in the inflation layer decay exponentially: $c(d) = 128 \cdot e^{-\alpha(d - r_{\text{robot}})}$ for $d > r_{\text{robot}}$, where $\alpha$ controls decay rate. Tuning $\alpha$ and inflation radius is a routine task: too aggressive and corridors become impassable; too lenient and the robot clips walls.

Data flows from sensors through costmaps to planners and out to the robot's actuators.

The global planner receives: a goal pose from a higher-level behaviour, the current pose from AMCL, and the global costmap. It runs a graph search (typically A* or the ROS-native NavFn, a wavefront propagation equivalent to Dijkstra) on the costmap grid.

Path cost combines distance and cell cost: $f(n) = g(n) + h(n), \quad w(n,m) = \text{dist}(n,m) \cdot (1 + k \cdot c(m))$

where $k$ scales obstacle avoidance weight relative to distance. Higher $k$ produces paths that hug the centre of corridors; lower $k$ produces shorter paths that graze walls.

ROS2 / Nav2 ships three global planners:

• NavFn — Dijkstra-based, reliable, slow on large maps.
• Smac Planner (2D) — A* with SE(2) lattice, respects turning radius.
• Smac Planner (Hybrid-A)* — full SE(2) search with Reeds-Shepp curves, mandatory for Ackermann vehicles that cannot rotate in place.

For a differential-drive robot in a warehouse, NavFn or Smac 2D is standard. For a forklift or car-like robot, Hybrid-A* is not optional.

The local planner converts the global reference path into safe velocity commands $(v, \omega)$ in real time. Two algorithms dominate:

Dynamic Window Approach (DWA): sample the reachable $(v, \omega)$ space (the dynamic window — velocities achievable within one timestep given acceleration limits), simulate each forward $T_{\text{sim}}$ seconds, score on heading to goal, distance to obstacles, and forward speed, command the best. Simple, fast, works in most indoor environments. Weakness: myopic — fails in U-shaped obstacles and narrow gaps.

Timed Elastic Band (TEB): represent the local trajectory as a sequence of poses with time stamps. Formulate trajectory optimisation as a sparse nonlinear least-squares problem — minimising path length, time, and obstacle clearance subject to kinematic constraints. Solved with $g^2o$ in $<10\,\text{ms}$ on a laptop CPU. TEB handles non-holonomic constraints natively, supports Ackermann and omni robots, and is better in tight spaces. Cost: more tuning parameters.

For new projects: start with DWA for simplicity; switch to TEB when the robot gets stuck in corridors or needs smooth curvature.

The global planner is only as good as the pose estimate it receives. AMCL (Adaptive Monte Carlo Localization) represents the robot's posterior pose distribution as a set of $N$ weighted particles $\{(q^{(i)}, w^{(i)})\}_{i=1}^N$:

1. Prediction: propagate each particle using the motion model $p(q_t \mid q_{t-1}, u_t)$ — typically a noisy odometry model.
2. Update: weight each particle by the likelihood $p(z_t \mid q^{(i)}, m)$ of the laser scan $z_t$ given the map $m$ and that particle's pose.
3. Resample: draw $N$ new particles proportional to weights (low-variance resampling to avoid particle depletion).

AMCL is asymptotically unbiased but has finite-sample noise. The "kidnapped robot" problem (sudden large displacement) causes particle collapse; global re-localisation requires spreading particles uniformly. In practice, AMCL with $N = 500$–$2000$ particles at 10 Hz is standard for indoor mobile robots with known maps. Without a good map — or outdoors — switch to LiDAR odometry + ICP (LOAM, KISS-ICP) instead.

Robots get stuck. A door closes, a box falls into the corridor, or sensor noise inflates an obstacle into the path. The navigation stack handles these with a recovery behavior chain executed in order when the local planner fails to make progress:

1. Clear costmap — erase the local costmap and try again. Handles transient sensor noise.
2. In-place rotation — rotate 360° to update the local map with fresh sensor data.
3. Back-up — move backwards a short distance to escape a tight corner.
4. Aggressive clear — wipe a larger portion of the global costmap and replan globally.

If all recoveries fail, the navigation stack aborts and reports failure to the behaviour tree or state machine above it. This escalation is intentional: it keeps the local planner from thrashing indefinitely and gives a higher-level arbitrator (human operator, replanning service) a chance to respond.

Well-tuned recovery behaviours are what separate a demo robot from a robot that works all day in a warehouse. Allocate more testing time to edge cases than to the happy path.

A minimal ROS2 Python node that publishes geometry_msgs/Twist commands to a robot illustrates the publish-subscribe pattern at the core of the nav stack:

import rclpy
from rclpy.node import Node
from geometry_msgs.msg import Twist

class VelPublisher(Node):
    def __init__(self):
        super().__init__('vel_publisher')
        self.pub = self.create_publisher(
            Twist, '/cmd_vel', qos_profile=10)
        # 10 Hz control loop
        self.timer = self.create_timer(0.1, self.timer_cb)
        self.get_logger().info('VelPublisher started')

    def timer_cb(self):
        msg = Twist()
        msg.linear.x  = 0.3   # m/s forward
        msg.angular.z = 0.1   # rad/s left turn
        self.pub.publish(msg)

def main():
    rclpy.init()
    node = VelPublisher()
    rclpy.spin(node)
    node.destroy_node()
    rclpy.shutdown()

if __name__ == '__main__':
    main()

In the full nav stack, cmd_vel is written by the local planner, not a hand-coded timer. The robot's hardware interface subscribes to /cmd_vel and converts it to wheel speeds via inverse kinematics. The same topic name is the interface contract across every layer.

Assembling the nav stack is only the beginning. Real deployments hit problems that benchmarks do not:

Map drift: AMCL confidence falls when the environment changes (moved furniture, seasonal lighting for camera-based maps). Periodic re-mapping or a live SLAM layer (e.g. Cartographer alongside AMCL) maintains accuracy.

Sensor calibration: laser-to-base-link transform errors of even $1\,\text{cm}$ degrade costmap accuracy and cause the robot to clip walls. Verify tf2 transforms with ros2 run tf2_tools view_frames before trusting the planner.

Footprint shape: the nav stack supports non-circular footprints (polygons). Using a circle for a rectangular robot wastes corridor width; the correct polygon keeps the full corridor available.

Latency: cmd_vel latency above $\sim 100\,\text{ms}$ destabilises DWA on fast robots. Profile the full pipeline — scan → costmap update → local plan → command — and identify bottlenecks.

The ROS navigation stack is mature software. Its failure modes are well-documented. Read the Nav2 tuning guide before filing bug reports — nine times out of ten, the answer is a parameter change, not a code fix.