Hi, I'm making  a driver for a position2d device, but I think I have something wrong in the ProcessMessage to catch the petition of commands, because the queue get full and the robot don't move.


Here I post the code of my position driver:



/***************************************************************************************************************/

#include "positionsabinadevice.h"
#include <playertime.h>
extern PlayerTime* GlobalTime;

/* WMWMWMWMWMWMWMWMWMWMWMWMWMWMWMWMWMWMWMWMWMWMWMWMWMW*/
/* Esta función no tiene que pertenecer a la clase. */
/* Es invocada desde player cada vez que un cliente
 * solicita una nueva instancia del driver.*/
/* WMWMWMWMWMWMWMWMWMWMWMWMWMWMWMWMWMWMWMWMWMWMWMWMWMW*/

Driver* Position_Sabina_Init(ConfigFile* cf, int section)
{
     return((Driver*)(new CPositionSabina(cf, section)));
}

/* WMWMWMWMWMWMWMWMWMWMWMWMWMWMWMWMWMWMWMWMWMWMWMWMWMW*/
/* Esta función no tiene que pertenecer a la clase. */
/* Sirve para informar al servidor (player) de la
 *  existencia de un nuevo driver para un dispositivo.*/
/* WMWMWMWMWMWMWMWMWMWMWMWMWMWMWMWMWMWMWMWMWMWMWMWMWMW*/

void Position_Sabina_Register(DriverTable* table)
{
  table->AddDriver("position_sabina", Position_Sabina_Init);
}

/* WMWMWMWMWMWMWMWMWMWMWMWMWMWMWMWMWMWMWMWMWMWMWMWMWMW*/
/* Constructor. */
/* WMWMWMWMWMWMWMWMWMWMWMWMWMWMWMWMWMWMWMWMWMWMWMWMWMW*/

CPositionSabina::CPositionSabina(ConfigFile* cf, int section):Driver(cf,section,true,PLAYER_MSGQUEUE_DEFAULT_MAXLEN,PLAYER_POSITION2D_CODE)
{
// Do we create a position interface?  
   if(cf->ReadDeviceAddr(&(this->position2d_id), section, "provides",
                         PLAYER_POSITION2D_CODE, -1,NULL ) ==0)
   {
     cout << "creamos el position2d" << endl;
     ant_veloc_mot_x=0;
     ant_veloc_mot_y=0;
     comand_serv=4; //actuar sobre los motores.
   }
}

/* WMWMWMWMWMWMWMWMWMWMWMWMWMWMWMWMWMWMWMWMWMWMWMWMWMW*/
/* Funcionalidad descrita en el .h              */
/* WMWMWMWMWMWMWMWMWMWMWMWMWMWMWMWMWMWMWMWMWMWMWMWMWMW*/

int CPositionSabina::Setup()
{
    if(conecta(PUERTO_SABINA,SABINA)!=0)
    {
       cout << "Error al establecer la conexión desde el driver de " <<
           "position_sabina con el servidor de Sabina." << endl;
       return(-1);
    }
    /* Despues de inicializar todo arrancamos el trhead. */
    StartThread();
    return(0);
}

/* WMWMWMWMWMWMWMWMWMWMWMWMWMWMWMWMWMWMWMWMWMWMWMWMWMW*/
/* Funcionalidad descrita en el .h              */
/* WMWMWMWMWMWMWMWMWMWMWMWMWMWMWMWMWMWMWMWMWMWMWMWMWMW*/

int CPositionSabina::Shutdown()
{
   /* Primero paramos el thread y despues finalizamos todo.*/
   StopThread();
   if( desconecta() == 0)
      return(0);
   else
      return (-1);
}

/* WMWMWMWMWMWMWMWMWMWMWMWMWMWMWMWMWMWMWMWMWMWMWMWMWMW*/
/* Funcionalidad descrita en el .h              */
/* WMWMWMWMWMWMWMWMWMWMWMWMWMWMWMWMWMWMWMWMWMWMWMWMWMW*/

