00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023
00024
00025
00026
00027
00028
00029
00030
00031
00032
00033
00034
00035
00036
00037
00038
00039
00040
00041
00042
00043
00044
00045
00046
00047 #ifndef __iri_jointspub__JOINTSPUBCONFIG_H__
00048 #define __iri_jointspub__JOINTSPUBCONFIG_H__
00049
00050 #include <dynamic_reconfigure/config_tools.h>
00051 #include <limits>
00052 #include <ros/node_handle.h>
00053 #include <dynamic_reconfigure/ConfigDescription.h>
00054 #include <dynamic_reconfigure/ParamDescription.h>
00055 #include <dynamic_reconfigure/Group.h>
00056 #include <dynamic_reconfigure/config_init_mutex.h>
00057 #include <boost/any.hpp>
00058
00059 namespace iri_jointspub
00060 {
00061 class JointspubConfigStatics;
00062
00063 class JointspubConfig
00064 {
00065 public:
00066 class AbstractParamDescription : public dynamic_reconfigure::ParamDescription
00067 {
00068 public:
00069 AbstractParamDescription(std::string n, std::string t, uint32_t l,
00070 std::string d, std::string e)
00071 {
00072 name = n;
00073 type = t;
00074 level = l;
00075 description = d;
00076 edit_method = e;
00077 }
00078
00079 virtual void clamp(JointspubConfig &config, const JointspubConfig &max, const JointspubConfig &min) const = 0;
00080 virtual void calcLevel(uint32_t &level, const JointspubConfig &config1, const JointspubConfig &config2) const = 0;
00081 virtual void fromServer(const ros::NodeHandle &nh, JointspubConfig &config) const = 0;
00082 virtual void toServer(const ros::NodeHandle &nh, const JointspubConfig &config) const = 0;
00083 virtual bool fromMessage(const dynamic_reconfigure::Config &msg, JointspubConfig &config) const = 0;
00084 virtual void toMessage(dynamic_reconfigure::Config &msg, const JointspubConfig &config) const = 0;
00085 virtual void getValue(const JointspubConfig &config, boost::any &val) const = 0;
00086 };
00087
00088 typedef boost::shared_ptr<AbstractParamDescription> AbstractParamDescriptionPtr;
00089 typedef boost::shared_ptr<const AbstractParamDescription> AbstractParamDescriptionConstPtr;
00090
00091 template <class T>
00092 class ParamDescription : public AbstractParamDescription
00093 {
00094 public:
00095 ParamDescription(std::string name, std::string type, uint32_t level,
00096 std::string description, std::string edit_method, T JointspubConfig::* f) :
00097 AbstractParamDescription(name, type, level, description, edit_method),
00098 field(f)
00099 {}
00100
00101 T (JointspubConfig::* field);
00102
00103 virtual void clamp(JointspubConfig &config, const JointspubConfig &max, const JointspubConfig &min) const
00104 {
00105 if (config.*field > max.*field)
00106 config.*field = max.*field;
00107
00108 if (config.*field < min.*field)
00109 config.*field = min.*field;
00110 }
00111
00112 virtual void calcLevel(uint32_t &comb_level, const JointspubConfig &config1, const JointspubConfig &config2) const
00113 {
00114 if (config1.*field != config2.*field)
00115 comb_level |= level;
00116 }
00117
00118 virtual void fromServer(const ros::NodeHandle &nh, JointspubConfig &config) const
00119 {
00120 nh.getParam(name, config.*field);
00121 }
00122
00123 virtual void toServer(const ros::NodeHandle &nh, const JointspubConfig &config) const
00124 {
00125 nh.setParam(name, config.*field);
00126 }
00127
00128 virtual bool fromMessage(const dynamic_reconfigure::Config &msg, JointspubConfig &config) const
00129 {
00130 return dynamic_reconfigure::ConfigTools::getParameter(msg, name, config.*field);
00131 }
00132
00133 virtual void toMessage(dynamic_reconfigure::Config &msg, const JointspubConfig &config) const
00134 {
00135 dynamic_reconfigure::ConfigTools::appendParameter(msg, name, config.*field);
00136 }
00137
00138 virtual void getValue(const JointspubConfig &config, boost::any &val) const
00139 {
00140 val = config.*field;
00141 }
00142 };
00143
00144 class AbstractGroupDescription : public dynamic_reconfigure::Group
00145 {
00146 public:
00147 AbstractGroupDescription(std::string n, std::string t, int p, int i, bool s)
00148 {
00149 name = n;
00150 type = t;
00151 parent = p;
00152 state = s;
00153 id = i;
00154 }
00155
00156 std::vector<AbstractParamDescriptionConstPtr> abstract_parameters;
00157 bool state;
00158
00159 virtual void toMessage(dynamic_reconfigure::Config &msg, const boost::any &config) const = 0;
00160 virtual bool fromMessage(const dynamic_reconfigure::Config &msg, boost::any &config) const =0;
00161 virtual void updateParams(boost::any &cfg, JointspubConfig &top) const= 0;
00162 virtual void setInitialState(boost::any &cfg) const = 0;
00163
00164
00165 void convertParams()
00166 {
00167 for(std::vector<AbstractParamDescriptionConstPtr>::const_iterator i = abstract_parameters.begin(); i != abstract_parameters.end(); i++)
00168 {
00169 parameters.push_back(dynamic_reconfigure::ParamDescription(**i));
00170 }
00171 }
00172 };
00173
00174 typedef boost::shared_ptr<AbstractGroupDescription> AbstractGroupDescriptionPtr;
00175 typedef boost::shared_ptr<const AbstractGroupDescription> AbstractGroupDescriptionConstPtr;
00176
00177 template<class T, class PT>
00178 class GroupDescription : public AbstractGroupDescription
00179 {
00180 public:
00181 GroupDescription(std::string name, std::string type, int parent, int id, bool s, T PT::* f) : AbstractGroupDescription(name, type, parent, id, s), field(f)
00182 {
00183 }
00184
00185 GroupDescription(const GroupDescription<T, PT>& g): AbstractGroupDescription(g.name, g.type, g.parent, g.id, g.state), field(g.field), groups(g.groups)
00186 {
00187 parameters = g.parameters;
00188 abstract_parameters = g.abstract_parameters;
00189 }
00190
00191 virtual bool fromMessage(const dynamic_reconfigure::Config &msg, boost::any &cfg) const
00192 {
00193 PT* config = boost::any_cast<PT*>(cfg);
00194 if(!dynamic_reconfigure::ConfigTools::getGroupState(msg, name, (*config).*field))
00195 return false;
00196
00197 for(std::vector<AbstractGroupDescriptionConstPtr>::const_iterator i = groups.begin(); i != groups.end(); i++)
00198 {
00199 boost::any n = &((*config).*field);
00200 if(!(*i)->fromMessage(msg, n))
00201 return false;
00202 }
00203
00204 return true;
00205 }
00206
00207 virtual void setInitialState(boost::any &cfg) const
00208 {
00209 PT* config = boost::any_cast<PT*>(cfg);
00210 T* group = &((*config).*field);
00211 group->state = state;
00212
00213 for(std::vector<AbstractGroupDescriptionConstPtr>::const_iterator i = groups.begin(); i != groups.end(); i++)
00214 {
00215 boost::any n = boost::any(&((*config).*field));
00216 (*i)->setInitialState(n);
00217 }
00218
00219 }
00220
00221 virtual void updateParams(boost::any &cfg, JointspubConfig &top) const
00222 {
00223 PT* config = boost::any_cast<PT*>(cfg);
00224
00225 T* f = &((*config).*field);
00226 f->setParams(top, abstract_parameters);
00227
00228 for(std::vector<AbstractGroupDescriptionConstPtr>::const_iterator i = groups.begin(); i != groups.end(); i++)
00229 {
00230 boost::any n = &((*config).*field);
00231 (*i)->updateParams(n, top);
00232 }
00233 }
00234
00235 virtual void toMessage(dynamic_reconfigure::Config &msg, const boost::any &cfg) const
00236 {
00237 const PT config = boost::any_cast<PT>(cfg);
00238 dynamic_reconfigure::ConfigTools::appendGroup<T>(msg, name, id, parent, config.*field);
00239
00240 for(std::vector<AbstractGroupDescriptionConstPtr>::const_iterator i = groups.begin(); i != groups.end(); i++)
00241 {
00242 (*i)->toMessage(msg, config.*field);
00243 }
00244 }
00245
00246 T (PT::* field);
00247 std::vector<JointspubConfig::AbstractGroupDescriptionConstPtr> groups;
00248 };
00249
00250 class DEFAULT
00251 {
00252 public:
00253 DEFAULT()
00254 {
00255 state = true;
00256 name = "Default";
00257 }
00258
00259 void setParams(JointspubConfig &config, const std::vector<AbstractParamDescriptionConstPtr> params)
00260 {
00261 for (std::vector<AbstractParamDescriptionConstPtr>::const_iterator i = params.begin(); i != params.end(); i++)
00262 {
00263 boost::any val;
00264 (*i)->getValue(config, val);
00265
00266
00267 }
00268 }
00269
00270
00271
00272 bool state;
00273 std::string name;
00274
00275
00276 }groups;
00277
00278
00279
00280
00281
00282
00283 bool __fromMessage__(dynamic_reconfigure::Config &msg)
00284 {
00285 const std::vector<AbstractParamDescriptionConstPtr> &__param_descriptions__ = __getParamDescriptions__();
00286 const std::vector<AbstractGroupDescriptionConstPtr> &__group_descriptions__ = __getGroupDescriptions__();
00287
00288 int count = 0;
00289 for (std::vector<AbstractParamDescriptionConstPtr>::const_iterator i = __param_descriptions__.begin(); i != __param_descriptions__.end(); i++)
00290 if ((*i)->fromMessage(msg, *this))
00291 count++;
00292
00293 for (std::vector<AbstractGroupDescriptionConstPtr>::const_iterator i = __group_descriptions__.begin(); i != __group_descriptions__.end(); i ++)
00294 {
00295 if ((*i)->id == 0)
00296 {
00297 boost::any n = boost::any(this);
00298 (*i)->updateParams(n, *this);
00299 (*i)->fromMessage(msg, n);
00300 }
00301 }
00302
00303 if (count != dynamic_reconfigure::ConfigTools::size(msg))
00304 {
00305 ROS_ERROR("JointspubConfig::__fromMessage__ called with an unexpected parameter.");
00306 ROS_ERROR("Booleans:");
00307 for (unsigned int i = 0; i < msg.bools.size(); i++)
00308 ROS_ERROR(" %s", msg.bools[i].name.c_str());
00309 ROS_ERROR("Integers:");
00310 for (unsigned int i = 0; i < msg.ints.size(); i++)
00311 ROS_ERROR(" %s", msg.ints[i].name.c_str());
00312 ROS_ERROR("Doubles:");
00313 for (unsigned int i = 0; i < msg.doubles.size(); i++)
00314 ROS_ERROR(" %s", msg.doubles[i].name.c_str());
00315 ROS_ERROR("Strings:");
00316 for (unsigned int i = 0; i < msg.strs.size(); i++)
00317 ROS_ERROR(" %s", msg.strs[i].name.c_str());
00318
00319
00320 return false;
00321 }
00322 return true;
00323 }
00324
00325
00326
00327 void __toMessage__(dynamic_reconfigure::Config &msg, const std::vector<AbstractParamDescriptionConstPtr> &__param_descriptions__, const std::vector<AbstractGroupDescriptionConstPtr> &__group_descriptions__) const
00328 {
00329 dynamic_reconfigure::ConfigTools::clear(msg);
00330 for (std::vector<AbstractParamDescriptionConstPtr>::const_iterator i = __param_descriptions__.begin(); i != __param_descriptions__.end(); i++)
00331 (*i)->toMessage(msg, *this);
00332
00333 for (std::vector<AbstractGroupDescriptionConstPtr>::const_iterator i = __group_descriptions__.begin(); i != __group_descriptions__.end(); i++)
00334 {
00335 if((*i)->id == 0)
00336 {
00337 (*i)->toMessage(msg, *this);
00338 }
00339 }
00340 }
00341
00342 void __toMessage__(dynamic_reconfigure::Config &msg) const
00343 {
00344 const std::vector<AbstractParamDescriptionConstPtr> &__param_descriptions__ = __getParamDescriptions__();
00345 const std::vector<AbstractGroupDescriptionConstPtr> &__group_descriptions__ = __getGroupDescriptions__();
00346 __toMessage__(msg, __param_descriptions__, __group_descriptions__);
00347 }
00348
00349 void __toServer__(const ros::NodeHandle &nh) const
00350 {
00351 const std::vector<AbstractParamDescriptionConstPtr> &__param_descriptions__ = __getParamDescriptions__();
00352 for (std::vector<AbstractParamDescriptionConstPtr>::const_iterator i = __param_descriptions__.begin(); i != __param_descriptions__.end(); i++)
00353 (*i)->toServer(nh, *this);
00354 }
00355
00356 void __fromServer__(const ros::NodeHandle &nh)
00357 {
00358 static bool setup=false;
00359
00360 const std::vector<AbstractParamDescriptionConstPtr> &__param_descriptions__ = __getParamDescriptions__();
00361 for (std::vector<AbstractParamDescriptionConstPtr>::const_iterator i = __param_descriptions__.begin(); i != __param_descriptions__.end(); i++)
00362 (*i)->fromServer(nh, *this);
00363
00364 const std::vector<AbstractGroupDescriptionConstPtr> &__group_descriptions__ = __getGroupDescriptions__();
00365 for (std::vector<AbstractGroupDescriptionConstPtr>::const_iterator i = __group_descriptions__.begin(); i != __group_descriptions__.end(); i++){
00366 if (!setup && (*i)->id == 0) {
00367 setup = true;
00368 boost::any n = boost::any(this);
00369 (*i)->setInitialState(n);
00370 }
00371 }
00372 }
00373
00374 void __clamp__()
00375 {
00376 const std::vector<AbstractParamDescriptionConstPtr> &__param_descriptions__ = __getParamDescriptions__();
00377 const JointspubConfig &__max__ = __getMax__();
00378 const JointspubConfig &__min__ = __getMin__();
00379 for (std::vector<AbstractParamDescriptionConstPtr>::const_iterator i = __param_descriptions__.begin(); i != __param_descriptions__.end(); i++)
00380 (*i)->clamp(*this, __max__, __min__);
00381 }
00382
00383 uint32_t __level__(const JointspubConfig &config) const
00384 {
00385 const std::vector<AbstractParamDescriptionConstPtr> &__param_descriptions__ = __getParamDescriptions__();
00386 uint32_t level = 0;
00387 for (std::vector<AbstractParamDescriptionConstPtr>::const_iterator i = __param_descriptions__.begin(); i != __param_descriptions__.end(); i++)
00388 (*i)->calcLevel(level, config, *this);
00389 return level;
00390 }
00391
00392 static const dynamic_reconfigure::ConfigDescription &__getDescriptionMessage__();
00393 static const JointspubConfig &__getDefault__();
00394 static const JointspubConfig &__getMax__();
00395 static const JointspubConfig &__getMin__();
00396 static const std::vector<AbstractParamDescriptionConstPtr> &__getParamDescriptions__();
00397 static const std::vector<AbstractGroupDescriptionConstPtr> &__getGroupDescriptions__();
00398
00399 private:
00400 static const JointspubConfigStatics *__get_statics__();
00401 };
00402
00403 template <>
00404 inline void JointspubConfig::ParamDescription<std::string>::clamp(JointspubConfig &config, const JointspubConfig &max, const JointspubConfig &min) const
00405 {
00406 return;
00407 }
00408
00409 class JointspubConfigStatics
00410 {
00411 friend class JointspubConfig;
00412
00413 JointspubConfigStatics()
00414 {
00415 JointspubConfig::GroupDescription<JointspubConfig::DEFAULT, JointspubConfig> Default("Default", "", 0, 0, true, &JointspubConfig::groups);
00416
00417 Default.convertParams();
00418
00419 __group_descriptions__.push_back(JointspubConfig::AbstractGroupDescriptionConstPtr(new JointspubConfig::GroupDescription<JointspubConfig::DEFAULT, JointspubConfig>(Default)));
00420
00421
00422 for (std::vector<JointspubConfig::AbstractGroupDescriptionConstPtr>::const_iterator i = __group_descriptions__.begin(); i != __group_descriptions__.end(); i++)
00423 {
00424 __description_message__.groups.push_back(**i);
00425 }
00426 __max__.__toMessage__(__description_message__.max, __param_descriptions__, __group_descriptions__);
00427 __min__.__toMessage__(__description_message__.min, __param_descriptions__, __group_descriptions__);
00428 __default__.__toMessage__(__description_message__.dflt, __param_descriptions__, __group_descriptions__);
00429 }
00430 std::vector<JointspubConfig::AbstractParamDescriptionConstPtr> __param_descriptions__;
00431 std::vector<JointspubConfig::AbstractGroupDescriptionConstPtr> __group_descriptions__;
00432 JointspubConfig __max__;
00433 JointspubConfig __min__;
00434 JointspubConfig __default__;
00435 dynamic_reconfigure::ConfigDescription __description_message__;
00436
00437 static const JointspubConfigStatics *get_instance()
00438 {
00439
00440
00441
00442
00443 static JointspubConfigStatics instance;
00444 return &instance;
00445 }
00446 };
00447
00448 inline const dynamic_reconfigure::ConfigDescription &JointspubConfig::__getDescriptionMessage__()
00449 {
00450 return __get_statics__()->__description_message__;
00451 }
00452
00453 inline const JointspubConfig &JointspubConfig::__getDefault__()
00454 {
00455 return __get_statics__()->__default__;
00456 }
00457
00458 inline const JointspubConfig &JointspubConfig::__getMax__()
00459 {
00460 return __get_statics__()->__max__;
00461 }
00462
00463 inline const JointspubConfig &JointspubConfig::__getMin__()
00464 {
00465 return __get_statics__()->__min__;
00466 }
00467
00468 inline const std::vector<JointspubConfig::AbstractParamDescriptionConstPtr> &JointspubConfig::__getParamDescriptions__()
00469 {
00470 return __get_statics__()->__param_descriptions__;
00471 }
00472
00473 inline const std::vector<JointspubConfig::AbstractGroupDescriptionConstPtr> &JointspubConfig::__getGroupDescriptions__()
00474 {
00475 return __get_statics__()->__group_descriptions__;
00476 }
00477
00478 inline const JointspubConfigStatics *JointspubConfig::__get_statics__()
00479 {
00480 const static JointspubConfigStatics *statics;
00481
00482 if (statics)
00483 return statics;
00484
00485 boost::mutex::scoped_lock lock(dynamic_reconfigure::__init_mutex__);
00486
00487 if (statics)
00488 return statics;
00489
00490 statics = JointspubConfigStatics::get_instance();
00491
00492 return statics;
00493 }
00494
00495
00496 }
00497
00498 #endif // __JOINTSPUBRECONFIGURATOR_H__