Goal pose off the gloabl costmap with nav2 and slam-toolbox

You’re not doing anything “wrong”. What you’re seeing is by design:

• In Nav2, the global costmap geometry is fixed by the first layer that provides it—in your case the StaticLayer that subscribes to /map.
• When StaticLayer is present, the costmap’s origin, width, height, and resolution are taken from the incoming nav_msgs/OccupancyGrid, and your width/height params are ignored.
• Planners (NavFn/Smac) cannot plan to a pose that is outside the global costmap bounds. tolerance helps if the goal is inside bounds but in lethal cells; it does not help if the goal is out of bounds.

So you have three viable patterns, depending on what behavior you want.

---

A) Keep a world-fixed (non-rolling) global costmap that “grows” as you explore

You need the /map message itself to grow. Nav2 will mirror whatever geometry the StaticLayer receives. slam_toolbox’s live map does expand as you explore, but if you try to send a goal beyond the current map extents, planning will fail until the map actually grows to include it.

Two practical ways to achieve “big initial area + dynamic growth”:

A1. Pad the map you feed into Nav2 (simple shim node)

Insert a tiny node between SLAM and Nav2 that:

• Subscribes to /map_raw (from slam_toolbox),
• If the robot gets within d_pad of any map edge or the goal is off-map, re-publishes /map with extra rows/cols of unknown cells around the perimeter (adjusting info.origin, info.width/height).
• Nav2’s StaticLayer will resize the global costmap automatically on the next message.

Minimal sketch:

# map_pad_node.py (ROS 2, Python rclpy) # 1) Subscribe nav_msgs/OccupancyGrid (/map_raw) # 2) Monitor robot pose in map frame + incoming goals # 3) If close to edges or goal out-of-bounds: create a larger OccupancyGrid with UNKNOWN=-1 padding # 4) Publish to /map (Nav2 subscribes here) # Pseudocode only: pad = N cells, new_width = old_width + 2*pad, update info.origin.x/y -= pad * resolution 

Pros: deterministic, works with any planner. Cons: you own a 40-line shim (but it’s dead simple).

A2. Ask slam_toolbox to publish the “full” map topic Nav2 uses

If your pipeline already publishes a growing /map (not a cropped/rolling view), just make sure:

global_costmap: global_costmap: plugins: ["static_layer", "obstacle_layer", "inflation_layer"] # StaticLayer first track_unknown_space: true # width/height are ignored when StaticLayer drives geometry 

…and don’t send goals beyond the currently published extents. Once you fly far enough that slam_toolbox expands the occupancy grid, you can plan there.

---

B) Use a rolling global costmap with a large window (supported out-of-the-box)

If you’re OK with a robot-centric global costmap, just turn it on:

global_costmap: global_costmap: rolling_window: true width: 50.0 # big enough to include typical goals height: 50.0 resolution: 0.05 plugins: ["obstacle_layer", "inflation_layer"] # usually drop StaticLayer in rolling mode track_unknown_space: true 

• Rolling global costmaps ignore the static map geometry; your width/height apply.
• This is the simplest way to guarantee goals “nearby” are inside the costmap, even if the static map hasn’t grown yet.

---

C) Clip or redirect off-map goals (defensive UX)

If an operator clicks off-map:

• Clamp the goal to the nearest in-bounds cell of the current map/costmap and re-publish it.
• Or reject with a hint (“goal outside map; explore or enlarge map”).

Tiny clamp idea:

# Given OccupancyGrid 'grid' and goal (x,y) in map frame: ix = clamp(int((x - origin_x)/res), 0, grid.info.width - 1) iy = clamp(int((y - origin_y)/res), 0, grid.info.height - 1) # Convert back to world coords and publish the adjusted goal 

---

Extra knobs that commonly trip people up

• allow_unknown (Smac/NavFn): lets the planner traverse unknown cells, but the goal must still lie inside the costmap bounds. Set it if you want paths through unknown:

  planner_server: ros__parameters: GridBased: # or SmacHybrid/Smac2D allow_unknown: true 

• tolerance in ComputePathToPose: helps if the goal is inside bounds but occupied; it doesn’t help if the goal is completely out of bounds.

• Layer order: put StaticLayer first only when you want the costmap geometry driven by the map. If you switch to rolling global costmap, typically drop StaticLayer and rely on obstacle/inflation layers.

---

Bottom line

• With StaticLayer active, Nav2’s global costmap size = your /map size.

• Planners cannot plan to off-map goals.

• Choose one:

  1. Publish a larger/expanding /map (padding unknowns when needed),
  2. Use a rolling global costmap with a big window, or
  3. Clamp off-map goals to the nearest valid pose.

Your interceding “map-pad” node is a perfectly valid solution for (A1); it’s exactly how I’d implement a world-fixed, ever-growing map while keeping the planner happy.

In case someone faces the same problem: I ended up creating an interceding node which takes the map from the slam algorithm and increases it when the robot approaches the limits of the map or when a goal off the costmap limits. The added areas of the map are marked as unknown.

Default:

slam_toolbox -> /map -> nav2

Now:

slam_toolbox -> /map_raw -> map_mod_node -> /map -> nav2

If someone finds a better solution, it would be nice to hear it =)