denso_controller.cpp
Go to the documentation of this file.
00001 
00025 #include "denso_robot_core/denso_controller.h"
00026 
00027 namespace denso_robot_core {
00028 
00029 DensoController::DensoController(const std::string& name, const int* mode)
00030   : DensoBase(name, mode)
00031 {
00032   for(int srvs = DensoBase::SRV_MIN; srvs <= DensoBase::SRV_MAX; srvs++) {
00033     BCAPService_Ptr service = boost::make_shared<bcap_service::BCAPService>();
00034     service->parseParams();
00035     switch(srvs){
00036       case DensoBase::SRV_ACT:
00037         service->put_Type("udp");
00038         break;
00039       default:
00040         service->put_Type("tcp");
00041         break;
00042     }
00043     m_vecService.push_back(service);
00044   }
00045 }
00046 
00047 DensoController::~DensoController()
00048 {
00049 
00050 }
00051 
00052 HRESULT DensoController::InitializeBCAP(const std::string& filename)
00053 {
00054   HRESULT      hr;
00055   XMLError     ret;
00056   XMLDocument  xmlDoc;
00057   XMLElement  *xmlCtrl, *xmlRob, *xmlTsk;
00058 
00059   for(int srvs = DensoBase::SRV_MIN; srvs <= DensoBase::SRV_MAX; srvs++) {
00060     hr = m_vecService[srvs]->Connect();
00061     if(FAILED(hr)) return hr;
00062   }
00063 
00064   ret = xmlDoc.LoadFile(filename.c_str());
00065   if(ret != XML_SUCCESS) return E_FAIL;
00066 
00067   hr = AddController();
00068   if(FAILED(hr)) return hr;
00069 
00070   xmlCtrl = xmlDoc.FirstChildElement(XML_CTRL_NAME);
00071   if(xmlCtrl == NULL) return E_FAIL;
00072 
00073   hr = AddVariable(xmlCtrl);
00074   if(FAILED(hr)) return hr;
00075 
00076   xmlRob = xmlCtrl->FirstChildElement(XML_ROBOT_NAME);
00077   if(xmlRob == NULL) return E_FAIL;
00078 
00079   hr = AddRobot(xmlRob);
00080   if(FAILED(hr)) return hr;
00081 
00082   xmlTsk = xmlCtrl->FirstChildElement(XML_TASK_NAME);
00083   if(xmlTsk == NULL) return E_FAIL;
00084 
00085   hr = AddTask(xmlTsk);
00086 
00087   return hr;
00088 }
00089 
00090 HRESULT DensoController::StartService(ros::NodeHandle& node)
00091 {
00092   DensoRobot_Vec::iterator itRob;
00093   for(itRob  = m_vecRobot.begin();
00094       itRob != m_vecRobot.end();
00095       itRob++)
00096   {
00097     (*itRob)->StartService(node);
00098   }
00099 
00100   DensoTask_Vec::iterator itTsk;
00101   for(itTsk  = m_vecTask.begin();
00102       itTsk != m_vecTask.end();
00103       itTsk++)
00104   {
00105     (*itTsk)->StartService(node);
00106   }
00107 
00108   DensoVariable_Vec::iterator itVar;
00109   for(itVar  = m_vecVar.begin();
00110       itVar != m_vecVar.end();
00111       itVar++)
00112   {
00113     (*itVar)->StartService(node);
00114   }
00115 
00116   m_serving = true;
00117 
00118   return S_OK;
00119 }
00120 
00121 HRESULT DensoController::StopService()
00122 {
00123   m_mtxSrv.lock();
00124   m_serving = false;
00125   m_mtxSrv.unlock();
00126 
00127   DensoRobot_Vec::iterator itRob;
00128   for(itRob  = m_vecRobot.begin();
00129       itRob != m_vecRobot.end();
00130       itRob++)
00131   {
00132     (*itRob)->StopService();
00133   }
00134 
00135   DensoTask_Vec::iterator itTsk;
00136   for(itTsk  = m_vecTask.begin();
00137       itTsk != m_vecTask.end();
00138       itTsk++)
00139   {
00140     (*itTsk)->StopService();
00141   }
00142 
00143   DensoVariable_Vec::iterator itVar;
00144   for(itVar  = m_vecVar.begin();
00145       itVar != m_vecVar.end();
00146       itVar++)
00147   {
00148     (*itVar)->StopService();
00149   }
00150 
00151   return S_OK;
00152 }
00153 
00154 bool DensoController::Update()
00155 {
00156   boost::mutex::scoped_lock lockSrv(m_mtxSrv);
00157   if(!m_serving) return false;
00158 
00159   DensoRobot_Vec::iterator itRob;
00160   for(itRob  = m_vecRobot.begin();
00161       itRob != m_vecRobot.end();
00162       itRob++)
00163   {
00164     (*itRob)->Update();
00165   }
00166 
00167   DensoTask_Vec::iterator itTsk;
00168   for(itTsk  = m_vecTask.begin();
00169       itTsk != m_vecTask.end();
00170       itTsk++)
00171   {
00172     (*itTsk)->Update();
00173   }
00174 
00175   DensoVariable_Vec::iterator itVar;
00176   for(itVar  = m_vecVar.begin();
00177       itVar != m_vecVar.end();
00178       itVar++)
00179   {
00180     (*itVar)->Update();
00181   }
00182 
00183   return true;
00184 }
00185 
00186 HRESULT DensoController::get_Robot(int index, DensoRobot_Ptr* robot)
00187 {
00188   if(robot == NULL) {
00189     return E_INVALIDARG;
00190   }
00191 
00192   DensoBase_Vec vecBase;
00193   vecBase.insert(vecBase.end(), m_vecRobot.begin(), m_vecRobot.end());
00194 
00195   DensoBase_Ptr pBase;
00196   HRESULT hr = DensoBase::get_Object(vecBase, index, &pBase);
00197   if(SUCCEEDED(hr)) {
00198     *robot = boost::dynamic_pointer_cast<DensoRobot>(pBase);
00199   }
00200 
00201   return hr;
00202 }
00203 
00204 HRESULT DensoController::get_Task(const std::string& name, DensoTask_Ptr* task)
00205 {
00206   if(task == NULL) {
00207     return E_INVALIDARG;
00208   }
00209 
00210   DensoBase_Vec vecBase;
00211   vecBase.insert(vecBase.end(), m_vecTask.begin(), m_vecTask.end());
00212 
00213   DensoBase_Ptr pBase;
00214   HRESULT hr = DensoBase::get_Object(vecBase, name, &pBase);
00215   if(SUCCEEDED(hr)) {
00216     *task = boost::dynamic_pointer_cast<DensoTask>(pBase);
00217   }
00218 
00219   return hr;
00220 }
00221 
00222 HRESULT DensoController::get_Variable(const std::string& name, DensoVariable_Ptr* var)
00223 {
00224   if(var == NULL) {
00225     return E_INVALIDARG;
00226   }
00227 
00228   DensoBase_Vec vecBase;
00229   vecBase.insert(vecBase.end(), m_vecVar.begin(), m_vecVar.end());
00230 
00231   DensoBase_Ptr pBase;
00232   HRESULT hr = DensoBase::get_Object(vecBase, name, &pBase);
00233   if(SUCCEEDED(hr)) {
00234     *var = boost::dynamic_pointer_cast<DensoVariable>(pBase);
00235   }
00236 
00237   return hr;
00238 }
00239 
00240 HRESULT DensoController::AddTask(XMLElement *xmlElem)
00241 {
00242   int objs;
00243   HRESULT hr;
00244 
00245   Name_Vec   vecName;
00246   hr = DensoBase::GetObjectNames(ID_CONTROLLER_GETTASKNAMES, vecName);
00247   if(SUCCEEDED(hr)) {
00248     for(objs = 0; objs < vecName.size(); objs++) {
00249       Handle_Vec vecHandle;
00250       hr = DensoBase::AddObject(
00251           ID_CONTROLLER_GETTASK, vecName[objs], vecHandle);
00252       if(FAILED(hr)) break;
00253 
00254       DensoTask_Ptr tsk(new DensoTask(this,
00255           m_vecService, vecHandle, vecName[objs], m_mode));
00256 
00257       hr = tsk->InitializeBCAP(xmlElem);
00258       if(FAILED(hr)) break;
00259 
00260       m_vecTask.push_back(tsk);
00261     }
00262   }
00263 
00264   return hr;
00265 }
00266 
00267 HRESULT DensoController::AddVariable(const std::string& name)
00268 {
00269   return DensoBase::AddVariable(ID_CONTROLLER_GETVARIABLE,
00270       name, m_vecVar);
00271 }
00272 
00273 HRESULT DensoController::AddVariable(XMLElement *xmlElem)
00274 {
00275   HRESULT hr = S_OK;
00276   XMLElement *xmlVar;
00277 
00278   for(xmlVar = xmlElem->FirstChildElement(XML_VARIABLE_NAME);
00279       xmlVar!= NULL;
00280       xmlVar = xmlVar->NextSiblingElement(XML_VARIABLE_NAME)) {
00281 
00282       hr = DensoBase::AddVariable(ID_CONTROLLER_GETVARIABLE,
00283           xmlVar, m_vecVar);
00284       if(FAILED(hr)) break;
00285   }
00286 
00287   return hr;
00288 }
00289 
00290 }


denso_robot_core
Author(s): DENSO WAVE INCORPORATED
autogenerated on Thu Jun 6 2019 21:00:11