Costmap

Costmaps are composed of multiple semantic layers that are combined into a final master grid for planning:

• Static Map Layer: Loads pre-built SLAM maps representing permanent structures like walls
• Obstacle Layer: Dynamically marks transient objects detected by LiDAR, depth cameras, or radar
• Inflation Layer: Expands obstacle boundaries by the robot's inscribed radius plus a safety buffer
• Voxel Layer: Handles 3D sensor data for platforms operating in non-planar environments Each layer independently updates at configurable rates, enabling efficient sensor fusion without blocking the planning cycle.

Each cell in a costmap stores a probability value between 0 (free) and 255 (occupied), with 128 representing unknown space. This probabilistic framework accommodates sensor noise and transient readings:

• Log-odds representation: Probabilities are stored in log-odds form for efficient Bayesian updates
• Thresholding: Cells above a configurable occupancy threshold are treated as lethal obstacles
• Unknown handling: Planners can be configured to treat unknown cells as free (optimistic) or lethal (conservative) The grid resolution is typically 0.05 meters per cell, balancing computational efficiency with spatial precision.

The inflation layer propagates costs outward from detected obstacles using a decay function, creating a gradient that planners use to maintain safe distances:

• Inscribed radius: The hard boundary within which the robot would certainly collide
• Circumscribed radius: The outer limit of the inflation field, defining the maximum cost influence
• Exponential decay: Cost values decrease exponentially from lethal (254) to neutral (0) with distance This gradient enables planners like the Dynamic Window Approach (DWA) to prefer paths that maximize clearance from obstacles rather than skimming dangerously close to boundaries.

Local costmaps operate within a sliding window centered on the robot, typically 3-5 meters square, that moves with the platform. This design provides several advantages:

• Constant memory footprint: Only the local vicinity is maintained, regardless of global map size
• Sensor horizon alignment: The window size matches the effective range of onboard perception sensors
• Dynamic obstacle handling: Transient objects enter and exit the window naturally as the robot moves
• Coordinate frame transformation: Sensor data is continuously transformed from sensor frames into the robot's base frame using TF2 transforms The global costmap, by contrast, maintains a larger fixed frame for high-level route planning.

Costmaps implement observation buffers that collect and fuse data from heterogeneous sensors before updating the grid:

• Clearing observations: Sensor rays that pass through space mark cells as free, removing stale obstacle markings
• Marking observations: Returns that hit surfaces mark cells as occupied
• Temporal filtering: Obstacles must persist for a minimum number of observations before being marked, reducing false positives from sensor noise
• Maximum range filtering: Readings beyond a configurable distance are discarded to prevent inaccurate far-field data from corrupting the map This fusion enables robust operation even when individual sensors produce noisy or intermittent readings.

Beyond binary free/occupied states, costmaps support custom cost functions that encode task-specific preferences into the planning space:

• Lethal zones: Absolute no-go areas like safety cages or cliff edges
• Neutral zones: Preferred travel corridors with zero additional cost
• Penalty zones: Areas with elevated but non-lethal costs, such as high-traffic intersections or fragile flooring
• Speed restriction zones: Regions where the planner should reduce velocity without altering the path These zones can be injected via polygon-stamped messages from behavior trees or mission planners, enabling dynamic reconfiguration of navigation policies without modifying the map.

A volumetric representation where each voxel stores the shortest distance to the nearest obstacle surface, with negative values indicating interior points. SDFs enable efficient collision checking by simply evaluating the signed distance at a query point. In costmap contexts, SDFs provide a mathematically smooth alternative to binary occupancy grids for gradient-based trajectory optimization.

A costmap plugin that artificially expands obstacle boundaries by propagating cost values outward from detected obstacles. This creates a safety buffer that accounts for the robot's physical footprint and localization uncertainty. The inflation radius is typically set to at least half the robot's inscribed radius, with cost decay functions—linear, exponential, or Gaussian—determining how quickly costs drop with distance.

A real-time control strategy that solves a finite-horizon optimization problem at each timestep using the costmap as its cost function landscape. MPC evaluates candidate trajectories against the costmap's occupancy probabilities, selecting the path that minimizes cumulative cost while respecting dynamic constraints. This enables smooth, anticipatory obstacle avoidance rather than reactive stopping.

A velocity-space local planner that samples achievable linear and angular velocities within the robot's dynamic window, then scores each trajectory against the costmap. DWA evaluates clearance, heading alignment, and velocity maximization to select the optimal command. It is the default local planner in ROS navigation and relies directly on the costmap for real-time obstacle scoring.

A 3D-aware costmap plugin that maintains a volumetric occupancy grid to handle obstacles that overhang or are sensed at different heights. Unlike the flat obstacle layer, the voxel layer can mark space above the robot as free while marking columns at ground level as occupied. This is critical for robots operating under tables, shelves, or in environments with non-floor-level obstructions.