BridgeConf.cpp
Go to the documentation of this file.
00001 /*
00002  * Copyright (c) 2008, AIST, the University of Tokyo and General Robotix Inc.
00003  * All rights reserved. This program is made available under the terms of the
00004  * Eclipse Public License v1.0 which accompanies this distribution, and is
00005  * available at http://www.eclipse.org/legal/epl-v10.html
00006  * Contributors:
00007  * National Institute of Advanced Industrial Science and Technology (AIST)
00008  * General Robotix Inc. 
00009  */
00015 #include "BridgeConf.h"
00016 
00017 #include <iostream>
00018 #include <fstream>
00019 #include <boost/regex.hpp>
00020 
00021 #if (BOOST_VERSION <= 103301)
00022 #include <boost/filesystem/path.hpp>
00023 #include <boost/filesystem/operations.hpp>
00024 #include <boost/filesystem/convenience.hpp> 
00025 #else
00026 #include <boost/filesystem.hpp>
00027 #endif
00028 
00029 
00030 using namespace std;
00031 using namespace boost;
00032 
00033 
00034 namespace {
00035   BridgeConf* bridgeConf = 0;
00036 }
00037 
00038 
00039 BridgeConf* BridgeConf::initialize(int argc, char* argv[])
00040 {
00041   bridgeConf = new BridgeConf(argc ,argv);
00042   return bridgeConf;
00043 }
00044 
00045   
00046 BridgeConf* BridgeConf::instance()
00047 {
00048   return bridgeConf;
00049 }
00050 
00051 
00052 BridgeConf::BridgeConf(int argc, char* argv[]) :
00053   options("Allowed options"),
00054   commandLineOptions("Allowed options")
00055 {
00056   isReady_ = false;
00057   initOptionsDescription();
00058   initLabelToDataTypeMap();
00059 
00060   setPreLoadModuleInfo();
00061   parseCommandLineOptions(argc, argv);
00062 }
00063 
00064 
00065 BridgeConf::~BridgeConf()
00066 {
00067 
00068 }
00069 
00070 
00071 void BridgeConf::initOptionsDescription()
00072 {
00073   options.add_options()
00074 
00075     ("name-server",
00076      program_options::value<string>()->default_value("127.0.0.1:2809"),
00077      "Nameserver used by OpenHRP (hostname:port)")
00078 
00079     ("server-name",
00080      program_options::value<string>()->default_value("ControllerBridge"),
00081      "Name of the OpenHRP controller factory server")
00082 
00083     ("robot-name",
00084      program_options::value<string>()->default_value(""),
00085      "Name of the virtual robot RTC type")
00086 
00087     ("module",
00088      program_options::value<vector<string> >(),
00089      "Module filename")
00090 
00091     ("out-port",
00092      program_options::value<vector<string> >(),
00093      "Set an out-port that transfers data from the simulator to the controller")
00094 
00095     ("in-port",
00096      program_options::value<vector<string> >(),
00097      "Set an in-port transfers data from the controller to the simulator")
00098 
00099     ("connection",
00100      program_options::value<vector<string> >(),
00101      "Port connection setting")
00102 
00103     ("periodic-rate",
00104      program_options::value<vector<string> >(), 
00105      "Periodic rate of execution context (INSTANCE_NAME:TIME_RATE[<=1.0])");
00106 
00107   commandLineOptions.add(options).add_options()
00108 
00109     ("config-file",
00110      program_options::value<string>(), 
00111      "configuration file of the controller bridge")
00112     
00113     ("help,h", "Show this help");
00114 }
00115 
00116 
00117 void BridgeConf::initLabelToDataTypeMap()
00118 {
00119   LabelToDataTypeIdMap& m = labelToDataTypeIdMap;
00120   
00121   m["JOINT_VALUE"]         = JOINT_VALUE;
00122   m["JOINT_VELOCITY"]      = JOINT_VELOCITY;
00123   m["JOINT_ACCELERATION"]  = JOINT_ACCELERATION;
00124   m["JOINT_TORQUE"]        = JOINT_TORQUE;
00125   m["EXTERNAL_FORCE"]      = EXTERNAL_FORCE;
00126   m["ABS_TRANSFORM"]       = ABS_TRANSFORM;
00127   m["ABS_VELOCITY"]        = ABS_VELOCITY;
00128   m["ABS_ACCELERATION"]    = ABS_ACCELERATION;
00129   m["FORCE_SENSOR"]        = FORCE_SENSOR;
00130   m["RATE_GYRO_SENSOR"]    = RATE_GYRO_SENSOR;
00131   m["ACCELERATION_SENSOR"] = ACCELERATION_SENSOR;
00132   m["COLOR_IMAGE"]         = COLOR_IMAGE;
00133   m["GRAYSCALE_IMAGE"]     = GRAYSCALE_IMAGE;
00134   m["DEPTH_IMAGE"]         = DEPTH_IMAGE;
00135   m["RANGE_SENSOR"]            = RANGE_SENSOR;
00136   m["CONSTRAINT_FORCE"]    = CONSTRAINT_FORCE;
00137   m["RATE_GYRO_SENSOR2"]    = RATE_GYRO_SENSOR2;
00138   m["ACCELERATION_SENSOR2"] = ACCELERATION_SENSOR2;
00139   m["ABS_TRANSFORM2"]       = ABS_TRANSFORM2;
00140 }
00141 
00142 
00143 void BridgeConf::parseCommandLineOptions(int argc, char* argv[])
00144 {
00145   isProcessingConfigFile = false;
00146 
00147   program_options::parsed_options parsed = 
00148       program_options::command_line_parser(argc, argv).options(commandLineOptions).allow_unregistered().run();      
00149   program_options::store(parsed, vmap); 
00150 
00151   if(vmap.count("help")){
00152     cout << commandLineOptions << endl;
00153   } else {
00154 
00155     if(vmap.count("config-file")){
00156       string fileName(vmap["config-file"].as<string>());
00157       ifstream ifs(fileName.c_str());
00158       
00159       if (ifs.fail()) {
00160             throw invalid_argument(string("cannot open the config file"));
00161       }
00162       program_options::store(program_options::parse_config_file(ifs, options), vmap);
00163       
00164       isProcessingConfigFile = true;
00165     }
00166     program_options::notify(vmap);
00167     
00168     parseOptions();
00169 
00170     isReady_ = true;
00171   }
00172 }
00173 
00174 
00175 void BridgeConf::parseOptions()
00176 {
00177   if(vmap.count("module")){
00178     vector<string> values = vmap["module"].as<vector<string> >();
00179     for(size_t i=0; i < values.size(); ++i){
00180       string modulePath( values[i] );
00181       if( filesystem::extension(filesystem::path( modulePath )).empty() )
00182       {
00183         modulePath += string( SUFFIX_SHARED_EXT );
00184       }
00185       addModuleInfo( modulePath );
00186     }
00187   }
00188 
00189   if(vmap.count("out-port")){
00190     setPortInfos("out-port", outPortInfos);
00191   }
00192 
00193   if(vmap.count("in-port")){
00194     setPortInfos("in-port", inPortInfos);
00195   }
00196 
00197   if(vmap.count("connection")){
00198     vector<string> values = vmap["connection"].as<vector<string> >();
00199     for(size_t i=0; i < values.size(); ++i){
00200       addPortConnection(values[i]);
00201     }
00202   }
00203 
00204   string server(expandEnvironmentVariables(vmap["name-server"].as<string>()));
00205   nameServerIdentifier = string("corbaloc:iiop:") + server + "/NameService";
00206 
00207   controllerName = expandEnvironmentVariables(vmap["server-name"].as<string>());
00208 
00209   virtualRobotRtcTypeName = expandEnvironmentVariables(vmap["robot-name"].as<string>());
00210   if(virtualRobotRtcTypeName==""){
00211       virtualRobotRtcTypeName=controllerName;
00212       virtualRobotRtcTypeName.append("(Robot)");
00213   }
00214 
00215   if(vmap.count("periodic-rate")){
00216     vector<string> values = vmap["periodic-rate"].as<vector<string> >();
00217     for(size_t i=0; i < values.size(); ++i){
00218       addTimeRateInfo(values[i]);
00219     }
00220   }
00221 }
00222 
00223 
00224 void BridgeConf::setPortInfos(const char* optionLabel, PortInfoMap& portInfos)
00225 {
00226   vector<string> ports = vmap[optionLabel].as<vector<string> >();
00227   for(size_t i=0; i < ports.size(); ++i){
00228     vector<string> parameters = extractParameters(ports[i]);
00229     int n = parameters.size();
00230     if(n < 2 || n > 4){
00231       throw invalid_argument(string("invalid in port setting"));
00232     }
00233     PortInfo info;
00234 
00235     info.portName = parameters[0];
00236     int j;
00237     for(j=1; j<3; j++){
00238         LabelToDataTypeIdMap::iterator it = labelToDataTypeIdMap.find(parameters[j]);
00239         if(it == labelToDataTypeIdMap.end() ){     // プロパティ名でないので識別名  //
00240             if(j==2)    // 3番目までにプロパティ名がないのでエラー //
00241                 throw invalid_argument(string("invalid data type"));
00242             string st=parameters[j];
00243             vector<string> owners = extractParameters(st, ',');
00244             for(size_t i=0; i<owners.size(); i++){
00245                 bool digit=true;
00246                 for(string::iterator itr=owners[i].begin(); itr!=owners[i].end(); itr++){
00247                     if(!isdigit(*itr)){
00248                         digit = false;             
00249                         break;
00250                     }
00251                 }
00252                 if(digit){  //識別名が数字なので画像データ 2つ以上指定するとエラー //
00253                     info.dataOwnerId = atoi(owners[i].c_str());
00254                     info.dataOwnerName.clear();
00255                     if(owners.size() > 1){
00256                         throw invalid_argument(string("invalid VisionSensor setting"));
00257                     }
00258                 }else{      //識別名が名前  //
00259                     info.dataOwnerId = -1;
00260                     info.dataOwnerName.push_back(owners[i]);
00261                 }
00262             }
00263         }else{  // プロパティ名 //
00264             info.dataTypeId = it->second;
00265             j++;
00266             break;
00267         }
00268     }
00269     if(j<n){    // まだパラメターがあるならサンプリング時間 //
00270         info.stepTime = atof(parameters[j].c_str());
00271     }else{
00272         info.stepTime = 0;
00273     }
00274 
00275     // プロパティがCONSTRAINT_FORCEのとき 識別名が2つ以上あってはいけない    //
00276     if(info.dataTypeId==CONSTRAINT_FORCE && info.dataOwnerName.size() > 1 )
00277         throw invalid_argument(string("invalid in port setting"));
00278     portInfos.insert(make_pair(info.portName, info));
00279   }
00280 }
00281 
00282 
00283 void BridgeConf::addPortConnection(const std::string& value)
00284 {
00285   vector<string> parameters = extractParameters(value);
00286   int n = parameters.size();
00287   if(n < 2 || n > 3){
00288     throw std::invalid_argument(string("Invalied port connection"));
00289   }
00290   PortConnection connection;
00291   connection.robotPortName = parameters[0];
00292   
00293   if(parameters.size() == 2){
00294     if(moduleInfoList.size() != 1){
00295       throw std::invalid_argument(string("RTC instance name must be specified in a connection option"));
00296     }
00297     connection.controllerPortName = parameters[1];
00298   } else {
00299     connection.controllerInstanceName = parameters[1];
00300     connection.controllerPortName = parameters[2];
00301   }
00302   portConnections.push_back(connection);
00303 }
00304 
00305 void BridgeConf::setPreLoadModuleInfo()
00306 {
00307     RTC::Manager& rtcManager = RTC::Manager::instance();
00308     std::vector<RTC::RtcBase*> components = rtcManager.getComponents();
00309     for(std::vector<RTC::RtcBase*>::iterator it=components.begin(); it != components.end(); ++it){ 
00310         ModuleInfo info;
00311         info.componentName = (*it)->get_component_profile()->type_name;
00312         info.fileName = "";
00313         info.initFuncName = "";
00314         info.isLoaded = true;
00315         info.rtcServant = *it;
00316         moduleInfoList.push_back(info);
00317     }
00318 }
00319 
00320 void BridgeConf::addModuleInfo(const std::string& value)
00321 {
00322   vector<string> parameters = extractParameters(value);
00323 
00324   if(parameters.size() == 0 || parameters.size() > 2){
00325     throw std::invalid_argument(std::string("invalid module set"));
00326   } else {
00327     ModuleInfo info;
00328     info.fileName = parameters[0];
00329     info.componentName = filesystem::basename(filesystem::path(info.fileName));
00330     if(parameters.size() == 1){
00331       info.initFuncName = info.componentName + "Init";
00332     } else {
00333       info.initFuncName = parameters[1];
00334     }
00335     info.isLoaded = false;
00336     
00337     moduleInfoList.push_back(info);
00338   }
00339 }
00340 
00341 void BridgeConf::addTimeRateInfo(const std::string& value) 
00342 {
00343   vector<string> parameters = extractParameters(value);
00344   if (parameters.size() == 0 || parameters.size() > 2) {
00345     throw std::invalid_argument(std::string("invalid time rate set"));
00346   } else {
00347     timeRateMap.insert( std::map<std::string, double>::value_type(parameters[0], atof(parameters[1].c_str())) );
00348   }
00349 }
00350 
00351 void BridgeConf::setupModules()
00352 {
00353   RTC::Manager& rtcManager = RTC::Manager::instance();
00354   ModuleInfoList::iterator moduleInfo = moduleInfoList.begin();
00355   while(moduleInfo != moduleInfoList.end()){
00356     if(!moduleInfo->isLoaded){
00357       rtcManager.load(moduleInfo->fileName.c_str(), moduleInfo->initFuncName.c_str());
00358       moduleInfo->isLoaded = true;
00359       moduleInfo->rtcServant = rtcManager.createComponent(moduleInfo->componentName.c_str());
00360     }
00361     ++moduleInfo;
00362   }
00363 }
00364 
00365 
00366 const char* BridgeConf::getOpenHRPNameServerIdentifier()
00367 {
00368   return nameServerIdentifier.c_str();
00369 }
00370 
00371 
00372 const char* BridgeConf::getControllerName()
00373 {
00374   return controllerName.c_str();
00375 }
00376 
00377 
00378 const char* BridgeConf::getVirtualRobotRtcTypeName()
00379 {
00380   return virtualRobotRtcTypeName.c_str();
00381 }
00382 
00383 std::vector<std::string> BridgeConf::extractParameters(const std::string& str, const char delimiter )
00384 {
00385   vector<string> result;
00386   string::size_type sepPos = 0;
00387   string::size_type nowPos = 0;
00388   
00389   while (sepPos != string::npos) {
00390     sepPos = str.find(delimiter, nowPos);
00391 
00392     if(isProcessingConfigFile){
00393       result.push_back(expandEnvironmentVariables(str.substr(nowPos, sepPos-nowPos)));
00394     } else {
00395       result.push_back(str.substr(nowPos, sepPos-nowPos));
00396     }
00397       
00398     nowPos = sepPos+1;
00399   }
00400   
00401   return result;
00402 }
00403 
00404 
00405 std::string BridgeConf::expandEnvironmentVariables(std::string str)
00406 {
00407   regex variablePattern("\\$([A-z][A-z_0-9]*)");
00408     
00409   match_results<string::const_iterator> result; 
00410   match_flag_type flags = match_default; 
00411     
00412   string::const_iterator start, end;
00413   start = str.begin();
00414   end = str.end();
00415   int pos = 0;
00416 
00417   vector< pair< int, int > > results;
00418 
00419   while ( regex_search(start, end, result, variablePattern, flags) ) {
00420     results.push_back(std::make_pair(pos+result.position(1), result.length(1)));
00421 
00422     // seek to the remaining part
00423     start = result[0].second;
00424     pos += result.length(0);
00425                 
00426     flags |= boost::match_prev_avail; 
00427     flags |= boost::match_not_bob; 
00428   }
00429 
00430   // replace the variables in reverse order
00431   while (!results.empty()) {
00432     int begin = results.back().first;
00433     int length = results.back().second;
00434 
00435     string envName = str.substr(begin, length);
00436     char* envValue = getenv(envName.c_str());
00437 
00438     if(envValue){
00439       str.replace(begin-1, length+1, string(envValue));
00440     }
00441     results.pop_back();
00442   }
00443 
00444   return str;
00445 }


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:15