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

Go to the documentation of this file.

#include <VisionControl.H>
#include <CommsManager.H>
#include <Logger.H>

// $Id: Type.cpp 68 2007-02-01 17:56:57Z fwph $

using namespace WURDE;

VisionControl::VisionControl(const std::string & streamname){
       init(streamname,STRAT_NORMAL,"Any");

       //global name will be set later
       m_haveGlobalname=false;
}

VisionControl::VisionControl(const ConnectionStrategy & strategy,const std::string & gblName){
       init("Any",strategy,gblName);

       if(strategy==STRAT_NORMAL){
              //then the global name really specifies the streamname,
              // and the global name should be assigned automagically once we have
              // confirmation that the stream has been started
              m_haveGlobalname=false;   
       }

       m_haveStream=false;
       
}

VisionControl::VisionControl(const std::string &streamname,const std::string &gblName){
       init(streamname,STRAT_NORMAL,gblName);
}

void VisionControl::init(const std::string &streamname,const ConnectionStrategy &strategy,const std::string &gblName){
  m_globalname=gblName;
  m_streamname=streamname;

  myCOMObject=NULL;
  
  //  data.source=streamname.c_str();
  requests=info;

  m_queue=false;
  m_supplier=false;
  m_doTimestamp=true;
  m_doTag=true;

  m_doPublishData=false;
  m_doPublishInfo=false;

  m_initialize=false;
  m_strategy=strategy;

  m_haveStream=true;
  m_haveGlobalname=true;
  m_doPublishData=false;
  m_doPublishInfo=false;

  m_doAutoPing=true;

  info.source=streamname;
  requests.source=m_globalname;
  info.target="Any";
  requests.target=streamname;
  info.timestamp.now();
  requests.timestamp.now();

  m_streamRequest=STAT_NULL;
  
  m_sourceModule="Unknown";
}


VisionControl::~VisionControl(){
  myManager=NULL;
  if(myCOMObject){

         //      delete myCOMObject;
  }
}


void VisionControl::activateSupplier(){
       if(m_haveStream){
              m_supplier=true;
              m_globalname=m_streamname;
              m_haveGlobalname=true;
              m_initialize=false;
              if(myCOMObject){
                      //                     myCOMObject=new CMUIPCRobotProtocol::CMUIPCVisionControl(m_globalname);
                     myCOMObject->setGlobalName(m_globalname);
                     myCOMObject->setStreamName(m_streamname);
                     myCOMObject->setInfo(&requests);
                     myCOMObject->setRequests(&info);
                     myCOMObject->setStaging(&data);
                     myCOMObject->activateSupplier();
                     myCOMObject->setAutoPing(m_doAutoPing);
                     myCOMObject->setInfoVector(&m_requestVector);
                     myCOMObject->setRequestVector(&m_infoVector);
                     myCOMObject->setDataVector(&m_dataVector);

                     data.source=m_streamname;
                     info.source=m_streamname;

                     if(m_queue){
                            myCOMObject->setQueueMode(true);
                     }

              }else{
                      g_fatal("Attempted to activateSupplier before setting a COMObject Please just use registerSupplier.");
                      exit(-1);
              }
        if(m_supplier){
                info.options.setCMode(C_SUPPLIER);
        }else{
                info.options.setCMode(C_CONSUMER);
        }
        if(m_supplier){
                info.stopOperators.setCMode(C_SUPPLIER);
        }else{
                info.stopOperators.setCMode(C_CONSUMER);
        }
        if(m_supplier){
                info.visionOperators.setCMode(C_SUPPLIER);
        }else{
                info.visionOperators.setCMode(C_CONSUMER);
        }
       }else{
              //this shouldn't happen, but there's no real problem with it.
              //it'd just be dumb.
              m_initialize=true;
              m_supplier=true;
       }
       
       requests=info;

}

void VisionControl::activateConsumer(){
       std::string localname;
       
       if(m_haveStream){
              m_initialize=false;
              m_supplier=false;
              if(myCOMObject&&myManager){
                      /* global name so we don't have conflicts with the naming service */
                      if(!m_haveGlobalname&&m_strategy==STRAT_NORMAL){
                              m_globalname=myManager->getName()+VISIONCONTROL+m_streamname+"Consumer";
                              m_haveGlobalname=true;
                      }

                     myCOMObject->setGlobalName(m_globalname);
                     myCOMObject->setStreamName(m_streamname);
                     requests.source=m_globalname;
                     data.source=m_streamname;
                     info.source=m_streamname;
                     myCOMObject->setInfo(&info);
                     myCOMObject->setRequests(&requests);
                     myCOMObject->setStaging(&data);
                     myCOMObject->setAutoPing(m_doAutoPing);
                     myCOMObject->activateConsumer();
                     myCOMObject->setInfoVector(&m_infoVector);
                     myCOMObject->setRequestVector(&m_requestVector);
                     myCOMObject->setDataVector(&m_dataVector);

                     if(m_queue){
                            myCOMObject->setQueueMode(true);
                     }
              }else{
                      g_fatal("Attempted to activateConsumer before setting a COMObject and CommsManager. Please use registerConsumer instead.");
                     exit(-1);
              }

        if(m_supplier){
                info.options.setCMode(C_SUPPLIER);
        }else{
                info.options.setCMode(C_CONSUMER);
        }
        if(m_supplier){
                info.stopOperators.setCMode(C_SUPPLIER);
        }else{
                info.stopOperators.setCMode(C_CONSUMER);
        }
        if(m_supplier){
                info.visionOperators.setCMode(C_SUPPLIER);
        }else{
                info.visionOperators.setCMode(C_CONSUMER);
        }
       }else{
              //no stream, so we can't connect yet. tell the system 
              //to initialize as soon as we've got one
              m_initialize=true;
       }

}

