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 }