30 #ifndef SWRI_ROSCPP_DYNAMIC_PARAMETERS_H_    31 #define SWRI_ROSCPP_DYNAMIC_PARAMETERS_H_    37 #include <boost/thread/mutex.hpp>    39 #include <yaml-cpp/yaml.h>    44 #include <dynamic_reconfigure/Config.h>    45 #include <dynamic_reconfigure/ConfigDescription.h>    46 #include <dynamic_reconfigure/GroupState.h>    47 #include <dynamic_reconfigure/Reconfigure.h>    65     std::vector<std::pair<std::string, int> > 
enums;
   141           dynamic_reconfigure::Reconfigure::Response &rsp)
   143       ROS_DEBUG(
"Got configuration change request");
   145       boost::mutex::scoped_lock lock(*mutex_);
   148       for (
size_t i = 0; 
i < req.config.doubles.size(); 
i++)
   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())
   154           ROS_ERROR(
"Could not find parameter '%s'", param.name.c_str());
   160           ROS_ERROR(
"Value '%s' was not a double type.", param.name.c_str());
   166           *iter->second.dbl = param.value;
   171           *iter->second.flt = (float)param.value;
   175       for (
size_t i = 0; 
i < req.config.ints.size(); 
i++)
   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())
   181           ROS_ERROR(
"Could not find parameter '%s'", param.name.c_str());
   187           ROS_ERROR(
"Value '%s' was not a int type.", param.name.c_str());
   191         *iter->second.integer = param.value;
   194       for (
size_t i = 0; 
i < req.config.bools.size(); 
i++)
   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())
   200           ROS_ERROR(
"Could not find parameter '%s'", param.name.c_str());
   206           ROS_ERROR(
"Value '%s' was not a bool type.", param.name.c_str());
   210         *iter->second.boolean = param.value;
   213       for (
size_t i = 0; 
i < req.config.strs.size(); 
i++)
   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())
   219           ROS_ERROR(
"Could not find parameter '%s'", param.name.c_str());
   225           ROS_ERROR(
"Value '%s' was not a string type.", param.name.c_str());
   229         *iter->second.str = param.value;
   232       updateCurrent(rsp.config);
   245       for (std::map<std::string, DynamicValue>::iterator value = values_.begin(); value != values_.end(); value++)
   249           dynamic_reconfigure::DoubleParameter 
param;
   250           param.name = value->first;
   251           param.value = *value->second.dbl;
   252           config.doubles.push_back(param);
   256           dynamic_reconfigure::DoubleParameter 
param;
   257           param.name = value->first;
   258           param.value = *value->second.flt;
   259           config.doubles.push_back(param);
   263           dynamic_reconfigure::IntParameter 
param;
   264           param.name = value->first;
   265           param.value = *value->second.integer;
   266           config.ints.push_back(param);
   270           dynamic_reconfigure::BoolParameter 
param;
   271           param.name = value->first;
   272           param.value = *value->second.boolean;
   273           config.bools.push_back(param);
   277           dynamic_reconfigure::StrParameter 
param;
   278           param.name = value->first;
   279           param.value = *value->second.str;
   280           config.strs.push_back(param);
   284       dynamic_reconfigure::GroupState gs;
   289       config.groups.push_back(gs);
   303       boost::mutex::scoped_lock lock(*mutex_);
   306       descr_pub_ = nh_->advertise<dynamic_reconfigure::ConfigDescription>(
"parameter_descriptions", 1, 
true);
   307       update_pub_ = nh_->advertise<dynamic_reconfigure::Config>(
"parameter_updates", 1, 
true);
   313       boost::mutex::scoped_lock lock(*mutex_);
   316       dynamic_reconfigure::ConfigDescription rdesc;
   317       dynamic_reconfigure::Group group;
   318       group.name = 
"Default";
   323       dynamic_reconfigure::GroupState gs;
   330       if (alphabetical_order)
   332         ordered_params_.clear();
   334         for (std::map<std::string, DynamicValue>::iterator it = values_.begin(); it != values_.end(); it++ )
   336           ordered_params_.push_back(it->first);
   339       for (
size_t i = 0; 
i < ordered_params_.size(); 
i++)
   341         std::map<std::string, DynamicValue>::iterator 
