Re: [Playerstage-users] Position2d driver - ResetOdometry

[scott <sc...@md...>]

>From the code it seems like you should implement SetOdometry in your driver. ResetOdometry() is calling SetOdometry(0,0,0).

>From client_libs/libplayerc++/position2dproxy.cc:
void
Position2dProxy::ResetOdometry()
{
  SetOdometry(0,0,0);
}



Gonzalo Fernando Perez Paina wrote:
> Hi all,
> I am programming a custom plugin driver for our robot (romaa) with
> position2d interface. In the ProcessMessage method, the section that
> respond to the PLAYER_POSITION2D_REQ_RESET_ODOM message is the following
> 
>  else if ( Message::MatchMessage( hdr, PLAYER_MSGTYPE_REQ,       //
> PLAYER_POSITION2D_REQ_RESET_ODOM   
>           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;
> ignorint" );
>       return ( -1 );
>     }
>     PLAYER_MSG0( 3, "reset odometry..." );
>     romaa_comms->reset_odometry( );
> 
>     Publish( this->position2d_addr, resp_queue,
>         PLAYER_MSGTYPE_RESP_ACK,
>         PLAYER_POSITION2D_REQ_RESET_ODOM );   
>     return ( 0 );
>   }
> 
> but when a clien program execute ResetOdometry() method to the
> Position2dProxy
> 
> // 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.ResetOdometry();
> 
> the server gives the following message
> warning : Unhandled message for driver device=0:6665:position2d:0
> type=request subtype=5 len=24
> 
> and the client gives
> playerc error   : got NACK from request
> 
> the subtype=5 message of position2d interface is
> PLAYER_POSITION2D_REQ_SET_ODOM
> #define PLAYER_POSITION2D_REQ_SET_ODOM 5
> #define PLAYER_POSITION2D_REQ_RESET_ODOM 6
> 
> The only way I could implement to respond the ResetOdometry methods is
> using the PLAYER_POSITION2D_REQ_SET_ODOM
> and checking if the set value is (0,0,0) to reset the odometry of the
> robot o set odometry if it is not (0,0,0)
> 
> else if( Message::MatchMessage( hdr, PLAYER_MSGTYPE_REQ,     //
> PLAYER_POSITION2D_REQ_SET_ODOM
>         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 <http://pose.pa> == 0) )
>     {
>       PLAYER_MSG0( 3, "reset odometry..." );
>       romaa_comms->reset_odometry( );
>     }
>     else
>     {
>       PLAYER_MSG0( 3, "set odometry..." );
>       romaa_comms->set_odometry( (float)set_odom_req->pose.px,
>           (float)set_odom_req->pose.py,
>           (float)set_odom_req->pose.pa <http://pose.pa> );
>     }
> 
>     Publish( this->position2d_addr, resp_queue,
>         PLAYER_MSGTYPE_RESP_ACK,
>         PLAYER_POSITION2D_REQ_SET_ODOM );
>     return (0);
>   }
> 
> 
> Although it is working I would like to ask you if am I doing something
> wrong in the ProcessMessage methods or this is a bug in the interface
> position2d messages? I do not have enought knowledge of Player structure
> to look for this error in the Player sources.
> Maybe someone could fix it if it's a bug....
> Thanks
> 
> -- 
> Gonzalo F. Perez Paina
> Centro de Investigación en Informática para la Ingeniería, CIII.
> Universidad Tecnológica Nacional. Facultad Regional Córdoba.
> Maestro M. López esq. Cruz Roja Argentina. CP X5016ZAA
> gp...@sc... <mailto:gp...@sc...>
> 
> 
> ------------------------------------------------------------------------
> 
> ------------------------------------------------------------------------------
> 
> 
> ------------------------------------------------------------------------
> 
> _______________________________________________
> Playerstage-users mailing list
> Pla...@li...
> https://lists.sourceforge.net/lists/listinfo/playerstage-users