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

Go to the documentation of this file.

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

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

using namespace WURDE;

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

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

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

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

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


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

         //      delete myCOMObject;
  }
}


void RobotDrive::activateSupplier(){
       if(m_haveStream){
              m_supplier=true;
              m_globalname=m_streamname;
              m_haveGlobalname=true;
              m_initialize=false;
              if(myCOMObject){
                      //                     myCOMObject=new CMUIPCRobotProtocol::CMUIPCRobotDrive(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.angularAccel.setCMode(C_SUPPLIER);
        }else{
                info.angularAccel.setCMode(C_CONSUMER);
        }
        if(m_supplier){
                info.transAccel.setCMode(C_SUPPLIER);
        }else{
                info.transAccel.setCMode(C_CONSUMER);
        }
        if(m_supplier){
                info.angularLimit.setCMode(C_SUPPLIER);
        }else{
                info.angularLimit.setCMode(C_CONSUMER);
        }
        if(m_supplier){
                info.angular.setCMode(C_SUPPLIER);
        }else{
                info.angular.setCMode(C_CONSUMER);
        }
        if(m_supplier){
                info.angularDecel.setCMode(C_SUPPLIER);
        }else{
                info.angularDecel.setCMode(C_CONSUMER);
        }
        if(m_supplier){
                info.trans.setCMode(C_SUPPLIER);
        }else{
                info.trans.setCMode(C_CONSUMER);
        }
        if(m_supplier){
                info.transDecel.setCMode(C_SUPPLIER);
        }else{
                info.transDecel.setCMode(C_CONSUMER);
        }
        if(m_supplier){
                info.brake.setCMode(C_SUPPLIER);
        }else{
                info.brake.setCMode(C_CONSUMER);
        }
        if(m_supplier){
                info.transLimit.setCMode(C_SUPPLIER);
        }else{
                info.transLimit.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 RobotDrive::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()+ROBOTDRIVE+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.angularAccel.setCMode(C_SUPPLIER);
        }else{
                info.angularAccel.setCMode(C_CONSUMER);
        }
        if(m_supplier){
                info.transAccel.setCMode(C_SUPPLIER);
        }else{
                info.transAccel.setCMode(C_CONSUMER);
        }
        if(m_supplier){
                info.angularLimit.setCMode(C_SUPPLIER);
        }else{
                info.angularLimit.setCMode(C_CONSUMER);
        }
        if(m_supplier){
                info.angular.setCMode(C_SUPPLIER);
        }else{
                info.angular.setCMode(C_CONSUMER);
        }
        if(m_supplier){
                info.angularDecel.setCMode(C_SUPPLIER);
        }else{
                info.angularDecel.setCMode(C_CONSUMER);
        }
        if(m_supplier){
                info.trans.setCMode(C_SUPPLIER);
        }else{
                info.trans.setCMode(C_CONSUMER);
        }
        if(m_supplier){
                info.transDecel.setCMode(C_SUPPLIER);
        }else{
                info.transDecel.setCMode(C_CONSUMER);
        }
        if(m_supplier){
                info.brake.setCMode(C_SUPPLIER);
        }else{
                info.brake.setCMode(C_CONSUMER);
        }
        if(m_supplier){
                info.transLimit.setCMode(C_SUPPLIER);
        }else{
                info.transLimit.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 RobotDrive::runUpdate(){
       if(myCOMObject){

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

void RobotDrive::setManager(CommsManager *myManager_){

  if(!myCOMObject){
    myManager=myManager_;
  }

}

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

       return false;
}

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

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


       return false;
}

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

void RobotDrive::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){
                     RobotDriveInfoStruct temp;
                     
                    
                     temp.source=info.source;
temp.angularAccel=info.angularAccel;
temp.transAccel=info.transAccel;
temp.angularLimit=info.angularLimit;
temp.angular=info.angular;
temp.mType=info.mType;
temp.angularDecel=info.angularDecel;
temp.trans=info.trans;
temp.target=info.target;
temp.transDecel=info.transDecel;
temp.timestamp=info.timestamp;
temp.brake=info.brake;
temp.transLimit=info.transLimit;

                     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){
                     RobotDriveInfoStruct temp;
        if(m_supplier){
                temp.angularAccel.setCMode(C_SUPPLIER);
        }else{
                temp.angularAccel.setCMode(C_CONSUMER);
        }
        if(m_supplier){
                temp.transAccel.setCMode(C_SUPPLIER);
        }else{
                temp.transAccel.setCMode(C_CONSUMER);
        }
        if(m_supplier){
                temp.angularLimit.setCMode(C_SUPPLIER);
        }else{
                temp.angularLimit.setCMode(C_CONSUMER);
        }
        if(m_supplier){
                temp.angular.setCMode(C_SUPPLIER);
        }else{
                temp.angular.setCMode(C_CONSUMER);
        }
        if(m_supplier){
                temp.angularDecel.setCMode(C_SUPPLIER);
        }else{
                temp.angularDecel.setCMode(C_CONSUMER);
        }
        if(m_supplier){
                temp.trans.setCMode(C_SUPPLIER);
        }else{
                temp.trans.setCMode(C_CONSUMER);
        }
        if(m_supplier){
                temp.transDecel.setCMode(C_SUPPLIER);
        }else{
                temp.transDecel.setCMode(C_CONSUMER);
        }
        if(m_supplier){
                temp.brake.setCMode(C_SUPPLIER);
        }else{
                temp.brake.setCMode(C_CONSUMER);
        }
        if(m_supplier){
                temp.transLimit.setCMode(C_SUPPLIER);
        }else{
                temp.transLimit.setCMode(C_CONSUMER);
        }
                     temp.source=requests.source;
temp.angularAccel=requests.angularAccel;
temp.transAccel=requests.transAccel;
temp.angularLimit=requests.angularLimit;
temp.angular=requests.angular;
temp.mType=requests.mType;
temp.trans=requests.trans;
temp.angularDecel=requests.angularDecel;
temp.target=requests.target;
temp.transDecel=requests.transDecel;
temp.timestamp=requests.timestamp;
temp.brake=requests.brake;
temp.transLimit=requests.transLimit;

                     m_requestVector.push(temp);

                     //              m_requestVector.push(requests);
              }

              m_doPublishInfo=true;
       }
}

