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>intmain( 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); }