# Autonomous Navigation Framework for Intelligent Robots Based on a Semantic Environment Modeling
## 3 TOSM-Based Autonomous Navigation Framework
### Semantic Autonomous Navigation
#### 3.3.2 Behavior Planner  
The behavior planner can be divided into two modules: the waypoint generator and the behavior handler. After receiving the next goal from the task planner, one of those two modules takes care of breaking down this behavior into atomic actions. The waypoint generator handles motion behaviors. While complex specific activities, such as entering and exiting an elevator or going up or downstairs, are processed by the behavior handler.  
The waypoint generator receives a goal point from the task planner, the current position from the localization module, and a local metric map obtained from the on-demand database and stored into the STM. It then uses the PRM algorithm proposed by [84], after being adapted for mobile robot navigation, to generate a feasible sequence of waypoints. We start by generating random samples \(\mathcal{N}_{i}\) on the metric map free space \(\mathcal{C}_{free}\). The PRM algorithm's goal is to generate a valid path between the current position \(\mathcal{N}_{p}\) and the goal \(\mathcal{N}_{g}\) by creating edges connecting two sample nodes \(\mathcal{E}_{k}(\mathcal{N}_{i},\mathcal{N}_{j})\). Initially, it generates \(n\) random samples on the 2D metric map, eliminating any sample outside of \(\mathcal{C}_{free}\) and re-generating new random samples until \(n\) valid samples are successfully generated, whereas in our approach, PRM works as a low-resolution path-planner, \(n\) can be of a smaller magnitude when compared to the overall map size, speeding up the query step. Thereon, it iterates through every sample generating valid paths connecting to the k-nearest neighbors. A path is considered valid if the two points can be connected by a straight line which is completely inside \(\mathcal{C}_{free}\) and is smaller than a maximum connection distance threshold. This graph generation depends only on the metric map and can be pre-generated by the robot when idle or while executing actions that do not require a large amount of processing power. Finally, to generate a waypoint sequence, \(\mathcal{N}_{p}\) and \(\mathcal{N}_{g}\) are added to the graph and the _[PERSON]'s algorithm_[98] is used to query the shortest path.  
The behavior handler receives a specific desired behavior from the task planner and then queries the behavior database for instructions on how to execute such activity. The behavior database (BDB) can be seen as the human implicit memory [93], which saves cognitive skills learned throughout one's life. The BDB stores finite state machines (FSM) able to coordinate single behaviors, such as the one shown in Figure 7. Each state (i.e., action) has a standardized ID and a possible events list that is used to check the transition to the next state. After obtaining the FSM description, the behavior handler sends the initial action ID to the action planner and waits for the returned events. Thereon, it uses the event list to choose the next state and sends it back to the action planner. This sequence is repeated until one of the two final states is reach: success or failure.  
Both the waypoint generator and the behavior handler emit a success signal to the task planner when the behavior is completed, allowing the planner dispatcher to send the next one. In case of failure, a homonymous signal is sent back instead, and the PDDL planner is able to perform re-planning starting from the current state.