11 #include <boost/any.hpp>
12 #include <dynamic_reconfigure/config_tools.h>
13 #include <dynamic_reconfigure/config_init_mutex.h>
15 #ifndef __pid__PIDCONFIG_H__
16 #define __pid__PIDCONFIG_H__
42 virtual void toMessage(dynamic_reconfigure::Config& msg,
const PidConfig& config)
const = 0;
53 ParamDescription(std::string name, std::string type, uint32_t level, std::string description,
88 return dynamic_reconfigure::ConfigTools::getParameter(msg, name, config.*
field);
93 dynamic_reconfigure::ConfigTools::appendParameter(msg, name, config.*
field);
117 virtual void toMessage(dynamic_reconfigure::Config& msg,
const boost::any& config)
const = 0;
118 virtual bool fromMessage(
const dynamic_reconfigure::Config& msg, boost::any& config)
const = 0;
124 for (std::vector<AbstractParamDescriptionConstPtr>::const_iterator i =
abstract_parameters.begin();
127 parameters.push_back(dynamic_reconfigure::ParamDescription(**i));
135 template <
class T,
class PT>
147 parameters = g.parameters;
151 virtual bool fromMessage(
const dynamic_reconfigure::Config& msg, boost::any& cfg)
const
153 PT* config = boost::any_cast<PT*>(cfg);
154 if (!dynamic_reconfigure::ConfigTools::getGroupState(msg, name, (*config).*
field))
157 for (std::vector<AbstractGroupDescriptionConstPtr>::const_iterator i =
groups.begin(); i !=
groups.end(); ++i)
159 boost::any n = &((*config).*
field);
160 if (!(*i)->fromMessage(msg, n))
169 PT* config = boost::any_cast<PT*>(cfg);
170 T* group = &((*config).*
field);
171 group->state =
state;
173 for (std::vector<AbstractGroupDescriptionConstPtr>::const_iterator i =
groups.begin(); i !=
groups.end(); ++i)
175 boost::any n = boost::any(&((*config).*
field));
176 (*i)->setInitialState(n);
182 PT* config = boost::any_cast<PT*>(cfg);
184 T*
f = &((*config).*
field);
187 for (std::vector<AbstractGroupDescriptionConstPtr>::const_iterator i =
groups.begin(); i !=
groups.end(); ++i)
189 boost::any n = &((*config).*
field);
190 (*i)->updateParams(n, top);
194 virtual void toMessage(dynamic_reconfigure::Config& msg,
const boost::any& cfg)
const
196 const PT config = boost::any_cast<PT>(cfg);
197 dynamic_reconfigure::ConfigTools::appendGroup<T>(msg, name,
id, parent, config.*
field);
199 for (std::vector<AbstractGroupDescriptionConstPtr>::const_iterator i =
groups.begin(); i !=
groups.end(); ++i)
201 (*i)->toMessage(msg, config.*
field);
206 std::vector<PidConfig::AbstractGroupDescriptionConstPtr>
groups;
220 for (std::vector<AbstractParamDescriptionConstPtr>::const_iterator _i = params.begin(); _i != params.end(); ++_i)
223 (*_i)->getValue(config, val);
225 if (
"Kp_scale" == (*_i)->name)
227 Kp_scale = boost::any_cast<double>(val);
229 if (
"Kp" == (*_i)->name)
231 Kp = boost::any_cast<double>(val);
233 if (
"Ki_scale" == (*_i)->name)
235 Ki_scale = boost::any_cast<double>(val);
237 if (
"Ki" == (*_i)->name)
239 Ki = boost::any_cast<double>(val);
241 if (
"Kd_scale" == (*_i)->name)
243 Kd_scale = boost::any_cast<double>(val);
245 if (
"Kd" == (*_i)->name)
247 Kd = boost::any_cast<double>(val);
291 for (std::vector<AbstractParamDescriptionConstPtr>::const_iterator i = __param_descriptions__.begin();
292 i != __param_descriptions__.end(); ++i)
293 if ((*i)->fromMessage(msg, *
this))
296 for (std::vector<AbstractGroupDescriptionConstPtr>::const_iterator i = __group_descriptions__.begin();
297 i != __group_descriptions__.end(); i++)
301 boost::any n = boost::any(
this);
302 (*i)->updateParams(n, *
this);
303 (*i)->fromMessage(msg, n);
307 if (count != dynamic_reconfigure::ConfigTools::size(msg))
309 ROS_ERROR(
"PidConfig::__fromMessage__ called with an unexpected parameter.");
311 for (
unsigned int i = 0; i < msg.bools.size(); i++)
312 ROS_ERROR(
" %s", msg.bools[i].name.c_str());
314 for (
unsigned int i = 0; i < msg.ints.size(); i++)
315 ROS_ERROR(
" %s", msg.ints[i].name.c_str());
317 for (
unsigned int i = 0; i < msg.doubles.size(); i++)
318 ROS_ERROR(
" %s", msg.doubles[i].name.c_str());
320 for (
unsigned int i = 0; i < msg.strs.size(); i++)
321 ROS_ERROR(
" %s", msg.strs[i].name.c_str());
332 const std::vector<AbstractParamDescriptionConstPtr>& __param_descriptions__,
333 const std::vector<AbstractGroupDescriptionConstPtr>& __group_descriptions__)
const
335 dynamic_reconfigure::ConfigTools::clear(msg);
336 for (std::vector<AbstractParamDescriptionConstPtr>::const_iterator i = __param_descriptions__.begin();
337 i != __param_descriptions__.end(); ++i)
338 (*i)->toMessage(msg, *
this);
340 for (std::vector<AbstractGroupDescriptionConstPtr>::const_iterator i = __group_descriptions__.begin();
341 i != __group_descriptions__.end(); ++i)
345 (*i)->toMessage(msg, *
this);
354 __toMessage__(msg, __param_descriptions__, __group_descriptions__);
360 for (std::vector<AbstractParamDescriptionConstPtr>::const_iterator i = __param_descriptions__.begin();
361 i != __param_descriptions__.end(); ++i)
362 (*i)->toServer(nh, *
this);
367 static bool setup =
false;
370 for (std::vector<AbstractParamDescriptionConstPtr>::const_iterator i = __param_descriptions__.begin();
371 i != __param_descriptions__.end(); ++i)
372 (*i)->fromServer(nh, *
this);
375 for (std::vector<AbstractGroupDescriptionConstPtr>::const_iterator i = __group_descriptions__.begin();
376 i != __group_descriptions__.end(); i++)
378 if (!
setup && (*i)->id == 0)
381 boost::any n = boost::any(
this);
382 (*i)->setInitialState(n);
392 for (std::vector<AbstractParamDescriptionConstPtr>::const_iterator i = __param_descriptions__.begin();
393 i != __param_descriptions__.end(); ++i)
394 (*i)->clamp(*
this, __max__, __min__);
401 for (std::vector<AbstractParamDescriptionConstPtr>::const_iterator i = __param_descriptions__.begin();
402 i != __param_descriptions__.end(); ++i)
403 (*i)->calcLevel(level, config, *
this);
445 "{'enum_description': 'Scale factor for K setting', 'enum': "
446 "[{'srcline': 7, 'description': 'Scale by 0.1', 'srcfile': "
447 "'/home/andy/Desktop/catkin_ws/src/pid/cfg/Pid.cfg', "
448 "'cconsttype': 'const double', 'value': 0.1, 'ctype': "
449 "'double', 'type': 'double', 'name': 'scale_tenth'}, "
450 "{'srcline': 8, 'description': 'No scaling', 'srcfile': "
451 "'/home/andy/Desktop/catkin_ws/src/pid/cfg/Pid.cfg', "
452 "'cconsttype': 'const double', 'value': 1.0, 'ctype': "
453 "'double', 'type': 'double', 'name': 'scale_unity'}, "
454 "{'srcline': 9, 'description': 'Scale by 10', 'srcfile': "
455 "'/home/andy/Desktop/catkin_ws/src/pid/cfg/Pid.cfg', "
456 "'cconsttype': 'const double', 'value': 10.0, 'ctype': "
457 "'double', 'type': 'double', 'name': 'scale_ten'}, {'srcline': "
458 "10, 'description': 'Scale by 100', 'srcfile': "
459 "'/home/andy/Desktop/catkin_ws/src/pid/cfg/Pid.cfg', "
460 "'cconsttype': 'const double', 'value': 100.0, 'ctype': "
461 "'double', 'type': 'double', 'name': 'scale_hundred'}]}",
467 "{'enum_description': 'Scale factor for K setting', 'enum': "
468 "[{'srcline': 7, 'description': 'Scale by 0.1', 'srcfile': "
469 "'/home/andy/Desktop/catkin_ws/src/pid/cfg/Pid.cfg', "
470 "'cconsttype': 'const double', 'value': 0.1, 'ctype': "
471 "'double', 'type': 'double', 'name': 'scale_tenth'}, "
472 "{'srcline': 8, 'description': 'No scaling', 'srcfile': "
473 "'/home/andy/Desktop/catkin_ws/src/pid/cfg/Pid.cfg', "
474 "'cconsttype': 'const double', 'value': 1.0, 'ctype': "
475 "'double', 'type': 'double', 'name': 'scale_unity'}, "
476 "{'srcline': 9, 'description': 'Scale by 10', 'srcfile': "
477 "'/home/andy/Desktop/catkin_ws/src/pid/cfg/Pid.cfg', "
478 "'cconsttype': 'const double', 'value': 10.0, 'ctype': "
479 "'double', 'type': 'double', 'name': 'scale_ten'}, {'srcline': "
480 "10, 'description': 'Scale by 100', 'srcfile': "
481 "'/home/andy/Desktop/catkin_ws/src/pid/cfg/Pid.cfg', "
482 "'cconsttype': 'const double', 'value': 100.0, 'ctype': "
483 "'double', 'type': 'double', 'name': 'scale_hundred'}]}",
515 "{'enum_description': 'Scale factor for K setting', 'enum': "
516 "[{'srcline': 7, 'description': 'Scale by 0.1', 'srcfile': "
517 "'/home/andy/Desktop/catkin_ws/src/pid/cfg/Pid.cfg', "
518 "'cconsttype': 'const double', 'value': 0.1, 'ctype': "
519 "'double', 'type': 'double', 'name': 'scale_tenth'}, "
520 "{'srcline': 8, 'description': 'No scaling', 'srcfile': "
521 "'/home/andy/Desktop/catkin_ws/src/pid/cfg/Pid.cfg', "
522 "'cconsttype': 'const double', 'value': 1.0, 'ctype': "
523 "'double', 'type': 'double', 'name': 'scale_unity'}, "
524 "{'srcline': 9, 'description': 'Scale by 10', 'srcfile': "
525 "'/home/andy/Desktop/catkin_ws/src/pid/cfg/Pid.cfg', "
526 "'cconsttype': 'const double', 'value': 10.0, 'ctype': "
527 "'double', 'type': 'double', 'name': 'scale_ten'}, {'srcline': "
528 "10, 'description': 'Scale by 100', 'srcfile': "
529 "'/home/andy/Desktop/catkin_ws/src/pid/cfg/Pid.cfg', "
530 "'cconsttype': 'const double', 'value': 100.0, 'ctype': "
531 "'double', 'type': 'double', 'name': 'scale_hundred'}]}",
537 "{'enum_description': 'Scale factor for K setting', 'enum': "
538 "[{'srcline': 7, 'description': 'Scale by 0.1', 'srcfile': "
539 "'/home/andy/Desktop/catkin_ws/src/pid/cfg/Pid.cfg', "
540 "'cconsttype': 'const double', 'value': 0.1, 'ctype': "
541 "'double', 'type': 'double', 'name': 'scale_tenth'}, "
542 "{'srcline': 8, 'description': 'No scaling', 'srcfile': "
543 "'/home/andy/Desktop/catkin_ws/src/pid/cfg/Pid.cfg', "
544 "'cconsttype': 'const double', 'value': 1.0, 'ctype': "
545 "'double', 'type': 'double', 'name': 'scale_unity'}, "
546 "{'srcline': 9, 'description': 'Scale by 10', 'srcfile': "
547 "'/home/andy/Desktop/catkin_ws/src/pid/cfg/Pid.cfg', "
548 "'cconsttype': 'const double', 'value': 10.0, 'ctype': "
549 "'double', 'type': 'double', 'name': 'scale_ten'}, {'srcline': "
550 "10, 'description': 'Scale by 100', 'srcfile': "
551 "'/home/andy/Desktop/catkin_ws/src/pid/cfg/Pid.cfg', "
552 "'cconsttype': 'const double', 'value': 100.0, 'ctype': "
553 "'double', 'type': 'double', 'name': 'scale_hundred'}]}",
585 "{'enum_description': 'Scale factor for K setting', 'enum': "
586 "[{'srcline': 7, 'description': 'Scale by 0.1', 'srcfile': "
587 "'/home/andy/Desktop/catkin_ws/src/pid/cfg/Pid.cfg', "
588 "'cconsttype': 'const double', 'value': 0.1, 'ctype': "
589 "'double', 'type': 'double', 'name': 'scale_tenth'}, "
590 "{'srcline': 8, 'description': 'No scaling', 'srcfile': "
591 "'/home/andy/Desktop/catkin_ws/src/pid/cfg/Pid.cfg', "
592 "'cconsttype': 'const double', 'value': 1.0, 'ctype': "
593 "'double', 'type': 'double', 'name': 'scale_unity'}, "
594 "{'srcline': 9, 'description': 'Scale by 10', 'srcfile': "
595 "'/home/andy/Desktop/catkin_ws/src/pid/cfg/Pid.cfg', "
596 "'cconsttype': 'const double', 'value': 10.0, 'ctype': "
597 "'double', 'type': 'double', 'name': 'scale_ten'}, {'srcline': "
598 "10, 'description': 'Scale by 100', 'srcfile': "
599 "'/home/andy/Desktop/catkin_ws/src/pid/cfg/Pid.cfg', "
600 "'cconsttype': 'const double', 'value': 100.0, 'ctype': "
601 "'double', 'type': 'double', 'name': 'scale_hundred'}]}",
607 "{'enum_description': 'Scale factor for K setting', 'enum': "
608 "[{'srcline': 7, 'description': 'Scale by 0.1', 'srcfile': "
609 "'/home/andy/Desktop/catkin_ws/src/pid/cfg/Pid.cfg', "
610 "'cconsttype': 'const double', 'value': 0.1, 'ctype': "
611 "'double', 'type': 'double', 'name': 'scale_tenth'}, "
612 "{'srcline': 8, 'description': 'No scaling', 'srcfile': "
613 "'/home/andy/Desktop/catkin_ws/src/pid/cfg/Pid.cfg', "
614 "'cconsttype': 'const double', 'value': 1.0, 'ctype': "
615 "'double', 'type': 'double', 'name': 'scale_unity'}, "
616 "{'srcline': 9, 'description': 'Scale by 10', 'srcfile': "
617 "'/home/andy/Desktop/catkin_ws/src/pid/cfg/Pid.cfg', "
618 "'cconsttype': 'const double', 'value': 10.0, 'ctype': "
619 "'double', 'type': 'double', 'name': 'scale_ten'}, {'srcline': "
620 "10, 'description': 'Scale by 100', 'srcfile': "
621 "'/home/andy/Desktop/catkin_ws/src/pid/cfg/Pid.cfg', "
622 "'cconsttype': 'const double', 'value': 100.0, 'ctype': "
623 "'double', 'type': 'double', 'name': 'scale_hundred'}]}",
652 for (std::vector<PidConfig::AbstractGroupDescriptionConstPtr>::const_iterator i =
__group_descriptions__.begin();
716 boost::mutex::scoped_lock lock(dynamic_reconfigure::__init_mutex__);
736 #endif // __PIDRECONFIGURATOR_H__