void VisionControl::runUpdate(){
       if(myCOMObject){

              myCOMObject->runUpdate(m_doPublishData,m_doPublishInfo);
              
              m_doPublishData=false;
              m_doPublishInfo=false;
              
       }
}

void VisionControl::setManager(CommsManager *myManager_){

  if(!myCOMObject){
    myManager=myManager_;
  }

}

void VisionControl::setQueueMode(const bool & val){

       if(!myCOMObject){
              m_queue=val;
       }else if(!m_queue&&val){
              m_queue=val;
              myCOMObject->setDataVector(&m_dataVector);
              myCOMObject->setQueueMode(true);

       }else if(!val&&m_queue){
              m_queue=val;
              myCOMObject->setQueueMode(false);
              while(!m_dataVector.empty()){
                     m_dataVector.pop();
              }
              while(!m_infoVector.empty()){
                     m_infoVector.pop();
              }
              while(!m_requestVector.empty()){
                     m_requestVector.pop();
              }
       }
}

bool VisionControl::newData(){
       if(m_queue){
              return !m_dataVector.empty();
       }else{
              if(myCOMObject){
                     return myCOMObject->newData();
              }
       }

       return false;
}

bool VisionControl::newInfo(){
       if(m_queue){
              if(m_supplier){
                     return !m_requestVector.empty();
              }

              return !m_infoVector.empty();
       }else{
              if(myCOMObject){
                     return myCOMObject->newInfo();
              }
       }


       return false;
}

void VisionControl::publishData(){
       if(m_supplier){
              if(m_doTimestamp){
                     data.timestamp.now();
              }
              if(m_doTag){
                     data.source=m_globalname;
              }
              if(m_queue){
                     VisionControlDataStruct temp;
                     temp.source=data.source;
temp.timestamp=data.timestamp;
 
                     m_dataVector.push(temp);
                              //                     m_dataVector.push(data);
              }
              m_doPublishData=true;
       }
}

void VisionControl::publishInfo(){
       if(m_supplier){

              info.mType=MESSAGE_INFO;
              if(m_doTimestamp){
                     info.timestamp.now();
              }

              if(m_doTag){
                     info.source=m_globalname;
                     info.target="Any";
              }

              if(m_queue){
                     VisionControlInfoStruct temp;
                     
                    
                     temp.source=info.source;
temp.target=info.target;
temp.options=info.options;
temp.timestamp=info.timestamp;
temp.stopOperators=info.stopOperators;
temp.visionOperators=info.visionOperators;
temp.mType=info.mType;

                     m_infoVector.push(temp);
                    
                     //              m_infoVector.push(info);
              }

              m_doPublishInfo=true;
       }else{

              requests.mType=MESSAGE_REQUEST;
              
              if(m_doTimestamp){
                     requests.timestamp.now();
              }

              if(m_doTag){
                     requests.source=m_globalname;
                     requests.target=m_streamname;
              }

              if(m_queue){
                     VisionControlInfoStruct temp;
        if(m_supplier){
                temp.options.setCMode(C_SUPPLIER);
        }else{
                temp.options.setCMode(C_CONSUMER);
        }
        if(m_supplier){
                temp.stopOperators.setCMode(C_SUPPLIER);
        }else{
                temp.stopOperators.setCMode(C_CONSUMER);
        }
        if(m_supplier){
                temp.visionOperators.setCMode(C_SUPPLIER);
        }else{
                temp.visionOperators.setCMode(C_CONSUMER);
        }
                     temp.source=requests.source;
temp.target=requests.target;
temp.options=requests.options;
temp.timestamp=requests.timestamp;
temp.stopOperators=requests.stopOperators;
temp.visionOperators=requests.visionOperators;
temp.mType=requests.mType;

                     m_requestVector.push(temp);

                     //              m_requestVector.push(requests);
              }

              m_doPublishInfo=true;
       }
}

VisionControlDataStruct VisionControl::getNextData(){
       if(m_queue){
              VisionControlDataStruct retval;
              retval.source="NULL";
              if(m_dataVector.empty()){
                     return data;
              }
             
              retval.source=m_dataVector.front().source;
retval.timestamp=m_dataVector.front().timestamp;

                       //             retval=m_dataVector.front();
              m_dataVector.pop();
              data=retval;
       
              return retval;
       }else{
              myCOMObject->resetData();
              return data;
       }
}