int CPositionSabina::ProcessMessage (MessageQueue* resp_queue, player_msghdr * hdr, void * data)
{

 if(Message::MatchMessage(hdr, PLAYER_MSGTYPE_CMD, PLAYER_POSITION2D_CMD_VEL,
                           this->position2d_id))
  {
    // get and send the latest motor command
    cout << "entra en el ProcessMessage de PLAYER_POSITION2D_CMD_VEL" << endl;
    player_position2d_cmd_vel_t position_cmd;
    position_cmd = *(player_position2d_cmd_vel_t*)data;


    // Almaceno las velocidades para enviarselas al servidor
    veloc_mot_x=(short)/*ntohl*/(position_cmd.vel.px/*xspeed*/);
    veloc_mot_y=(short)/* ntohl*/(position_cmd.vel.py/*yspeed*/);
    cout << "veloc_mot_x: " << veloc_mot_x << ", veloc_mot_y: " << veloc_mot_y << endl;
    PLAYER_MSG2(2,"sending motor commands %f:%f",
                position_cmd.vel.px,
                position_cmd.vel.pa);
    // Si las velocidades han cambiado con respecto a las últimas enviadas
    // las mando, sino no.
    if((veloc_mot_x!=ant_veloc_mot_x) || (veloc_mot_y!=ant_veloc_mot_y))
     {
       //Primero mando el comando que indica al servidor que quiero actuar
       // sobre los motores.
       while(envia_comando(comand_serv) != 0)
          cout << "Error al enviar comando (motores) al servidor." << endl;

       // y luego el valor de los motores.
       while(EnviaVelocidades() != 0)
           cout << "Error al enviar velocidades al servidor." << endl;

       ant_veloc_mot_x=veloc_mot_x;
       ant_veloc_mot_y=veloc_mot_y;
      }
   
    return(0);

  } // final del if MatchMessage



   return(-1);  // Devuelve -1 si no ha sido capaz de coger el mensaje
}  // Final del ProcessMessage


/* WMWMWMWMWMWMWMWMWMWMWMWMWMWMWMWMWMWMWMWMWMWMWMWMWMW*/
/* Funcionalidad descrita en el .h              */
/* WMWMWMWMWMWMWMWMWMWMWMWMWMWMWMWMWMWMWMWMWMWMWMWMWMW*/

int CPositionSabina::EnviaVelocidades()
{
   char veloc[4]; // 2 enteros(velo_x y velo_y) de 2 bytes cada uno.

   veloc[0]=(char)veloc_mot_x;
   veloc[1]=(char)(veloc_mot_x>>8);
   veloc[2]=(char)veloc_mot_y;
   veloc[3]=(char)(veloc_mot_y>>8);

   if (send(num_s,(const char*) veloc,4*sizeof(char), 0) != 4*sizeof(char))
     return (-1);
   else
     return 0;
}

/* Desde el Main estaré a la espera de que llegue un comando para actuar sobre
 * los motores de Sabina, y cuando llegue se lo mandaré al servidor de Sabina.*/

void CPositionSabina::Main()
{
//   player_position2d_data_t comando;
   player_position2d_data_t dato;
//   int i;
   char num_motores=2;
   bool enviado=false;
  
   for(;;)
   {

  /*Cambio la comprobacion de un nuevo comando por la espera del mismo, cuando llegue uno sigue */

      ProcessMessages();
     
      // La primera vez mando el número de dispositivos position   
      if(enviado == false)
      {
         //Primero mando el comando 
             while(envia_comando(comand_serv) != 0)
               cout << "Error al enviar comando (motores) al servidor." << endl;
         //Y luego el numero de motores
         if(send(num_s,(const char*)&num_motores,sizeof(char),0) != sizeof(char))
                cout << "Error al enviar el num. de motores al servidor." << endl;
         enviado=true;         
      }


      // Actualizacion de los datos del position2d para mandarselos al cliente

      // Envio los Datos al Cliente.

      //Primero el estado de los motores
      if((ant_veloc_mot_x==0) && (ant_veloc_mot_y==0))//Si estan parados...
        dato.stall=(uint8_t)1;
      else
    dato.stall=(uint8_t)0;
     
      // Y las velocidades
      dato.vel.px/*xspeed*/=/*htonl*/(ant_veloc_mot_x);
      dato.vel.py/*yspeed*/=/*htonl*/(ant_veloc_mot_y);
     
      // Llamo a PutData para almacenar el dato en el buffer de datos.
      //PutData((uint8_t*) &dato,sizeof(dato),0,0);
 Publish(this->position2d_id,NULL,PLAYER_MSGTYPE_DATA,PLAYER_POSITION2D_DATA_STATE,
        &dato,sizeof(dato));

   }
}

/**********************************************************************************************************/