00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023
00024
00025
00026
00027
00028
00029
00043
00044
00045 #include "p_driver.h"
00046 using namespace Stg;
00047
00048 InterfacePosition::InterfacePosition( player_devaddr_t addr,
00049 StgDriver* driver,
00050 ConfigFile* cf,
00051 int section )
00052
00053 : InterfaceModel( addr, driver, cf, section, "position" )
00054 {
00055
00056 }
00057
00058 int InterfacePosition::ProcessMessage(QueuePointer &resp_queue,
00059 player_msghdr_t* hdr,
00060 void* data)
00061 {
00062 ModelPosition* mod = (ModelPosition*)this->mod;
00063
00064
00065 if(Message::MatchMessage(hdr, PLAYER_MSGTYPE_CMD,
00066 PLAYER_POSITION2D_CMD_VEL,
00067 this->addr))
00068 {
00069
00070 player_position2d_cmd_vel_t* pcmd = (player_position2d_cmd_vel_t*)data;
00071
00072
00073
00074
00075 mod->SetSpeed( pcmd->vel.px, pcmd->vel.py, pcmd->vel.pa );
00076
00077 return 0;
00078 }
00079
00080
00081 else if(Message::MatchMessage(hdr, PLAYER_MSGTYPE_CMD,
00082 PLAYER_POSITION2D_CMD_POS,
00083 this->addr))
00084 {
00085
00086 player_position2d_cmd_pos_t* pcmd = (player_position2d_cmd_pos_t*)data;
00087
00088 mod->GoTo( pcmd->pos.px, pcmd->pos.py, pcmd->pos.pa );
00089 return 0;
00090 }
00091
00092
00093 else if(Message::MatchMessage(hdr, PLAYER_MSGTYPE_CMD,
00094 PLAYER_POSITION2D_CMD_CAR,
00095 this->addr))
00096 {
00097
00098 player_position2d_cmd_car_t* pcmd = (player_position2d_cmd_car_t*)data;
00099
00100 mod->SetSpeed( pcmd->velocity, 0, pcmd->angle );
00101 return 0;
00102 }
00103
00104
00105 else if(Message::MatchMessage(hdr, PLAYER_MSGTYPE_REQ,
00106 PLAYER_POSITION2D_REQ_GET_GEOM,
00107 this->addr))
00108 {
00109 if(hdr->size == 0)
00110 {
00111 Geom geom = this->mod->GetGeom();
00112
00113
00114 player_position2d_geom_t pgeom;
00115 pgeom.pose.px = geom.pose.x;
00116 pgeom.pose.py = geom.pose.y;
00117 pgeom.pose.pyaw = geom.pose.a;
00118
00119 pgeom.size.sl = geom.size.x;
00120 pgeom.size.sw = geom.size.y;
00121
00122 this->driver->Publish( this->addr, resp_queue,
00123 PLAYER_MSGTYPE_RESP_ACK,
00124 PLAYER_POSITION2D_REQ_GET_GEOM,
00125 (void*)&pgeom, sizeof(pgeom), NULL );
00126 return 0;
00127 }
00128 else
00129 {
00130 PRINT_ERR2("config request len is invalid (%d != %d)",
00131 (int)hdr->size, 0);
00132 return(-1);
00133 }
00134 }
00135
00136 else if(Message::MatchMessage(hdr, PLAYER_MSGTYPE_REQ,
00137 PLAYER_POSITION2D_REQ_RESET_ODOM,
00138 this->addr))
00139 {
00140 if(hdr->size == 0)
00141 {
00142 PRINT_DEBUG( "resetting odometry" );
00143
00144 mod->est_pose.x = 0;
00145 mod->est_pose.y = 0;
00146 mod->est_pose.z = 0;
00147 mod->est_pose.a = 0;
00148
00149 this->driver->Publish( this->addr, resp_queue,
00150 PLAYER_MSGTYPE_RESP_ACK,
00151 PLAYER_POSITION2D_REQ_RESET_ODOM );
00152 return 0;
00153 }
00154 else
00155 {
00156 PRINT_ERR2("config request len is invalid (%d != %d)",
00157 (int)hdr->size, 0);
00158 return -1;
00159 }
00160 }
00161
00162 else if(Message::MatchMessage(hdr, PLAYER_MSGTYPE_REQ,
00163 PLAYER_POSITION2D_REQ_SET_ODOM,
00164 this->addr))
00165 {
00166 if(hdr->size == sizeof(player_position2d_set_odom_req_t))
00167 {
00168 player_position2d_set_odom_req_t* req =
00169 (player_position2d_set_odom_req_t*)data;
00170
00171
00172 mod->est_pose.x = req->pose.px;
00173 mod->est_pose.y = req->pose.py;
00174
00175 mod->est_pose.a = req->pose.pa;
00176
00177 PRINT_DEBUG3( "set odometry to (%.2f,%.2f,%.2f)",
00178 pose.x,
00179 pose.y,
00180 pose.a );
00181
00182 this->driver->Publish( this->addr, resp_queue,
00183 PLAYER_MSGTYPE_RESP_ACK,
00184 PLAYER_POSITION2D_REQ_SET_ODOM );
00185 return(0);
00186 }
00187 else
00188 {
00189 PRINT_ERR2("config request len is invalid (%d != %d)",
00190 (int)hdr->size, (int)sizeof(player_position2d_set_odom_req_t));
00191 return(-1);
00192 }
00193 }
00194
00195 else if(Message::MatchMessage(hdr, PLAYER_MSGTYPE_REQ,
00196 PLAYER_POSITION2D_REQ_MOTOR_POWER,
00197 this->addr))
00198 {
00199 if(hdr->size == sizeof(player_position2d_power_config_t))
00200 {
00201 player_position2d_power_config_t* req =
00202 (player_position2d_power_config_t*)data;
00203
00204 int motors_on = req->state;
00205
00206 PRINT_WARN1( "Stage ignores motor power state (%d)",
00207 motors_on );
00208 this->driver->Publish( this->addr, resp_queue,
00209 PLAYER_MSGTYPE_RESP_ACK,
00210 PLAYER_POSITION2D_REQ_MOTOR_POWER );
00211 return(0);
00212 }
00213 else
00214 {
00215 PRINT_ERR2("config request len is invalid (%d != %d)",
00216 (int)hdr->size, (int)sizeof(player_position2d_power_config_t));
00217 return(-1);
00218 }
00219 }
00220
00221 else if(Message::MatchMessage(hdr, PLAYER_MSGTYPE_REQ,
00222 PLAYER_POSITION2D_REQ_POSITION_MODE,
00223 this->addr))
00224 {
00225 if(hdr->size == sizeof(player_position2d_position_mode_req_t))
00226 {
00227
00228
00229
00230
00231
00232
00233
00234 PRINT_WARN2( "Put model %s into %s control mode", this->mod->Token(), mod ? "POSITION" : "VELOCITY" );
00235 PRINT_WARN( "set control mode not yet implemented") ;
00236
00237 this->driver->Publish( this->addr, resp_queue,
00238 PLAYER_MSGTYPE_RESP_ACK,
00239 PLAYER_POSITION2D_REQ_POSITION_MODE );
00240 return(0);
00241 }
00242 else
00243 {
00244 PRINT_ERR2("config request len is invalid (%d != %d)",
00245 (int)hdr->size,
00246 (int)sizeof(player_position2d_position_mode_req_t));
00247 return(-1);
00248 }
00249 }
00250
00251
00252
00253
00254 PRINT_WARN2( "position doesn't support msg with type %d subtype %d",
00255 hdr->type, hdr->subtype);
00256 return(-1);
00257 }
00258
00259 void InterfacePosition::Publish( void )
00260 {
00261
00262
00263 ModelPosition* mod = (ModelPosition*)this->mod;
00264
00265
00266
00267
00268 player_position2d_data_t ppd;
00269 bzero( &ppd, sizeof(ppd) );
00270
00271
00272
00273 ppd.pos.px = mod->est_pose.x;
00274 ppd.pos.py = mod->est_pose.y;
00275
00276 ppd.pos.pa = mod->est_pose.a;
00277
00278 Velocity v = mod->GetVelocity();
00279
00280 ppd.vel.px = v.x;
00281 ppd.vel.py = v.y;
00282 ppd.vel.pa = v.a;
00283
00284
00285 ppd.stall = this->mod->Stalled();
00286
00287
00288 this->driver->Publish( this->addr,
00289 PLAYER_MSGTYPE_DATA, PLAYER_POSITION2D_DATA_STATE,
00290 (void*)&ppd, sizeof(ppd), NULL);
00291 }