param = values_.find(ordered_params_[
i]);
   348           dynamic_reconfigure::BoolParameter desc;
   349           desc.name = param->first;
   351           rdesc.max.bools.push_back(desc);
   353           rdesc.min.bools.push_back(desc);
   354           desc.value = param->second.Default.b;
   355           rdesc.dflt.bools.push_back(desc);
   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);
   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);
   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);
   399           dynamic_reconfigure::StrParameter desc;
   400           desc.name = param->first;
   402           rdesc.max.strs.push_back(desc);
   404           rdesc.min.strs.push_back(desc);
   405           desc.value = param->second.default_string;
   406           rdesc.dflt.strs.push_back(desc);
   409         dynamic_reconfigure::ParamDescription desc;
   410         desc.name = param->first;
   413         desc.description = param->second.description;
   416         if (!param->second.enums.empty())
   418           YAML::Emitter emitter;
   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;
   427           for (
size_t j = 0; j < param->second.enums.size(); j++)
   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;
   440           emitter << YAML::EndSeq << YAML::EndMap;
   442           desc.edit_method = emitter.c_str();
   445         group.parameters.push_back(desc);
   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);
   453       dynamic_reconfigure::Config config;
   454       updateCurrent(config);
   456       set_service_ = nh_->advertiseService(
"set_parameters",
   459       ordered_params_.clear();
   464       std::map<std::string, DynamicValue>::iterator iter = values_.find(param);
   465       if (iter == values_.end())
   467         ROS_ERROR(
"Tried to add enum to nonexistant param %s", param.c_str());
   471       iter->second.enums.insert(iter->second.enums.end(), 
enums.begin(), 
enums.end());
   481       dynamic_reconfigure::Config config;
   482       updateCurrent(config);
   488       std::map<std::string, DynamicValue>::iterator iter = values_.find(name);
   489       if (iter == values_.end())
   491         ROS_ERROR(
"Tried to get nonexistant param %s", name.c_str());
   496         ROS_ERROR(
"Tried to load parameter %s with the wrong type: double.", name.c_str());
   500       return *iter->second.dbl;
   506       std::map<std::string, DynamicValue>::iterator iter = values_.find(name);
   507       if (iter == values_.end())
   509         ROS_ERROR(
"Tried to get nonexistant param %s", name.c_str());
   514         ROS_ERROR(
"Tried to load parameter %s with the wrong type: float.", name.c_str());
   518       return *iter->second.flt;
   523       std::map<std::string, DynamicValue>::iterator iter = values_.find(name);
   524       if (iter == values_.end())
   526         ROS_ERROR(
"Tried to get nonexistant param %s", name.c_str());
   531         ROS_ERROR(
"Tried to load parameter %s with the wrong type: int.", name.c_str());
   535       return *iter->second.integer;
   541       std::map<std::string, DynamicValue>::iterator iter = values_.find(name);
   542       if (iter == values_.end())
   544         ROS_ERROR(
"Tried to get nonexistant param %s", name.c_str());
   549         ROS_ERROR(
"Tried to load parameter %s with the wrong type: bool.", name.c_str());
   553       return *iter->second.boolean;
   559       std::map<std::string, DynamicValue>::iterator iter = values_.find(name);
   560       if (iter == values_.end())
   562         ROS_ERROR(
"Tried to get nonexistant param %s", name.c_str());
   567         ROS_ERROR(
"Tried to load parameter %s with the wrong type: string.", name.c_str());
   571       return *iter->second.str;
   584       return boost::mutex::scoped_lock(*mutex_);
   589     void get(
const std::string &
name,
   593       const float min = -100,
   594       const float max = 100)
   603       values_[
name] = value;
   604       ordered_params_.push_back(name);
   606       std::string resolved_name = nh_->resolveName(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);
   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)
   628       values_[
name] = value;
   629       ordered_params_.push_back(name);
   631       variable.data = value.
