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)