/home/fwph/code/wurde/rde/core/capabilities/Egomotion.cpp

Go to the documentation of this file.
00001 
00029 #include <Egomotion.H>
00030 #include <CommsManager.H>
00031 #include <Logger.H>
00032 
00033 // $Id: Type.cpp 68 2007-02-01 17:56:57Z fwph $
00034 
00035 using namespace WURDE;
00036 
00037 Egomotion::Egomotion(const std::string & streamname){
00038        init(streamname,STRAT_NORMAL,"Any");
00039 
00040        //global name will be set later
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               //then the global name really specifies the streamname,
00049               // and the global name should be assigned automagically once we have
00050               // confirmation that the stream has been started
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   //  data.source=streamname.c_str();
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          //      delete myCOMObject;
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                       //                     myCOMObject=new CMUIPCRobotProtocol::CMUIPCEgomotion(m_globalname);
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               //this shouldn't happen, but there's no real problem with it.
00147               //it'd just be dumb.
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                       /* global name so we don't have conflicts with the naming service */
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               //no stream, so we can't connect yet. tell the system 
00197               //to initialize as soon as we've got one
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                               //                     m_dataVector.push(data);
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                      //              m_infoVector.push(info);
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                      //              m_requestVector.push(requests);
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                        //             retval=m_dataVector.front();
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                               //                     retval=m_requestVector.front();
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                      // the requests struct really should get the values in 
00421                      // info
00422                      requests=info;
00423                      
00424                      return retval;
00425               }
00426        }else{
00427               myCOMObject->resetInfo();
00428               if(m_supplier){
00429                      return requests;
00430               }else{
00431                      // the requests struct really should get the values in 
00432                      // info
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 

Generated on Thu Feb 1 15:31:52 2007 for WURDE by  doxygen 1.5.1