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
00030
00046
00047
00048
00049
00050 #include <string.h>
00051 #include <libgen.h>
00052 #include <libplayercore/globals.h>
00053
00054 #include "p_driver.h"
00055 using namespace Stg;
00056
00057
00058 extern bool player_quiet_startup;
00059 extern PlayerTime* GlobalTime;
00060
00061 #define DRIVER_ERROR(X) printf( "Stage driver error: %s\n", X )
00062
00063 #ifndef PLAYER_CAPABILITIES_REQ
00064 #ifdef PLAYER_CAPABILTIES_REQ
00065 #define PLAYER_CAPABILITIES_REQ PLAYER_CAPABILTIES_REQ
00066 #endif
00067 #endif
00068
00070
00071
00072
00073
00074 InterfaceSimulation::InterfaceSimulation( player_devaddr_t addr,
00075 StgDriver* driver,
00076 ConfigFile* cf,
00077 int section )
00078 : Interface( addr, driver, cf, section )
00079 {
00080 printf( "a Stage world" ); fflush(stdout);
00081
00082
00083 Stg::Init( &player_argc, &player_argv );
00084
00085 StgDriver::usegui = cf->ReadBool(section, "usegui", 1 );
00086
00087 const char* worldfile_name = cf->ReadString(section, "worldfile", NULL );
00088
00089 if( worldfile_name == NULL )
00090 {
00091 PRINT_ERR1( "device \"%s\" uses the Stage driver but has "
00092 "no \"model\" value defined. You must specify a "
00093 "model name that matches one of the models in "
00094 "the worldfile.",
00095 worldfile_name );
00096 return;
00097 }
00098
00099 char fullname[MAXPATHLEN];
00100
00101 if( worldfile_name[0] == '/' )
00102 strcpy( fullname, worldfile_name );
00103 else
00104 {
00105 char *tmp = strdup(cf->filename);
00106 snprintf( fullname, MAXPATHLEN,
00107 "%s/%s", dirname(tmp), worldfile_name );
00108 free(tmp);
00109 }
00110
00111
00112
00113
00114
00115
00116
00117
00118
00119
00120
00121
00122
00123 StgDriver::world = ( StgDriver::usegui ? new WorldGui( 400, 300, worldfile_name ) : new World(worldfile_name));
00124 assert(StgDriver::world);
00125 puts("");
00126
00127 StgDriver::world->Load( fullname );
00128
00129
00130
00131
00132
00133
00134
00135
00136
00137
00138
00139
00140 delete GlobalTime;
00141 GlobalTime = new StTime( driver );
00142 assert(GlobalTime);
00143
00144
00145
00146
00147 StgDriver::world->Start();
00148
00149
00150
00151 driver->alwayson = TRUE;
00152
00153 puts( "" );
00154 }
00155
00156 int InterfaceSimulation::ProcessMessage(QueuePointer &resp_queue,
00157 player_msghdr_t* hdr,
00158 void* data)
00159 {
00160 if(Message::MatchMessage(hdr, PLAYER_MSGTYPE_REQ, PLAYER_CAPABILITIES_REQ, addr))
00161 {
00162 PLAYER_ERROR1("%p\n", data);
00163 player_capabilities_req_t & cap_req = * reinterpret_cast<player_capabilities_req_t *> (data);
00164 if (cap_req.type == PLAYER_MSGTYPE_REQ && (cap_req.subtype == PLAYER_SIMULATION_REQ_SET_POSE3D || cap_req.subtype == PLAYER_SIMULATION_REQ_GET_POSE3D))
00165 {
00166 this->driver->Publish(addr, resp_queue, PLAYER_MSGTYPE_RESP_ACK, PLAYER_CAPABILITIES_REQ);
00167 return 0;
00168 }
00169 }
00170
00171
00172 if(Message::MatchMessage(hdr, PLAYER_MSGTYPE_REQ,
00173 PLAYER_SIMULATION_REQ_GET_POSE2D,
00174 this->addr))
00175 {
00176 player_simulation_pose2d_req_t* req =
00177 (player_simulation_pose2d_req_t*)data;
00178
00179 PRINT_DEBUG1( "Stage: received request for the 2D position of object \"%s\"\n", req->name );
00180
00181
00182 Model* mod = StgDriver::world->GetModel( req->name );
00183
00184 if( mod )
00185 {
00186 Pose pose = mod->GetPose();
00187
00188 PRINT_DEBUG3( "Stage: returning location [ %.2f, %.2f, %.2f ]\n",
00189 pose.x, pose.y, pose.a );
00190
00191 player_simulation_pose2d_req_t reply;
00192 memcpy( &reply, req, sizeof(reply));
00193 reply.pose.px = pose.x;
00194 reply.pose.py = pose.y;
00195 reply.pose.pa = pose.a;
00196
00197 this->driver->Publish( this->addr, resp_queue,
00198 PLAYER_MSGTYPE_RESP_ACK,
00199 PLAYER_SIMULATION_REQ_GET_POSE2D,
00200 (void*)&reply, sizeof(reply), NULL );
00201 return(0);
00202 }
00203 else
00204 {
00205 PRINT_WARN1( "Stage: GET_POSE2D request: simulation model \"%s\" not found", req->name );
00206 return(-1);
00207 }
00208 }
00209
00210
00211 if(Message::MatchMessage(hdr, PLAYER_MSGTYPE_REQ,
00212 PLAYER_SIMULATION_REQ_SET_POSE2D,
00213 this->addr))
00214 {
00215 player_simulation_pose2d_req_t* req =
00216 (player_simulation_pose2d_req_t*)data;
00217
00218
00219 Model* mod = StgDriver::world->GetModel( req->name );
00220
00221 if( mod )
00222 {
00223 PRINT_DEBUG4( "Stage: moving \"%s\" to [ %.2f, %.2f, %.2f ]\n",
00224 req->name, req->pose.px, req->pose.py, req->pose.pa );
00225
00226 Pose pose = mod->GetPose();
00227 pose.x = req->pose.px;
00228 pose.y = req->pose.py;
00229 pose.a = req->pose.pa;
00230
00231 mod->SetPose( pose );
00232
00233 this->driver->Publish(this->addr, resp_queue,
00234 PLAYER_MSGTYPE_RESP_ACK,
00235 PLAYER_SIMULATION_REQ_SET_POSE2D);
00236 return(0);
00237 }
00238 else
00239 {
00240 PRINT_WARN1( "SETPOSE2D request: simulation model \"%s\" not found", req->name );
00241 return(-1);
00242 }
00243 }
00244
00245
00246 else if(Message::MatchMessage(hdr, PLAYER_MSGTYPE_REQ,
00247 PLAYER_SIMULATION_REQ_GET_POSE3D,
00248 this->addr))
00249 {
00250 player_simulation_pose3d_req_t* req =
00251 (player_simulation_pose3d_req_t*)data;
00252
00253 PRINT_DEBUG1( "Stage: received request for the 3D position of object \"%s\"\n", req->name );
00254
00255
00256 Model* mod = StgDriver::world->GetModel( req->name );
00257
00258 if( mod )
00259 {
00260 Pose pose = mod->GetPose();
00261
00262 PRINT_DEBUG4( "Stage: returning location [ %.2f, %.2f, %.2f, %.2f ]\n",
00263 pose.x, pose.y, pose.z, pose.a );
00264
00265 player_simulation_pose3d_req_t reply;
00266 memcpy( &reply, req, sizeof(reply));
00267 reply.pose.px = pose.x;
00268 reply.pose.py = pose.y;
00269 reply.pose.pz = pose.z;
00270 reply.pose.proll = 0;
00271 reply.pose.ppitch = 0;
00272 reply.pose.pyaw = pose.a;
00273 reply.simtime = mod->GetWorld()->SimTimeNow();
00274
00275 this->driver->Publish( this->addr, resp_queue,
00276 PLAYER_MSGTYPE_RESP_ACK,
00277 PLAYER_SIMULATION_REQ_GET_POSE3D,
00278 (void*)&reply, sizeof(reply), NULL );
00279 return(0);
00280 }
00281 else
00282 {
00283 PRINT_WARN1( "Stage: GET_POSE3D request: simulation model \"%s\" not found", req->name );
00284 return(-1);
00285 }
00286 }
00287
00288
00289 if(Message::MatchMessage(hdr, PLAYER_MSGTYPE_REQ,
00290 PLAYER_SIMULATION_REQ_SET_POSE3D,
00291 this->addr))
00292 {
00293 player_simulation_pose3d_req_t* req =
00294 (player_simulation_pose3d_req_t*)data;
00295
00296
00297 Model* mod = StgDriver::world->GetModel( req->name );
00298
00299 if( mod )
00300 {
00301 PRINT_DEBUG5( "Stage: moving \"%s\" to [ %.2f, %.2f, %.2f %.2f ]\n",
00302 req->name, req->pose.px, req->pose.py, req->pose.pz, req->pose.pyaw );
00303
00304 Pose pose = mod->GetPose();
00305 pose.x = req->pose.px;
00306 pose.y = req->pose.py;
00307 pose.z = req->pose.pz;
00308 pose.a = req->pose.pyaw;
00309
00310
00311 mod->SetPose( pose );
00312
00313 this->driver->Publish(this->addr, resp_queue,
00314 PLAYER_MSGTYPE_RESP_ACK,
00315 PLAYER_SIMULATION_REQ_SET_POSE3D);
00316 return(0);
00317 }
00318 else
00319 {
00320 PRINT_WARN1( "SETPOSE2D request: simulation model \"%s\" not found", req->name );
00321 return(-1);
00322 }
00323 }
00324
00325
00326
00327 else if(Message::MatchMessage(hdr, PLAYER_MSGTYPE_REQ,
00328 PLAYER_SIMULATION_REQ_SET_PROPERTY,
00329 this->addr))
00330 {
00331 player_simulation_property_req_t* req =
00332 (player_simulation_property_req_t*)data;
00333
00334
00335
00336
00337
00338 if( strncmp(req->prop, "color", (size_t)req->prop_count) )
00339 {
00340 PRINT_WARN1("Property \"%s\" can not be set. Options are \"color\"", req->prop);
00341 return(-1);
00342 }
00343
00344
00345 if(req->value_count != sizeof(float)*4)
00346 {
00347 PRINT_WARN("Value given by SetProperty must be an array of 4 floats\n");
00348 return(-1);
00349 }
00350
00351
00352
00353 Model* mod = StgDriver::world->GetModel( req->name );
00354
00355
00356 if( mod )
00357 {
00358 int ack = 0;
00359 float *col = (float *)req->value;
00360 Color newColour = Color(col[0], col[1], col[2], col[3]);
00361
00362 mod->SetColor(newColour);
00363
00364 this->driver->Publish(this->addr, resp_queue,
00365 ack==0 ? PLAYER_MSGTYPE_RESP_ACK : PLAYER_MSGTYPE_RESP_NACK,
00366 PLAYER_SIMULATION_REQ_SET_PROPERTY);
00367 return(0);
00368 }
00369 else
00370 {
00371 PRINT_WARN1( "SET_PROPERTY request: simulation model \"%s\" not found", req->name );
00372 return(-1);
00373 }
00374 }
00375
00376
00377 else if(Message::MatchMessage(hdr, PLAYER_MSGTYPE_REQ,
00378 PLAYER_SIMULATION_REQ_GET_PROPERTY,
00379 this->addr))
00380 {
00381 player_simulation_property_req_t* req =
00382 (player_simulation_property_req_t*)data;
00383
00384
00385
00386
00387
00388
00389 if( !(strncmp(req->prop, "color", (size_t)req->prop_count) ))
00390 {
00391
00392 if(req->value_count != sizeof(float)*4)
00393 {
00394 PRINT_WARN("Colour requires an array of 4 floats to store\n");
00395 return(-1);
00396 }
00397
00398
00399 Model* mod = StgDriver::world->GetModel( req->name );
00400
00401 if( mod )
00402 {
00403 Color newColour = mod->GetColor();
00404
00405 float col[4];
00406 col[0] = newColour.r;
00407 col[1] = newColour.g;
00408 col[2] = newColour.b;
00409 col[3] = newColour.a;
00410
00411
00412 memcpy(req->value, col, req->value_count);
00413
00414
00415
00416 player_simulation_property_req_t reply;
00417 memcpy( &reply, req, sizeof(reply));
00418
00419
00420 memcpy(reply.value, col, reply.value_count);
00421
00422
00423 this->driver->Publish( this->addr, resp_queue,
00424 PLAYER_MSGTYPE_RESP_ACK,
00425 PLAYER_SIMULATION_REQ_GET_PROPERTY,
00426 (void*)&reply, sizeof(reply), NULL );
00427
00428 return(0);
00429 }
00430 else
00431 {
00432 PRINT_WARN1( "GET_PROPERTY request: simulation model \"%s\" not found", req->name );
00433 return(-1);
00434 }
00435 }
00436 else if( strncmp(req->prop, "time", (size_t)req->prop_count ) == 0 )
00437 {
00438
00439 if(req->value_count != sizeof(uint64_t))
00440 {
00441 PRINT_WARN("Simulation time requires a uint64_t to store\n");
00442 return(-1);
00443 }
00444
00445
00446
00447 Model* mod = StgDriver::world->GetModel( req->name );
00448
00449 if( mod )
00450 {
00451
00452 player_simulation_property_req_t reply;
00453 memcpy( &reply, req, sizeof(reply));
00454
00455
00456 *(uint64_t*)&reply.value = mod->GetWorld()->SimTimeNow();
00457
00458 this->driver->Publish( this->addr, resp_queue,
00459 PLAYER_MSGTYPE_RESP_ACK,
00460 PLAYER_SIMULATION_REQ_GET_PROPERTY,
00461 (void*)&reply, sizeof(reply), NULL );
00462
00463 return(0);
00464 }
00465 else
00466 {
00467 PRINT_WARN1( "GET_PROPERTY request: simulation model \"%s\" not found", req->name );
00468 return(-1);
00469 }
00470
00471 }
00472 else
00473 {
00474 PRINT_WARN1("Property \"%s\" is not accessible. Options are \"color\", \"_mp_color\", or \"colour\" for changing colour. \"simtime\" or \"sim_time\" for getting the simulation time.", req->prop);
00475 return(-1);
00476 }
00477
00478 }
00479
00480 else
00481 {
00482
00483 PRINT_WARN2( "simulation doesn't support msg with type/subtype %d/%d",
00484 hdr->type, hdr->subtype);
00485 return(-1);
00486 }
00487 }