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

Go to the documentation of this file.

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

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

using namespace WURDE;

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

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

VectorMoverTransport::VectorMoverTransport(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;
       
}

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

void VectorMoverTransport::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";
}


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

         //      delete myCOMObject;
  }
}


void VectorMoverTransport::activateSupplier(){
       if(m_haveStream){
              m_supplier=true;
              m_globalname=m_streamname;
              m_haveGlobalname=true;
              m_initialize=false;
              if(myCOMObject){
                      //                     myCOMObject=new CMUIPCRobotProtocol::CMUIPCVectorMoverTransport(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);
              }
        data.hasGoal=0;
        data.distanceToGoal=0;
        data.angleToGoal=0;
        if(m_supplier){
                info.absolute.setCMode(C_SUPPLIER);
        }else{
                info.absolute.setCMode(C_CONSUMER);
        }
        if(m_supplier){
                info.maxTranslationVelocity.setCMode(C_SUPPLIER);
        }else{
                info.maxTranslationVelocity.setCMode(C_CONSUMER);
        }
        if(m_supplier){
                info.goalHasLocation.setCMode(C_SUPPLIER);
        }else{
                info.goalHasLocation.setCMode(C_CONSUMER);
        }
        if(m_supplier){
                info.goalHasTheta.setCMode(C_SUPPLIER);
        }else{
                info.goalHasTheta.setCMode(C_CONSUMER);
        }
        if(m_supplier){
                info.brake.setCMode(C_SUPPLIER);
        }else{
                info.brake.setCMode(C_CONSUMER);
        }
        if(m_supplier){
                info.maxRotationVelocity.setCMode(C_SUPPLIER);
        }else{
                info.maxRotationVelocity.setCMode(C_CONSUMER);
        }
        if(m_supplier){
                info.goalPose.setCMode(C_SUPPLIER);
        }else{
                info.goalPose.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 VectorMoverTransport::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()+VECTORMOVERTRANSPORT+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);
              }

        data.hasGoal=0;
        data.distanceToGoal=0;
        data.angleToGoal=0;
        if(m_supplier){
                info.absolute.setCMode(C_SUPPLIER);
        }else{
                info.absolute.setCMode(C_CONSUMER);
        }
        if(m_supplier){
                info.maxTranslationVelocity.setCMode(C_SUPPLIER);
        }else{
                info.maxTranslationVelocity.setCMode(C_CONSUMER);
        }
        if(m_supplier){
                info.goalHasLocation.setCMode(C_SUPPLIER);
        }else{
                info.goalHasLocation.setCMode(C_CONSUMER);
        }
        if(m_supplier){
                info.goalHasTheta.setCMode(C_SUPPLIER);
        }else{
                info.goalHasTheta.setCMode(C_CONSUMER);
        }
        if(m_supplier){
                info.brake.setCMode(C_SUPPLIER);
        }else{
                info.brake.setCMode(C_CONSUMER);
        }
        if(m_supplier){
                info.maxRotationVelocity.setCMode(C_SUPPLIER);
        }else{
                info.maxRotationVelocity.setCMode(C_CONSUMER);
        }
        if(m_supplier){
                info.goalPose.setCMode(C_SUPPLIER);
        }else{
                info.goalPose.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 VectorMoverTransport::runUpdate(){
       if(myCOMObject){

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

void VectorMoverTransport::setManager(CommsManager *myManager_){

  if(!myCOMObject){
    myManager=myManager_;
  }

}

void VectorMoverTransport::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 VectorMoverTransport::newData(){
       if(m_queue){
              return !m_dataVector.empty();
       }else{
              if(myCOMObject){
                     return myCOMObject->newData();
              }
       }

       return false;
}

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

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


       return false;
}

void VectorMoverTransport::publishData(){
       if(m_supplier){
              if(m_doTimestamp){
                     data.timestamp.now();
              }
              if(m_doTag){
                     data.source=m_globalname;
              }
              if(m_queue){
                     VectorMoverTransportDataStruct temp;
        temp.hasGoal=0;
        temp.distanceToGoal=0;
        temp.angleToGoal=0;
                     temp.hasGoal=data.hasGoal;
temp.source=data.source;
temp.timestamp=data.timestamp;
temp.distanceToGoal=data.distanceToGoal;
temp.angleToGoal=data.angleToGoal;
 
                     m_dataVector.push(temp);
                              //                     m_dataVector.push(data);
              }
              m_doPublishData=true;
       }
}

void VectorMoverTransport::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){
                     VectorMoverTransportInfoStruct temp;
                     
                    
                     temp.source=info.source;
temp.absolute=info.absolute;
temp.maxTranslationVelocity=info.maxTranslationVelocity;
temp.goalHasLocation=info.goalHasLocation;
temp.mType=info.mType;
temp.target=info.target;
temp.timestamp=info.timestamp;
temp.goalHasTheta=info.goalHasTheta;
temp.brake=info.brake;
temp.maxRotationVelocity=info.maxRotationVelocity;
temp.goalPose=info.goalPose;

                     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){
                     VectorMoverTransportInfoStruct temp;
        if(m_supplier){
                temp.absolute.setCMode(C_SUPPLIER);
        }else{
                temp.absolute.setCMode(C_CONSUMER);
        }
        if(m_supplier){
                temp.maxTranslationVelocity.setCMode(C_SUPPLIER);
        }else{
                temp.maxTranslationVelocity.setCMode(C_CONSUMER);
        }
        if(m_supplier){
                temp.goalHasLocation.setCMode(C_SUPPLIER);
        }else{
                temp.goalHasLocation.setCMode(C_CONSUMER);
        }
        if(m_supplier){
                temp.goalHasTheta.setCMode(C_SUPPLIER);
        }else{
                temp.goalHasTheta.setCMode(C_CONSUMER);
        }
        if(m_supplier){
                temp.brake.setCMode(C_SUPPLIER);
        }else{
                temp.brake.setCMode(C_CONSUMER);
        }
        if(m_supplier){
                temp.maxRotationVelocity.setCMode(C_SUPPLIER);
        }else{
                temp.maxRotationVelocity.setCMode(C_CONSUMER);
        }
        if(m_supplier){
                temp.goalPose.setCMode(C_SUPPLIER);
        }else{
                temp.goalPose.setCMode(C_CONSUMER);
        }
                     temp.source=requests.source;
temp.absolute=requests.absolute;
temp.maxTranslationVelocity=requests.maxTranslationVelocity;
temp.goalHasLocation=requests.goalHasLocation;
temp.mType=requests.mType;
temp.target=requests.target;
temp.goalHasTheta=requests.goalHasTheta;
temp.timestamp=requests.timestamp;
temp.brake=requests.brake;
temp.maxRotationVelocity=requests.maxRotationVelocity;
temp.goalPose=requests.goalPose;

                     m_requestVector.push(temp);

                     //              m_requestVector.push(requests);
              }

              m_doPublishInfo=true;
       }
}