flt;
   632       variable.mutex = mutex_;
   634       std::string resolved_name = nh_->resolveName(name);
   636       nh_->param(name, *value.
flt, default_value);
   637       ROS_INFO(
"Read dynamic parameter %s = %f", name.c_str(), *variable);
   642     void get(
const std::string &
name,
   644       const double default_value,
   645       const std::string description = 
"None.",
   646       const double min = -100,
   647       const double max = 100)
   656       values_[
name] = value;
   657       ordered_params_.push_back(name);
   659       std::string resolved_name = nh_->resolveName(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);
   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)
   681       values_[
name] = value;
   682       ordered_params_.push_back(name);
   684       variable.data = value.
dbl;
   685       variable.mutex = mutex_;
   687       std::string resolved_name = nh_->resolveName(name);
   689       nh_->param(name, *value.
dbl, default_value);
   690       ROS_INFO(
"Read dynamic parameter %s = %lf", name.c_str(), *variable);
   694     void get(
const std::string &
name,
   696       const int default_value,
   697       const std::string description = 
"None.",
   698       const int min = -100,
   708       values_[
name] = value;
   709       ordered_params_.push_back(name);
   711       std::string resolved_name = nh_->resolveName(name);
   713       nh_->param(name, *value.
integer, default_value);
   715       ROS_INFO(
"Read dynamic parameter %s = %i", name.c_str(), variable);
   719     void get(
const std::string &
name,
   721       const int default_value,
   722       const std::string description = 
"None.",
   723       const int min = -100,
   733       values_[
name] = value;
   734       ordered_params_.push_back(name);
   737       variable.mutex = mutex_;
   739       std::string resolved_name = nh_->resolveName(name);
   741       nh_->param(name, *value.
integer, default_value);
   742       ROS_INFO(
"Read dynamic parameter %s = %i", name.c_str(), *variable);
   747     void get(
const std::string &
name,
   749       const bool default_value,
   750       const std::string description = 
"None.")
   757       values_[
name] = value;
   758       ordered_params_.push_back(name);
   760       std::string resolved_name = nh_->resolveName(name);
   762       nh_->param(name, *value.
boolean, default_value);
   764       ROS_INFO(
"Read dynamic parameter %s = %s", name.c_str(), variable ? 
"true" : 
"false");
   768     void get(
const std::string &
name,
   770       const bool default_value,
   771       const std::string description = 
"None.")
   778       values_[
name] = value;
   779       ordered_params_.push_back(name);
   782       variable.mutex = mutex_;
   784       std::string resolved_name = nh_->resolveName(name);
   786       nh_->param(name, *value.
boolean, default_value);
   787       ROS_INFO(
"Read dynamic parameter %s = %s", name.c_str(), *variable ? 
"true" : 
"false");
   792     void get(
const std::string &
name,
   793       std::string &variable,
   794       const std::string default_value,
   795       const std::string description = 
"None.")
   802       values_[
name] = value;
   803       ordered_params_.push_back(name);
   805       std::string resolved_name = nh_->resolveName(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());
   813     void get(
const std::string &
name,
   814       StringParam &variable,
   815       const std::string default_value,
   816       const std::string description = 
"None.")
   823       values_[
name] = value;
   824       ordered_params_.push_back(name);
   826       variable.data = value.
str;
   827       variable.mutex = mutex_;
   829       std::string resolved_name = nh_->resolveName(name);
   831       nh_->param(name, *value.
str, default_value);
   832       ROS_INFO(
"Read dynamic parameter %s = %s", name.c_str(), (*variable).c_str());
   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
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)
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
ros::Publisher update_pub_
union swri::DynamicValue::@0 Default
TypedParam< float > FloatParam
std::string default_string
int getInt(const std::string &name)
ros::ServiceServer set_service_
union swri::DynamicValue::@2 Max
ros::Publisher descr_pub_
void addEnums(const std::string ¶m, const std::vector< std::pair< std::string, int > > &enums)