Bienvenido: Ingresar

No tienes permisos para la acción RecommendPage en esta página.

Quitar mensaje
location: Robotica / PlayerStageRoMAA / PlayerDriverRoMAA1 / Messages

Procesamiento de mensajes y publicación de datos

ProcessMessage

ProcessMessage provee de la principal funcionalidad del servidor Player. Diferentes interfaces interactuan unas con otras enviando y recibiendo mensajes a traves de Player. Los diferentes tipos de mensajes pueden verse aquí. Cada interfaz soporta un subconjunto de mensajes. Por ejemplo los tipos de mensajes definidos para la interfaz position2d puden verse aquí.

Tipos de mensajes

Interface position2d (motor control / odometry)

Publicación de odometría

// Update position2d data                                                                              
player_position2d_data_t pos2d_data;                                                                   
memset( &pos2d_data, 0, sizeof( player_position2d_data_t) );                                           
                                                                                                           
romaa_comms->update_odometry();                                                                        
romaa_comms->get_odometry( pos2d_data.pos.px,                                                          
                           pos2d_data.pos.py,                                                          
                           pos2d_data.pos.pa );                                                        
                                                                                                           
this->Publish( this->position2d_addr,                                                                  
                PLAYER_MSGTYPE_DATA, PLAYER_POSITION2D_DATA_STATE,                                     
                (void*)&pos2d_data, sizeof( player_position2d_data_t ), NULL );   

utilizando el tipo de dato player_position2d_data_t.

Actualización de velocidad

// CMD Message type (hdr->type)
if ( Message::MatchMessage( hdr, PLAYER_MSGTYPE_CMD,
                            PLAYER_POSITION2D_CMD_VEL,
                            this->position2d_addr ) )
{
  memcpy( &position2d_cmd_vel, data, sizeof( player_position2d_cmd_vel_t) );

  PLAYER_MSG2(1, "sending motor commands: %f %f",
        position2d_cmd_vel.vel.px,
        position2d_cmd_vel.vel.pa);

  romaa_comms->set_speed( position2d_cmd_vel.vel.px, position2d_cmd_vel.vel.pa );
  return (0);
  }

utilizando el tipo de dato player_position2d_cmd_vel_t.

Programa cliente

Con la publicación de la odometría y la actualización de velocidad hacia el robot, se puede controlar la interfaz position2d a través del siguiente programa cliente (mediante el Proxy Position2dProxy)

#include <iostream>
#include <libplayerc++/playerc++.h>

int
main( int argc, char *argv[] )
{
  // Connect to the local player process on port 6665
  PlayerCc::PlayerClient romaa_robot( "localhost", 6665 );

  // Create a position2d proxy
  PlayerCc::Position2dProxy romaa_pos2d( &romaa_robot, 0 );

  romaa_pos2d.SetSpeed( 10, 0 );

  std::cout << "loop..." << std::endl;
  for( int i = 0; i < 100; i++ )
  {
    // For 10 seconds move into a circle
    romaa_robot.Read();

 ";
    std::cout << "px: " << romaa_pos2d.GetXPos() << " - ";
    std::cout << "py: " << romaa_pos2d.GetYPos() << " - ";
    std::cout << "pa: " << romaa_pos2d.GetYaw() << std::endl;

    usleep(50000);
  }
  return 0;
}

Cliente playerv

El programa cliente playerv se bloquea al subscribirce a la interfac position2d y no funciona. Esto se debe a que playerv al ejecutarse envia el mensaje PLAYER_POSITION2D_REQ_GET_GEOM solicitando la geometría del robot para así poder dibujar el cuerpo en pantalla. Se responde a este mensaje de la siguiente manera

