2.1.1 Coverage Planners

This section presents the algorithms employed for the discovery of portable objects. While a comprehensive technical analysis of these algorithms is beyond the scope of this work, they are described with sufficient detail to enable understanding of their application within our framework. For in-depth technical details and formal derivations, the reader is referred to the original publications cited herein. To demonstrate the different navigators, a costmap of the Pulse Breakout room is used. This map was obtained during testing of the coverage methods. Furthermore, Nav2 Macenski et al., 2020 is used for low-level motion planning and execution as well as real-time control and costmap generation.

Preprocessing¶

Before the map can be used to plan a path, a common preprocessing algorithm can be abstracted. Coverage path planners generally use either a binary map of the free space (free = 1, occupied = 0) or the polygons constructed from the boundaries of the free space.

The input to the algorithm can be any 2D image displaying the free and occupied pixels. In this case, the input is the costmap directly. First, the costmap is thresholded to create a binary costmap, which is the first output.

Then, using this binary costmap, the contours are extracted (Figure 3).

Morphology-Based Skeleton Path Planner¶

The first implemented method, based on the work of Becoy et al., 2025, Niu et al., 2025, derives a coverage path from the morphological skeleton of the free space. See the algorithm below.

Input: Occupancy grid $\mathcal{M}$, robot start position $\mathbf{p}_0 \in \mathbb{R}^2$
Output: Ordered waypoint segments $\mathcal{P} = [P_1, P_2, \ldots, P_k]$

1. $B \leftarrow$ threshold($\mathcal{M}$, lethal_threshold)

2. Extract contours from $B$, group by containment into polygon groups $G_{poly}$

3. For each polygon group $g \in G_{poly}$:

   1. $S \leftarrow$ medialAxis($B$)     (skimage skeletonize)

   2. $W \leftarrow$ skeletonToWorldCoords($S$)

   3. $G_{graph} \leftarrow$ buildKDTreeGraph($W$, radius $= r_{path}$)

   4. bridgeDisconnectedComponents($G_{graph}$)

   5. $L \leftarrow$ findLeafNodes($G_{graph}$)     (nodes with degree 1)

   6. $v \leftarrow$ nearestLeaf($\mathbf{p}_0$, $L$)

   7. $\mathcal{Q} \leftarrow [v]$, $\quad V_{visited} \leftarrow \emptyset$

   8. While $V_{visited} \neq L$:

      1. $V_{visited} \leftarrow V_{visited} \cup \{v\}$

      2. $r \leftarrow$ nearestLeafByGraphDistance($v$, $L \setminus V_{visited}$, $G_{graph}$)

      3. $\pi \leftarrow$ shortestPath($v$, $r$, $G_{graph}$)

      4. append $\pi$ to $\mathcal{Q}$

      5. $v \leftarrow r$

   9. $P_i \leftarrow [W[u] \;\forall\, u \in \mathcal{Q}]$

   10. append $P_i$ to $\mathcal{P}$, set $\mathbf{p}_0 \leftarrow P_i[\text{last}]$

4. sanitiseWaypoints($\mathcal{P}$, $\mathcal{M}$)

5. Return $\mathcal{P}$

The free space is extracted using the methods discussed in the previous section. The polygons are used and converted to a binary map so as not to produce too many branches for the robot to navigate through. This map is then skeletonized to extract waypoints, see Figure 4.

Once the waypoints are obtained, a graph is constructed and leaf nodes are extracted. In the graph, leaf nodes are defined by their single connection to the rest of the skeleton; these are treated as endpoints of the path.

Finally, starting from the leaf node closest to the robot, the full path is planned by recursively tracing the graph from the current leaf node to the nearest unvisited leaf node. Use the slider below to visualize the full path.

Grid-Based Spanning-Tree Planner¶

Secondly, a grid-based spanning-tree algorithm is used to plan a full coverage path over the map. This planner is based on the work of Gabriely & Rimon, 2001.

Input: Occupancy grid $\mathcal{M}$, robot start position $\mathbf{p}_0 \in \mathbb{R}^2$, scale $s = 0.06$
Output: Ordered waypoint segments $\mathcal{P} = [P_1, P_2, \ldots, P_k]$

1. $B \leftarrow$ threshold($\mathcal{M}$, lethal_threshold)

2. $B_{down} \leftarrow$ resize($B$, scale $= s$, interpolation $=$ NEAREST)

3. $G \leftarrow$ gridGraph($B_{down}$)     (4-connected)

4. Remove all edges $(u, v)$ where $B_{down}[u] = 0$ or $B_{down}[v] = 0$

5. $T \leftarrow \emptyset$

6. For each connected component $\mathcal{C} \in G$:

   1. $T \leftarrow T \cup$ DFS_tree($\mathcal{C}$)

7. For each edge $(u, v) \in T$:     (edge subdivision)

   1. $m \leftarrow \frac{u + v}{2}$

   2. replace $(u, v)$ with edges $(u, m)$ and $(m, v)$

8. $P_{perim} \leftarrow \mathbf{0}^{H \times W}$, $\quad r \leftarrow \lfloor \frac{1}{4s} \rfloor$

9. For each node $p \in T$:

   1. $p_{img} \leftarrow \frac{p + 0.5}{s}$

   2. drawFilledRect($P_{perim}$, centre $= p_{img}$, halfsize $= r$)

10. $\mathcal{C} \leftarrow$ findContours($P_{perim}$, RETR_EXTERNAL)

11. For each contour $c \in \mathcal{C}$:

    1. $W \leftarrow$ pixelToWorld($c$)

    2. If $|W| < 2$: skip

    3. $idx \leftarrow \arg\min_i \|W[i] - \mathbf{p}_0\|_2$

    4. $P_i \leftarrow [W[idx], \ldots, W[-1], W[0], \ldots, W[idx-1]]$

    5. append $P_i$ to $\mathcal{P}$, set $\mathbf{p}_0 \leftarrow P_i[\text{last}]$

12. sanitiseWaypoints($\mathcal{P}$, $\mathcal{M}$)

13. Return $\mathcal{P}$

Unlike the skeleton planner, this planner uses the binary costmap directly. The binary costmap is further discretized to a grid whose resolution is a fraction of that of the original costmap (Figure 7).

A depth-first search (DFS) is conducted over the free cells, from which sparse waypoints are created.

A black image with the same resolution as the original costmap is then created, and white squares with half the cell width are placed in the center of each cell and between adjacent cells. Waypoints are created from the contours of this tree. These waypoints represent a circumnavigation around the DFS tree.

A segment from this circumnavigation is then removed from the path

A lab cleanup robot must not blindly follow waypoints. If it does, it might collide with obstacles near the map boundary or collide with moving objects.
Nav2 implements several low-level real-time trajectory planners that plan inside the live-updating costmap Macenski et al., 2023. They handle how the waypoints are followed and how much influence the map ought to have on the generated trajectory. The system described in this section implements an MPPI controller Williams et al., 2017 due to its superior performance in highly dynamic environments, such as labs where students or other people frequently walk Elbouhy et al., 2024. Other planners that were considered include DWB (Dynamic Window Approach Behavior-based) Thrun et al., 1997 and RPP (Regulated Pure Pursuit) Macenski et al., 2023.