19 #include <rtm/Manager.h>
20 #include <rtm/RTObject.h>
21 #include <rtm/NVUtil.h>
28 using namespace boost;
31 const bool CONTROLLER_BRIDGE_DEBUG =
false;
36 : bridgeConf(bridgeConf),
37 rtcManager(rtcManager),
41 if(CONTROLLER_BRIDGE_DEBUG){
42 cout <<
"Controller_impl::Controller_impl" << endl;
46 RTC::RtcBase* rtc =
rtcManager->createComponent(
"VirtualRobot");
53 if(CONTROLLER_BRIDGE_DEBUG){
54 cout <<
"Controller_impl::~Controller_impl" << endl;
64 RTC::Manager&
rtcManager = RTC::Manager::instance();
66 string nameServer =
rtcManager.getConfig()[
"corba.nameservers"];
67 cout <<
"setting naming" << endl;
68 int comPos = nameServer.find(
",");
70 comPos = nameServer.length();
72 nameServer = nameServer.substr(0, comPos);
76 RTC::RTObject_var rtcRef;
77 string rtcName = it->first;
78 if ( rtcName !=
"" ) {
79 string rtcNamingName = rtcName +
".rtc";
81 CORBA::Object_var objRef =
naming->resolve(rtcNamingName.c_str());
82 if ( CORBA::is_nil(objRef) ) {
83 cerr << rtcName <<
" is not found." << endl;
84 throw OpenHRP::Controller::ControllerException(
string(rtcName+
" is not found.").c_str());
86 rtcRef = RTC::RTObject::_narrow(objRef);
87 if(CORBA::is_nil(rtcRef)){
88 cerr << rtcName <<
" is not an RTC object." << endl;
89 throw OpenHRP::Controller::ControllerException(
string(rtcName+
" is not an RTC object.").c_str());
93 cerr << ex._rep_id() << endl;
94 cerr <<
"exception in Controller_impl::detectRtcs" << endl;
96 cerr <<
"unknown exception in Controller_impl::detectRtcs()" << endl;
99 if (!CORBA::is_nil(rtcRef)) {
104 cout <<
"setup RT components" << endl;
110 RTC::RTObject_var rtcRef;
117 rtcRef = rtcServant->getObjRef();
123 RtcInfoMap::iterator it =
rtcInfoMap.find(rtcName);
126 string rtcNamingName = rtcName +
".rtc";
127 CORBA::Object_var objRef =
naming->resolve(rtcNamingName.c_str());
128 if(CORBA::is_nil(objRef)){
129 cerr << rtcName <<
" is not found." << endl;
130 throw OpenHRP::Controller::ControllerException(
string(rtcName+
" is not found.").c_str());
132 rtcRef = RTC::RTObject::_narrow(objRef);
133 if(CORBA::is_nil(rtcRef)){
134 cerr << rtcName <<
" is not an RTC object." << endl;
135 throw OpenHRP::Controller::ControllerException(
string(rtcName+
" is not an RTC object.").c_str());
139 if(!CORBA::is_nil(rtcRef)){
141 rtcInfoMap.insert(make_pair(rtcName,rtcInfo));
147 for(CORBA::ULong
i=0;
i < rtcList->length(); ++
i){
156 for(CORBA::ULong
i=0;
i < ports->length(); ++
i){
157 RTC::PortProfile_var
profile = ports[
i]->get_port_profile();
158 std::string portName(
profile->name);
159 string::size_type index = portName.rfind(
".");
160 if (index != string::npos) portName = portName.substr(index+1);
161 rtcInfo->portMap[portName] = ports[
i];
169 if((*it)->rtcRef->_is_equivalent(new_rtcRef))
174 rtcInfo->rtcRef = new_rtcRef;
176 string rtcName = (string)rtcInfo->rtcRef->get_component_profile()->instance_name;
179 rtcInfo->timeRate = 1.0;
180 rtcInfo->timeRateCounter = 0.0;
184 rtcInfo->timeRate = (double)p->second;
185 rtcInfo->timeRateCounter = 1.0 - rtcInfo->timeRate;
187 rtcInfo->timeRate = 0.0;
188 rtcInfo->timeRateCounter = 0.0;
190 cout <<
"periodic-rate (" << rtcName <<
") = " << rtcInfo->timeRate << endl;
195 #ifdef OPENRTM_VERSION_042
196 RTC::ExecutionContextServiceList_var eclist = rtcInfo->rtcRef->get_execution_context_services();
197 for(CORBA::ULong
i=0;
i < eclist->length(); ++
i){
198 if(!CORBA::is_nil(eclist[
i])){
199 rtcInfo->execContext = ExtTrigExecutionContextService_Type::_narrow(eclist[
i]);
200 if(!CORBA::is_nil(rtcInfo->execContext)){
201 cout <<
"detected the ExtTrigExecutionContext" << endl;
207 RTC::ExecutionContextList_var eclist = rtcInfo->rtcRef->get_owned_contexts();
208 for(CORBA::ULong
i=0;
i < eclist->length(); ++
i){
209 if(!CORBA::is_nil(eclist[
i])){
210 rtcInfo->execContext = ExtTrigExecutionContextService_Type::_narrow(eclist[
i]);
211 if(!CORBA::is_nil(rtcInfo->execContext)){
212 cout <<
"detected the ExtTrigExecutionContext" << endl;
228 if(controllerInstanceName.empty()){
230 controllerInstanceName =
"";
237 int connected =
false;
239 RtcInfoMap::iterator p =
rtcInfoMap.find(controllerInstanceName);
244 if(q == rtcInfo->portMap.end()){
246 cerr << controllerInstanceName <<
" does not have a port ";
248 throw OpenHRP::Controller::ControllerException(
"not found a port");
253 if(!robotPortHandler){
255 cerr <<
"The robot does not have a port named " << connection.
robotPortName <<
"\n";
256 throw OpenHRP::Controller::ControllerException(
"not found a port");
260 if(!CORBA::is_nil(robotPortRef)){
261 if(dynamic_pointer_cast<OutPortHandler>(robotPortHandler)){
262 connected =
connectPorts(robotPortRef, controllerPortRef);
264 connected =
connectPorts(controllerPortRef, robotPortRef);
272 cout <<
" ...ok" << endl;
273 }
else if(connected == -1){
274 cerr <<
"Connection failed." << endl;
275 }
else if(connected == 1){
276 cerr <<
" It has already been connected." << endl;
284 RTC::ConnectorProfileList_var connectorProfiles = inPort->get_connector_profiles();
285 for(CORBA::ULong
i=0;
i < connectorProfiles->length(); ++
i){
286 RTC::ConnectorProfile& connectorProfile = connectorProfiles[
i];
289 for(CORBA::ULong j=0; j < connectedPorts.length(); ++j){
291 if(connectedPortRef->_is_equivalent(outPort)){
297 RTC::ConnectorProfile cprof;
298 cprof.connector_id =
"";
299 cprof.name = CORBA::string_dup(
"connector0");
300 cprof.ports.length(2);
301 cprof.ports[0] = Port_Service_Type::_duplicate(inPort);
302 cprof.ports[1] = Port_Service_Type::_duplicate(outPort);
304 CORBA_SeqUtil::push_back(cprof.properties,
305 NVUtil::newNV(
"dataport.dataflow_type",
307 #ifdef OPENRTM_VERSION_042
308 CORBA_SeqUtil::push_back(cprof.properties,
309 NVUtil::newNV(
"dataport.interface_type",
311 CORBA_SeqUtil::push_back(cprof.properties,
312 NVUtil::newNV(
"dataport.subscription_type",
315 CORBA_SeqUtil::push_back(cprof.properties,
316 NVUtil::newNV(
"dataport.interface_type",
318 CORBA_SeqUtil::push_back(cprof.properties,
319 NVUtil::newNV(
"dataport.subscription_type",
322 RTC::ReturnCode_t result = inPort->connect(cprof);
324 if(result == RTC::RTC_OK)
339 this->viewSimulator = ViewSimulator::_duplicate(
viewSimulator);
345 if(CONTROLLER_BRIDGE_DEBUG){
346 cout <<
"Controller_impl::start" << endl;
357 cameras =
new CameraSequence(0);
362 cerr << ex._rep_id() << endl;
363 cerr <<
"exception in Controller_impl::start" << endl;
365 cerr <<
"unknown exception in Controller_impl::start()" << endl;
382 (
const std::string& linkName, DynamicsSimulator::LinkDataType linkDataType)
384 DblSequence_var
data;
385 dynamicsSimulator->getCharacterLinkData(modelName.c_str(), linkName.c_str(), linkDataType,
data.out());
392 DblSequence_var
data;
401 ImageData_var imageData =
cameras[cameraId]->getImageData();
402 return imageData._retn();
404 ImageData* imageData =
new ImageData;
405 imageData->floatData.length(0);
406 imageData->longData.length(0);
407 imageData->octetData.length(0);
415 if(CONTROLLER_BRIDGE_DEBUG){
416 cout <<
"Controller_impl::input" << endl;
444 DynamicsSimulator::LinkDataType linkDataType,
445 const DblSequence& linkData)
448 linkDataType, linkData);
453 if(CONTROLLER_BRIDGE_DEBUG){
454 cout <<
"Controller_impl::output" << endl;
460 info.flushed =
false;
469 if(CONTROLLER_BRIDGE_DEBUG){
470 cout <<
"Controller_impl::control" << endl;
477 if(!CORBA::is_nil(rtcInfo->execContext)){
478 rtcInfo->timeRateCounter += rtcInfo->timeRate;
479 if(rtcInfo->timeRateCounter + rtcInfo->timeRate/2.0 > 1.0){
480 rtcInfo->execContext->tick();
481 rtcInfo->timeRateCounter -= 1.0;
500 if(CONTROLLER_BRIDGE_DEBUG){
501 cout <<
"Controller_impl::destroy()" << endl;
503 PortableServer::POA_var poa = _default_POA();
504 PortableServer::ObjectId_var
id = poa->servant_to_id(
this);
505 poa->deactivate_object(
id);
512 if(CONTROLLER_BRIDGE_DEBUG){
513 cout <<
"Controller_impl::initialize()" << endl;
524 throw OpenHRP::Controller::ControllerException(
"Error OutPort StepTime");
528 cerr << ex._rep_id() << endl;
529 cerr <<
"exception in initializeController" << endl;
530 throw OpenHRP::Controller::ControllerException(
"");
531 }
catch(std::invalid_argument& ex){
532 cerr <<
"invalid argument : " << ex.what() << endl;
533 throw OpenHRP::Controller::ControllerException(
"");
540 if(CONTROLLER_BRIDGE_DEBUG){
541 cout <<
"Controller_impl::shutdown()" << endl;
555 if(!CORBA::is_nil(rtcInfo->execContext)){
556 if( RTC::PRECONDITION_NOT_MET == rtcInfo->execContext->activate_component(rtcInfo->rtcRef) )
558 rtcInfo->execContext->reset_component(rtcInfo->rtcRef);
559 rtcInfo->execContext->activate_component(rtcInfo->rtcRef);
564 RTC::ExecutionContextList_var eclist =
virtualRobotRTC->get_owned_contexts();
565 for(CORBA::ULong
i=0;
i < eclist->length(); ++
i){
566 if(!CORBA::is_nil(eclist[
i])){
568 if(!CORBA::is_nil(execContext)){
569 if( RTC::PRECONDITION_NOT_MET == execContext->activate_component(
virtualRobotRTC->getObjRef()) )
584 std::vector<ExtTrigExecutionContextService_Var_Type> vecExecContext;
587 if(!CORBA::is_nil(rtcInfo->execContext)){
588 #ifdef OPENRTM_VERISON110
589 rtcInfo->execContext->deactivate_component(rtcInfo->rtcRef);
590 vecExecContext.push_back(rtcInfo->execContext);
592 if ( RTC::RTC_OK == rtcInfo->execContext->deactivate_component(rtcInfo->rtcRef) ){
593 vecExecContext.push_back(rtcInfo->execContext);
598 RTC::ExecutionContextList_var eclist =
virtualRobotRTC->get_owned_contexts();
599 for(CORBA::ULong
i=0;
i < eclist->length(); ++
i){
600 if(!CORBA::is_nil(eclist[
i])){
602 if(!CORBA::is_nil(execContext)){
604 vecExecContext.push_back(execContext);
609 for( std::vector<ExtTrigExecutionContextService_Var_Type>::iterator ite = vecExecContext.begin();
610 ite != vecExecContext.end(); ++ite ){
611 if(!CORBA::is_nil( *ite )){
616 cerr << ex._rep_id() << endl;
617 cerr <<
"exception in Controller_impl::deactiveComponents" << endl;
619 cerr <<
"unknown exception in Controller_impl::initialize()" << endl;