parameter_pa_ros.cpp
Go to the documentation of this file.
1 /******************************************************************************
2 * *
3 * parameter_pa_ros.c *
4 * ================== *
5 * *
6 *******************************************************************************
7 * *
8 * github repository *
9 * https://github.com/TUC-ProAut/ros_parameter *
10 * *
11 * Chair of Automation Technology, Technische Universität Chemnitz *
12 * https://www.tu-chemnitz.de/etit/proaut *
13 * *
14 *******************************************************************************
15 * *
16 * New BSD License *
17 * *
18 * Copyright (c) 2015-2018, Peter Weissig, Technische Universität Chemnitz *
19 * All rights reserved. *
20 * *
21 * Redistribution and use in source and binary forms, with or without *
22 * modification, are permitted provided that the following conditions are met: *
23 * * Redistributions of source code must retain the above copyright *
24 * notice, this list of conditions and the following disclaimer. *
25 * * Redistributions in binary form must reproduce the above copyright *
26 * notice, this list of conditions and the following disclaimer in the *
27 * documentation and/or other materials provided with the distribution. *
28 * * Neither the name of the Technische Universität Chemnitz nor the *
29 * names of its contributors may be used to endorse or promote products *
30 * derived from this software without specific prior written permission. *
31 * *
32 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" *
33 * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE *
34 * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE *
35 * ARE DISCLAIMED. IN NO EVENT SHALL <COPYRIGHT HOLDER> BE LIABLE FOR ANY *
36 * DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES *
37 * (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR *
38 * SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER *
39 * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT *
40 * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY *
41 * OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH *
42 * DAMAGE. *
43 * *
44 ******************************************************************************/
45 
46 // local headers
48 
49 // ros headers
50 #include <ros/package.h>
51 #if ROS_VERSION_MINIMUM(1, 12, 0)
52  // everything after ros indigo
53  #include <xmlrpcpp/XmlRpcValue.h>
54 #else //#if ROS_VERSION_MINIMUM(1, 12, 0)
55  // only for ros indigo
56  #include <XmlRpcValue.h>
57 #endif //#if ROS_VERSION_MINIMUM(1, 12, 0)
58 
59 // standard headers
60 #include <sstream>
61 
62 //**************************[load - bool]**************************************
63 bool cParameterPaRos::load (const std::string name,
64  bool &value, const bool print_default) const {
65 
66  bool result;
67 
68  result = nh_.getParam(resolveRessourcename(name), value);
69 
70  loadSub(name, boolToStr(value), print_default, result);
71 
72  return result;
73 }
74 
75 //**************************[load - string]************************************
76 bool cParameterPaRos::load (const std::string name,
77  std::string &value, const bool print_default) const {
78 
79  bool result;
80 
81  result = nh_.getParam(resolveRessourcename(name), value);
82 
83  loadSub(name, value, print_default, result);
84 
85  return result;
86 }
87 
88 //**************************[loadTopic]****************************************
89 bool cParameterPaRos::loadTopic (const std::string name,
90  std::string &value, const bool print_default) const {
91 
92  bool result;
93  std::string value_resolved;
94 
95  result = nh_.getParam(resolveRessourcename(name), value);
96  value_resolved = resolveRessourcename(value);
97 
98  if (value_resolved == value) {
99  loadSub(name, value, print_default, result);
100  } else {
101  loadSub(name, value + " == " + value_resolved, print_default, result);
102  value = value_resolved;
103  }
104 
105  return result;
106 }
107 
108 //**************************[loadPath]*****************************************
109 bool cParameterPaRos::loadPath (const std::string name,
110  std::string &value, const bool print_default) const {
111 
112  bool result;
113 
114  result = nh_.getParam(resolveRessourcename(name), value);
115  replaceFindpack(value);
116 
117  loadSub(name, value, print_default, result);
118 
119  return result;
120 }
121 
122 //**************************[load - int]***************************************
123 bool cParameterPaRos::load (const std::string name,
124  int &value, const bool print_default) const {
125 
126  bool result;
127 
128  result = nh_.getParam(resolveRessourcename(name), value);
129 
130  std::stringstream value_s;
131  value_s << value;
132  loadSub(name, value_s.str(), print_default, result);
133 
134  return result;
135 }
136 
137 //**************************[load - double]************************************
138 bool cParameterPaRos::load (const std::string name,
139  double &value, const bool print_default) const {
140 
141  bool result;
142 
143  result = nh_.getParam(resolveRessourcename(name), value);
144 
145  std::stringstream value_s;
146  value_s << value;
147  loadSub(name, value_s.str(), print_default, result);
148 
149  return result;
150 }
151 
152 //**************************[load - vector<bool>]******************************
153 bool cParameterPaRos::load (const std::string name,
154  std::vector<bool> &value, const bool print_default) const {
155 
156  bool result;
157 
158  std::vector<bool> value_temp;
159  result = nh_.getParam(resolveRessourcename(name), value_temp);
160  if (result) {value = value_temp;}
161 
162  std::stringstream value_s;
163  value_s << "[";
164  if (value.size() > 0) {
165  value_s << boolToStr(value[0]);
166  for (int i = 1; i < value.size(); i++) {
167  value_s << ", " << boolToStr(value[i]);
168  }
169  }
170  value_s << "]";
171  loadSub(name, value_s.str(), print_default, result);
172 
173  return result;
174 }
175 
176 //**************************[load - vector<string>]****************************
177 bool cParameterPaRos::load (const std::string name,
178  std::vector<std::string> &value, const bool print_default) const {
179 
180  bool result;
181 
182  std::vector<std::string> value_temp;
183  result = nh_.getParam(resolveRessourcename(name), value_temp);
184  if (result) {value = value_temp;}
185 
186  std::stringstream value_s;
187  value_s << "[";
188  if (value.size() > 0) {
189  value_s << value[0];
190  for (int i = 1; i < value.size(); i++) {
191  value_s << ", " << value[i];
192  }
193  }
194  value_s << "]";
195  loadSub(name, value_s.str(), print_default, result);
196 
197  return result;
198 }
199 
200 //**************************[load - vector<int>]*******************************
201 bool cParameterPaRos::load (const std::string name,
202  std::vector<int> &value, const bool print_default) const {
203 
204  bool result;
205 
206  std::vector<int> value_temp;
207  result = nh_.getParam(resolveRessourcename(name), value_temp);
208  if (result) {value = value_temp;}
209 
210  std::stringstream value_s;
211  value_s << "[";
212  if (value.size() > 0) {
213  value_s << value[0];
214  for (int i = 1; i < value.size(); i++) {
215  value_s << ", " << value[i];
216  }
217  }
218  value_s << "]";
219  loadSub(name, value_s.str(), print_default, result);
220 
221  return result;
222 }
223 
224 //**************************[load - vector<double>]****************************
225 bool cParameterPaRos::load (const std::string name,
226  std::vector<double> &value, const bool print_default) const {
227 
228  bool result;
229 
230  std::vector<double> value_temp;
231  result = nh_.getParam(resolveRessourcename(name), value_temp);
232  if (result) {value = value_temp;}
233 
234  std::stringstream value_s;
235  value_s << "[";
236  if (value.size() > 0) {
237  value_s << value[0];
238  for (int i = 1; i < value.size(); i++) {
239  value_s << ", " << value[i];
240  }
241  }
242  value_s << "]";
243  loadSub(name, value_s.str(), print_default, result);
244 
245  return result;
246 }
247 
248 //**************************[load - matrix]************************************
249 bool cParameterPaRos::load (const std::string name,
250  Eigen::MatrixXf &value, const bool print_default) const {
251 
252  bool result = true;
253 
254  Eigen::MatrixXf mat;
255 
257  nh_.getParam(resolveRessourcename(name), xml);
258 
259  // check type and minimum size
260  if((xml.getType() != XmlRpc::XmlRpcValue::TypeArray) ||
261  (xml.size() < 1)) {
262  result = false;
263  } else {
264  for (int y = 0; y < xml.size(); y++) {
265  // check type and minimum size
266  if ((xml[y].getType() != XmlRpc::XmlRpcValue::TypeArray) ||
267  (xml[y].size() < 1)) {
268  result = false;
269  break;
270  }
271 
272  // set and check size
273  if (y == 0) {
274  mat.resize((int) xml.size(),(int) xml[y].size());
275  } else if (xml[y].size() != mat.cols()){
276  result = false;
277  break;
278  }
279 
280  for (int x = 0; x < xml[y].size(); x++) {
281  if (result == false) {break;}
282 
283  switch (xml[y][x].getType()) {
285  mat(y,x) = (double) xml[y][x];
286  break;
288  mat(y,x) = (int) xml[y][x];
289  break;
290 
291  default:
292  result = false;
293  break;
294  }
295  }
296  if (result == false) {
297  break;
298  }
299  }
300  }
301 
302  if (result) {value = mat;}
303 
304  std::stringstream value_s;
305  value_s << '[';
306  for (int y = 0; y < value.rows(); y++) {
307  if (y > 0) { value_s << ", ";}
308  value_s << '[';
309  for (int x = 0; x < value.cols(); x++) {
310  if (x > 0) { value_s << ", ";}
311  value_s << value(y,x);
312  }
313  value_s << ']';
314  }
315  value_s << ']';
316 
317  // Vergleich
318  value_s << std::endl << "Vergleich:" << std::endl << value;
319 
320  loadSub(name, value_s.str(), print_default, result);
321 
322  return result;
323 }
324 
325 //**************************[replaceFindpack]**********************************
326 bool cParameterPaRos::replaceFindpack(std::string &path) {
327 
328  while(1) {
329  // check for string "$(find ...)"
330  std::string::size_type start;
331  start = (int) path.find("$(find ");
332  if ((start < 0) || (start >= path.length())) {return true;}
333 
334  std::string::size_type end;
335  end = path.find(")", (std::string::size_type) start);
336  if ((end < 0) || (end >= path.length())) {return false;}
337 
338  // get replacement
339  std::string replace;
340  try {
341  replace = ros::package::command(
342  path.substr(start + 2, end - start - 2));
343  } catch (std::runtime_error &e) {
344  return false;
345  }
346  if (replace == "") { return false;}
347 
348  // extract white spaces from replacement
349  std::string::size_type pos_save = 0;
350  std::string::size_type pos_current;
351  for (pos_current = 0; pos_current < replace.length(); pos_current++) {
352  char c;
353  c = replace[pos_current];
354 
355  if ((c == '\r') || (c == '\n') || (c == ' ') || (c == '\t')) {
356  continue;
357  }
358 
359  if (pos_save != pos_current) {
360  replace[pos_save] = c;
361  }
362  pos_save++;
363  }
364  if (pos_save != replace.length()) {
365  replace.resize(pos_save);
366  }
367 
368  // replace
369  path = path.substr(0,start) + replace + path.substr(end + 1);
370  }
371 }
372 
373 //**************************[resolveRessourcename]*****************************
374 std::string cParameterPaRos::resolveRessourcename(const std::string name) {
375 
376  if (name == "") {return "";}
377 
378  bool full_path = false;
379  bool end_with_slash = name[name.length()-1] == '/';
380  std::list<std::string> parts = splitRessourcename(name);
381 
382  if (parts.empty()) {
383  return "";
384  }
385 
386  if (parts.front() == "" ) {
387  full_path = true;
388  parts.pop_front();
389  } else if (parts.front() == "." ) {
390  parts.front() = "..";
391  parts.push_front("~");
392  }
393 
394  std::list<std::string>::iterator iter = parts.begin();
395  while (iter != parts.end()) {
396 
397  // check for expansion to full ressource name
398  if (((parts.front() == "~") || (parts.front() == "")) &&
399  (full_path == false)) {
400 
401  // get full ressource name (including node name)
402  std::list<std::string> temp = splitRessourcename(
403  ros::names::resolve("~"));
404  std::list<std::string>::iterator iter_temp = temp.end();
405  iter_temp--; // to point to last element (not beyond)
406 
407  // remove current node name (if not wanted)
408  // This happens only if absolute ressource name is implicitly
409  // needed - e.g. by calling with "../xyz".
410  if (parts.front() != "~") {
411  iter_temp--;
412  }
413 
414  // removing part, which was expanded
415  parts.pop_front();
416 
417  for ( ; iter_temp != temp.end(); iter_temp--) {
418  parts.push_front(*iter_temp);
419  }
420 
421  // updated variables
422  full_path = true;
423  iter = parts.begin();
424  continue;
425  }
426 
427  // check for "" or "."
428  if ((*iter == "") or (*iter == ".")) {
429  iter = parts.erase(iter);
430  continue;
431  }
432 
433  // check for "../"
434  if (*iter == "..") {
435  if (iter == parts.begin()) {
436  if (full_path) {
437  // there is no higher ressource name then "/"
438  // so a "/../xyz" will result in "/xyz"
439  iter = parts.erase(iter);
440  continue;
441  } else {
442  // force full ressource name before continueing
443  parts.push_front("");
444  continue;
445  }
446  }
447  // remove last ressource name
448  iter--;
449  iter = parts.erase(iter);
450  iter = parts.erase(iter);
451  continue;
452  }
453 
454  iter++;
455  }
456 
457  // concate resulting string
458  std::string result;
459  if (full_path) { result = "/";}
460 
461  if (parts.size() > 0) {
462  result+= parts.front();
463  }
464  for (iter = ++parts.begin() ; iter != parts.end(); iter++) {
465  result+= '/';
466  result+= *iter;
467  }
468 
469  if (end_with_slash && (parts.size() > 0)) {
470  result+= '/';
471  }
472 
473  return result;
474 }
475 
476 //**************************[boolToStr]****************************************
477 std::string cParameterPaRos::boolToStr(const bool value) {
478 
479  if (value) {
480  return "true";
481  } else {
482  return "false";
483  }
484 }
485 
487 
488 //**************************[load_topic]***************************************
489 bool cParameterPaRos::load_topic (const std::string name,
490  std::string &value, const bool print_default) const {
491 
492  return loadTopic(name, value, print_default);
493 }
494 
495 //**************************[load_path]****************************************
496 bool cParameterPaRos::load_path (const std::string name,
497  std::string &value, const bool print_default) const {
498 
499  return loadPath(name, value, print_default);
500 }
501 
502 //**************************[replace_findpack]*********************************
503 bool cParameterPaRos::replace_findpack(std::string &path) {
504 
505  return replaceFindpack(path);
506 }
507 
508 //**************************[resolve_ressourcename]****************************
509 void cParameterPaRos::resolve_ressourcename(std::string &name) {
510 
511  name = resolveRessourcename(name);
512 }
513 
514 //**************************[bool_to_str]**************************************
515 std::string cParameterPaRos::bool_to_str(const bool value) {
516 
517  return boolToStr(value);
518 }
519 
521 //**************************[loadSub]******************************************
522 void cParameterPaRos::loadSub(const std::string &n, const std::string &v,
523  const bool p, const bool r) const {
524 
525  std::string result;
526 
527  if (r) {
528  result = "load parameter " + n + " (" + v + ")";
529  } else {
530  result = "parameter " + n + " not set";
531 
532  if (p) {
533  result+= " (defaults to " + v + ")";
534  }
535  }
536 
537  ROS_INFO_STREAM(result);
538 }
539 
540 //**************************[splitRessourcename]*******************************
541 std::list<std::string> cParameterPaRos::splitRessourcename(
542  const std::string name) {
543 
544  std::list<std::string> result;
545  int count = 0; // number of parts (seperated by slashes)
546  std::string::size_type pos = 0; // current position within string
547 
548  while(pos < name.length()) {
549  // check for string "/"
550  std::string::size_type end;
551  end = (int) name.find("/", pos);
552 
553  if ((end < 0) || (end > name.length())) {
554  end = name.length();
555  }
556 
557  if (pos < end) {
558  result.push_back(name.substr(pos, end - pos));
559  } else {
560  result.push_back("");
561  }
562 
563  pos = end + 1;
564  }
565 
566  return result;
567 }
ROSCPP_DECL void start()
int size() const
bool loadPath(const std::string name, std::string &value, const bool print_default=true) const
bool load_path(const std::string name, std::string &value, const bool print_default=true) const
ROSCPP_DECL std::string resolve(const std::string &name, bool remap=true)
ros::NodeHandle nh_
Type const & getType() const
bool loadTopic(const std::string name, std::string &value, const bool print_default=true) const
bool load_topic(const std::string name, std::string &value, const bool print_default=true) const
deprecated function names, just for compatibility
bool load(const std::string name, bool &value, const bool print_default=true) const
static bool replaceFindpack(std::string &path)
static std::list< std::string > splitRessourcename(const std::string name)
ROSLIB_DECL std::string command(const std::string &cmd)
static void resolve_ressourcename(std::string &name)
static std::string boolToStr(const bool value)
static std::string resolveRessourcename(const std::string name)
void loadSub(const std::string &n, const std::string &v, const bool p, const bool r) const
private functions
#define ROS_INFO_STREAM(args)
bool getParam(const std::string &key, std::string &s) const
static bool replace_findpack(std::string &path)
static std::string bool_to_str(const bool value)


parameter_pa
Author(s):
autogenerated on Fri Jun 7 2019 21:42:21