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