VisionControlInfoStruct VisionControl::getNextInfo(){
       if(m_queue){
              VisionControlInfoStruct retval;
        if(m_supplier){
                retval.options.setCMode(C_SUPPLIER);
        }else{
                retval.options.setCMode(C_CONSUMER);
        }
        if(m_supplier){
                retval.stopOperators.setCMode(C_SUPPLIER);
        }else{
                retval.stopOperators.setCMode(C_CONSUMER);
        }
        if(m_supplier){
                retval.visionOperators.setCMode(C_SUPPLIER);
        }else{
                retval.visionOperators.setCMode(C_CONSUMER);
        }
              retval.source="NULL";
              if(m_supplier){
                     if(m_requestVector.empty()){
                            return requests;
                     }
                     retval.source=m_requestVector.front().source;
retval.target=m_requestVector.front().target;
retval.options=m_requestVector.front().options;
retval.timestamp=m_requestVector.front().timestamp;
retval.stopOperators=m_requestVector.front().stopOperators;
retval.visionOperators=m_requestVector.front().visionOperators;
retval.mType=m_requestVector.front().mType;

                              //                     retval=m_requestVector.front();
        if(m_supplier){
                retval.options.setCMode(C_SUPPLIER);
        }else{
                retval.options.setCMode(C_CONSUMER);
        }
        if(m_supplier){
                retval.stopOperators.setCMode(C_SUPPLIER);
        }else{
                retval.stopOperators.setCMode(C_CONSUMER);
        }
        if(m_supplier){
                retval.visionOperators.setCMode(C_SUPPLIER);
        }else{
                retval.visionOperators.setCMode(C_CONSUMER);
        }
                     m_requestVector.pop();
                     requests=retval;
                     return retval;
              }else{
                     if(m_infoVector.empty()){
                            return info;
                     }
                     retval.source=m_infoVector.front().source;
retval.target=m_infoVector.front().target;
retval.options=m_infoVector.front().options;
retval.timestamp=m_infoVector.front().timestamp;
retval.stopOperators=m_infoVector.front().stopOperators;
retval.visionOperators=m_infoVector.front().visionOperators;
retval.mType=m_infoVector.front().mType;

        if(m_supplier){
                retval.options.setCMode(C_SUPPLIER);
        }else{
                retval.options.setCMode(C_CONSUMER);
        }
        if(m_supplier){
                retval.stopOperators.setCMode(C_SUPPLIER);
        }else{
                retval.stopOperators.setCMode(C_CONSUMER);
        }
        if(m_supplier){
                retval.visionOperators.setCMode(C_SUPPLIER);
        }else{
                retval.visionOperators.setCMode(C_CONSUMER);
        }
                     m_infoVector.pop();
                     info=retval;
                     // the requests struct really should get the values in 
                     // info
                     requests=info;
                     
                     return retval;
              }
       }else{
              myCOMObject->resetInfo();
              if(m_supplier){
        if(m_supplier){
                requests.options.setCMode(C_SUPPLIER);
        }else{
                requests.options.setCMode(C_CONSUMER);
        }
        if(m_supplier){
                requests.stopOperators.setCMode(C_SUPPLIER);
        }else{
                requests.stopOperators.setCMode(C_CONSUMER);
        }
        if(m_supplier){
                requests.visionOperators.setCMode(C_SUPPLIER);
        }else{
                requests.visionOperators.setCMode(C_CONSUMER);
        }
                     return requests;
              }else{
        if(m_supplier){
                info.options.setCMode(C_SUPPLIER);
        }else{
                info.options.setCMode(C_CONSUMER);
        }
        if(m_supplier){
                info.stopOperators.setCMode(C_SUPPLIER);
        }else{
                info.stopOperators.setCMode(C_CONSUMER);
        }
        if(m_supplier){
                info.visionOperators.setCMode(C_SUPPLIER);
        }else{
                info.visionOperators.setCMode(C_CONSUMER);
        }
                     // the requests struct really should get the values in 
                     // info
                     requests=info;
                     return info;
              }
       }
}

void VisionControl::setStreamName(const std::string & name){

       if(m_supplier&&name=="Any"){
              return;
       }

       m_streamname=name;
       m_haveStream=true;

       if(m_supplier){
              m_globalname=name;
       }
       
       if(myCOMObject){
              myCOMObject->setStreamName(name);
       }

       if(m_initialize){
              if(m_supplier){
                     activateSupplier();
              }else{
                     activateConsumer();
              }
       }

       m_streamRequest=STAT_OKAY;
}

void VisionControl::setGlobalName(const std::string & name){
       
       if(name=="Any"){
              return;
       }

       m_globalname=name;
       m_haveGlobalname=true;
       if(m_supplier){
              m_streamname=name;
       }

       if(myCOMObject){
              myCOMObject->setGlobalName(name);
       }
}

void VisionControl::doPing(){
       if(myCOMObject){
              myCOMObject->doPing();
       }
}

void VisionControl::setAutoPing(const bool & val){
       m_doAutoPing=val; 
       if(myCOMObject){
              myCOMObject->setAutoPing(val);
       }
}

---