Controller_impl.cpp
Go to the documentation of this file.
00001 /*
00002  * Copyright (c) 2008, AIST, the University of Tokyo and General Robotix Inc.
00003  * All rights reserved. This program is made available under the terms of the
00004  * Eclipse Public License v1.0 which accompanies this distribution, and is
00005  * available at http://www.eclipse.org/legal/epl-v10.html
00006  * Contributors:
00007  * National Institute of Advanced Industrial Science and Technology (AIST)
00008  * General Robotix Inc. 
00009  */
00015 #include "Controller_impl.h"
00016 
00017 #include <string>
00018 #include <iostream>
00019 #include <rtm/Manager.h>
00020 #include <rtm/RTObject.h>
00021 #include <rtm/NVUtil.h>
00022 
00023 #include <hrpCorba/ORBwrap.h>
00024 
00025 #include "VirtualRobotRTC.h"
00026 
00027 using namespace std;
00028 using namespace boost;
00029 
00030 namespace {
00031     const bool CONTROLLER_BRIDGE_DEBUG = false;
00032 }
00033 
00034 
00035 Controller_impl::Controller_impl(RTC::Manager* rtcManager, BridgeConf* bridgeConf)
00036     :   bridgeConf(bridgeConf),
00037         rtcManager(rtcManager),
00038         modelName(""),
00039         bRestart(false)
00040 {
00041     if(CONTROLLER_BRIDGE_DEBUG){
00042         cout << "Controller_impl::Controller_impl" << endl;
00043     }
00044     VirtualRobotRTC::registerFactory(rtcManager, bridgeConf->getVirtualRobotRtcTypeName());
00045 
00046     RTC::RtcBase* rtc = rtcManager->createComponent("VirtualRobot");
00047     virtualRobotRTC = dynamic_cast<VirtualRobotRTC*>(rtc);
00048 }
00049 
00050 
00051 Controller_impl::~Controller_impl()
00052 {
00053     if(CONTROLLER_BRIDGE_DEBUG){
00054         cout << "Controller_impl::~Controller_impl" << endl;
00055     }
00056     delete naming;
00057 
00058     virtualRobotRTC->isOwnedByController = false;
00059 }
00060 
00061 
00062 void Controller_impl::detectRtcs()
00063 {
00064     RTC::Manager& rtcManager = RTC::Manager::instance();
00065 
00066     string nameServer = rtcManager.getConfig()["corba.nameservers"];
00067     cout << "setting naming" << endl;
00068     int comPos = nameServer.find(",");
00069     if (comPos < 0){
00070         comPos = nameServer.length();
00071     }
00072     nameServer = nameServer.substr(0, comPos);
00073     naming = new RTC::CorbaNaming(rtcManager.getORB(), nameServer.c_str());
00074 
00075     for (TimeRateMap::iterator it = bridgeConf->timeRateMap.begin(); it != bridgeConf->timeRateMap.end(); ++it){
00076         RTC::RTObject_var rtcRef;
00077         string rtcName = it->first;
00078         if ( rtcName != "" ) {
00079             string rtcNamingName = rtcName + ".rtc";
00080             try {
00081                 CORBA::Object_var objRef = naming->resolve(rtcNamingName.c_str());
00082                 if ( CORBA::is_nil(objRef) ) {
00083                     cerr << rtcName << " is not found." << endl;
00084                     throw OpenHRP::Controller::ControllerException(string(rtcName+" is not found.").c_str());  
00085                 } else {
00086                     rtcRef = RTC::RTObject::_narrow(objRef);
00087                     if(CORBA::is_nil(rtcRef)){
00088                         cerr << rtcName << " is not an RTC object." << endl;
00089                         throw OpenHRP::Controller::ControllerException(string(rtcName+" is not an RTC object.").c_str());  
00090                     }
00091                 }
00092             } catch(CORBA_SystemException& ex) {
00093                 cerr << ex._rep_id() << endl;
00094                 cerr << "exception in Controller_impl::detectRtcs" << endl;
00095             } catch(...){
00096                 cerr << "unknown exception in Controller_impl::detectRtcs()" <<  endl;
00097             }
00098         }
00099         if (!CORBA::is_nil(rtcRef)) {
00100             addRtcVectorWithConnection(rtcRef);
00101         }
00102     }
00103 
00104     cout << "setup RT components" << endl;
00105 
00106     for(size_t i=0; i < bridgeConf->portConnections.size(); ++i){
00107 
00108         PortConnection& connection = bridgeConf->portConnections[i];
00109 
00110         RTC::RTObject_var rtcRef;
00111         string rtcName;
00112 
00113         if(connection.controllerInstanceName.empty()){
00114             if(!bridgeConf->moduleInfoList.empty()){
00115                 RTC::RtcBase* rtcServant = bridgeConf->moduleInfoList.front().rtcServant;
00116                 rtcName = "";                                                 
00117                 rtcRef = rtcServant->getObjRef();
00118             }
00119         } else {
00120             rtcName = connection.controllerInstanceName;
00121         }
00122          
00123         RtcInfoMap::iterator it = rtcInfoMap.find(rtcName);
00124         if(it==rtcInfoMap.end()){
00125             if(rtcName!=""){
00126                 string rtcNamingName = rtcName + ".rtc";
00127                 CORBA::Object_var objRef = naming->resolve(rtcNamingName.c_str());
00128                 if(CORBA::is_nil(objRef)){
00129                     cerr << rtcName << " is not found." << endl;
00130                     throw OpenHRP::Controller::ControllerException(string(rtcName+" is not found.").c_str());
00131                 } else {
00132                     rtcRef = RTC::RTObject::_narrow(objRef);
00133                     if(CORBA::is_nil(rtcRef)){
00134                         cerr << rtcName << " is not an RTC object." << endl;
00135                         throw OpenHRP::Controller::ControllerException(string(rtcName+" is not an RTC object.").c_str());
00136                     }
00137                 }
00138             }
00139             if(!CORBA::is_nil(rtcRef)){
00140                 RtcInfoPtr rtcInfo = addRtcVectorWithConnection(rtcRef);
00141                 rtcInfoMap.insert(make_pair(rtcName,rtcInfo));
00142             }
00143         }
00144     }
00145 
00146     RTC::RTCList_var rtcList = virtualRobotRTC->getConnectedRtcs();
00147     for(CORBA::ULong i=0; i < rtcList->length(); ++i){
00148         addRtcVectorWithConnection(rtcList[i]);
00149     }
00150 }
00151 
00152 
00153 void Controller_impl::makePortMap(RtcInfoPtr& rtcInfo)
00154 {
00155         Port_Service_List_Var_Type ports = rtcInfo->rtcRef->get_ports();
00156     for(CORBA::ULong i=0; i < ports->length(); ++i){
00157         RTC::PortProfile_var profile = ports[i]->get_port_profile();
00158         std::string portName(profile->name);
00159         string::size_type index = portName.rfind(".");
00160         if (index != string::npos) portName = portName.substr(index+1);
00161         rtcInfo->portMap[portName] = ports[i];
00162     }
00163 }
00164 
00165 Controller_impl::RtcInfoPtr Controller_impl::addRtcVectorWithConnection(RTC::RTObject_var new_rtcRef)
00166 {
00167     RtcInfoVector::iterator it = rtcInfoVector.begin();
00168     for( ; it != rtcInfoVector.end(); ++it){
00169         if((*it)->rtcRef->_is_equivalent(new_rtcRef))
00170             return *it;
00171     }
00172 
00173     RtcInfoPtr rtcInfo(new RtcInfo());
00174     rtcInfo->rtcRef = new_rtcRef;
00175     makePortMap(rtcInfo);
00176     string rtcName = (string)rtcInfo->rtcRef->get_component_profile()->instance_name;
00177 
00178     if ( bridgeConf->timeRateMap.size() == 0 ) {
00179         rtcInfo->timeRate = 1.0;
00180         rtcInfo->timeRateCounter = 0.0;
00181     } else {
00182         TimeRateMap::iterator p = bridgeConf->timeRateMap.find(rtcName);
00183         if ( p != bridgeConf->timeRateMap.end() ) {
00184             rtcInfo->timeRate = (double)p->second;
00185             rtcInfo->timeRateCounter = 1.0 - rtcInfo->timeRate;
00186         } else {
00187             rtcInfo->timeRate = 0.0;
00188             rtcInfo->timeRateCounter = 0.0;
00189         }
00190         cout << "periodic-rate (" << rtcName << ") = " << rtcInfo->timeRate << endl;
00191     }
00192     rtcInfoVector.push_back(rtcInfo);
00193 
00194 
00195 #ifdef OPENRTM_VERSION_042
00196     RTC::ExecutionContextServiceList_var eclist = rtcInfo->rtcRef->get_execution_context_services();
00197     for(CORBA::ULong i=0; i < eclist->length(); ++i){
00198         if(!CORBA::is_nil(eclist[i])){
00199             rtcInfo->execContext = ExtTrigExecutionContextService_Type::_narrow(eclist[i]);
00200             if(!CORBA::is_nil(rtcInfo->execContext)){
00201                 cout << "detected the ExtTrigExecutionContext" << endl;
00202             }
00203             break;
00204         }
00205     }
00206 #else
00207     RTC::ExecutionContextList_var eclist = rtcInfo->rtcRef->get_owned_contexts();
00208     for(CORBA::ULong i=0; i < eclist->length(); ++i){
00209         if(!CORBA::is_nil(eclist[i])){
00210             rtcInfo->execContext = ExtTrigExecutionContextService_Type::_narrow(eclist[i]);
00211             if(!CORBA::is_nil(rtcInfo->execContext)){
00212                 cout << "detected the ExtTrigExecutionContext" << endl;
00213             }
00214             break;
00215         }
00216     }
00217 #endif
00218     return rtcInfo;
00219 }
00220 
00221 void Controller_impl::setupRtcConnections()
00222 {
00223     for(size_t i=0; i < bridgeConf->portConnections.size(); ++i){
00224 
00225         const PortConnection& connection = bridgeConf->portConnections[i];
00226 
00227         string controllerInstanceName = connection.controllerInstanceName;
00228         if(controllerInstanceName.empty()){
00229             if(!bridgeConf->moduleInfoList.empty()){
00230                 controllerInstanceName = "";
00231             }
00232         }
00233 
00234         cout << "connect " << virtualRobotRTC->getInstanceName() << ":" << connection.robotPortName;
00235         cout << " <--> " << controllerInstanceName << ":" << connection.controllerPortName;
00236 
00237         int connected = false;
00238 
00239         RtcInfoMap::iterator p = rtcInfoMap.find(controllerInstanceName);
00240         if(p != rtcInfoMap.end()){
00241             RtcInfoPtr rtcInfo = p->second;
00242 
00243             PortMap::iterator q = rtcInfo->portMap.find(connection.controllerPortName);
00244             if(q == rtcInfo->portMap.end()){
00245                 cerr << "\n";
00246                 cerr << controllerInstanceName << " does not have a port ";
00247                 cerr << connection.controllerPortName << "\n";
00248                 throw OpenHRP::Controller::ControllerException("not found a port");
00249             } else {
00250                 Port_Service_Ptr_Type controllerPortRef = q->second;
00251 
00252                 PortHandlerPtr robotPortHandler = virtualRobotRTC->getPortHandler(connection.robotPortName);
00253                 if(!robotPortHandler){
00254                     cerr << "\n";
00255                     cerr << "The robot does not have a port named " << connection.robotPortName << "\n";
00256                     throw OpenHRP::Controller::ControllerException("not found a port");
00257                 } else {
00258                         Port_Service_Ptr_Type robotPortRef = robotPortHandler->portRef;
00259 
00260                     if(!CORBA::is_nil(robotPortRef)){
00261                         if(dynamic_pointer_cast<OutPortHandler>(robotPortHandler)){
00262                             connected = connectPorts(robotPortRef, controllerPortRef);
00263                         } else {
00264                             connected = connectPorts(controllerPortRef, robotPortRef);
00265                         }
00266                     }
00267                 }
00268             }
00269         }
00270 
00271         if(!connected){
00272             cout << " ...ok" << endl;
00273         } else if(connected == -1){
00274             cerr << "Connection failed." << endl;
00275         } else if(connected == 1){
00276             cerr << " It has already been connected." << endl;
00277         }
00278     }
00279 }
00280 
00281 
00282 int Controller_impl::connectPorts(Port_Service_Ptr_Type outPort, Port_Service_Ptr_Type inPort)
00283 {
00284     RTC::ConnectorProfileList_var connectorProfiles = inPort->get_connector_profiles();
00285     for(CORBA::ULong i=0; i < connectorProfiles->length(); ++i){
00286         RTC::ConnectorProfile& connectorProfile = connectorProfiles[i];
00287         Port_Service_List_Type& connectedPorts = connectorProfile.ports;
00288 
00289         for(CORBA::ULong j=0; j < connectedPorts.length(); ++j){
00290                 Port_Service_Ptr_Type connectedPortRef = connectedPorts[j];
00291             if(connectedPortRef->_is_equivalent(outPort)){
00292                 return 1;
00293             }
00294         }
00295     }
00296     // connect ports
00297     RTC::ConnectorProfile cprof;
00298     cprof.connector_id = "";
00299     cprof.name = CORBA::string_dup("connector0");
00300     cprof.ports.length(2);
00301     cprof.ports[0] = Port_Service_Type::_duplicate(inPort);
00302     cprof.ports[1] = Port_Service_Type::_duplicate(outPort);
00303 
00304     CORBA_SeqUtil::push_back(cprof.properties,
00305                        NVUtil::newNV("dataport.dataflow_type",
00306                                      "Push"));
00307 #ifdef OPENRTM_VERSION_042
00308     CORBA_SeqUtil::push_back(cprof.properties,
00309                        NVUtil::newNV("dataport.interface_type",
00310                                      "CORBA_Any"));
00311     CORBA_SeqUtil::push_back(cprof.properties,
00312                        NVUtil::newNV("dataport.subscription_type",
00313                                      "Flush"));
00314 #else
00315     CORBA_SeqUtil::push_back(cprof.properties,
00316                        NVUtil::newNV("dataport.interface_type",
00317                                      "corba_cdr"));
00318     CORBA_SeqUtil::push_back(cprof.properties,
00319                        NVUtil::newNV("dataport.subscription_type",
00320                                      "flush"));
00321 #endif
00322     RTC::ReturnCode_t result = inPort->connect(cprof);
00323 
00324     if(result == RTC::RTC_OK)
00325         return 0;
00326     else
00327         return -1;
00328 }
00329 
00330 
00331 void Controller_impl::setDynamicsSimulator(DynamicsSimulator_ptr dynamicsSimulator)
00332 {
00333     this->dynamicsSimulator = DynamicsSimulator::_duplicate(dynamicsSimulator);
00334 }
00335 
00336 
00337 void Controller_impl::setViewSimulator(ViewSimulator_ptr viewSimulator)
00338 {
00339     this->viewSimulator = ViewSimulator::_duplicate(viewSimulator);
00340 }
00341 
00342 
00343 void Controller_impl::start()
00344 {
00345     if(CONTROLLER_BRIDGE_DEBUG){
00346         cout << "Controller_impl::start" << endl;
00347     }
00348 
00349     controlTime = 0.0;
00350     try{
00351         if( bRestart ){
00352             restart();
00353         } else {
00354             if(!CORBA::is_nil(viewSimulator)) {
00355                 viewSimulator->getCameraSequenceOf(modelName.c_str(), cameras);
00356             }else{
00357                 cameras = new CameraSequence(0);
00358             }
00359             activeComponents();
00360         }
00361     } catch(CORBA_SystemException& ex){
00362         cerr << ex._rep_id() << endl;
00363         cerr << "exception in Controller_impl::start" << endl;
00364     } catch(...){
00365         cerr << "unknown exception in Controller_impl::start()" <<  endl;
00366     }
00367 }
00368 
00369 
00370 SensorState& Controller_impl::getCurrentSensorState()
00371 {
00372     if(!sensorStateUpdated){
00373         dynamicsSimulator->getCharacterSensorState(modelName.c_str(), sensorState);
00374         sensorStateUpdated = true;
00375     }
00376 
00377     return sensorState;
00378 }
00379 
00380 
00381 DblSequence* Controller_impl::getLinkDataFromSimulator
00382 (const std::string& linkName, DynamicsSimulator::LinkDataType linkDataType)
00383 {
00384     DblSequence_var data;
00385     dynamicsSimulator->getCharacterLinkData(modelName.c_str(), linkName.c_str(), linkDataType, data.out());
00386     return data._retn();
00387 }
00388 
00389 
00390 DblSequence* Controller_impl::getSensorDataFromSimulator(const std::string& sensorName)
00391 {
00392     DblSequence_var data;
00393     dynamicsSimulator->getCharacterSensorValues(modelName.c_str(), sensorName.c_str(), data.out());
00394     return data._retn();
00395 }
00396 
00397 
00398 ImageData* Controller_impl::getCameraImageFromSimulator(int cameraId)
00399 {
00400     if(cameras->length()!=0){
00401         ImageData_var imageData = cameras[cameraId]->getImageData();
00402         return imageData._retn();
00403     }else{
00404         ImageData* imageData = new ImageData;
00405         imageData->floatData.length(0);
00406         imageData->longData.length(0);
00407         imageData->octetData.length(0);
00408         return imageData;
00409     }
00410 }
00411 
00412 
00413 void Controller_impl::input()
00414 {
00415     if(CONTROLLER_BRIDGE_DEBUG){
00416         cout << "Controller_impl::input" << endl;
00417     }
00418 
00419     sensorStateUpdated = false;
00420 
00421     virtualRobotRTC->inputDataFromSimulator(this);
00422 }
00423 
00424 
00425 DblSequence& Controller_impl::getJointDataSeqRef(DynamicsSimulator::LinkDataType linkDataType)
00426 {
00427     return outputJointValueSeqInfos[linkDataType].values;
00428 }
00429 
00430 
00431 void Controller_impl::flushJointDataSeqToSimulator(DynamicsSimulator::LinkDataType linkDataType)
00432 {
00433     JointValueSeqInfoMap::iterator p = outputJointValueSeqInfos.find(linkDataType);
00434     if(p != outputJointValueSeqInfos.end()){
00435         JointValueSeqInfo& info = p->second;
00436         if(!info.flushed){
00437             dynamicsSimulator->setCharacterAllLinkData(modelName.c_str(), linkDataType, info.values);
00438             info.flushed = true;
00439         }
00440     }
00441 }
00442 
00443 void Controller_impl::flushLinkDataToSimulator(const std::string& linkName,
00444                                                DynamicsSimulator::LinkDataType linkDataType,
00445                                                const DblSequence& linkData)
00446 {
00447     dynamicsSimulator->setCharacterLinkData(modelName.c_str(), linkName.c_str(),
00448                                         linkDataType, linkData);
00449 }
00450 
00451 void Controller_impl::output()
00452 {
00453     if(CONTROLLER_BRIDGE_DEBUG){
00454         cout << "Controller_impl::output" << endl;
00455     }
00456 
00457     for(JointValueSeqInfoMap::iterator p = outputJointValueSeqInfos.begin();
00458       p != outputJointValueSeqInfos.end(); ++p){
00459         JointValueSeqInfo& info = p->second;
00460         info.flushed = false;
00461     }
00462 
00463     virtualRobotRTC->outputDataToSimulator(this);
00464 }
00465 
00466 
00467 void Controller_impl::control()
00468 {
00469     if(CONTROLLER_BRIDGE_DEBUG){
00470         cout << "Controller_impl::control" << endl;
00471     }
00472 
00473   virtualRobotRTC->writeDataToOutPorts(this);
00474 
00475     for(RtcInfoVector::iterator p = rtcInfoVector.begin(); p != rtcInfoVector.end(); ++p){
00476         RtcInfoPtr& rtcInfo = *p;
00477         if(!CORBA::is_nil(rtcInfo->execContext)){
00478                 rtcInfo->timeRateCounter += rtcInfo->timeRate;
00479             if(rtcInfo->timeRateCounter + rtcInfo->timeRate/2.0 > 1.0){
00480                 rtcInfo->execContext->tick();
00481                 rtcInfo->timeRateCounter -= 1.0;
00482             }
00483         }
00484     }
00485 
00486     virtualRobotRTC->readDataFromInPorts(this);
00487 
00488   controlTime += timeStep;
00489 }
00490 
00491 
00492 void Controller_impl::stop()
00493 {
00494     deactiveComponents();
00495 }
00496 
00497 
00498 void Controller_impl::destroy()
00499 {
00500     if(CONTROLLER_BRIDGE_DEBUG){
00501         cout << "Controller_impl::destroy()" << endl;
00502     }
00503     PortableServer::POA_var poa = _default_POA();
00504     PortableServer::ObjectId_var id = poa->servant_to_id(this);
00505     poa->deactivate_object(id);
00506 }
00507 
00508 // TAWARA INSERT CODE 2009/01/14 START
00509 
00510 void Controller_impl::initialize()
00511 {
00512     if(CONTROLLER_BRIDGE_DEBUG){
00513         cout << "Controller_impl::initialize()" << endl;
00514     }
00515 
00516     if( virtualRobotRTC)
00517     {
00518         if( virtualRobotRTC->isOwnedByController ){
00519             bRestart = false;
00520         } else {
00521             virtualRobotRTC->isOwnedByController = true;
00522             try{
00523                 if(!virtualRobotRTC->checkOutPortStepTime(timeStep))
00524                     throw OpenHRP::Controller::ControllerException("Error OutPort StepTime"); 
00525                 detectRtcs();
00526                 setupRtcConnections();
00527             } catch(CORBA_SystemException& ex){
00528                 cerr << ex._rep_id() << endl;
00529                 cerr << "exception in initializeController" << endl;
00530                 throw OpenHRP::Controller::ControllerException(""); 
00531             } catch(std::invalid_argument& ex){
00532                 cerr << "invalid argument : " << ex.what() << endl;
00533                 throw OpenHRP::Controller::ControllerException(""); 
00534             } 
00535         }
00536     }
00537 }
00538 void Controller_impl::shutdown()
00539 {
00540     if(CONTROLLER_BRIDGE_DEBUG){
00541         cout << "Controller_impl::shutdown()" << endl;
00542     }
00543     destroy();
00544     rtcManager->terminate();
00545 }
00546 
00547 void Controller_impl::restart()
00548 {
00549     activeComponents();
00550 }
00551 void Controller_impl::activeComponents()
00552 {
00553     for(RtcInfoVector::iterator p = rtcInfoVector.begin(); p != rtcInfoVector.end(); ++p){
00554         RtcInfoPtr& rtcInfo = *p;
00555         if(!CORBA::is_nil(rtcInfo->execContext)){
00556             if( RTC::PRECONDITION_NOT_MET == rtcInfo->execContext->activate_component(rtcInfo->rtcRef) )
00557             {
00558                 rtcInfo->execContext->reset_component(rtcInfo->rtcRef);
00559                 rtcInfo->execContext->activate_component(rtcInfo->rtcRef);
00560             }
00561         }
00562     }
00563 
00564     RTC::ExecutionContextList_var eclist = virtualRobotRTC->get_owned_contexts();
00565     for(CORBA::ULong i=0; i < eclist->length(); ++i){
00566         if(!CORBA::is_nil(eclist[i])){
00567             ExtTrigExecutionContextService_Var_Type execContext = ExtTrigExecutionContextService_Type::_narrow(eclist[i]);
00568             if(!CORBA::is_nil(execContext)){
00569                 if( RTC::PRECONDITION_NOT_MET == execContext->activate_component(virtualRobotRTC->getObjRef()) )
00570                 {
00571                     execContext->reset_component(virtualRobotRTC->getObjRef());
00572                     execContext->tick();
00573                     execContext->activate_component(virtualRobotRTC->getObjRef());
00574                 }
00575                 execContext->tick();
00576             }
00577             break;
00578         }
00579     }
00580 }
00581 
00582 void Controller_impl::deactiveComponents()
00583 {
00584     std::vector<ExtTrigExecutionContextService_Var_Type> vecExecContext;
00585     for(RtcInfoVector::iterator p = rtcInfoVector.begin(); p != rtcInfoVector.end(); ++p){
00586         RtcInfoPtr& rtcInfo = *p;
00587         if(!CORBA::is_nil(rtcInfo->execContext)){
00588 #ifdef OPENRTM_VERISON110
00589             rtcInfo->execContext->deactivate_component(rtcInfo->rtcRef);
00590             vecExecContext.push_back(rtcInfo->execContext);
00591 #else
00592             if ( RTC::RTC_OK == rtcInfo->execContext->deactivate_component(rtcInfo->rtcRef) ){
00593                 vecExecContext.push_back(rtcInfo->execContext);
00594             }
00595 #endif
00596         }
00597     }
00598     RTC::ExecutionContextList_var eclist = virtualRobotRTC->get_owned_contexts();
00599     for(CORBA::ULong i=0; i < eclist->length(); ++i){
00600         if(!CORBA::is_nil(eclist[i])){
00601             ExtTrigExecutionContextService_Var_Type execContext = ExtTrigExecutionContextService_Type::_narrow(eclist[i]);
00602             if(!CORBA::is_nil(execContext)){
00603                 execContext->deactivate_component(virtualRobotRTC->getObjRef());
00604                 vecExecContext.push_back(execContext);
00605             }
00606             break;
00607         }
00608     }
00609     for( std::vector<ExtTrigExecutionContextService_Var_Type>::iterator ite = vecExecContext.begin();
00610          ite != vecExecContext.end();   ++ite   ){
00611         if(!CORBA::is_nil( *ite )){
00612             try{
00613                 // Trigger onDeactivated in component.
00614                 (*ite)->tick();
00615             } catch ( CORBA_SystemException& ex){
00616                 cerr << ex._rep_id() << endl;
00617                 cerr << "exception in Controller_impl::deactiveComponents" << endl;
00618             } catch (...) {
00619                 cerr << "unknown exception in Controller_impl::initialize()" <<  endl;
00620             }
00621         }
00622     }
00623 }


openhrp3
Author(s): AIST, General Robotix Inc., Nakamura Lab of Dept. of Mechano Informatics at University of Tokyo
autogenerated on Sun Apr 2 2017 03:43:53