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 __ipa_canopen_ros_simple__CANOPEN_ROSCONFIG_H__
00048 #define __ipa_canopen_ros_simple__CANOPEN_ROSCONFIG_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 ipa_canopen_ros_simple
00060 {
00061 class canopen_rosConfigStatics;
00062
00063 class canopen_rosConfig
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(canopen_rosConfig &config, const canopen_rosConfig &max, const canopen_rosConfig &min) const = 0;
00080 virtual void calcLevel(uint32_t &level, const canopen_rosConfig &config1, const canopen_rosConfig &config2) const = 0;
00081 virtual void fromServer(const ros::NodeHandle &nh, canopen_rosConfig &config) const = 0;
00082 virtual void toServer(const ros::NodeHandle &nh, const canopen_rosConfig &config) const = 0;
00083 virtual bool fromMessage(const dynamic_reconfigure::Config &msg, canopen_rosConfig &config) const = 0;
00084 virtual void toMessage(dynamic_reconfigure::Config &msg, const canopen_rosConfig &config) const = 0;
00085 virtual void getValue(const canopen_rosConfig &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 canopen_rosConfig::* f) :
00097 AbstractParamDescription(name, type, level, description, edit_method),
00098 field(f)
00099 {}
00100
00101 T (canopen_rosConfig::* field);
00102
00103 virtual void clamp(canopen_rosConfig &config, const canopen_rosConfig &max, const canopen_rosConfig &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 canopen_rosConfig &config1, const canopen_rosConfig &config2) const
00113 {
00114 if (config1.*field != config2.*field)
00115 comb_level |= level;
00116 }
00117
00118 virtual void fromServer(const ros::NodeHandle &nh, canopen_rosConfig &config) const
00119 {
00120 nh.getParam(name, config.*field);
00121 }
00122
00123 virtual void toServer(const ros::NodeHandle &nh, const canopen_rosConfig &config) const
00124 {
00125 nh.setParam(name, config.*field);
00126 }
00127
00128 virtual bool fromMessage(const dynamic_reconfigure::Config &msg, canopen_rosConfig &config) const
00129 {
00130 return dynamic_reconfigure::ConfigTools::getParameter(msg, name, config.*field);
00131 }
00132
00133 virtual void toMessage(dynamic_reconfigure::Config &msg, const canopen_rosConfig &config) const
00134 {
00135 dynamic_reconfigure::ConfigTools::appendParameter(msg, name, config.*field);
00136 }
00137
00138 virtual void getValue(const canopen_rosConfig &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, canopen_rosConfig &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, canopen_rosConfig &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<canopen_rosConfig::AbstractGroupDescriptionConstPtr> groups;
00248 };
00249
00250 class DEFAULT
00251 {
00252 public:
00253 DEFAULT()
00254 {
00255 state = true;
00256 name = "Default";
00257 }
00258
00259 void setParams(canopen_rosConfig &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 if("can_device"==(*i)->name){can_device = boost::any_cast<std::string>(val);}
00267 if("can_baudrate"==(*i)->name){can_baudrate = boost::any_cast<std::string>(val);}
00268 if("modul_ids"==(*i)->name){modul_ids = boost::any_cast<std::string>(val);}
00269 if("joint_names"==(*i)->name){joint_names = boost::any_cast<std::string>(val);}
00270 if("robot_description"==(*i)->name){robot_description = boost::any_cast<std::string>(val);}
00271 }
00272 }
00273
00274 std::string can_device;
00275 std::string can_baudrate;
00276 std::string modul_ids;
00277 std::string joint_names;
00278 std::string robot_description;
00279
00280 bool state;
00281 std::string name;
00282
00283
00284 }groups;
00285
00286
00287
00288
00289 std::string can_device;
00290
00291 std::string can_baudrate;
00292
00293 std::string modul_ids;
00294
00295 std::string joint_names;
00296
00297 std::string robot_description;
00298
00299
00300 bool __fromMessage__(dynamic_reconfigure::Config &msg)
00301 {
00302 const std::vector<AbstractParamDescriptionConstPtr> &__param_descriptions__ = __getParamDescriptions__();
00303 const std::vector<AbstractGroupDescriptionConstPtr> &__group_descriptions__ = __getGroupDescriptions__();
00304
00305 int count = 0;
00306 for (std::vector<AbstractParamDescriptionConstPtr>::const_iterator i = __param_descriptions__.begin(); i != __param_descriptions__.end(); i++)
00307 if ((*i)->fromMessage(msg, *this))
00308 count++;
00309
00310 for (std::vector<AbstractGroupDescriptionConstPtr>::const_iterator i = __group_descriptions__.begin(); i != __group_descriptions__.end(); i ++)
00311 {
00312 if ((*i)->id == 0)
00313 {
00314 boost::any n = boost::any(this);
00315 (*i)->updateParams(n, *this);
00316 (*i)->fromMessage(msg, n);
00317 }
00318 }
00319
00320 if (count != dynamic_reconfigure::ConfigTools::size(msg))
00321 {
00322 ROS_ERROR("canopen_rosConfig::__fromMessage__ called with an unexpected parameter.");
00323 ROS_ERROR("Booleans:");
00324 for (unsigned int i = 0; i < msg.bools.size(); i++)
00325 ROS_ERROR(" %s", msg.bools[i].name.c_str());
00326 ROS_ERROR("Integers:");
00327 for (unsigned int i = 0; i < msg.ints.size(); i++)
00328 ROS_ERROR(" %s", msg.ints[i].name.c_str());
00329 ROS_ERROR("Doubles:");
00330 for (unsigned int i = 0; i < msg.doubles.size(); i++)
00331 ROS_ERROR(" %s", msg.doubles[i].name.c_str());
00332 ROS_ERROR("Strings:");
00333 for (unsigned int i = 0; i < msg.strs.size(); i++)
00334 ROS_ERROR(" %s", msg.strs[i].name.c_str());
00335
00336
00337 return false;
00338 }
00339 return true;
00340 }
00341
00342
00343
00344 void __toMessage__(dynamic_reconfigure::Config &msg, const std::vector<AbstractParamDescriptionConstPtr> &__param_descriptions__, const std::vector<AbstractGroupDescriptionConstPtr> &__group_descriptions__) const
00345 {
00346 dynamic_reconfigure::ConfigTools::clear(msg);
00347 for (std::vector<AbstractParamDescriptionConstPtr>::const_iterator i = __param_descriptions__.begin(); i != __param_descriptions__.end(); i++)
00348 (*i)->toMessage(msg, *this);
00349
00350 for (std::vector<AbstractGroupDescriptionConstPtr>::const_iterator i = __group_descriptions__.begin(); i != __group_descriptions__.end(); i++)
00351 {
00352 if((*i)->id == 0)
00353 {
00354 (*i)->toMessage(msg, *this);
00355 }
00356 }
00357 }
00358
00359 void __toMessage__(dynamic_reconfigure::Config &msg) const
00360 {
00361 const std::vector<AbstractParamDescriptionConstPtr> &__param_descriptions__ = __getParamDescriptions__();
00362 const std::vector<AbstractGroupDescriptionConstPtr> &__group_descriptions__ = __getGroupDescriptions__();
00363 __toMessage__(msg, __param_descriptions__, __group_descriptions__);
00364 }
00365
00366 void __toServer__(const ros::NodeHandle &nh) const
00367 {
00368 const std::vector<AbstractParamDescriptionConstPtr> &__param_descriptions__ = __getParamDescriptions__();
00369 for (std::vector<AbstractParamDescriptionConstPtr>::const_iterator i = __param_descriptions__.begin(); i != __param_descriptions__.end(); i++)
00370 (*i)->toServer(nh, *this);
00371 }
00372
00373 void __fromServer__(const ros::NodeHandle &nh)
00374 {
00375 static bool setup=false;
00376
00377 const std::vector<AbstractParamDescriptionConstPtr> &__param_descriptions__ = __getParamDescriptions__();
00378 for (std::vector<AbstractParamDescriptionConstPtr>::const_iterator i = __param_descriptions__.begin(); i != __param_descriptions__.end(); i++)
00379 (*i)->fromServer(nh, *this);
00380
00381 const std::vector<AbstractGroupDescriptionConstPtr> &__group_descriptions__ = __getGroupDescriptions__();
00382 for (std::vector<AbstractGroupDescriptionConstPtr>::const_iterator i = __group_descriptions__.begin(); i != __group_descriptions__.end(); i++){
00383 if (!setup && (*i)->id == 0) {
00384 setup = true;
00385 boost::any n = boost::any(this);
00386 (*i)->setInitialState(n);
00387 }
00388 }
00389 }
00390
00391 void __clamp__()
00392 {
00393 const std::vector<AbstractParamDescriptionConstPtr> &__param_descriptions__ = __getParamDescriptions__();
00394 const canopen_rosConfig &__max__ = __getMax__();
00395 const canopen_rosConfig &__min__ = __getMin__();
00396 for (std::vector<AbstractParamDescriptionConstPtr>::const_iterator i = __param_descriptions__.begin(); i != __param_descriptions__.end(); i++)
00397 (*i)->clamp(*this, __max__, __min__);
00398 }
00399
00400 uint32_t __level__(const canopen_rosConfig &config) const
00401 {
00402 const std::vector<AbstractParamDescriptionConstPtr> &__param_descriptions__ = __getParamDescriptions__();
00403 uint32_t level = 0;
00404 for (std::vector<AbstractParamDescriptionConstPtr>::const_iterator i = __param_descriptions__.begin(); i != __param_descriptions__.end(); i++)
00405 (*i)->calcLevel(level, config, *this);
00406 return level;
00407 }
00408
00409 static const dynamic_reconfigure::ConfigDescription &__getDescriptionMessage__();
00410 static const canopen_rosConfig &__getDefault__();
00411 static const canopen_rosConfig &__getMax__();
00412 static const canopen_rosConfig &__getMin__();
00413 static const std::vector<AbstractParamDescriptionConstPtr> &__getParamDescriptions__();
00414 static const std::vector<AbstractGroupDescriptionConstPtr> &__getGroupDescriptions__();
00415
00416 private:
00417 static const canopen_rosConfigStatics *__get_statics__();
00418 };
00419
00420 template <>
00421 inline void canopen_rosConfig::ParamDescription<std::string>::clamp(canopen_rosConfig &config, const canopen_rosConfig &max, const canopen_rosConfig &min) const
00422 {
00423 return;
00424 }
00425
00426 class canopen_rosConfigStatics
00427 {
00428 friend class canopen_rosConfig;
00429
00430 canopen_rosConfigStatics()
00431 {
00432 canopen_rosConfig::GroupDescription<canopen_rosConfig::DEFAULT, canopen_rosConfig> Default("Default", "", 0, 0, true, &canopen_rosConfig::groups);
00433
00434 __min__.can_device = "";
00435
00436 __max__.can_device = "";
00437
00438 __default__.can_device = "";
00439
00440 Default.abstract_parameters.push_back(canopen_rosConfig::AbstractParamDescriptionConstPtr(new canopen_rosConfig::ParamDescription<std::string>("can_device", "str", 0, "Autogenerated parameter based on model.", "", &canopen_rosConfig::can_device)));
00441
00442 __param_descriptions__.push_back(canopen_rosConfig::AbstractParamDescriptionConstPtr(new canopen_rosConfig::ParamDescription<std::string>("can_device", "str", 0, "Autogenerated parameter based on model.", "", &canopen_rosConfig::can_device)));
00443
00444 __min__.can_baudrate = "";
00445
00446 __max__.can_baudrate = "";
00447
00448 __default__.can_baudrate = "1000";
00449
00450 Default.abstract_parameters.push_back(canopen_rosConfig::AbstractParamDescriptionConstPtr(new canopen_rosConfig::ParamDescription<std::string>("can_baudrate", "str", 0, "Autogenerated parameter based on model.", "", &canopen_rosConfig::can_baudrate)));
00451
00452 __param_descriptions__.push_back(canopen_rosConfig::AbstractParamDescriptionConstPtr(new canopen_rosConfig::ParamDescription<std::string>("can_baudrate", "str", 0, "Autogenerated parameter based on model.", "", &canopen_rosConfig::can_baudrate)));
00453
00454 __min__.modul_ids = "";
00455
00456 __max__.modul_ids = "";
00457
00458 __default__.modul_ids = "";
00459
00460 Default.abstract_parameters.push_back(canopen_rosConfig::AbstractParamDescriptionConstPtr(new canopen_rosConfig::ParamDescription<std::string>("modul_ids", "str", 0, "Autogenerated parameter based on model.", "", &canopen_rosConfig::modul_ids)));
00461
00462 __param_descriptions__.push_back(canopen_rosConfig::AbstractParamDescriptionConstPtr(new canopen_rosConfig::ParamDescription<std::string>("modul_ids", "str", 0, "Autogenerated parameter based on model.", "", &canopen_rosConfig::modul_ids)));
00463
00464 __min__.joint_names = "";
00465
00466 __max__.joint_names = "";
00467
00468 __default__.joint_names = "[torso_1_joint, torso_2_joint, torso_3_joint]";
00469
00470 Default.abstract_parameters.push_back(canopen_rosConfig::AbstractParamDescriptionConstPtr(new canopen_rosConfig::ParamDescription<std::string>("joint_names", "str", 0, "Autogenerated parameter based on model.", "", &canopen_rosConfig::joint_names)));
00471
00472 __param_descriptions__.push_back(canopen_rosConfig::AbstractParamDescriptionConstPtr(new canopen_rosConfig::ParamDescription<std::string>("joint_names", "str", 0, "Autogenerated parameter based on model.", "", &canopen_rosConfig::joint_names)));
00473
00474 __min__.robot_description = "";
00475
00476 __max__.robot_description = "";
00477
00478 __default__.robot_description = "";
00479
00480 Default.abstract_parameters.push_back(canopen_rosConfig::AbstractParamDescriptionConstPtr(new canopen_rosConfig::ParamDescription<std::string>("robot_description", "str", 0, "Autogenerated parameter based on model.", "", &canopen_rosConfig::robot_description)));
00481
00482 __param_descriptions__.push_back(canopen_rosConfig::AbstractParamDescriptionConstPtr(new canopen_rosConfig::ParamDescription<std::string>("robot_description", "str", 0, "Autogenerated parameter based on model.", "", &canopen_rosConfig::robot_description)));
00483
00484 Default.convertParams();
00485
00486 __group_descriptions__.push_back(canopen_rosConfig::AbstractGroupDescriptionConstPtr(new canopen_rosConfig::GroupDescription<canopen_rosConfig::DEFAULT, canopen_rosConfig>(Default)));
00487
00488
00489 for (std::vector<canopen_rosConfig::AbstractGroupDescriptionConstPtr>::const_iterator i = __group_descriptions__.begin(); i != __group_descriptions__.end(); i++)
00490 {
00491 __description_message__.groups.push_back(**i);
00492 }
00493 __max__.__toMessage__(__description_message__.max, __param_descriptions__, __group_descriptions__);
00494 __min__.__toMessage__(__description_message__.min, __param_descriptions__, __group_descriptions__);
00495 __default__.__toMessage__(__description_message__.dflt, __param_descriptions__, __group_descriptions__);
00496 }
00497 std::vector<canopen_rosConfig::AbstractParamDescriptionConstPtr> __param_descriptions__;
00498 std::vector<canopen_rosConfig::AbstractGroupDescriptionConstPtr> __group_descriptions__;
00499 canopen_rosConfig __max__;
00500 canopen_rosConfig __min__;
00501 canopen_rosConfig __default__;
00502 dynamic_reconfigure::ConfigDescription __description_message__;
00503
00504 static const canopen_rosConfigStatics *get_instance()
00505 {
00506
00507
00508
00509
00510 static canopen_rosConfigStatics instance;
00511 return &instance;
00512 }
00513 };
00514
00515 inline const dynamic_reconfigure::ConfigDescription &canopen_rosConfig::__getDescriptionMessage__()
00516 {
00517 return __get_statics__()->__description_message__;
00518 }
00519
00520 inline const canopen_rosConfig &canopen_rosConfig::__getDefault__()
00521 {
00522 return __get_statics__()->__default__;
00523 }
00524
00525 inline const canopen_rosConfig &canopen_rosConfig::__getMax__()
00526 {
00527 return __get_statics__()->__max__;
00528 }
00529
00530 inline const canopen_rosConfig &canopen_rosConfig::__getMin__()
00531 {
00532 return __get_statics__()->__min__;
00533 }
00534
00535 inline const std::vector<canopen_rosConfig::AbstractParamDescriptionConstPtr> &canopen_rosConfig::__getParamDescriptions__()
00536 {
00537 return __get_statics__()->__param_descriptions__;
00538 }
00539
00540 inline const std::vector<canopen_rosConfig::AbstractGroupDescriptionConstPtr> &canopen_rosConfig::__getGroupDescriptions__()
00541 {
00542 return __get_statics__()->__group_descriptions__;
00543 }
00544
00545 inline const canopen_rosConfigStatics *canopen_rosConfig::__get_statics__()
00546 {
00547 const static canopen_rosConfigStatics *statics;
00548
00549 if (statics)
00550 return statics;
00551
00552 boost::mutex::scoped_lock lock(dynamic_reconfigure::__init_mutex__);
00553
00554 if (statics)
00555 return statics;
00556
00557 statics = canopen_rosConfigStatics::get_instance();
00558
00559 return statics;
00560 }
00561
00562
00563 }
00564
00565 #endif // __CANOPEN_ROSRECONFIGURATOR_H__