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
Commands: Se utilizan para darle instrucciones al driver cuando no se requiere una respuesta. La interfaz position2d usa comandos para recibir velocidades para los motores.
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 iteracción del loop Main del driver.
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);
}