Go to the documentation of this file.00001
00002
00003
00004
00005
00006
00007
00008
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
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
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
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 }