20 #include <boost/bind.hpp>
23 using namespace boost;
26 const bool CONTROLLER_BRIDGE_DEBUG =
false;
32 static const char* spec[] = {
33 "implementation_id",
"VirtualRobot",
34 "type_name",
"VirtualRobot",
35 "description",
"This component enables controller components to"
36 "access the I/O of a virtual robot in a OpenHRP simulation",
39 "category",
"OpenHRP",
40 "activity_type",
"DataFlowComponent",
43 "lang_type",
"compile",
47 if(CONTROLLER_BRIDGE_DEBUG){
48 cout <<
"initVirtualRobotRTC()" << endl;
52 profile.setDefault(
"type_name", componentTypeName);
54 manager->registerFactory(
profile,
55 RTC::Create<VirtualRobotRTC>,
56 RTC::Delete<VirtualRobotRTC>);
61 :
RTC::DataFlowComponentBase(manager)
63 if(CONTROLLER_BRIDGE_DEBUG){
64 cout <<
"VirtualRobotRTC::VirtualRobotRTC" << endl;
74 PortInfoMap::iterator it;
77 #ifdef OPENRTM_VERSION_042
81 cerr <<
"createOutPortHandler(" << it->second.portName <<
") failed" << std::endl;
87 #ifdef OPENRTM_VERSION_042
91 cerr <<
"createInPortHandler(" << it->second.portName <<
") failed" << std::endl;
103 if(CONTROLLER_BRIDGE_DEBUG){
104 cout <<
"VirtualRobotRTC::~VirtualRobotRTC" << endl;
108 #ifdef OPENRTM_VERSION_042
328 string::size_type index =
name.rfind(
".");
329 if (index != string::npos)
name =
name.substr(index+1);
335 portHandler = p->second;
339 portHandler = q->second;
351 handler->portRef = Port_Service_Type::_nil();
355 handler->portRef = Port_Service_Type::_nil();
360 for(CORBA::ULong
i=0;
i < ports->length(); ++
i){
362 RTC::PortProfile_var
profile = ports[
i]->get_port_profile();
366 portHandler->portRef = ports[
i];
374 RTC::RTCList* rtcList =
new RTC::RTCList;
376 set<string> foundRtcNames;
393 RTC::PortProfile_var portProfile = portRef->get_port_profile();
394 string portName(portProfile->name);
396 RTC::ConnectorProfileList_var connectorProfiles = portRef->get_connector_profiles();
398 for(CORBA::ULong
i=0;
i < connectorProfiles->length(); ++
i){
399 RTC::ConnectorProfile& connectorProfile = connectorProfiles[
i];
402 for(CORBA::ULong j=0; j < connectedPorts.length(); ++j){
404 RTC::PortProfile_var connectedPortProfile = connectedPortRef->get_port_profile();
405 RTC::RTObject_var connectedRtcRef = RTC::RTObject::_duplicate(connectedPortProfile->owner);
406 RTC::RTObject_var thisRef = RTC::RTObject::_duplicate(getObjRef());
408 if(!CORBA::is_nil(connectedRtcRef) && !connectedRtcRef->_is_equivalent(thisRef)){
410 for(; ii<rtcList.length(); ii++){
411 if(rtcList[ii]->_is_equivalent(connectedRtcRef))
415 RTC::ComponentProfile_var componentProfile = connectedRtcRef->get_component_profile();
416 string connectedRtcName(componentProfile->instance_name);
418 cout <<
"detected a port connection: ";
419 cout <<
"\"" << portName <<
"\" of " << getInstanceName() <<
" <--> \"";
420 cout << connectedPortProfile->name <<
"\" of " << connectedRtcName << endl;
422 if(ii == rtcList.length()){
424 #ifdef OPENRTM_VERSION_042
425 RTC::ExecutionContextServiceList_var execServices = connectedRtcRef->get_execution_context_services();
427 for(CORBA::ULong k=0; k < execServices->length(); k++) {
428 RTC::ExecutionContextService_var execContext = execServices[k];
431 ExtTrigExecutionContextService_Type::_narrow(execContext);
433 if(!CORBA::is_nil(extTrigExecContext)){
434 CORBA::ULong
n = rtcList.length();
435 rtcList.length(
n + 1);
436 rtcList[
n] = connectedRtcRef;
437 foundRtcNames.insert(connectedRtcName);
441 RTC::ExecutionContextList_var execServices = connectedRtcRef->get_owned_contexts();
443 for(CORBA::ULong k=0; k < execServices->length(); k++) {
444 RTC::ExecutionContext_var execContext = execServices[k];
447 ExtTrigExecutionContextService_Type::_narrow(execContext);
449 if(!CORBA::is_nil(extTrigExecContext)){
450 CORBA::ULong
n = rtcList.length();
451 rtcList.length(
n + 1);
452 rtcList[
n] = connectedRtcRef;
453 foundRtcNames.insert(connectedRtcName);
468 double controlTimeStep = controller->
getTimeStep();
470 double stepTime = it->second->stepTime;
472 double w = controlTime-(
int)(controlTime/stepTime)*stepTime + controlTimeStep/2;
473 if(w >= stepTime) w=0;
474 if(w < controlTimeStep )
475 it->second->inputDataFromSimulator(controller);
477 it->second->inputDataFromSimulator(controller);
486 it->second->outputDataToSimulator(controller);
494 double controlTimeStep = controller->
getTimeStep();
496 double stepTime = it->second->stepTime;
498 double w = controlTime-(
int)(controlTime/stepTime)*stepTime + controlTimeStep/2;
499 if(w >= stepTime) w=0;
500 if(w < controlTimeStep )
501 it->second->writeDataToPort();
503 it->second->writeDataToPort();
512 it->second->readDataFromPort(controller);
525 double stepTime = it->second->stepTime;
526 if(stepTime && stepTime < controlTimeStep){
527 cerr <<
"OutPort(" << it->second->portName <<
") : Output interval(" << stepTime <<
") must be longer than the control interval(" << controlTimeStep <<
")." << std::endl;