# State Machines

Para la creación de máquinas de estado existen dos *executives* en ros: [smach](http://wiki.ros.org/smach) y [teer](http://wiki.ros.org/executive_teer), ambos tienen sus ventajas y desventajas. En bender se está utilizando *smach*.


Las máquinas de estados de smach deben ser programadas en **python**. . . Al menos nos ahorramos la parte de la compilación.

---

## Documentación

 * [smach wiki](http://wiki.ros.org/smach/Documentation): Documentación básica sobre conceptos, estados y contenedores utilizados
 * [smach wiki tutorials](http://wiki.ros.org/smach/Tutorials): Tutoriales para todos los niveles de smach. Ojo que son escritos por los diseñadores del package, sino que por usuarios, entonces no capturan todo lo que se puede hacer con smach, ni la mejor forma de hacerlo.
 * [smach_ros pkg documentation](http://docs.ros.org/fuerte/api/smach_ros/html/python/smach_ros-module.html): Documentación de objetos y funciones del package *smach_ros*.
 * [smach pkg documentation](http://docs.ros.org/groovy/api/smach/html/python/smach-module.html): Documentación de objetos y funciones del package *smach*.

---

## Instalación

* `sudo apt-get install ros-fuerte-executive-smach`
* `sudo apt-get install ros-fuerte-executive-smach-visualization`

---

## Herramientas

#### Eclipse

La idea es usar algún IDE que provea soporte para python y ROS, con funciones como *autocomplete* y búsqueda de librerias. Simplififica mucho la vida.

Además de la [instalación](http://wiki.ros.org/IDEs) es necesario realizar la configuración para trabajar con python (explicada en el mismo link). Antes de poder ocupar cualquier package en eclipse o ante cambios en las dependencias, es necesario ejecutar `make eclipse-project` desde el directorio del package.

#### Smach Introspection Server

Es una interfaz provista por el package [smach_viewer](http://wiki.ros.org/smach_viewer) que permite monitorear las máquinas de estado de smach, estados y variables, por medio de una interfaz gráfica amigable. También permite setear 'a mano' el estado inicial de una máquina, lo que puede ser útil para debbugeo/programación.

Uso del introspection server: [Viewing State Machines](http://wiki.ros.org/smach/Tutorials/Smach%20Viewer) (*Basta agregar 4 líneas de código (: *)

---

## Outcomes

Para la definición de outcomes, se ocuparán **como norma**, al menos los siguientes outcomes, 

* succeeded: Finalización ok
* aborted: Aborto debido a fallo (por ejemplo: una excepción)
* preempted: Aborto debido a orden superior (por ejemplo: stop button, fin de estado paralelo, etc.)

de ser necesario, se puede utilizar cualquier otro outcome que se desee. Tal norma es importante para simplificar la creación de máquinas de estados y poder implementar cosas como el *stop button*.

Ejemplo: Outcomes obligatiorios + outcomes extras

```
#!/usr/bin/env python
...
class Foo(smach.State):
    def __init__(self):
        smach.State.__init__(self, outcomes=['succeeded','aborted','preempted','my_state_outcome_1'])

    def execute(self, userdata):
        ...
        return 'succeeded'

if __name__ == '__main__':
    ...
    sm = smach.StateMachine(outcomes=['succeeded','aborted','preempted','my_sm_outcome_1'])
    ...

```

#### Preempt, request_preempt() & preempt_requested()

Es importante especificar como se manejan los casos de *preempt* en bender. Se ocupa el sistema de *preempt* para los casos de **aborto del estado, debido a una orden superior (de mayor peso)**. Un *preempt* típicamente no sucede espontaneamente, por una condición interna del estado, sino que debe haber una señal que gatille esa acción, por ejemplo:

* Contenedores de *Concurrencia*: señales de *preempt* enviada debido a término de estado hermano. 
* Stop button: El *stop button* es un caso particular de concurrencia, en donde se envían señales de preempt a todos los estados actuales.
* Padre que reciba señal de *preempt* (típicamente) reenvía la señal a sus hijos en ejecución actuales.
* otros . . .

Ya que un *preempt* es un outcome gatillado, smach provee un mecanismo de chequeo de tal señal: `preempt_requested()`  y otro de notificación a sus hijos `request_preempt()`. En éste [tutorial](http://wiki.ros.org/smach/Tutorials/State%20Preemption%20Implementation) se explica el funcionamiento de tales funciones.

Ante un preempt es posible que la máquina quede ''pausada'' y posteriormente sea retomada en el mismo estado, por lo que junto con el chequeo de la señal, se debe hacer almacenamiento de variables importantes para retomar la ejecución posteriormente.

Si no se hace el chequeo del *preempt*, el estado trabajará ininterrumpidamente hasta su término, lo que puede ser peligroso en estados que involucren movimientos del robot. Por lo tanto, es necesario implementar tal mecanismo y ejecutar los procedimientos necesarios para detener el estado (por ejemplo, con señales de stop a actuadores).
 

---

## macros

Las macros son  *StateMachines* que implementan una funcionalidad en específico y existen para ser anidadas en una *StateMachine* de mayor nivel. El uso de macros permite reutilizar el código y ayuda con el control de bugs, pues todos los cambios necesarios se hacen en un mismo lugar.


#### \_\_init\_\_.py

Tal como está explicado [acá](http://effbot.org/pyfaq/what-is-init-py-used-for.htm) y en la [guia de python](http://wiki.ros.org/PyStyleGuide), es importante la creación de archivos `__init__.py` (típicamente vacios) para poder hacer includes con namespaces y evitar overloads de nombres de archivos .py entre cada package.

Para el caso de las macros, se recomienda que todos los imports de macros se realicen utilizando un namespace. Por ejemplo, se presenta la siguiente configuración utilizada en bender:

* bender_macros/
    * src/
        * bender_macros/
            * \_\_init\_\_.py
            * nav/
                * \_\_init\_\_.py
                * GoToPose.py
                * GoToPlace.py
            * vision/
                * \_\_init\_\_.py
                * . . .
            * my_macro.py

con lo que un import de una macro de navegación quedaría como:

`from bender_macros.nav import GoToPose.py`


#### main() vs. getInstance()

Para simplificar el testeo y anidación de macros, se promueve el uso de las funciones `main()` y `getInstance()` como en el siguiente ejemplo:


* Macro Example: /bender_macros/src/bender_macros/my_folder/MyMacro.py
```
#!/usr/bin/env python
...

def getInstance():
    
    # Machine creation
    sm = smach.StateMachine(outcomes = ['succeeded','aborted','preempted'])
    ...
    
    # Fill machine
    with sm:
        smach.StateMachine.add('...', ... )
        ...
    ...
    # return machine
    return sm
 
if __name__ == '__main__':

    rospy.init_node('my_node')

    # create machine
    sm = getInstance()

    # introspection server
    sis = smach_ros.IntrospectionServer('my_machine', sm, '/MY_MACHINE_SM')
    sis.start()
    outcome = sm.execute()
    sis.stop()

```

* Macro Usage: /bender_behaviors/src/bender_behaviors/my_folder/MyBehavior.py
```
#!/usr/bin/env python

# Import macro!
from bender_macros.my_folder import MyMacro
...

def getInstance():
    ...
    
    # Fill machine
    with sm:

        # use macro
        smach.StateMachine.add('MY_MACRO', MyMacro.getInstance(),... )
        ...
    ...
 
```


`main()`: Permite testear la macro sin tener que correr todo el behavior. Además, acá se agrega el introspection server, para que sólo sea cargado una vez en todo el proceso. Cuando la máquina debe tener un input, es posible dar valores dummy:

```
# main
if __name__ == '__main__':

    ...
    sm = getInstance()

    # generate dummy userdata
    dummy_ud = smach.UserData()
    dummy_ud.my_sm_place = 'living_room'
    dummy_ud.my_sm_number = 3216.03584021

    ...
    # execute with dummy data
    outcome = sm.execute(dummy_ud)
 
```

`getInstance()`: Utilizado para hacer la anidación de macros. Retorna el objeto de tipo `smach.state_machine`. En caso de desear que un mismo archivo .py retorne más macros, queda a gusto del programador la implementación de funciones similares (i.e, `myGetInstance1()`, ...). Si se desea que la macro retornada sea creada según ciertos parámetros, siéntase libre de hacer modificaciones como `getInstance(var1,var2,...)`


#### State SETUP

Al trabajar en máquinas de estados de alto nivel (macros) es altamente probable que se requiera que cierto servicio esté activado o que se hayan cargado ciertas configuraciones para poder operar normalmente. Por lo anterior, se recomienda la creación de un(os) estado(s) SETUP encargado de realizar los procedimientos necesarios. En el fondo, se recomienda **asumir que nadie realizo todas las preparaciones que necesitamos**.


---

## Más sobre: Monitor State

La información básica sobre el *Monitor State* puede ser encontrada en los [tutoriales](http://wiki.ros.org/smach/Tutorials/MonitorState) o en la [documentación](http://docs.ros.org/fuerte/api/smach_ros/html/python/smach_ros.monitor_state.MonitorState-class.html).

Se debe definir un callback para atender el tópico seleccionado: `def monitor_cb(ud,msg)`. Tal callback debe retornar `True` ó `False`:

* monitor_cb - return 'False': Termina la ejecución del monitor con outcome `'invalid'`
* monitor_cb - return 'True': Continua atendiendo mensajes (`'valid'`). Sin outcome.


## Más sobre: Concurrence Container

#### out_cb

kjsañdkcjñsadklcj

#### child_term_cb

sadcascasdc

#### monitor_cb

ascsadcsadcasd