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;
53 if(CONTROLLER_BRIDGE_DEBUG){
54 cout <<
"Controller_impl::~Controller_impl" << endl;
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;
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);
307 #ifdef OPENRTM_VERSION_042 322 RTC::ReturnCode_t result = inPort->connect(cprof);
324 if(result == RTC::RTC_OK)
333 this->dynamicsSimulator = DynamicsSimulator::_duplicate(dynamicsSimulator);
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;
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;
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);
565 for(CORBA::ULong
i=0;
i < eclist->length(); ++
i){
566 if(!CORBA::is_nil(eclist[
i])){
568 if(!CORBA::is_nil(execContext)){
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);
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;
SDOPackage::NameValue newNV(const char *name, Value value)
png_infop png_charpp int png_charpp profile
VirtualRobotRTC * virtualRobotRTC
RTObject_impl * createComponent(const char *comp_args)
int connectPorts(Port_Service_Ptr_Type outPort, Port_Service_Ptr_Type inPort)
void readDataFromInPorts(Controller_impl *controller)
Modifications controlling boost library behavior.
void makePortMap(RtcInfoPtr &rtcInfo)
void writeDataToOutPorts(Controller_impl *controller)
void outputDataToSimulator(Controller_impl *controller)
OpenRTM::ExtTrigExecutionContextService_var ExtTrigExecutionContextService_Var_Type
RTC::Manager * rtcManager
ImageData * getCameraImageFromSimulator(int cameraId)
PortHandlerPtr getPortHandler(const std::string &name)
boost::shared_ptr< RtcInfo > RtcInfoPtr
#define CORBA_SystemException
CORBA::Object_ptr resolve(const CosNaming::Name &name)
DblSequence & getJointDataSeqRef(DynamicsSimulator::LinkDataType linkDataType)
RTC::PortServiceList_var Port_Service_List_Var_Type
void deactiveComponents()
virtual void setDynamicsSimulator(DynamicsSimulator_ptr dynamicsSimulator)
std::string controllerInstanceName
static Manager & instance()
void setupRtcConnections()
PortConnectionList portConnections
boost::shared_ptr< PortHandler > PortHandlerPtr
JointValueSeqInfoMap outputJointValueSeqInfos
coil::Properties & getConfig()
SensorState & getCurrentSensorState()
DynamicsSimulator_var dynamicsSimulator
std::string robotPortName
RTC::CorbaNaming * naming
void flushJointDataSeqToSimulator(DynamicsSimulator::LinkDataType linkDataType)
CameraSequence_var cameras
void inputDataFromSimulator(Controller_impl *controller)
void flushLinkDataToSimulator(const std::string &linkName, DynamicsSimulator::LinkDataType linkDataType, const DblSequence &linkData)
virtual void setViewSimulator(ViewSimulator_ptr viewSimulator)
Controller_impl(RTC::Manager *rtcManager, BridgeConf *bridgeConf)
RTObject_ptr getObjRef() const
const char * getInstanceName()
RTC::PortServiceList Port_Service_List_Type
virtual ExecutionContextList * get_owned_contexts()
DblSequence * getLinkDataFromSimulator(const std::string &linkName, DynamicsSimulator::LinkDataType linkDataType)
bool checkOutPortStepTime(double controlTimeStep)
static void registerFactory(RTC::Manager *manager, const char *componentTypeName)
RTC::RTCList * getConnectedRtcs()
RtcInfoVector rtcInfoVector
RTC::PortService_ptr Port_Service_Ptr_Type
void push_back(CorbaSequence &seq, SequenceElement elem)
Controller_impl::RtcInfoPtr addRtcVectorWithConnection(RTC::RTObject_var rtcRef)
DblSequence * getSensorDataFromSimulator(const std::string &sensorName)
virtual void initialize()
std::string controllerPortName
const char * getVirtualRobotRtcTypeName()
ViewSimulator_var viewSimulator
SensorState_var sensorState
ModuleInfoList moduleInfoList