2.2 Robot Behavior

A common approach in robotics for navigation- and perception-heavy tasks is to use a global behavior tree that describes the robot’s actions in different situations. This architecture, as shown in Figure 1, is used here to describe and execute the cleaning strategy. The Py Trees for ROS python package is used to implement a behaviour tree due to its high level of documentation, aswell as native support in ROS 2.

The behavior tree provides a hierarchical approach for coordinating navigation, perception, and cleaning actions. It also provides a clear structure for debugging. As seen in Figure 1, the robot first explores the environment, then pauses its coverage task whenever an object is detected, approaches the object, checks whether it is still visible, picks it up, and places it in the appropriate basket.

Input: Coverage segments $\mathcal{P}$, planner type $\tau$
Output: Task result (success / cancelled / stopped)

1. $\mathcal{P} \leftarrow$ sortByLength($\mathcal{P}$, descending)

2. $n \leftarrow |\mathcal{P}|$, $\;i \leftarrow 0$

3. While $\mathcal{P} \neq \emptyset$:

   1. $P_i \leftarrow$ dequeue($\mathcal{P}$), $\;i \leftarrow i + 1$

   2. nav2.goThroughPoses(toROSPath($P_i$))

   3. Repeat until nav2.isTaskComplete():

      1. $f \leftarrow$ nav2.getFeedback()

      2. publishFeedback($i$, $n$, $f$.remainingPoses)

      3. If cancelRequested: nav2.cancelTask(), return CANCELLED

      4. If stopRequested: nav2.cancelTask(), return STOPPED

      5. If pauseRequested:

         1. nav2.cancelTask()

         2. $P_{rem} \leftarrow$ computeRemainingSegment($P_i$, getRobotPos())

         3. prepend $P_{rem}$ to $\mathcal{P}$     (resume from current pose)

         4. Repeat until $\neg$pauseRequested: spinOnce()

         5. nav2.goThroughPoses(toROSPath($P_{rem}$))

   4. publishFeedback($i$, $n$, 0)

4. Return SUCCESS