Nodo RRT como FSM

Nodo RRT

Esta semana estuve trabajando en el nodo de planificación RRT. Funciona como una máquina finita de estados con 5:

1. BUILD GRAPH. En este estado se carga el mapa y se detectan los obstáculos. Así, se crea un árbol RRT sin destino que cubre el espacio libre del mapa.

2. WAIT GOAL. Estado de reposo. El robot espera a recibir un destino.

3. EXTEND GRAPH. Una vez existe un goal válido, se extiende el grafo con el nodo start y goal.

4. PLAN ROUTE. Se construye el camino dentro del árbol.

5. PILOTING. El robot navega por los waypoints de la ruta.

Este nodo reutiliza el árbol RRT generado al principio.