dynamic_parameters.h
Go to the documentation of this file.
1 // *****************************************************************************
2 //
3 // Copyright (c) 2019, Southwest Research Institute® (SwRI®)
4 // All rights reserved.
5 //
6 // Redistribution and use in source and binary forms, with or without
7 // modification, are permitted provided that the following conditions are met:
8 // * Redistributions of source code must retain the above copyright
9 // notice, this list of conditions and the following disclaimer.
10 // * Redistributions in binary form must reproduce the above copyright
11 // notice, this list of conditions and the following disclaimer in the
12 // documentation and/or other materials provided with the distribution.
13 // * Neither the name of Southwest Research Institute® (SwRI®) nor the
14 // names of its contributors may be used to endorse or promote products
15 // derived from this software without specific prior written permission.
16 //
17 // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
18 // AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
19 // IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
20 // ARE DISCLAIMED. IN NO EVENT SHALL SOUTHWEST RESEARCH INSTITUTE BE LIABLE FOR ANY
21 // DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
22 // (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
23 // LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
24 // ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
25 // (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
26 // SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
27 //
28 // *****************************************************************************
29 
30 #ifndef SWRI_ROSCPP_DYNAMIC_PARAMETERS_H_
31 #define SWRI_ROSCPP_DYNAMIC_PARAMETERS_H_
32 
33 #include <map>
34 #include <string>
35 #include <sstream>
36 
37 #include <boost/thread/mutex.hpp>
38 
39 #include <yaml-cpp/yaml.h>
40 
41 #include <ros/console.h>
42 #include <ros/node_handle.h>
43 
44 #include <dynamic_reconfigure/Config.h>
45 #include <dynamic_reconfigure/ConfigDescription.h>
46 #include <dynamic_reconfigure/GroupState.h>
47 #include <dynamic_reconfigure/Reconfigure.h>
48 
49 namespace swri
50 {
51  struct DynamicValue
52  {
53  enum Type
54  {
55  Bool = 0,
56  Float = 1,
57  Double = 2,
58  Int = 3,
59  String = 4
60  };
61 
63  std::string name;
64  std::string description;
65  std::vector<std::pair<std::string, int> > enums;
66 
67  //pointer to the parameter to update on change
73 
74  // Defaults, maximum and minimum values for this parameter
75  union
76  {
77  double d;
78  bool b;
79  int i;
80  } Default;
81  union
82  {
83  double d;
84  int i;
85  } Min;
86  union
87  {
88  double d;
89  int i;
90  } Max;
91  std::string default_string;// to get around union issues with strings
92  };
93 
94  template <class T>
95  struct TypedParam
96  {
99 
100  inline
101  T operator* ()
102  {
103  return *data;
104  }
105 
106  T get()
107  {
108  mutex->lock();
109  T d = *data;
110  mutex->unlock();
111 
112  return d;
113  }
114  };
115 
121 
122 
124  {
129 
130  std::map<std::string, DynamicValue> values_;
131 
132  // stores the order that the parameters were added in
133  std::vector<std::string> ordered_params_;
134 
135  boost::function<void(DynamicParameters&)> on_change_;
136 
137 
139 
140  bool setConfigCallback(dynamic_reconfigure::Reconfigure::Request &req,
141  dynamic_reconfigure::Reconfigure::Response &rsp)
142  {
143  ROS_DEBUG("Got configuration change request");
144 
145  boost::mutex::scoped_lock lock(*mutex_);
146 
147  // update the parameters
148  for (size_t i = 0; i < req.config.doubles.size(); i++)
149  {
150  dynamic_reconfigure::DoubleParameter param = req.config.doubles[i];
151  std::map<std::string, DynamicValue>::iterator iter = values_.find(param.name);
152  if (iter == values_.end())
153  {
154  ROS_ERROR("Could not find parameter '%s'", param.name.c_str());
155  continue;
156  }
157 
158  if (iter->second.type != DynamicValue::Double && iter->second.type != DynamicValue::Float)
159  {
160  ROS_ERROR("Value '%s' was not a double type.", param.name.c_str());
161  continue;
162  }
163 
164  if (iter->second.type == DynamicValue::Double)
165  {
166  *iter->second.dbl = param.value;
167  }
168 
169  if (iter->second.type == DynamicValue::Float)
170  {
171  *iter->second.flt = (float)param.value;
172  }
173  }
174 
175  for (size_t i = 0; i < req.config.ints.size(); i++)
176  {
177  dynamic_reconfigure::IntParameter param = req.config.ints[i];
178  std::map<std::string, DynamicValue>::iterator iter = values_.find(param.name);
179  if (iter == values_.end())
180  {
181  ROS_ERROR("Could not find parameter '%s'", param.name.c_str());
182  continue;
183  }
184 
185  if (iter->second.type != DynamicValue::Int)
186  {
187  ROS_ERROR("Value '%s' was not a int type.", param.name.c_str());
188  continue;
189  }
190 
191  *iter->second.integer = param.value;
192  }
193 
194  for (size_t i = 0; i < req.config.bools.size(); i++)
195  {
196  dynamic_reconfigure::BoolParameter param = req.config.bools[i];
197  std::map<std::string, DynamicValue>::iterator iter = values_.find(param.name);
198  if (iter == values_.end())
199  {
200  ROS_ERROR("Could not find parameter '%s'", param.name.c_str());
201  continue;
202  }
203 
204  if (iter->second.type != DynamicValue::Bool)
205  {
206  ROS_ERROR("Value '%s' was not a bool type.", param.name.c_str());
207  continue;
208  }
209 
210  *iter->second.boolean = param.value;
211  }
212 
213  for (size_t i = 0; i < req.config.strs.size(); i++)
214  {
215  dynamic_reconfigure::StrParameter param = req.config.strs[i];
216  std::map<std::string, DynamicValue>::iterator iter = values_.find(param.name);
217  if (iter == values_.end())
218  {
219  ROS_ERROR("Could not find parameter '%s'", param.name.c_str());
220  continue;
221  }
222 
223  if (iter->second.type != DynamicValue::String)
224  {
225  ROS_ERROR("Value '%s' was not a string type.", param.name.c_str());
226  continue;
227  }
228 
229  *iter->second.str = param.value;
230  }
231 
232  updateCurrent(rsp.config);
233 
234  if (on_change_)
235  {
236  on_change_(*this);
237  }
238 
239  return true;
240  }
241 
242  // Updates a config with the current parameter values
243  void updateCurrent(dynamic_reconfigure::Config& config)
244  {
245  for (std::map<std::string, DynamicValue>::iterator value = values_.begin(); value != values_.end(); value++)
246  {
247  if (value->second.type == DynamicValue::Double)
248  {
249  dynamic_reconfigure::DoubleParameter param;
250  param.name = value->first;
251  param.value = *value->second.dbl;
252  config.doubles.push_back(param);
253  }
254  else if (value->second.type == DynamicValue::Float)
255  {
256  dynamic_reconfigure::DoubleParameter param;
257  param.name = value->first;
258  param.value = *value->second.flt;
259  config.doubles.push_back(param);
260  }
261  else if (value->second.type == DynamicValue::Int)
262  {
263  dynamic_reconfigure::IntParameter param;
264  param.name = value->first;
265  param.value = *value->second.integer;
266  config.ints.push_back(param);
267  }
268  else if (value->second.type == DynamicValue::Bool)
269  {
270  dynamic_reconfigure::BoolParameter param;
271  param.name = value->first;
272  param.value = *value->second.boolean;
273  config.bools.push_back(param);
274  }
275  else if (value->second.type == DynamicValue::String)
276  {
277  dynamic_reconfigure::StrParameter param;
278  param.name = value->first;
279  param.value = *value->second.str;
280  config.strs.push_back(param);
281  }
282  }
283 
284  dynamic_reconfigure::GroupState gs;
285  gs.name = "Default";
286  gs.state = true;
287  gs.id = 0;
288  gs.parent = 0;
289  config.groups.push_back(gs);
290  update_pub_.publish(config);
291  }
292 
293  public:
294 
295  DynamicParameters() : mutex_(new boost::mutex)
296  {
297 
298  }
299 
300  // Sets up the node handle and publishers. Be sure to call this before finalize or any of the 'get*' calls.
302  {
303  boost::mutex::scoped_lock lock(*mutex_);
305 
306  descr_pub_ = nh_->advertise<dynamic_reconfigure::ConfigDescription>("parameter_descriptions", 1, true);
307  update_pub_ = nh_->advertise<dynamic_reconfigure::Config>("parameter_updates", 1, true);
308  }
309 
310  // Publishes the configuration parameters that have been added
311  void finalize(bool alphabetical_order = true)
312  {
313  boost::mutex::scoped_lock lock(*mutex_);
314 
315  // publish the configs as one group
316  dynamic_reconfigure::ConfigDescription rdesc;
317  dynamic_reconfigure::Group group;
318  group.name = "Default";
319  group.type = "";
320  group.id = 0;
321  group.parent = 0;
322 
323  dynamic_reconfigure::GroupState gs;
324  gs.name = "Default";
325  gs.state = true;
326  gs.id = 0;
327  gs.parent = 0;
328 
329  // sort by alphabetical if we want it
330  if (alphabetical_order)
331  {
332  ordered_params_.clear();
333 
334  for (std::map<std::string, DynamicValue>::iterator it = values_.begin(); it != values_.end(); it++ )
335  {
336  ordered_params_.push_back(it->first);
337  }
338  }
339  for (size_t i = 0; i < ordered_params_.size(); i++)
340  {
341  std::map<std::string, DynamicValue>::iterator param = values_.find(ordered_params_[i]);
342 
343  std::string type;
344  if (param->second.type == DynamicValue::Bool)
345  {
346  type = "bool";
347 
348  dynamic_reconfigure::BoolParameter desc;
349  desc.name = param->first;
350  desc.value = true;
351  rdesc.max.bools.push_back(desc);
352  desc.value = false;
353  rdesc.min.bools.push_back(desc);
354  desc.value = param->second.Default.b;
355  rdesc.dflt.bools.push_back(desc);
356  }
357  else if (param->second.type == DynamicValue::Float)
358  {
359  type = "double";
360 
361  dynamic_reconfigure::DoubleParameter desc;
362  desc.name = param->first;
363  desc.value = param->second.Max.d;
364  rdesc.max.doubles.push_back(desc);
365  desc.value = param->second.Min.d;
366  rdesc.min.doubles.push_back(desc);
367  desc.value = param->second.Default.d;
368  rdesc.dflt.doubles.push_back(desc);
369  }
370  else if (param->second.type == DynamicValue::Double)
371  {
372  type = "double";
373 
374  dynamic_reconfigure::DoubleParameter desc;
375  desc.name = param->first;
376  desc.value = param->second.Max.d;
377  rdesc.max.doubles.push_back(desc);
378  desc.value = param->second.Min.d;
379  rdesc.min.doubles.push_back(desc);
380  desc.value = param->second.Default.d;
381  rdesc.dflt.doubles.push_back(desc);
382  }
383  else if (param->second.type == DynamicValue::Int)
384  {
385  type = "int";
386 
387  dynamic_reconfigure::IntParameter desc;
388  desc.name = param->first;
389  desc.value = param->second.Max.i;
390  rdesc.max.ints.push_back(desc);
391  desc.value = param->second.Min.i;
392  rdesc.min.ints.push_back(desc);
393  desc.value = param->second.Default.i;
394  rdesc.dflt.ints.push_back(desc);
395  }
396  else if (param->second.type == DynamicValue::String)
397  {
398  type = "str";
399  dynamic_reconfigure::StrParameter desc;
400  desc.name = param->first;
401  desc.value = "";
402  rdesc.max.strs.push_back(desc);
403  desc.value = "";
404  rdesc.min.strs.push_back(desc);
405  desc.value = param->second.default_string;
406  rdesc.dflt.strs.push_back(desc);
407  }
408 
409  dynamic_reconfigure::ParamDescription desc;
410  desc.name = param->first;
411  desc.type = type;
412  desc.level = 0;
413  desc.description = param->second.description;
414 
415  // If this is an enum, let's make the edit method string
416  if (!param->second.enums.empty())
417  {
418  YAML::Emitter emitter;
419 
420  emitter << YAML::Flow << YAML::SingleQuoted;
421  emitter << YAML::BeginMap;
422  emitter << YAML::Key << "enum_description";
423  emitter << YAML::Value << desc.description;
424  emitter << YAML::Key << "enum";
425  emitter << YAML::Value << YAML::BeginSeq;
426 
427  for (size_t j = 0; j < param->second.enums.size(); j++)
428  {
429  emitter << YAML::BeginMap;
430  emitter << YAML::Key << "srcline" << YAML::Value << 0;
431  emitter << YAML::Key << "description" << YAML::Value << "Unknown";
432  emitter << YAML::Key << "srcfile" << YAML::Value << "dynamic_parameters.h";
433  emitter << YAML::Key << "cconsttype" << YAML::Value << "const int";
434  emitter << YAML::Key << "value" << YAML::Value << param->second.enums[j].second;
435  emitter << YAML::Key << "ctype" << YAML::Value << "int";
436  emitter << YAML::Key << "type" << YAML::Value << "int";
437  emitter << YAML::Key << "name" << YAML::Value << param->second.enums[j].first;
438  emitter << YAML::EndMap;
439  }
440  emitter << YAML::EndSeq << YAML::EndMap;
441 
442  desc.edit_method = emitter.c_str();
443  }
444 
445  group.parameters.push_back(desc);
446  }
447  rdesc.max.groups.push_back(gs);
448  rdesc.min.groups.push_back(gs);
449  rdesc.dflt.groups.push_back(gs);
450  rdesc.groups.push_back(group);
451  descr_pub_.publish(rdesc);
452 
453  dynamic_reconfigure::Config config;
454  updateCurrent(config);
455 
456  set_service_ = nh_->advertiseService("set_parameters",
458 
459  ordered_params_.clear();// to save memory
460  }
461 
462  void addEnums(const std::string& param, const std::vector<std::pair<std::string, int> >& enums)
463  {
464  std::map<std::string, DynamicValue>::iterator iter = values_.find(param);
465  if (iter == values_.end())
466  {
467  ROS_ERROR("Tried to add enum to nonexistant param %s", param.c_str());
468  return;
469  }
470 
471  iter->second.enums.insert(iter->second.enums.end(), enums.begin(), enums.end());
472  }
473 
474  void setCallback(boost::function<void(DynamicParameters&)> fun)
475  {
476  on_change_ = fun;
477  }
478 
479  void update()
480  {
481  dynamic_reconfigure::Config config;
482  updateCurrent(config);
483  }
484 
485  //for use in the on change callback
486  double getDouble(const std::string& name)
487  {
488  std::map<std::string, DynamicValue>::iterator iter = values_.find(name);
489  if (iter == values_.end())
490  {
491  ROS_ERROR("Tried to get nonexistant param %s", name.c_str());
492  return 0.0;
493  }
494  if (iter->second.type != DynamicValue::Double)
495  {
496  ROS_ERROR("Tried to load parameter %s with the wrong type: double.", name.c_str());
497  return 0.0;
498  }
499 
500  return *iter->second.dbl;
501  }
502 
503  //for use in the on change callback
504  float getFloat(const std::string& name)
505  {
506  std::map<std::string, DynamicValue>::iterator iter = values_.find(name);
507  if (iter == values_.end())
508  {
509  ROS_ERROR("Tried to get nonexistant param %s", name.c_str());
510  return 0.0f;
511  }
512  if (iter->second.type != DynamicValue::Float)
513  {
514  ROS_ERROR("Tried to load parameter %s with the wrong type: float.", name.c_str());
515  return 0.0f;
516  }
517 
518  return *iter->second.flt;
519  }
520 
521  int getInt(const std::string& name)
522  {
523  std::map<std::string, DynamicValue>::iterator iter = values_.find(name);
524  if (iter == values_.end())
525  {
526  ROS_ERROR("Tried to get nonexistant param %s", name.c_str());
527  return 0.0f;
528  }
529  if (iter->second.type != DynamicValue::Int)
530  {
531  ROS_ERROR("Tried to load parameter %s with the wrong type: int.", name.c_str());
532  return 0.0f;
533  }
534 
535  return *iter->second.integer;
536  }
537 
538  //for use in the on change callback
539  bool getBool(const std::string& name)
540  {
541  std::map<std::string, DynamicValue>::iterator iter = values_.find(name);
542  if (iter == values_.end())
543  {
544  ROS_ERROR("Tried to get nonexistant param %s", name.c_str());
545  return false;
546  }
547  if (iter->second.type != DynamicValue::Bool)
548  {
549  ROS_ERROR("Tried to load parameter %s with the wrong type: bool.", name.c_str());
550  return false;
551  }
552 
553  return *iter->second.boolean;
554  }
555 
556  //for use in the on change callback
557  std::string getString(const std::string& name)
558  {
559  std::map<std::string, DynamicValue>::iterator iter = values_.find(name);
560  if (iter == values_.end())
561  {
562  ROS_ERROR("Tried to get nonexistant param %s", name.c_str());
563  return "";
564  }
565  if (iter->second.type != DynamicValue::String)
566  {
567  ROS_ERROR("Tried to load parameter %s with the wrong type: string.", name.c_str());
568  return "";
569  }
570 
571  return *iter->second.str;
572  }
573 
574 
575  inline
576  boost::mutex& mutex()
577  {
578  return *mutex_;
579  }
580 
581  inline
582  boost::mutex::scoped_lock lock_guard()
583  {
584  return boost::mutex::scoped_lock(*mutex_);
585  }
586 
587  // for use with on change callbacks
588  inline
589  void get(const std::string &name,
590  float &variable,
591  const float default_value,
592  const std::string description = "None.",
593  const float min = -100,
594  const float max = 100)
595  {
596  DynamicValue value;
597  value.type = DynamicValue::Float;
598  value.description = description;
599  value.Min.d = min;
600  value.Max.d = max;
601  value.Default.d = default_value;
602  value.flt = boost::shared_ptr<float>(new float);
603  values_[name] = value;
604  ordered_params_.push_back(name);
605 
606  std::string resolved_name = nh_->resolveName(name);
607  //_used_params.insert(resolved_name);
608  nh_->param(name, *value.flt, default_value);
609  variable = *value.flt;
610  ROS_INFO("Read dynamic parameter %s = %f", name.c_str(), variable);
611  }
612 
613  inline
614  void get(const std::string &name,
615  FloatParam &variable,
616  const float default_value,
617  const std::string description = "None.",
618  const float min = -100,
619  const float max = 100)
620  {
621  DynamicValue value;
622  value.type = DynamicValue::Float;
623  value.description = description;
624  value.Min.d = min;
625  value.Max.d = max;
626  value.Default.d = default_value;
627  value.flt = boost::shared_ptr<float>(new float);
628  values_[name] = value;
629  ordered_params_.push_back(name);
630 
631  variable.data = value.flt;
632  variable.mutex = mutex_;
633 
634  std::string resolved_name = nh_->resolveName(name);
635  //_used_params.insert(resolved_name);
636  nh_->param(name, *value.flt, default_value);
637  ROS_INFO("Read dynamic parameter %s = %f", name.c_str(), *variable);
638  }
639 
640  // for use with on change callbacks
641  inline
642  void get(const std::string &name,
643  double &variable,
644  const double default_value,
645  const std::string description = "None.",
646  const double min = -100,
647  const double max = 100)
648  {
649  DynamicValue value;
650  value.type = DynamicValue::Double;
651  value.description = description;
652  value.Min.d = min;
653  value.Max.d = max;
654  value.Default.d = default_value;
655  value.dbl = boost::shared_ptr<double>(new double);
656  values_[name] = value;
657  ordered_params_.push_back(name);
658 
659  std::string resolved_name = nh_->resolveName(name);
660  //_used_params.insert(resolved_name);
661  nh_->param(name, *value.dbl, default_value);
662  variable = *value.dbl;
663  ROS_INFO("Read dynamic parameter %s = %lf", name.c_str(), variable);
664  }
665 
666  inline
667  void get(const std::string &name,
668  DoubleParam &variable,
669  const double default_value,
670  const std::string description = "None.",
671  const double min = -100,
672  const double max = 100)
673  {
674  DynamicValue value;
675  value.type = DynamicValue::Double;
676  value.description = description;
677  value.Min.d = min;
678  value.Max.d = max;
679  value.Default.d = default_value;
680  value.dbl = boost::shared_ptr<double>(new double);
681  values_[name] = value;
682  ordered_params_.push_back(name);
683 
684  variable.data = value.dbl;
685  variable.mutex = mutex_;
686 
687  std::string resolved_name = nh_->resolveName(name);
688  //_used_params.insert(resolved_name);
689  nh_->param(name, *value.dbl, default_value);
690  ROS_INFO("Read dynamic parameter %s = %lf", name.c_str(), *variable);
691  }
692 
693  inline
694  void get(const std::string &name,
695  int &variable,
696  const int default_value,
697  const std::string description = "None.",
698  const int min = -100,
699  const int max = 100)
700  {
701  DynamicValue value;
702  value.type = DynamicValue::Int;
703  value.description = description;
704  value.Min.i = min;
705  value.Max.i = max;
706  value.Default.i = default_value;
707  value.integer = boost::shared_ptr<int>(new int);
708  values_[name] = value;
709  ordered_params_.push_back(name);
710 
711  std::string resolved_name = nh_->resolveName(name);
712  //_used_params.insert(resolved_name);
713  nh_->param(name, *value.integer, default_value);
714  variable = *value.integer;
715  ROS_INFO("Read dynamic parameter %s = %i", name.c_str(), variable);
716  }
717 
718  inline
719  void get(const std::string &name,
720  IntParam &variable,
721  const int default_value,
722  const std::string description = "None.",
723  const int min = -100,
724  const int max = 100)
725  {
726  DynamicValue value;
727  value.type = DynamicValue::Int;
728  value.description = description;
729  value.Min.i = min;
730  value.Max.i = max;
731  value.Default.i = default_value;
732  value.integer = boost::shared_ptr<int>(new int);
733  values_[name] = value;
734  ordered_params_.push_back(name);
735 
736  variable.data = value.integer;
737  variable.mutex = mutex_;
738 
739  std::string resolved_name = nh_->resolveName(name);
740  //_used_params.insert(resolved_name);
741  nh_->param(name, *value.integer, default_value);
742  ROS_INFO("Read dynamic parameter %s = %i", name.c_str(), *variable);
743  }
744 
745  // for use with on change callbacks
746  inline
747  void get(const std::string &name,
748  bool &variable,
749  const bool default_value,
750  const std::string description = "None.")
751  {
752  DynamicValue value;
753  value.type = DynamicValue::Bool;
754  value.description = description;
755  value.Default.b = default_value;
756  value.boolean = boost::shared_ptr<bool>(new bool);
757  values_[name] = value;
758  ordered_params_.push_back(name);
759 
760  std::string resolved_name = nh_->resolveName(name);
761  //_used_params.insert(resolved_name);
762  nh_->param(name, *value.boolean, default_value);
763  variable = *value.boolean;
764  ROS_INFO("Read dynamic parameter %s = %s", name.c_str(), variable ? "true" : "false");
765  }
766 
767  inline
768  void get(const std::string &name,
769  BoolParam &variable,
770  const bool default_value,
771  const std::string description = "None.")
772  {
773  DynamicValue value;
774  value.type = DynamicValue::Bool;
775  value.description = description;
776  value.Default.b = default_value;
777  value.boolean = boost::shared_ptr<bool>(new bool);
778  values_[name] = value;
779  ordered_params_.push_back(name);
780 
781  variable.data = value.boolean;
782  variable.mutex = mutex_;
783 
784  std::string resolved_name = nh_->resolveName(name);
785  //_used_params.insert(resolved_name);
786  nh_->param(name, *value.boolean, default_value);
787  ROS_INFO("Read dynamic parameter %s = %s", name.c_str(), *variable ? "true" : "false");
788  }
789 
790  // for use with on change callbacks
791  inline
792  void get(const std::string &name,
793  std::string &variable,
794  const std::string default_value,
795  const std::string description = "None.")
796  {
797  DynamicValue value;
798  value.type = DynamicValue::String;
799  value.description = description;
800  value.default_string = default_value;
801  value.str = boost::shared_ptr<std::string>(new std::string());
802  values_[name] = value;
803  ordered_params_.push_back(name);
804 
805  std::string resolved_name = nh_->resolveName(name);
806  //_used_params.insert(resolved_name);
807  nh_->param(name, *value.str, default_value);
808  variable = *value.str;
809  ROS_INFO("Read dynamic parameter %s = %s", name.c_str(), variable.c_str());
810  }
811 
812  inline
813  void get(const std::string &name,
814  StringParam &variable,
815  const std::string default_value,
816  const std::string description = "None.")
817  {
818  DynamicValue value;
819  value.type = DynamicValue::String;
820  value.description = description;
821  value.default_string = default_value;
822  value.str = boost::shared_ptr<std::string>(new std::string());
823  values_[name] = value;
824  ordered_params_.push_back(name);
825 
826  variable.data = value.str;
827  variable.mutex = mutex_;
828 
829  std::string resolved_name = nh_->resolveName(name);
830  //_used_params.insert(resolved_name);
831  nh_->param(name, *value.str, default_value);
832  ROS_INFO("Read dynamic parameter %s = %s", name.c_str(), (*variable).c_str());
833  }
834  };
835 } // namespace swri_param
836 #endif // SWRI_ROSCPP_DYNAMIC_PARAMETERS_H_
std::vector< std::string > ordered_params_
void finalize(bool alphabetical_order=true)
boost::shared_ptr< boost::mutex > mutex
void publish(const boost::shared_ptr< M > &message) const
void initialize(ros::NodeHandle &pnh)
void updateCurrent(dynamic_reconfigure::Config &config)
float getFloat(const std::string &name)
boost::shared_ptr< std::string > str
std::string getString(const std::string &name)
bool setConfigCallback(dynamic_reconfigure::Reconfigure::Request &req, dynamic_reconfigure::Reconfigure::Response &rsp)
std::map< std::string, DynamicValue > values_
bool getBool(const std::string &name)
boost::shared_ptr< double > dbl
TypedParam< bool > BoolParam
def default_value(type_)
union swri::DynamicValue::@1 Min
double getDouble(const std::string &name)
boost::mutex::scoped_lock lock_guard()
void setCallback(boost::function< void(DynamicParameters &)> fun)
static void param(const ros::NodeHandle &nh, const std::string &name, int &variable, const int default_value)
Definition: parameters.h:102
#define ROS_INFO(...)
boost::shared_ptr< bool > boolean
boost::shared_ptr< boost::mutex > mutex_
boost::shared_ptr< float > flt
boost::shared_ptr< int > integer
std::vector< std::pair< std::string, int > > enums
boost::shared_ptr< ros::NodeHandle > nh_
TypedParam< double > DoubleParam
TypedParam< int > IntParam
boost::function< void(DynamicParameters &)> on_change_
TypedParam< std::string > StringParam
boost::shared_ptr< T > data
union swri::DynamicValue::@0 Default
int min(int a, int b)
TypedParam< float > FloatParam
std::string default_string
int getInt(const std::string &name)
ros::ServiceServer set_service_
union swri::DynamicValue::@2 Max
void addEnums(const std::string &param, const std::vector< std::pair< std::string, int > > &enums)
#define ROS_ERROR(...)
#define ROS_DEBUG(...)


swri_roscpp
Author(s):
autogenerated on Tue Apr 6 2021 02:50:35