VirtualRobotRTC.cpp
Go to the documentation of this file.
00001 // -*- mode: c++; c-basic-offset: 2; -*-
00002 /*
00003  * Copyright (c) 2008, AIST, the University of Tokyo and General Robotix Inc.
00004  * All rights reserved. This program is made available under the terms of the
00005  * Eclipse Public License v1.0 which accompanies this distribution, and is
00006  * available at http://www.eclipse.org/legal/epl-v10.html
00007  * Contributors:
00008  * National Institute of Advanced Industrial Science and Technology (AIST)
00009  * General Robotix Inc.
00010  */
00016 #include"VirtualRobotRTC.h"
00017 #include "Controller_impl.h"
00018 
00019 #include <iostream>
00020 #include <boost/bind.hpp>
00021 
00022 using namespace std;
00023 using namespace boost;
00024 
00025 namespace {
00026   const bool CONTROLLER_BRIDGE_DEBUG = false;
00027 }
00028 
00029 
00030 void VirtualRobotRTC::registerFactory(RTC::Manager* manager, const char* componentTypeName)
00031 {
00032   static const char* spec[] = {
00033     "implementation_id", "VirtualRobot",
00034     "type_name",         "VirtualRobot",
00035     "description",       "This component enables controller components to"
00036     "access the I/O of a virtual robot in a OpenHRP simulation",
00037     "version",           "1.0",
00038     "vendor",            "AIST",
00039     "category",          "OpenHRP",
00040     "activity_type",     "DataFlowComponent",
00041     "max_instance",      "1",
00042     "language",          "C++",
00043     "lang_type",         "compile",
00044     ""
00045   };
00046 
00047   if(CONTROLLER_BRIDGE_DEBUG){
00048     cout << "initVirtualRobotRTC()" << endl;
00049   }
00050 
00051   RTC::Properties profile(spec);
00052   profile.setDefault("type_name", componentTypeName);
00053 
00054   manager->registerFactory(profile,
00055                            RTC::Create<VirtualRobotRTC>,
00056                            RTC::Delete<VirtualRobotRTC>);
00057 }
00058 
00059 
00060 VirtualRobotRTC::VirtualRobotRTC(RTC::Manager* manager)
00061   : RTC::DataFlowComponentBase(manager)
00062 {
00063   if(CONTROLLER_BRIDGE_DEBUG){
00064     cout << "VirtualRobotRTC::VirtualRobotRTC" << endl;
00065   }
00066 
00067   isOwnedByController = false;
00068 }
00069 
00070 RTC::ReturnCode_t VirtualRobotRTC::onInitialize()
00071 {
00072   BridgeConf* bridgeConf = BridgeConf::instance();
00073 
00074   PortInfoMap::iterator it;
00075 
00076   for(it = bridgeConf->outPortInfos.begin(); it != bridgeConf->outPortInfos.end(); ++it){
00077 #ifdef OPENRTM_VERSION_042
00078       createOutPortHandler(it->second);
00079 #else
00080     if (!createOutPortHandler(it->second)){
00081       cerr << "createOutPortHandler(" << it->second.portName << ") failed" << std::endl;
00082     }
00083 #endif
00084   }
00085 
00086   for(it = bridgeConf->inPortInfos.begin(); it != bridgeConf->inPortInfos.end(); ++it){
00087 #ifdef OPENRTM_VERSION_042
00088       createInPortHandler(it->second);
00089 #else
00090     if (!createInPortHandler(it->second)){
00091       cerr << "createInPortHandler(" << it->second.portName << ") failed" << std::endl;
00092     }
00093 #endif
00094   }
00095 
00096   updatePortObjectRefs();
00097   return RTC::RTC_OK;
00098 }
00099 
00100 
00101 VirtualRobotRTC::~VirtualRobotRTC()
00102 {
00103   if(CONTROLLER_BRIDGE_DEBUG){
00104     cout << "VirtualRobotRTC::~VirtualRobotRTC" << endl;
00105   }
00106 }
00107 
00108 #ifdef OPENRTM_VERSION_042
00109 
00110 void VirtualRobotRTC::createOutPortHandler(PortInfo& portInfo)
00111 {
00112   DataTypeId dataTypeId = portInfo.dataTypeId;
00113 
00114   if(portInfo.dataOwnerName.empty()){
00115     switch(dataTypeId) {
00116 
00117     case JOINT_VALUE:
00118     case JOINT_VELOCITY:
00119     case JOINT_ACCELERATION:
00120     case JOINT_TORQUE:
00121     case FORCE_SENSOR:
00122     case RATE_GYRO_SENSOR:
00123     case ACCELERATION_SENSOR:
00124       registerOutPortHandler(new SensorStateOutPortHandler(portInfo));
00125       break;
00126 
00127     case COLOR_IMAGE:
00128       registerOutPortHandler(new ColorImageOutPortHandler(portInfo));
00129       break;
00130 
00131     case GRAYSCALE_IMAGE:
00132       registerOutPortHandler(new GrayScaleImageOutPortHandler(portInfo));
00133       break;
00134 
00135     case DEPTH_IMAGE:
00136       registerOutPortHandler(new DepthImageOutPortHandler(portInfo));
00137       break;
00138 
00139     default:
00140       break;
00141     }
00142   } else {
00143 
00144     switch(dataTypeId) {
00145 
00146     case JOINT_VALUE:
00147     case JOINT_VELOCITY:
00148     case JOINT_ACCELERATION:
00149     case JOINT_TORQUE:
00150     case ABS_TRANSFORM:
00151     case ABS_VELOCITY:
00152     case ABS_ACCELERATION:
00153     case CONSTRAINT_FORCE:
00154       registerOutPortHandler(new LinkDataOutPortHandler(portInfo));
00155       break;
00156     case ABS_TRANSFORM2:
00157       registerOutPortHandler(new AbsTransformOutPortHandler(portInfo));
00158       break;
00159 
00160     case FORCE_SENSOR:
00161     case RATE_GYRO_SENSOR:
00162     case ACCELERATION_SENSOR:
00163     case RANGE_SENSOR:
00164       registerOutPortHandler(new SensorDataOutPortHandler(portInfo));
00165       break;
00166 
00167     default:
00168       break;
00169     }
00170   }
00171 }
00172 
00173 
00174 void VirtualRobotRTC::createInPortHandler(PortInfo& portInfo)
00175 {
00176   DataTypeId dataTypeId = portInfo.dataTypeId;
00177   if(portInfo.dataOwnerName.empty()){
00178     switch(dataTypeId) {
00179     case JOINT_VALUE:
00180     case JOINT_VELOCITY:
00181     case JOINT_ACCELERATION:
00182     case JOINT_TORQUE:
00183       registerInPortHandler(new JointDataSeqInPortHandler(portInfo));
00184       break;
00185     default:
00186       break;
00187     }
00188   }else{
00189     switch(dataTypeId){
00190     case JOINT_VALUE:
00191     case JOINT_VELOCITY:
00192     case JOINT_ACCELERATION:
00193     case JOINT_TORQUE:
00194       registerInPortHandler(new LinkDataInPortHandler(portInfo));
00195       break;
00196     case ABS_TRANSFORM:
00197     case ABS_VELOCITY:
00198     case ABS_ACCELERATION:
00199     case EXTERNAL_FORCE:
00200       registerInPortHandler(new LinkDataInPortHandler(portInfo));
00201       break;
00202     case ABS_TRANSFORM2:
00203       registerInPortHandler(new AbsTransformInPortHandler(portInfo));
00204       break;
00205     default:
00206       break;
00207     }
00208   }
00209 }
00210 
00211 #else
00212 
00213 bool VirtualRobotRTC::createOutPortHandler(PortInfo& portInfo)
00214 {
00215   DataTypeId dataTypeId = portInfo.dataTypeId;
00216   bool ret = false;
00217 
00218   if(portInfo.dataOwnerName.empty()){
00219     switch(dataTypeId) {
00220 
00221     case JOINT_VALUE:
00222     case JOINT_VELOCITY:
00223     case JOINT_ACCELERATION:
00224     case JOINT_TORQUE:
00225     case FORCE_SENSOR:
00226     case RATE_GYRO_SENSOR:
00227     case ACCELERATION_SENSOR:
00228       ret = registerOutPortHandler(new SensorStateOutPortHandler(portInfo));
00229       break;
00230 
00231     case COLOR_IMAGE:
00232       ret = registerOutPortHandler(new ColorImageOutPortHandler(portInfo));
00233       break;
00234 
00235     case GRAYSCALE_IMAGE:
00236       ret = registerOutPortHandler(new GrayScaleImageOutPortHandler(portInfo));
00237       break;
00238 
00239     case DEPTH_IMAGE:
00240       ret = registerOutPortHandler(new DepthImageOutPortHandler(portInfo));
00241       break;
00242 
00243     default:
00244       break;
00245     }
00246   } else {
00247 
00248     switch(dataTypeId) {
00249 
00250     case JOINT_VALUE:
00251     case JOINT_VELOCITY:
00252     case JOINT_ACCELERATION:
00253     case JOINT_TORQUE:
00254     case ABS_TRANSFORM:
00255     case ABS_VELOCITY:
00256     case ABS_ACCELERATION:
00257     case CONSTRAINT_FORCE:
00258       ret = registerOutPortHandler(new LinkDataOutPortHandler(portInfo));
00259       break;
00260     case ABS_TRANSFORM2:
00261       ret = registerOutPortHandler(new AbsTransformOutPortHandler(portInfo));
00262       break;
00263 
00264     case FORCE_SENSOR:
00265     case RATE_GYRO_SENSOR:
00266     case ACCELERATION_SENSOR:
00267     case RANGE_SENSOR:
00268       ret = registerOutPortHandler(new SensorDataOutPortHandler(portInfo));
00269       break;
00270     case RATE_GYRO_SENSOR2:
00271       ret = registerOutPortHandler(new GyroSensorOutPortHandler(portInfo));
00272       break;
00273     case ACCELERATION_SENSOR2:
00274       ret = registerOutPortHandler(new AccelerationSensorOutPortHandler(portInfo));
00275       break;
00276 
00277     default:
00278       break;
00279     }
00280   }
00281   return ret;
00282 }
00283 
00284 
00285 bool VirtualRobotRTC::createInPortHandler(PortInfo& portInfo)
00286 {
00287   DataTypeId dataTypeId = portInfo.dataTypeId;
00288   bool ret = false;
00289   if(portInfo.dataOwnerName.empty()){
00290     switch(dataTypeId) {
00291     case JOINT_VALUE:
00292     case JOINT_VELOCITY:
00293     case JOINT_ACCELERATION:
00294     case JOINT_TORQUE:
00295       ret = registerInPortHandler(new JointDataSeqInPortHandler(portInfo));
00296       break;
00297     default:
00298       break;
00299     }
00300   }else{
00301     switch(dataTypeId){
00302     case JOINT_VALUE:
00303     case JOINT_VELOCITY:
00304     case JOINT_ACCELERATION:
00305     case JOINT_TORQUE:
00306       ret = registerInPortHandler(new LinkDataInPortHandler(portInfo));
00307       break;
00308     case ABS_TRANSFORM2:
00309       ret = registerInPortHandler(new AbsTransformInPortHandler(portInfo));
00310       break;
00311     case ABS_TRANSFORM:
00312     case ABS_VELOCITY:
00313     case ABS_ACCELERATION:
00314     case EXTERNAL_FORCE:
00315       ret = registerInPortHandler(new LinkDataInPortHandler(portInfo));
00316       break;
00317     default:
00318       break;
00319     }
00320   }
00321   return ret;
00322 }
00323 #endif
00324 
00325 PortHandlerPtr VirtualRobotRTC::getPortHandler(const std::string& name_)
00326 {
00327   string name(name_);
00328   string::size_type index = name.rfind(".");
00329   if (index != string::npos) name = name.substr(index+1);
00330 
00331   PortHandlerPtr portHandler;
00332 
00333   OutPortHandlerMap::iterator p = outPortHandlers.find(name);
00334   if(p != outPortHandlers.end()){
00335     portHandler = p->second;
00336   } else {
00337     InPortHandlerMap::iterator q = inPortHandlers.find(name);
00338     if(q != inPortHandlers.end()){
00339       portHandler = q->second;
00340     }
00341   }
00342 
00343   return portHandler;
00344 }
00345 
00346 
00347 void VirtualRobotRTC::updatePortObjectRefs()
00348 {
00349   for(OutPortHandlerMap::iterator it = outPortHandlers.begin(); it != outPortHandlers.end(); ++it){
00350     OutPortHandlerPtr& handler = it->second;
00351     handler->portRef = Port_Service_Type::_nil();
00352   }
00353   for(InPortHandlerMap::iterator it = inPortHandlers.begin(); it != inPortHandlers.end(); ++it){
00354     InPortHandlerPtr& handler = it->second;
00355     handler->portRef = Port_Service_Type::_nil();
00356   }
00357 
00358   Port_Service_List_Var_Type ports = get_ports();
00359 
00360   for(CORBA::ULong i=0; i < ports->length(); ++i){
00361 
00362     RTC::PortProfile_var profile = ports[i]->get_port_profile();
00363     PortHandlerPtr portHandler = getPortHandler(string(profile->name));
00364 
00365     if(portHandler){
00366       portHandler->portRef = ports[i];
00367     }
00368   }
00369 }
00370 
00371 
00372 RTC::RTCList* VirtualRobotRTC::getConnectedRtcs()
00373 {
00374   RTC::RTCList* rtcList = new RTC::RTCList;
00375 
00376   set<string> foundRtcNames;
00377 
00378   for(OutPortHandlerMap::iterator it = outPortHandlers.begin(); it != outPortHandlers.end(); ++it){
00379     OutPortHandlerPtr& handler = it->second;
00380     addConnectedRtcs(handler->portRef, *rtcList, foundRtcNames);
00381   }
00382   for(InPortHandlerMap::iterator it = inPortHandlers.begin(); it != inPortHandlers.end(); ++it){
00383     InPortHandlerPtr& handler = it->second;
00384     addConnectedRtcs(handler->portRef, *rtcList, foundRtcNames);
00385   }
00386 
00387   return rtcList;
00388 }
00389 
00390 
00391 void VirtualRobotRTC::addConnectedRtcs(Port_Service_Ptr_Type portRef, RTC::RTCList& rtcList, std::set<std::string>& foundRtcNames)
00392 {
00393     RTC::PortProfile_var portProfile = portRef->get_port_profile();
00394     string portName(portProfile->name);
00395 
00396     RTC::ConnectorProfileList_var connectorProfiles = portRef->get_connector_profiles();
00397 
00398     for(CORBA::ULong i=0; i < connectorProfiles->length(); ++i){
00399         RTC::ConnectorProfile& connectorProfile = connectorProfiles[i];
00400         Port_Service_List_Type& connectedPorts = connectorProfile.ports;
00401 
00402         for(CORBA::ULong j=0; j < connectedPorts.length(); ++j){
00403                 Port_Service_Ptr_Type connectedPortRef = connectedPorts[j];
00404             RTC::PortProfile_var connectedPortProfile = connectedPortRef->get_port_profile();
00405             RTC::RTObject_var connectedRtcRef = RTC::RTObject::_duplicate(connectedPortProfile->owner);
00406             RTC::RTObject_var thisRef = RTC::RTObject::_duplicate(getObjRef());
00407 
00408             if(!CORBA::is_nil(connectedRtcRef) && !connectedRtcRef->_is_equivalent(thisRef)){
00409                 CORBA::ULong ii=0;
00410                 for(; ii<rtcList.length(); ii++){
00411                     if(rtcList[ii]->_is_equivalent(connectedRtcRef))
00412                         break;
00413                 }
00414 
00415                 RTC::ComponentProfile_var componentProfile = connectedRtcRef->get_component_profile();
00416                 string connectedRtcName(componentProfile->instance_name);
00417 
00418                 cout << "detected a port connection: ";
00419                 cout << "\"" << portName << "\" of " << getInstanceName() << " <--> \"";
00420                 cout << connectedPortProfile->name << "\" of " << connectedRtcName << endl;
00421 
00422                 if(ii == rtcList.length()){
00423 
00424 #ifdef OPENRTM_VERSION_042
00425                         RTC::ExecutionContextServiceList_var execServices = connectedRtcRef->get_execution_context_services();
00426 
00427                     for(CORBA::ULong k=0; k < execServices->length(); k++) {
00428                         RTC::ExecutionContextService_var execContext = execServices[k];
00429 
00430                         ExtTrigExecutionContextService_Var_Type extTrigExecContext =
00431                                 ExtTrigExecutionContextService_Type::_narrow(execContext);
00432 
00433                         if(!CORBA::is_nil(extTrigExecContext)){
00434                             CORBA::ULong n = rtcList.length();
00435                             rtcList.length(n + 1);
00436                             rtcList[n] = connectedRtcRef;
00437                             foundRtcNames.insert(connectedRtcName);
00438                         }
00439                     }
00440 #else
00441                     RTC::ExecutionContextList_var execServices = connectedRtcRef->get_owned_contexts();
00442 
00443                     for(CORBA::ULong k=0; k < execServices->length(); k++) {
00444                         RTC::ExecutionContext_var execContext = execServices[k];
00445 
00446                         ExtTrigExecutionContextService_Var_Type extTrigExecContext =
00447                                 ExtTrigExecutionContextService_Type::_narrow(execContext);
00448 
00449                         if(!CORBA::is_nil(extTrigExecContext)){
00450                             CORBA::ULong n = rtcList.length();
00451                             rtcList.length(n + 1);
00452                             rtcList[n] = connectedRtcRef;
00453                             foundRtcNames.insert(connectedRtcName);
00454                         }
00455                     }
00456 #endif
00457 
00458                 }
00459             }
00460         }
00461     }
00462 }
00463 
00464 
00465 void VirtualRobotRTC::inputDataFromSimulator(Controller_impl* controller)
00466 {
00467     double controlTime = controller->controlTime;
00468     double controlTimeStep = controller->getTimeStep();
00469     for(OutPortHandlerMap::iterator it = outPortHandlers.begin(); it != outPortHandlers.end(); ++it){
00470         double stepTime = it->second->stepTime;
00471         if(stepTime){
00472             double w = controlTime-(int)(controlTime/stepTime)*stepTime + controlTimeStep/2;
00473             if(w >= stepTime) w=0;
00474             if(w < controlTimeStep )
00475                 it->second->inputDataFromSimulator(controller);
00476         }else{
00477             it->second->inputDataFromSimulator(controller);
00478         }
00479     }
00480 }
00481 
00482 
00483 void VirtualRobotRTC::outputDataToSimulator(Controller_impl* controller)
00484 {
00485   for(InPortHandlerMap::iterator it = inPortHandlers.begin(); it != inPortHandlers.end(); ++it){
00486     it->second->outputDataToSimulator(controller);
00487   }
00488 }
00489 
00490 
00491 void VirtualRobotRTC::writeDataToOutPorts(Controller_impl* controller)
00492 {
00493     double controlTime = controller->controlTime;
00494     double controlTimeStep = controller->getTimeStep();
00495     for(OutPortHandlerMap::iterator it = outPortHandlers.begin(); it != outPortHandlers.end(); ++it){
00496         double stepTime = it->second->stepTime;
00497         if(stepTime){
00498             double w = controlTime-(int)(controlTime/stepTime)*stepTime + controlTimeStep/2;
00499             if(w >= stepTime) w=0;
00500             if(w < controlTimeStep )
00501                 it->second->writeDataToPort();
00502         }else{
00503                 it->second->writeDataToPort();
00504         }
00505     }
00506 }
00507 
00508 
00509 void VirtualRobotRTC::readDataFromInPorts(Controller_impl* controller)
00510 {
00511   for(InPortHandlerMap::iterator it = inPortHandlers.begin(); it != inPortHandlers.end(); ++it){
00512     it->second->readDataFromPort(controller);
00513   }
00514 }
00515 
00516 void VirtualRobotRTC::stop()
00517 {
00518 
00519 }
00520 
00521 bool VirtualRobotRTC::checkOutPortStepTime(double controlTimeStep)
00522 {
00523     bool ret = true;
00524     for(OutPortHandlerMap::iterator it = outPortHandlers.begin(); it != outPortHandlers.end(); ++it){
00525         double stepTime = it->second->stepTime;
00526         if(stepTime && stepTime < controlTimeStep){
00527             cerr << "OutPort(" << it->second->portName << ") : Output interval(" << stepTime << ") must be longer than the control interval(" << controlTimeStep << ")." << std::endl;
00528             ret &= false;
00529         }
00530     }
00531     return ret;
00532 }


openhrp3
Author(s): AIST, General Robotix Inc., Nakamura Lab of Dept. of Mechano Informatics at University of Tokyo
autogenerated on Thu Apr 11 2019 03:30:19