Driver de Player para el control 2.0 del robot RoMAA

Tabla de Contenidos

Driver de Player para el control 2.0 del robot RoMAA

Objetivos

Desarrollar el nuevo driver del robot RoMAA para la versión 2.0 del sistema de control embebido compatible con Player versión 3.0.

Control embebido 2.0

El control embebido tiene tres modos de funcionamiento

A lazo abierto mediante comandos de PWM a cada motor
A lazo cerrado en velocidad independiente para cada motor
Lazo cerrado cross-couplind utilizando comandos de velocidad lineal y angular

Para más detalles del control embebido ver "Hardware de Control de Plataforma Robótica Móvil con Arquitectura ARM y RTOS. Caracterización".

Player driver

Para la programación de un driver de Player se utilizan las librerías incluidas en player que permite generar un driver plugin. Los drivers plugin se carga en tiempo de ejecución del servidor. La instalación de Player incluye un ejemplo de programación de driver plugin que puede encontrarse, junto con el archivo CMakeList.txt para la compilación, en ${prefix}/share/player/example.

Estructura del driver plugin (player v3.0)

#include <libplayercore/playercore.h>

class Romaa : public ThreadedDriver
{
  public:  
    // Constructor
    Romaa(ConfigFile* cf, int section);

    // This method will be invoked on each incoming message
    virtual int ProcessMessage(QueuePointer &resp_queue, 
        player_msghdr * hdr,
        void * data);

  private:
    // Main function for device thread.
    virtual void Main();
    virtual int MainSetup();
    virtual void MainQuit();
};

El driver plugin se programa como una clase de C++ heredara de la clase ThreadedDriver. Esta clase tiene los siguientes métodos virtuales

MainSetup():Realiza funciones de inicialización especificas del dispositivo por ej. abrir y configurar un puerto de comunicación serie; también ejecuta el thread del driver. El método MainSetup se llama cada vez que un cliente se subscribe al driver.
MainQuit(): Es el opuesto al método MainSetup. MainQuit asegura la terminación de dispositivos como por ej. cerrar un puerto de comunicación serie; también detiene el thread del driver.
Main(): Es la función principal del thread del dispositivo, donde se incluyen todas las interacciones con el dispositivo.
ProcessMessage():Este método se invoca con cada mensaje entrante al servidor y ofrece la posibilidad de enviar una respuesta publicando un mensaje utilizando la función miembro pública Publish.

El server player inicia el plugin driver llamando a player_driver_init (función en C), esta función llama a Romaa_Register, la cual agrega el driver a una tabla de drivers del servidor Player mediante método AddDriver que llama a Romaa_Init. Romaa_Init crea un nuevo objeto Romaa y devuelve un puntero al driver. Una vez creado el objeto Romaa se ejecuta el constructor. En el constructor se cargan los datos del archivo de configuración (.cfg) del servidor Player. Luego se ejecuta MainSetup y el método principal Main.

/* need the extern to avoid C++ name-mangling  */
extern "C" {
  int player_driver_init(DriverTable* table)
  { 
    puts("romaa driver initializing");
    Romaa_Register(table);
    puts("romaa driver done");
    return(0);
  }
} 

void Romaa_Register(DriverTable* table)
{
  table->AddDriver("romaa", Romaa_Init);
} 
  
Driver*  Romaa_Init(ConfigFile* cf, int section)
{
  // Create and return a new instance of this driver
  return((Driver*)(new Romaa(cf, section)));
}

El método Main es el núcleo del driver plugin y se ejecuta en un thread lo que significa que corre en paralelo con otros drivers. La mayor parte del método Main esta contenida en un loop infinito. El método Main tiene que llamar unas pocas funciones específicas:

pthread_testcancel:Verifica si el thread fue terminado (killed). La función saldrá del loop y ejecutara la el método MainQuit.
ProcessMessages: Le pasa el control a Player que llamará la función ProcessMessage() del plugin con cada mensaje esperando en la cola de mensajes.
usleep: Es un comando de sleep para liberar los recursos utilizados por el driver.

Romaa::Main() 
{
  // The main loop; interact with the device here
  for(;;)
  {
    // test if we are supposed to cancel
    pthread_testcancel();

    // Process incoming messages.  Romaa::ProcessMessage() is
    // called on each message.
    ProcessMessages();

    // Interact with the device, and push out the resulting data, using
    // Driver::Publish()

    // Sleep (you might, for example, block on a read() instead)
    usleep(100000);
  }
}

Procesamiento de mensajes

Las diferentes interfaces en Player interactúan unas con otras enviando y recibiendo mensajes a través de Player. Algunos de los diferentes tipos de mensajes son PLAYER_MSGTYPE_DATA (datos), PLAYER_MSGTYPE_CMD (comando), PLAYER_MSGTYPE_REQ (petición), PLAYER_MSGTYPE_RESP_ACK (respuesta), etc.

Tipos de mensajes

Commands: Se utilizan para darle instrucciones al driver cuando no se requiere una respuesta
Requests: Son mensajes desde otros drivers para acceder a datos que no son publicados regularmente o enviar comandos que requieren algún tipo de respuesta
Data: Los mensajes de datos son publicados en cada iteración del loop Main del driver.

Además, cada interfaz soporta un subconjunto de los mensajes anteriores. Los mensajes se transfieren como un puntero a objetos Message. Un método importante de la clase Messages es MatchMessage que se utiliza dentro de ProcessMessage para determinar si el encabezado de un mensaje coincide con algunos de los tipos preestablecidos.

Archivo de configuración

Driver para interfaz position2d

Los comandos de la interfaz position2d implementados son: PLAYER_POSITION2D_CMD_VEL, PLAYER_POSITION2D_REQ_RESET_ODOM, PLAYER_POSITION2D_REQ_SET_ODOM, PLAYER_POSITION2D_REQ_GET_GEOM, PLAYER_POSITION2D_REQ_MOTOR_POWER, PLAYER_POSITION2D_DATA_STATE

El archivo de configuración para cargar el driver es el siguiente

driver
(
  name        "romaa"
  plugin      "libromaa"
  provides    [ "position2d:0" ]
  port        "/dev/ttyUSB0"
  baudrate    115200
  motor_pid   [ 20.0 10.0 0.0 ]
  vw_pid      [ 8.0 4.0 0.0 ]
  t_odometry  25
  t_loop      20
)

Driver para interfaz position1d

Los comandos de la interfaz position1d implementados son