// REQ Message type (hdr->type)
else if ( Message::MatchMessage ( hdr, PLAYER_MSGTYPE_REQ,
                                  PLAYER_POSITION2D_REQ_GET_GEOM,
                                  this->position2d_addr ) ) 
{
  memset( &position2d_geom, 0, sizeof( player_position2d_geom_t ) );

  romaa_comms->get_geometry_body_size( position2d_geom.size.sw,
      position2d_geom.size.sl,
      position2d_geom.size.sh );
  romaa_comms->get_geometry_odometry_center( position2d_geom.pose.px,
      position2d_geom.pose.py );

  this->Publish( this->position2d_addr, resp_queue,
                 PLAYER_MSGTYPE_RESP_ACK,
                 PLAYER_POSITION2D_REQ_GET_GEOM,
                 (void*)&position2d_geom, sizeof( player_position2d_geom_t ) );

  return (0);
}

utilizando el tipo de dato player_position2d_geom_t.

Cliente playerjoy

El cliente playerjoy al iniciar envia el mensaje PLAYER_POSITION2D_REQ_MOTOR_POWER, que se responde de la siguiente manera

else if ( Message::MatchMessage ( hdr, PLAYER_MSGTYPE_REQ,
                                  PLAYER_POSITION2D_REQ_MOTOR_POWER,
                                  this->position2d_addr ) )
{
  this->Publish( this->position2d_addr, resp_queue,
      PLAYER_MSGTYPE_RESP_ACK, PLAYER_POSITION2D_REQ_MOTOR_POWER );
  return (0);
}

Reset Odometry

Para el caso de resetear la odometria utilizando el método de Position2dProxy::ResetOdometry() del lado del cliente, en el driver del lado del servidor se detecto el error que el tipo de mensaje recibido no es PLAYER_POSITION2D_REQ_RESET_ODOM sino PLAYER_POSITION2D_REQ_SET_ODOM; este error (en el subtipo de mensaje) puede estar del lado del server o del cliente, lo cual no fue determinado.

  else if ( Message::MatchMessage( hdr, PLAYER_MSGTYPE_REQ,
        PLAYER_POSITION2D_REQ_RESET_ODOM,
        this->position2d_addr ) )
  {
    // reset position to 0,0,0: no args 
    if ( hdr->size != 0 )
    {
      PLAYER_WARN( "Arg to reset position request is wrong size; ignoring" );
      return(-1);
    }
    romaa_comms->reset_odometry();

    this->Publish( this->position2d_addr, resp_queue,
        PLAYER_MSGTYPE_RESP_ACK,
        PLAYER_POSITION2D_REQ_RESET_ODOM );
    return (0);
  }

Set Odometry

Al llamar el método Position2dProxy::SetOdometry() de lado del cliente, en el servido se obtiene el mensaje correspondiente PLAYER_POSITION2D_REQ_SET_ODOM pero debido al error antes mencionado en ResetOdometry() se procesa el caso de seteo de odometria en (0,0,0) para salvar dicho error.

  else if ( Message::MatchMessage( hdr, PLAYER_MSGTYPE_REQ,
        PLAYER_POSITION2D_REQ_SET_ODOM,
        this->position2d_addr ) )
  {
    if ( hdr->size != sizeof( player_position2d_set_odom_req_t ) )
    {
      PLAYER_WARN("Arg to odometry set requests wrong size; ignoring");
      return(-1);
    }
    player_position2d_set_odom_req_t* set_odom_req =
      (player_position2d_set_odom_req_t*)data;

    if ( ( set_odom_req->pose.px == 0) &&
        ( set_odom_req->pose.py == 0) &&
        ( set_odom_req->pose.pa == 0) )
    {
      romaa_comms->reset_odometry();
    }
    else
    {
      romaa_comms->set_odometry( set_odom_req->pose.px,
          set_odom_req->pose.py,
          set_odom_req->pose.pa );
    }

    this->Publish( this->position2d_addr, resp_queue,
        PLAYER_MSGTYPE_RESP_ACK,
        PLAYER_POSITION2D_REQ_SET_ODOM );
    return (0);
  }