BridgeConf.cpp
Go to the documentation of this file.
1 /*
2  * Copyright (c) 2008, AIST, the University of Tokyo and General Robotix Inc.
3  * All rights reserved. This program is made available under the terms of the
4  * Eclipse Public License v1.0 which accompanies this distribution, and is
5  * available at http://www.eclipse.org/legal/epl-v10.html
6  * Contributors:
7  * National Institute of Advanced Industrial Science and Technology (AIST)
8  * General Robotix Inc.
9  */
15 #include "BridgeConf.h"
16 
17 #include <iostream>
18 #include <fstream>
19 #include <boost/regex.hpp>
20 
21 #if (BOOST_VERSION <= 103301)
22 #include <boost/filesystem/path.hpp>
23 #include <boost/filesystem/operations.hpp>
24 #include <boost/filesystem/convenience.hpp>
25 #else
26 #include <boost/filesystem.hpp>
27 #endif
28 
29 
30 using namespace std;
31 using namespace boost;
32 
33 
34 namespace {
35  BridgeConf* bridgeConf = 0;
36 }
37 
38 
39 BridgeConf* BridgeConf::initialize(int argc, char* argv[])
40 {
41  bridgeConf = new BridgeConf(argc ,argv);
42  return bridgeConf;
43 }
44 
45 
47 {
48  return bridgeConf;
49 }
50 
51 
52 BridgeConf::BridgeConf(int argc, char* argv[]) :
53  options("Allowed options"),
54  commandLineOptions("Allowed options")
55 {
56  isReady_ = false;
59 
61  parseCommandLineOptions(argc, argv);
62 }
63 
64 
66 {
67 
68 }
69 
70 
72 {
73  options.add_options()
74 
75  ("name-server",
76  program_options::value<string>()->default_value("127.0.0.1:2809"),
77  "Nameserver used by OpenHRP (hostname:port)")
78 
79  ("server-name",
80  program_options::value<string>()->default_value("ControllerBridge"),
81  "Name of the OpenHRP controller factory server")
82 
83  ("robot-name",
84  program_options::value<string>()->default_value(""),
85  "Name of the virtual robot RTC type")
86 
87  ("module",
88  program_options::value<vector<string> >(),
89  "Module filename")
90 
91  ("out-port",
92  program_options::value<vector<string> >(),
93  "Set an out-port that transfers data from the simulator to the controller")
94 
95  ("in-port",
96  program_options::value<vector<string> >(),
97  "Set an in-port transfers data from the controller to the simulator")
98 
99  ("connection",
100  program_options::value<vector<string> >(),
101  "Port connection setting")
102 
103  ("periodic-rate",
104  program_options::value<vector<string> >(),
105  "Periodic rate of execution context (INSTANCE_NAME:TIME_RATE[<=1.0])");
106 
107  commandLineOptions.add(options).add_options()
108 
109  ("config-file",
110  program_options::value<string>(),
111  "configuration file of the controller bridge")
112 
113  ("help,h", "Show this help");
114 }
115 
116 
118 {
120 
121  m["JOINT_VALUE"] = JOINT_VALUE;
122  m["JOINT_VELOCITY"] = JOINT_VELOCITY;
123  m["JOINT_ACCELERATION"] = JOINT_ACCELERATION;
124  m["JOINT_TORQUE"] = JOINT_TORQUE;
125  m["EXTERNAL_FORCE"] = EXTERNAL_FORCE;
126  m["ABS_TRANSFORM"] = ABS_TRANSFORM;
127  m["ABS_VELOCITY"] = ABS_VELOCITY;
128  m["ABS_ACCELERATION"] = ABS_ACCELERATION;
129  m["FORCE_SENSOR"] = FORCE_SENSOR;
130  m["RATE_GYRO_SENSOR"] = RATE_GYRO_SENSOR;
131  m["ACCELERATION_SENSOR"] = ACCELERATION_SENSOR;
132  m["COLOR_IMAGE"] = COLOR_IMAGE;
133  m["GRAYSCALE_IMAGE"] = GRAYSCALE_IMAGE;
134  m["DEPTH_IMAGE"] = DEPTH_IMAGE;
135  m["RANGE_SENSOR"] = RANGE_SENSOR;
136  m["CONSTRAINT_FORCE"] = CONSTRAINT_FORCE;
137  m["RATE_GYRO_SENSOR2"] = RATE_GYRO_SENSOR2;
138  m["ACCELERATION_SENSOR2"] = ACCELERATION_SENSOR2;
139  m["ABS_TRANSFORM2"] = ABS_TRANSFORM2;
140 }
141 
142 
143 void BridgeConf::parseCommandLineOptions(int argc, char* argv[])
144 {
145  isProcessingConfigFile = false;
146 
147  program_options::parsed_options parsed =
148  program_options::command_line_parser(argc, argv).options(commandLineOptions).allow_unregistered().run();
149  program_options::store(parsed, vmap);
150 
151  if(vmap.count("help")){
152  cout << commandLineOptions << endl;
153  } else {
154 
155  if(vmap.count("config-file")){
156  string fileName(vmap["config-file"].as<string>());
157  ifstream ifs(fileName.c_str());
158 
159  if (ifs.fail()) {
160  throw invalid_argument(string("cannot open the config file"));
161  }
162  program_options::store(program_options::parse_config_file(ifs, options), vmap);
163 
164  isProcessingConfigFile = true;
165  }
166  program_options::notify(vmap);
167 
168  parseOptions();
169 
170  isReady_ = true;
171  }
172 }
173 
174 
176 {
177  if(vmap.count("module")){
178  vector<string> values = vmap["module"].as<vector<string> >();
179  for(size_t i=0; i < values.size(); ++i){
180  string modulePath( values[i] );
181  if( filesystem::extension(filesystem::path( modulePath )).empty() )
182  {
183  modulePath += string( SUFFIX_SHARED_EXT );
184  }
185  addModuleInfo( modulePath );
186  }
187  }
188 
189  if(vmap.count("out-port")){
190  setPortInfos("out-port", outPortInfos);
191  }
192 
193  if(vmap.count("in-port")){
194  setPortInfos("in-port", inPortInfos);
195  }
196 
197  if(vmap.count("connection")){
198  vector<string> values = vmap["connection"].as<vector<string> >();
199  for(size_t i=0; i < values.size(); ++i){
200  addPortConnection(values[i]);
201  }
202  }
203 
204  string server(expandEnvironmentVariables(vmap["name-server"].as<string>()));
205  nameServerIdentifier = string("corbaloc:iiop:") + server + "/NameService";
206 
207  controllerName = expandEnvironmentVariables(vmap["server-name"].as<string>());
208 
209  virtualRobotRtcTypeName = expandEnvironmentVariables(vmap["robot-name"].as<string>());
210  if(virtualRobotRtcTypeName==""){
212  virtualRobotRtcTypeName.append("(Robot)");
213  }
214 
215  if(vmap.count("periodic-rate")){
216  vector<string> values = vmap["periodic-rate"].as<vector<string> >();
217  for(size_t i=0; i < values.size(); ++i){
218  addTimeRateInfo(values[i]);
219  }
220  }
221 }
222 
223 
224 void BridgeConf::setPortInfos(const char* optionLabel, PortInfoMap& portInfos)
225 {
226  vector<string> ports = vmap[optionLabel].as<vector<string> >();
227  for(size_t i=0; i < ports.size(); ++i){
228  vector<string> parameters = extractParameters(ports[i]);
229  int n = parameters.size();
230  if(n < 2 || n > 4){
231  throw invalid_argument(string("invalid in port setting"));
232  }
233  PortInfo info;
234 
235  info.portName = parameters[0];
236  int j;
237  for(j=1; j<3; j++){
238  LabelToDataTypeIdMap::iterator it = labelToDataTypeIdMap.find(parameters[j]);
239  if(it == labelToDataTypeIdMap.end() ){ // プロパティ名でないので識別名  //
240  if(j==2) // 3番目までにプロパティ名がないのでエラー //
241  throw invalid_argument(string("invalid data type"));
242  string st=parameters[j];
243  vector<string> owners = extractParameters(st, ',');
244  for(size_t i=0; i<owners.size(); i++){
245  bool digit=true;
246  for(string::iterator itr=owners[i].begin(); itr!=owners[i].end(); itr++){
247  if(!isdigit(*itr)){
248  digit = false;
249  break;
250  }
251  }
252  if(digit){ //識別名が数字なので画像データ 2つ以上指定するとエラー //
253  info.dataOwnerId = atoi(owners[i].c_str());
254  info.dataOwnerName.clear();
255  if(owners.size() > 1){
256  throw invalid_argument(string("invalid VisionSensor setting"));
257  }
258  }else{ //識別名が名前 //
259  info.dataOwnerId = -1;
260  info.dataOwnerName.push_back(owners[i]);
261  }
262  }
263  }else{ // プロパティ名 //
264  info.dataTypeId = it->second;
265  j++;
266  break;
267  }
268  }
269  if(j<n){ // まだパラメターがあるならサンプリング時間 //
270  info.stepTime = atof(parameters[j].c_str());
271  }else{
272  info.stepTime = 0;
273  }
274 
275  // プロパティがCONSTRAINT_FORCEのとき 識別名が2つ以上あってはいけない  //
276  if(info.dataTypeId==CONSTRAINT_FORCE && info.dataOwnerName.size() > 1 )
277  throw invalid_argument(string("invalid in port setting"));
278  portInfos.insert(make_pair(info.portName, info));
279  }
280 }
281 
282 
283 void BridgeConf::addPortConnection(const std::string& value)
284 {
285  vector<string> parameters = extractParameters(value);
286  int n = parameters.size();
287  if(n < 2 || n > 3){
288  throw std::invalid_argument(string("Invalied port connection"));
289  }
290  PortConnection connection;
291  connection.robotPortName = parameters[0];
292 
293  if(parameters.size() == 2){
294  if(moduleInfoList.size() != 1){
295  throw std::invalid_argument(string("RTC instance name must be specified in a connection option"));
296  }
297  connection.controllerPortName = parameters[1];
298  } else {
299  connection.controllerInstanceName = parameters[1];
300  connection.controllerPortName = parameters[2];
301  }
302  portConnections.push_back(connection);
303 }
304 
306 {
307  RTC::Manager& rtcManager = RTC::Manager::instance();
308  std::vector<RTC::RtcBase*> components = rtcManager.getComponents();
309  for(std::vector<RTC::RtcBase*>::iterator it=components.begin(); it != components.end(); ++it){
311  info.componentName = (*it)->get_component_profile()->type_name;
312  info.fileName = "";
313  info.initFuncName = "";
314  info.isLoaded = true;
315  info.rtcServant = *it;
316  moduleInfoList.push_back(info);
317  }
318 }
319 
320 void BridgeConf::addModuleInfo(const std::string& value)
321 {
322  vector<string> parameters = extractParameters(value);
323 
324  if(parameters.size() == 0 || parameters.size() > 2){
325  throw std::invalid_argument(std::string("invalid module set"));
326  } else {
328  info.fileName = parameters[0];
329  info.componentName = filesystem::basename(filesystem::path(info.fileName));
330  if(parameters.size() == 1){
331  info.initFuncName = info.componentName + "Init";
332  } else {
333  info.initFuncName = parameters[1];
334  }
335  info.isLoaded = false;
336 
337  moduleInfoList.push_back(info);
338  }
339 }
340 
341 void BridgeConf::addTimeRateInfo(const std::string& value)
342 {
343  vector<string> parameters = extractParameters(value);
344  if (parameters.size() == 0 || parameters.size() > 2) {
345  throw std::invalid_argument(std::string("invalid time rate set"));
346  } else {
347  timeRateMap.insert( std::map<std::string, double>::value_type(parameters[0], atof(parameters[1].c_str())) );
348  }
349 }
350 
352 {
353  RTC::Manager& rtcManager = RTC::Manager::instance();
354  ModuleInfoList::iterator moduleInfo = moduleInfoList.begin();
355  while(moduleInfo != moduleInfoList.end()){
356  if(!moduleInfo->isLoaded){
357  rtcManager.load(moduleInfo->fileName.c_str(), moduleInfo->initFuncName.c_str());
358  moduleInfo->isLoaded = true;
359  moduleInfo->rtcServant = rtcManager.createComponent(moduleInfo->componentName.c_str());
360  }
361  ++moduleInfo;
362  }
363 }
364 
365 
367 {
368  return nameServerIdentifier.c_str();
369 }
370 
371 
373 {
374  return controllerName.c_str();
375 }
376 
377 
379 {
380  return virtualRobotRtcTypeName.c_str();
381 }
382 
383 std::vector<std::string> BridgeConf::extractParameters(const std::string& str, const char delimiter )
384 {
385  vector<string> result;
386  string::size_type sepPos = 0;
387  string::size_type nowPos = 0;
388 
389  while (sepPos != string::npos) {
390  sepPos = str.find(delimiter, nowPos);
391 
393  result.push_back(expandEnvironmentVariables(str.substr(nowPos, sepPos-nowPos)));
394  } else {
395  result.push_back(str.substr(nowPos, sepPos-nowPos));
396  }
397 
398  nowPos = sepPos+1;
399  }
400 
401  return result;
402 }
403 
404 
405 std::string BridgeConf::expandEnvironmentVariables(std::string str)
406 {
407  regex variablePattern("\\$([A-z][A-z_0-9]*)");
408 
409  match_results<string::const_iterator> result;
410  match_flag_type flags = match_default;
411 
412  string::const_iterator start, end;
413  start = str.begin();
414  end = str.end();
415  int pos = 0;
416 
417  vector< pair< int, int > > results;
418 
419  while ( regex_search(start, end, result, variablePattern, flags) ) {
420  results.push_back(std::make_pair(pos+result.position(1), result.length(1)));
421 
422  // seek to the remaining part
423  start = result[0].second;
424  pos += result.length(0);
425 
426  flags |= boost::match_prev_avail;
427  flags |= boost::match_not_bob;
428  }
429 
430  // replace the variables in reverse order
431  while (!results.empty()) {
432  int begin = results.back().first;
433  int length = results.back().second;
434 
435  string envName = str.substr(begin, length);
436  char* envValue = getenv(envName.c_str());
437 
438  if(envValue){
439  str.replace(begin-1, length+1, string(envValue));
440  }
441  results.pop_back();
442  }
443 
444  return str;
445 }
RATE_GYRO_SENSOR2
@ RATE_GYRO_SENSOR2
Definition: BridgeConf.h:52
basename
std::string basename(const std::string name)
Definition: projectGenerator.cpp:33
BridgeConf::BridgeConf
BridgeConf(int argc, char *argv[])
Definition: BridgeConf.cpp:52
BridgeConf::initialize
static BridgeConf * initialize(int argc, char *argv[])
Definition: BridgeConf.cpp:39
BridgeConf::commandLineOptions
boost::program_options::options_description commandLineOptions
Definition: BridgeConf.h:122
i
png_uint_32 i
Definition: png.h:2732
ABS_TRANSFORM2
@ ABS_TRANSFORM2
Definition: BridgeConf.h:54
BridgeConf::outPortInfos
PortInfoMap outPortInfos
Definition: BridgeConf.h:109
RANGE_SENSOR
@ RANGE_SENSOR
Definition: BridgeConf.h:50
DEPTH_IMAGE
@ DEPTH_IMAGE
Definition: BridgeConf.h:49
GRAYSCALE_IMAGE
@ GRAYSCALE_IMAGE
Definition: BridgeConf.h:48
BridgeConf::extractParameters
std::vector< std::string > extractParameters(const std::string &str, const char delimiter=':')
Definition: BridgeConf.cpp:383
BridgeConf::virtualRobotRtcTypeName
std::string virtualRobotRtcTypeName
Definition: BridgeConf.h:127
ModuleInfo
Definition: BridgeConf.h:76
BridgeConf::nameServerIdentifier
std::string nameServerIdentifier
Definition: BridgeConf.h:129
BridgeConf.h
BridgeConf::setPreLoadModuleInfo
void setPreLoadModuleInfo()
Definition: BridgeConf.cpp:305
PortInfoMap
std::map< std::string, PortInfo > PortInfoMap
Definition: BridgeConf.h:65
autoplay.n
n
Definition: autoplay.py:12
BridgeConf::addModuleInfo
void addModuleInfo(const std::string &value)
Definition: BridgeConf.cpp:320
start
png_size_t start
Definition: png.h:1493
BridgeConf::expandEnvironmentVariables
std::string expandEnvironmentVariables(std::string str)
Definition: BridgeConf.cpp:405
boost
Modifications controlling boost library behavior.
Definition: ColladaUtil.h:306
BridgeConf::timeRateMap
TimeRateMap timeRateMap
Definition: BridgeConf.h:116
ACCELERATION_SENSOR2
@ ACCELERATION_SENSOR2
Definition: BridgeConf.h:53
BridgeConf::controllerName
std::string controllerName
Definition: BridgeConf.h:128
BridgeConf::initLabelToDataTypeMap
void initLabelToDataTypeMap()
Definition: BridgeConf.cpp:117
CONSTRAINT_FORCE
@ CONSTRAINT_FORCE
Definition: BridgeConf.h:51
BridgeConf::moduleInfoList
ModuleInfoList moduleInfoList
Definition: BridgeConf.h:112
BridgeConf::~BridgeConf
~BridgeConf()
Definition: BridgeConf.cpp:65
BridgeConf::labelToDataTypeIdMap
LabelToDataTypeIdMap labelToDataTypeIdMap
Definition: BridgeConf.h:107
FORCE_SENSOR
@ FORCE_SENSOR
Definition: BridgeConf.h:44
value
png_voidp int value
Definition: png.h:2110
SUFFIX_SHARED_EXT
#define SUFFIX_SHARED_EXT
Definition: BridgeConf.h:23
BridgeConf::parseCommandLineOptions
void parseCommandLineOptions(int argc, char *argv[])
Definition: BridgeConf.cpp:143
BridgeConf::vmap
boost::program_options::variables_map vmap
Definition: BridgeConf.h:120
BridgeConf::LabelToDataTypeIdMap
std::map< std::string, DataTypeId > LabelToDataTypeIdMap
Definition: BridgeConf.h:106
BridgeConf::getOpenHRPNameServerIdentifier
const char * getOpenHRPNameServerIdentifier()
Definition: BridgeConf.cpp:366
BridgeConf::setPortInfos
void setPortInfos(const char *optionLabel, PortInfoMap &portInfos)
Definition: BridgeConf.cpp:224
ABS_TRANSFORM
@ ABS_TRANSFORM
Definition: BridgeConf.h:41
PortConnection::controllerInstanceName
std::string controllerInstanceName
Definition: BridgeConf.h:70
BridgeConf::initOptionsDescription
void initOptionsDescription()
Definition: BridgeConf.cpp:71
COLOR_IMAGE
@ COLOR_IMAGE
Definition: BridgeConf.h:47
JOINT_VALUE
@ JOINT_VALUE
Definition: BridgeConf.h:36
BridgeConf::getVirtualRobotRtcTypeName
const char * getVirtualRobotRtcTypeName()
Definition: BridgeConf.cpp:378
BridgeConf::addPortConnection
void addPortConnection(const std::string &value)
Definition: BridgeConf.cpp:283
BridgeConf::parseOptions
void parseOptions()
Definition: BridgeConf.cpp:175
ACCELERATION_SENSOR
@ ACCELERATION_SENSOR
Definition: BridgeConf.h:46
JOINT_TORQUE
@ JOINT_TORQUE
Definition: BridgeConf.h:39
BridgeConf::setupModules
void setupModules()
Definition: BridgeConf.cpp:351
BridgeConf::instance
static BridgeConf * instance()
Definition: BridgeConf.cpp:46
RATE_GYRO_SENSOR
@ RATE_GYRO_SENSOR
Definition: BridgeConf.h:45
BridgeConf::inPortInfos
PortInfoMap inPortInfos
Definition: BridgeConf.h:110
BridgeConf::portConnections
PortConnectionList portConnections
Definition: BridgeConf.h:114
flags
png_uint_32 int flags
Definition: png.h:1653
JOINT_ACCELERATION
@ JOINT_ACCELERATION
Definition: BridgeConf.h:38
ABS_ACCELERATION
@ ABS_ACCELERATION
Definition: BridgeConf.h:43
BridgeConf::isReady_
bool isReady_
Definition: BridgeConf.h:124
BridgeConf::getControllerName
const char * getControllerName()
Definition: BridgeConf.cpp:372
BridgeConf::isProcessingConfigFile
bool isProcessingConfigFile
Definition: BridgeConf.h:125
JOINT_VELOCITY
@ JOINT_VELOCITY
Definition: BridgeConf.h:37
BridgeConf::addTimeRateInfo
void addTimeRateInfo(const std::string &value)
Definition: BridgeConf.cpp:341
BridgeConf
Definition: BridgeConf.h:87
info
backing_store_ptr info
Definition: jmemsys.h:181
ABS_VELOCITY
@ ABS_VELOCITY
Definition: BridgeConf.h:42
BridgeConf::options
boost::program_options::options_description options
Definition: BridgeConf.h:121
PortConnection::robotPortName
std::string robotPortName
Definition: BridgeConf.h:69
length
png_bytep png_bytep png_size_t length
Definition: png.h:1538
PortConnection
Definition: BridgeConf.h:68
PortConnection::controllerPortName
std::string controllerPortName
Definition: BridgeConf.h:71
EXTERNAL_FORCE
@ EXTERNAL_FORCE
Definition: BridgeConf.h:40
PortInfo
Definition: BridgeConf.h:57


openhrp3
Author(s): AIST, General Robotix Inc., Nakamura Lab of Dept. of Mechano Informatics at University of Tokyo
autogenerated on Wed Sep 7 2022 02:51:02