00001
00029 #include <Egomotion.H>
00030 #include <CommsManager.H>
00031 #include <Logger.H>
00032
00033
00034
00035 using namespace WURDE;
00036
00037 Egomotion::Egomotion(const std::string & streamname){
00038 init(streamname,STRAT_NORMAL,"Any");
00039
00040
00041 m_haveGlobalname=false;
00042 }
00043
00044 Egomotion::Egomotion(const ConnectionStrategy & strategy,const std::string & gblName){
00045 init("Any",strategy,gblName);
00046
00047 if(strategy==STRAT_NORMAL){
00048
00049
00050
00051 m_haveGlobalname=false;
00052 }
00053
00054 m_haveStream=false;
00055
00056 }
00057
00058 Egomotion::Egomotion(const std::string &streamname,const std::string &gblName){
00059 init(streamname,STRAT_NORMAL,gblName);
00060 }
00061
00062 void Egomotion::init(const std::string &streamname,const ConnectionStrategy &strategy,const std::string &gblName){
00063 m_globalname=gblName;
00064 m_streamname=streamname;
00065
00066 myCOMObject=NULL;
00067
00068
00069 requests=info;
00070
00071 m_queue=false;
00072 m_supplier=false;
00073 m_doTimestamp=true;
00074 m_doTag=true;
00075
00076 m_doPublishData=false;
00077 m_doPublishInfo=false;
00078
00079 m_initialize=false;
00080 m_strategy=strategy;
00081
00082 m_haveStream=true;
00083 m_haveGlobalname=true;
00084 m_doPublishData=false;
00085 m_doPublishInfo=false;
00086
00087 m_doAutoPing=true;
00088
00089 info.source=streamname;
00090 requests.source=m_globalname;
00091 info.target="Any";
00092 requests.target=streamname;
00093 info.timestamp.now();
00094 requests.timestamp.now();
00095
00096 m_streamRequest=STAT_NULL;
00097
00098 m_sourceModule="Unknown";
00099 }
00100
00101
00102 Egomotion::~Egomotion(){
00103 myManager=NULL;
00104 if(myCOMObject){
00105
00106
00107 }
00108 }
00109
00110
00111 void Egomotion::activateSupplier(){
00112 if(m_haveStream){
00113 m_supplier=true;
00114 m_globalname=m_streamname;
00115 m_haveGlobalname=true;
00116 m_initialize=false;
00117 if(myCOMObject){
00118
00119 myCOMObject->setGlobalName(m_globalname);
00120 myCOMObject->setStreamName(m_streamname);
00121 myCOMObject->setInfo(&requests);
00122 myCOMObject->setRequests(&info);
00123 myCOMObject->setStaging(&data);
00124 myCOMObject->activateSupplier();
00125 myCOMObject->setAutoPing(m_doAutoPing);
00126 myCOMObject->setInfoVector(&m_requestVector);
00127 myCOMObject->setRequestVector(&m_infoVector);
00128 myCOMObject->setDataVector(&m_dataVector);
00129
00130 data.source=m_streamname;
00131 info.source=m_streamname;
00132
00133 if(m_queue){
00134 myCOMObject->setQueueMode(true);
00135 }
00136
00137 }else{
00138 g_fatal("Attempted to activateSupplier before setting a COMObject Please just use registerSupplier.");
00139 exit(-1);
00140 }
00141 data.angularVelocity=0;
00142 data.angularAccel=0;
00143 data.transAccel=0;
00144 data.transVelocity=0;
00145 }else{
00146
00147
00148 m_initialize=true;
00149 m_supplier=true;
00150 }
00151
00152 requests=info;
00153
00154 }
00155
00156 void Egomotion::activateConsumer(){
00157 std::string localname;
00158
00159 if(m_haveStream){
00160 m_initialize=false;
00161 m_supplier=false;
00162 if(myCOMObject&&myManager){
00163
00164 if(!m_haveGlobalname&&m_strategy==STRAT_NORMAL){
00165 m_globalname=myManager->getName()+EGOMOTION+m_streamname+"Consumer";
00166 m_haveGlobalname=true;
00167 }
00168
00169 myCOMObject->setGlobalName(m_globalname);
00170 myCOMObject->setStreamName(m_streamname);
00171 requests.source=m_globalname;
00172 data.source=m_streamname;
00173 info.source=m_streamname;
00174 myCOMObject->setInfo(&info);
00175 myCOMObject->setRequests(&requests);
00176 myCOMObject->setStaging(&data);
00177 myCOMObject->setAutoPing(m_doAutoPing);
00178 myCOMObject->activateConsumer();
00179 myCOMObject->setInfoVector(&m_infoVector);
00180 myCOMObject->setRequestVector(&m_requestVector);
00181 myCOMObject->setDataVector(&m_dataVector);
00182
00183 if(m_queue){
00184 myCOMObject->setQueueMode(true);
00185 }
00186 }else{
00187 g_fatal("Attempted to activateConsumer before setting a COMObject and CommsManager. Please use registerConsumer instead.");
00188 exit(-1);
00189 }
00190
00191 data.angularVelocity=0;
00192 data.angularAccel=0;
00193 data.transAccel=0;
00194 data.transVelocity=0;
00195 }else{
00196
00197
00198 m_initialize=true;
00199 }
00200
00201 }
00202
00203 void Egomotion::runUpdate(){
00204 if(myCOMObject){
00205
00206 myCOMObject->runUpdate(m_doPublishData,m_doPublishInfo);
00207
00208 m_doPublishData=false;
00209 m_doPublishInfo=false;
00210
00211 }
00212 }
00213
00214 void Egomotion::setManager(CommsManager *myManager_){
00215
00216 if(!myCOMObject){
00217 myManager=myManager_;
00218 }
00219
00220 }
00221
00222 void Egomotion::setQueueMode(const bool & val){
00223
00224 if(!myCOMObject){
00225 m_queue=val;
00226 }else if(!m_queue&&val){
00227 m_queue=val;
00228 myCOMObject->setDataVector(&m_dataVector);
00229 myCOMObject->setQueueMode(true);
00230
00231 }else if(!val&&m_queue){
00232 m_queue=val;
00233 myCOMObject->setQueueMode(false);
00234 while(!m_dataVector.empty()){
00235 m_dataVector.pop();
00236 }
00237 while(!m_infoVector.empty()){
00238 m_infoVector.pop();
00239 }
00240 while(!m_requestVector.empty()){
00241 m_requestVector.pop();
00242 }
00243 }
00244 }
00245
00246 bool Egomotion::newData(){
00247 if(m_queue){
00248 return !m_dataVector.empty();
00249 }else{
00250 if(myCOMObject){
00251 return myCOMObject->newData();
00252 }
00253 }
00254
00255 return false;
00256 }
00257
00258 bool Egomotion::newInfo(){
00259 if(m_queue){
00260 if(m_supplier){
00261 return !m_requestVector.empty();
00262 }
00263
00264 return !m_infoVector.empty();
00265 }else{
00266 if(myCOMObject){
00267 return myCOMObject->newInfo();
00268 }
00269 }
00270
00271
00272 return false;
00273 }
00274
00275 void Egomotion::publishData(){
00276 if(m_supplier){
00277 if(m_doTimestamp){
00278 data.timestamp.now();
00279 }
00280 if(m_doTag){
00281 data.source=m_globalname;
00282 }
00283 if(m_queue){
00284 EgomotionDataStruct temp;
00285 temp.angularVelocity=0;
00286 temp.angularAccel=0;
00287 temp.transAccel=0;
00288 temp.transVelocity=0;
00289 temp.angularVelocity=data.angularVelocity;
00290 temp.source=data.source;
00291 temp.angularAccel=data.angularAccel;
00292 temp.transAccel=data.transAccel;
00293 temp.location=data.location;
00294 temp.timestamp=data.timestamp;
00295 temp.transVelocity=data.transVelocity;
00296
00297 m_dataVector.push(temp);
00298
00299 }
00300 m_doPublishData=true;
00301 }
00302 }
00303
00304 void Egomotion::publishInfo(){
00305 if(m_supplier){
00306
00307 info.mType=MESSAGE_INFO;
00308 if(m_doTimestamp){
00309 info.timestamp.now();
00310 }
00311
00312 if(m_doTag){
00313 info.source=m_globalname;
00314 info.target="Any";
00315 }
00316
00317 if(m_queue){
00318 EgomotionInfoStruct temp;
00319
00320
00321 temp.source=info.source;
00322 temp.target=info.target;
00323 temp.timestamp=info.timestamp;
00324 temp.mType=info.mType;
00325
00326 m_infoVector.push(temp);
00327
00328
00329 }
00330
00331 m_doPublishInfo=true;
00332 }else{
00333
00334 requests.mType=MESSAGE_REQUEST;
00335
00336 if(m_doTimestamp){
00337 requests.timestamp.now();
00338 }
00339
00340 if(m_doTag){
00341 requests.source=m_globalname;
00342 requests.target=m_streamname;
00343 }
00344
00345 if(m_queue){
00346 EgomotionInfoStruct temp;
00347 temp.source=requests.source;
00348 temp.target=requests.target;
00349 temp.timestamp=requests.timestamp;
00350 temp.mType=requests.mType;
00351
00352 m_requestVector.push(temp);
00353
00354
00355 }
00356
00357 m_doPublishInfo=true;
00358 }
00359 }
00360
00361 EgomotionDataStruct Egomotion::getNextData(){
00362 if(m_queue){
00363 EgomotionDataStruct retval;
00364 retval.angularVelocity=0;
00365 retval.angularAccel=0;
00366 retval.transAccel=0;
00367 retval.transVelocity=0;
00368 retval.source="NULL";
00369 if(m_dataVector.empty()){
00370 return data;
00371 }
00372
00373 retval.angularVelocity=m_dataVector.front().angularVelocity;
00374 retval.source=m_dataVector.front().source;
00375 retval.angularAccel=m_dataVector.front().angularAccel;
00376 retval.transAccel=m_dataVector.front().transAccel;
00377 retval.location=m_dataVector.front().location;
00378 retval.timestamp=m_dataVector.front().timestamp;
00379 retval.transVelocity=m_dataVector.front().transVelocity;
00380
00381
00382 m_dataVector.pop();
00383 data=retval;
00384
00385 return retval;
00386 }else{
00387 myCOMObject->resetData();
00388 return data;
00389 }
00390 }
00391
00392 EgomotionInfoStruct Egomotion::getNextInfo(){
00393 if(m_queue){
00394 EgomotionInfoStruct retval;
00395 retval.source="NULL";
00396 if(m_supplier){
00397 if(m_requestVector.empty()){
00398 return requests;
00399 }
00400 retval.source=m_requestVector.front().source;
00401 retval.target=m_requestVector.front().target;
00402 retval.timestamp=m_requestVector.front().timestamp;
00403 retval.mType=m_requestVector.front().mType;
00404
00405
00406 m_requestVector.pop();
00407 requests=retval;
00408 return retval;
00409 }else{
00410 if(m_infoVector.empty()){
00411 return info;
00412 }
00413 retval.source=m_infoVector.front().source;
00414 retval.target=m_infoVector.front().target;
00415 retval.timestamp=m_infoVector.front().timestamp;
00416 retval.mType=m_infoVector.front().mType;
00417
00418 m_infoVector.pop();
00419 info=retval;
00420
00421
00422 requests=info;
00423
00424 return retval;
00425 }
00426 }else{
00427 myCOMObject->resetInfo();
00428 if(m_supplier){
00429 return requests;
00430 }else{
00431
00432
00433 requests=info;
00434 return info;
00435 }
00436 }
00437 }
00438
00439 void Egomotion::setStreamName(const std::string & name){
00440
00441 if(m_supplier&&name=="Any"){
00442 return;
00443 }
00444
00445 m_streamname=name;
00446 m_haveStream=true;
00447
00448 if(m_supplier){
00449 m_globalname=name;
00450 }
00451
00452 if(myCOMObject){
00453 myCOMObject->setStreamName(name);
00454 }
00455
00456 if(m_initialize){
00457 if(m_supplier){
00458 activateSupplier();
00459 }else{
00460 activateConsumer();
00461 }
00462 }
00463
00464 m_streamRequest=STAT_OKAY;
00465 }
00466
00467 void Egomotion::setGlobalName(const std::string & name){
00468
00469 if(name=="Any"){
00470 return;
00471 }
00472
00473 m_globalname=name;
00474 m_haveGlobalname=true;
00475 if(m_supplier){
00476 m_streamname=name;
00477 }
00478
00479 if(myCOMObject){
00480 myCOMObject->setGlobalName(name);
00481 }
00482 }
00483
00484 void Egomotion::doPing(){
00485 if(myCOMObject){
00486 myCOMObject->doPing();
00487 }
00488 }
00489
00490 void Egomotion::setAutoPing(const bool & val){
00491 m_doAutoPing=val;
00492 if(myCOMObject){
00493 myCOMObject->setAutoPing(val);
00494 }
00495 }
00496
00497