VectorMoverTransportDataStruct VectorMoverTransport::getNextData(){
       if(m_queue){
              VectorMoverTransportDataStruct retval;
        retval.hasGoal=0;
        retval.distanceToGoal=0;
        retval.angleToGoal=0;
              retval.source="NULL";
              if(m_dataVector.empty()){
                     return data;
              }
             
              retval.source=m_dataVector.front().source;
retval.angleToGoal=m_dataVector.front().angleToGoal;
retval.hasGoal=m_dataVector.front().hasGoal;
retval.timestamp=m_dataVector.front().timestamp;
retval.distanceToGoal=m_dataVector.front().distanceToGoal;

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

VectorMoverTransportInfoStruct VectorMoverTransport::getNextInfo(){
       if(m_queue){
              VectorMoverTransportInfoStruct retval;
        if(m_supplier){
                retval.absolute.setCMode(C_SUPPLIER);
        }else{
                retval.absolute.setCMode(C_CONSUMER);
        }
        if(m_supplier){
                retval.maxTranslationVelocity.setCMode(C_SUPPLIER);
        }else{
                retval.maxTranslationVelocity.setCMode(C_CONSUMER);
        }
        if(m_supplier){
                retval.goalHasLocation.setCMode(C_SUPPLIER);
        }else{
                retval.goalHasLocation.setCMode(C_CONSUMER);
        }
        if(m_supplier){
                retval.goalHasTheta.setCMode(C_SUPPLIER);
        }else{
                retval.goalHasTheta.setCMode(C_CONSUMER);
        }
        if(m_supplier){
                retval.brake.setCMode(C_SUPPLIER);
        }else{
                retval.brake.setCMode(C_CONSUMER);
        }
        if(m_supplier){
                retval.maxRotationVelocity.setCMode(C_SUPPLIER);
        }else{
                retval.maxRotationVelocity.setCMode(C_CONSUMER);
        }
        if(m_supplier){
                retval.goalPose.setCMode(C_SUPPLIER);
        }else{
                retval.goalPose.setCMode(C_CONSUMER);
        }
              retval.source="NULL";
              if(m_supplier){
                     if(m_requestVector.empty()){
                            return requests;
                     }
                     retval.source=m_requestVector.front().source;
retval.absolute=m_requestVector.front().absolute;
retval.maxTranslationVelocity=m_requestVector.front().maxTranslationVelocity;
retval.goalHasLocation=m_requestVector.front().goalHasLocation;
retval.mType=m_requestVector.front().mType;
retval.target=m_requestVector.front().target;
retval.goalHasTheta=m_requestVector.front().goalHasTheta;
retval.timestamp=m_requestVector.front().timestamp;
retval.brake=m_requestVector.front().brake;
retval.maxRotationVelocity=m_requestVector.front().maxRotationVelocity;
retval.goalPose=m_requestVector.front().goalPose;

                              //                     retval=m_requestVector.front();
        if(m_supplier){
                retval.absolute.setCMode(C_SUPPLIER);
        }else{
                retval.absolute.setCMode(C_CONSUMER);
        }
        if(m_supplier){
                retval.maxTranslationVelocity.setCMode(C_SUPPLIER);
        }else{
                retval.maxTranslationVelocity.setCMode(C_CONSUMER);
        }
        if(m_supplier){
                retval.goalHasLocation.setCMode(C_SUPPLIER);
        }else{
                retval.goalHasLocation.setCMode(C_CONSUMER);
        }
        if(m_supplier){
                retval.goalHasTheta.setCMode(C_SUPPLIER);
        }else{
                retval.goalHasTheta.setCMode(C_CONSUMER);
        }
        if(m_supplier){
                retval.brake.setCMode(C_SUPPLIER);
        }else{
                retval.brake.setCMode(C_CONSUMER);
        }
        if(m_supplier){
                retval.maxRotationVelocity.setCMode(C_SUPPLIER);
        }else{
                retval.maxRotationVelocity.setCMode(C_CONSUMER);
        }
        if(m_supplier){
                retval.goalPose.setCMode(C_SUPPLIER);
        }else{
                retval.goalPose.setCMode(C_CONSUMER);
        }
                     m_requestVector.pop();
                     requests=retval;
                     return retval;
              }else{
                     if(m_infoVector.empty()){
                            return info;
                     }
                     retval.source=m_infoVector.front().source;
retval.absolute=m_infoVector.front().absolute;
retval.maxTranslationVelocity=m_infoVector.front().maxTranslationVelocity;
retval.goalHasLocation=m_infoVector.front().goalHasLocation;
retval.mType=m_infoVector.front().mType;
retval.target=m_infoVector.front().target;
retval.goalHasTheta=m_infoVector.front().goalHasTheta;
retval.timestamp=m_infoVector.front().timestamp;
retval.brake=m_infoVector.front().brake;
retval.maxRotationVelocity=m_infoVector.front().maxRotationVelocity;
retval.goalPose=m_infoVector.front().goalPose;

        if(m_supplier){
                retval.absolute.setCMode(C_SUPPLIER);
        }else{
                retval.absolute.setCMode(C_CONSUMER);
        }
        if(m_supplier){
                retval.maxTranslationVelocity.setCMode(C_SUPPLIER);
        }else{
                retval.maxTranslationVelocity.setCMode(C_CONSUMER);
        }
        if(m_supplier){
                retval.goalHasLocation.setCMode(C_SUPPLIER);
        }else{
                retval.goalHasLocation.setCMode(C_CONSUMER);
        }
        if(m_supplier){
                retval.goalHasTheta.setCMode(C_SUPPLIER);
        }else{
                retval.goalHasTheta.setCMode(C_CONSUMER);
        }
        if(m_supplier){
                retval.brake.setCMode(C_SUPPLIER);
        }else{
                retval.brake.setCMode(C_CONSUMER);
        }
        if(m_supplier){
                retval.maxRotationVelocity.setCMode(C_SUPPLIER);
        }else{
                retval.maxRotationVelocity.setCMode(C_CONSUMER);
        }
        if(m_supplier){
                retval.goalPose.setCMode(C_SUPPLIER);
        }else{
                retval.goalPose.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.absolute.setCMode(C_SUPPLIER);
        }else{
                requests.absolute.setCMode(C_CONSUMER);
        }
        if(m_supplier){
                requests.maxTranslationVelocity.setCMode(C_SUPPLIER);
        }else{
                requests.maxTranslationVelocity.setCMode(C_CONSUMER);
        }
        if(m_supplier){
                requests.goalHasLocation.setCMode(C_SUPPLIER);
        }else{
                requests.goalHasLocation.setCMode(C_CONSUMER);
        }
        if(m_supplier){
                requests.goalHasTheta.setCMode(C_SUPPLIER);
        }else{
                requests.goalHasTheta.setCMode(C_CONSUMER);
        }
        if(m_supplier){
                requests.brake.setCMode(C_SUPPLIER);
        }else{
                requests.brake.setCMode(C_CONSUMER);
        }
        if(m_supplier){
                requests.maxRotationVelocity.setCMode(C_SUPPLIER);
        }else{
                requests.maxRotationVelocity.setCMode(C_CONSUMER);
        }
        if(m_supplier){
                requests.goalPose.setCMode(C_SUPPLIER);
        }else{
                requests.goalPose.setCMode(C_CONSUMER);
        }
                     return requests;
              }else{
        if(m_supplier){
                info.absolute.setCMode(C_SUPPLIER);
        }else{
                info.absolute.setCMode(C_CONSUMER);
        }
        if(m_supplier){
                info.maxTranslationVelocity.setCMode(C_SUPPLIER);
        }else{
                info.maxTranslationVelocity.setCMode(C_CONSUMER);
        }
        if(m_supplier){
                info.goalHasLocation.setCMode(C_SUPPLIER);
        }else{
                info.goalHasLocation.setCMode(C_CONSUMER);
        }
        if(m_supplier){
                info.goalHasTheta.setCMode(C_SUPPLIER);
        }else{
                info.goalHasTheta.setCMode(C_CONSUMER);
        }
        if(m_supplier){
                info.brake.setCMode(C_SUPPLIER);
        }else{
                info.brake.setCMode(C_CONSUMER);
        }
        if(m_supplier){
                info.maxRotationVelocity.setCMode(C_SUPPLIER);
        }else{
                info.maxRotationVelocity.setCMode(C_CONSUMER);
        }
        if(m_supplier){
                info.goalPose.setCMode(C_SUPPLIER);
        }else{
                info.goalPose.setCMode(C_CONSUMER);
        }
                     // the requests struct really should get the values in 
                     // info
                     requests=info;
                     return info;
              }
       }
}

void VectorMoverTransport::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 VectorMoverTransport::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 VectorMoverTransport::doPing(){
       if(myCOMObject){
              myCOMObject->doPing();
       }
}

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

---