RobotDriveDataStruct RobotDrive::getNextData(){
       if(m_queue){
              RobotDriveDataStruct 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;
       }
}

RobotDriveInfoStruct RobotDrive::getNextInfo(){
       if(m_queue){
              RobotDriveInfoStruct retval;
        if(m_supplier){
                retval.angularAccel.setCMode(C_SUPPLIER);
        }else{
                retval.angularAccel.setCMode(C_CONSUMER);
        }
        if(m_supplier){
                retval.transAccel.setCMode(C_SUPPLIER);
        }else{
                retval.transAccel.setCMode(C_CONSUMER);
        }
        if(m_supplier){
                retval.angularLimit.setCMode(C_SUPPLIER);
        }else{
                retval.angularLimit.setCMode(C_CONSUMER);
        }
        if(m_supplier){
                retval.angular.setCMode(C_SUPPLIER);
        }else{
                retval.angular.setCMode(C_CONSUMER);
        }
        if(m_supplier){
                retval.angularDecel.setCMode(C_SUPPLIER);
        }else{
                retval.angularDecel.setCMode(C_CONSUMER);
        }
        if(m_supplier){
                retval.trans.setCMode(C_SUPPLIER);
        }else{
                retval.trans.setCMode(C_CONSUMER);
        }
        if(m_supplier){
                retval.transDecel.setCMode(C_SUPPLIER);
        }else{
                retval.transDecel.setCMode(C_CONSUMER);
        }
        if(m_supplier){
                retval.brake.setCMode(C_SUPPLIER);
        }else{
                retval.brake.setCMode(C_CONSUMER);
        }
        if(m_supplier){
                retval.transLimit.setCMode(C_SUPPLIER);
        }else{
                retval.transLimit.setCMode(C_CONSUMER);
        }
              retval.source="NULL";
              if(m_supplier){
                     if(m_requestVector.empty()){
                            return requests;
                     }
                     retval.source=m_requestVector.front().source;
retval.angularAccel=m_requestVector.front().angularAccel;
retval.transAccel=m_requestVector.front().transAccel;
retval.angularLimit=m_requestVector.front().angularLimit;
retval.angular=m_requestVector.front().angular;
retval.mType=m_requestVector.front().mType;
retval.trans=m_requestVector.front().trans;
retval.angularDecel=m_requestVector.front().angularDecel;
retval.target=m_requestVector.front().target;
retval.transDecel=m_requestVector.front().transDecel;
retval.timestamp=m_requestVector.front().timestamp;
retval.brake=m_requestVector.front().brake;
retval.transLimit=m_requestVector.front().transLimit;

                              //                     retval=m_requestVector.front();
        if(m_supplier){
                retval.angularAccel.setCMode(C_SUPPLIER);
        }else{
                retval.angularAccel.setCMode(C_CONSUMER);
        }
        if(m_supplier){
                retval.transAccel.setCMode(C_SUPPLIER);
        }else{
                retval.transAccel.setCMode(C_CONSUMER);
        }
        if(m_supplier){
                retval.angularLimit.setCMode(C_SUPPLIER);
        }else{
                retval.angularLimit.setCMode(C_CONSUMER);
        }
        if(m_supplier){
                retval.angular.setCMode(C_SUPPLIER);
        }else{
                retval.angular.setCMode(C_CONSUMER);
        }
        if(m_supplier){
                retval.angularDecel.setCMode(C_SUPPLIER);
        }else{
                retval.angularDecel.setCMode(C_CONSUMER);
        }
        if(m_supplier){
                retval.trans.setCMode(C_SUPPLIER);
        }else{
                retval.trans.setCMode(C_CONSUMER);
        }
        if(m_supplier){
                retval.transDecel.setCMode(C_SUPPLIER);
        }else{
                retval.transDecel.setCMode(C_CONSUMER);
        }
        if(m_supplier){
                retval.brake.setCMode(C_SUPPLIER);
        }else{
                retval.brake.setCMode(C_CONSUMER);
        }
        if(m_supplier){
                retval.transLimit.setCMode(C_SUPPLIER);
        }else{
                retval.transLimit.setCMode(C_CONSUMER);
        }
                     m_requestVector.pop();
                     requests=retval;
                     return retval;
              }else{
                     if(m_infoVector.empty()){
                            return info;
                     }
                     retval.source=m_infoVector.front().source;
retval.angularAccel=m_infoVector.front().angularAccel;
retval.transAccel=m_infoVector.front().transAccel;
retval.angularLimit=m_infoVector.front().angularLimit;
retval.angular=m_infoVector.front().angular;
retval.mType=m_infoVector.front().mType;
retval.trans=m_infoVector.front().trans;
retval.angularDecel=m_infoVector.front().angularDecel;
retval.target=m_infoVector.front().target;
retval.transDecel=m_infoVector.front().transDecel;
retval.timestamp=m_infoVector.front().timestamp;
retval.brake=m_infoVector.front().brake;
retval.transLimit=m_infoVector.front().transLimit;

        if(m_supplier){
                retval.angularAccel.setCMode(C_SUPPLIER);
        }else{
                retval.angularAccel.setCMode(C_CONSUMER);
        }
        if(m_supplier){
                retval.transAccel.setCMode(C_SUPPLIER);
        }else{
                retval.transAccel.setCMode(C_CONSUMER);
        }
        if(m_supplier){
                retval.angularLimit.setCMode(C_SUPPLIER);
        }else{
                retval.angularLimit.setCMode(C_CONSUMER);
        }
        if(m_supplier){
                retval.angular.setCMode(C_SUPPLIER);
        }else{
                retval.angular.setCMode(C_CONSUMER);
        }
        if(m_supplier){
                retval.angularDecel.setCMode(C_SUPPLIER);
        }else{
                retval.angularDecel.setCMode(C_CONSUMER);
        }
        if(m_supplier){
                retval.trans.setCMode(C_SUPPLIER);
        }else{
                retval.trans.setCMode(C_CONSUMER);
        }
        if(m_supplier){
                retval.transDecel.setCMode(C_SUPPLIER);
        }else{
                retval.transDecel.setCMode(C_CONSUMER);
        }
        if(m_supplier){
                retval.brake.setCMode(C_SUPPLIER);
        }else{
                retval.brake.setCMode(C_CONSUMER);
        }
        if(m_supplier){
                retval.transLimit.setCMode(C_SUPPLIER);
        }else{
                retval.transLimit.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.angularAccel.setCMode(C_SUPPLIER);
        }else{
                requests.angularAccel.setCMode(C_CONSUMER);
        }
        if(m_supplier){
                requests.transAccel.setCMode(C_SUPPLIER);
        }else{
                requests.transAccel.setCMode(C_CONSUMER);
        }
        if(m_supplier){
                requests.angularLimit.setCMode(C_SUPPLIER);
        }else{
                requests.angularLimit.setCMode(C_CONSUMER);
        }
        if(m_supplier){
                requests.angular.setCMode(C_SUPPLIER);
        }else{
                requests.angular.setCMode(C_CONSUMER);
        }
        if(m_supplier){
                requests.angularDecel.setCMode(C_SUPPLIER);
        }else{
                requests.angularDecel.setCMode(C_CONSUMER);
        }
        if(m_supplier){
                requests.trans.setCMode(C_SUPPLIER);
        }else{
                requests.trans.setCMode(C_CONSUMER);
        }
        if(m_supplier){
                requests.transDecel.setCMode(C_SUPPLIER);
        }else{
                requests.transDecel.setCMode(C_CONSUMER);
        }
        if(m_supplier){
                requests.brake.setCMode(C_SUPPLIER);
        }else{
                requests.brake.setCMode(C_CONSUMER);
        }
        if(m_supplier){
                requests.transLimit.setCMode(C_SUPPLIER);
        }else{
                requests.transLimit.setCMode(C_CONSUMER);
        }
                     return requests;
              }else{
        if(m_supplier){
                info.angularAccel.setCMode(C_SUPPLIER);
        }else{
                info.angularAccel.setCMode(C_CONSUMER);
        }
        if(m_supplier){
                info.transAccel.setCMode(C_SUPPLIER);
        }else{
                info.transAccel.setCMode(C_CONSUMER);
        }
        if(m_supplier){
                info.angularLimit.setCMode(C_SUPPLIER);
        }else{
                info.angularLimit.setCMode(C_CONSUMER);
        }
        if(m_supplier){
                info.angular.setCMode(C_SUPPLIER);
        }else{
                info.angular.setCMode(C_CONSUMER);
        }
        if(m_supplier){
                info.angularDecel.setCMode(C_SUPPLIER);
        }else{
                info.angularDecel.setCMode(C_CONSUMER);
        }
        if(m_supplier){
                info.trans.setCMode(C_SUPPLIER);
        }else{
                info.trans.setCMode(C_CONSUMER);
        }
        if(m_supplier){
                info.transDecel.setCMode(C_SUPPLIER);
        }else{
                info.transDecel.setCMode(C_CONSUMER);
        }
        if(m_supplier){
                info.brake.setCMode(C_SUPPLIER);
        }else{
                info.brake.setCMode(C_CONSUMER);
        }
        if(m_supplier){
                info.transLimit.setCMode(C_SUPPLIER);
        }else{
                info.transLimit.setCMode(C_CONSUMER);
        }
                     // the requests struct really should get the values in 
                     // info
                     requests=info;
                     return info;
              }
       }
}

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

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

---