31 #ifndef RTT_DYNAMIC_RECONFIGURE_SERVER_H 32 #define RTT_DYNAMIC_RECONFIGURE_SERVER_H 46 #include <dynamic_reconfigure/Reconfigure.h> 47 #include <dynamic_reconfigure/Config.h> 48 #include <dynamic_reconfigure/ConfigDescription.h> 50 #include <rtt/rtt-config.h> 51 #if !defined(RTT_VERSION_GTE) 52 #define RTT_VERSION_GTE(major,minor,patch) \ 53 ((RTT_VERSION_MAJOR > major) || (RTT_VERSION_MAJOR == major && \ 54 (RTT_VERSION_MINOR > minor) || (RTT_VERSION_MINOR == minor && \ 55 (RTT_VERSION_PATCH >= patch)))) 60 template <
class ConfigType>
class Server;
70 template <
class ConfigType>
76 template <
class ConfigType>
86 static void getMin(ConfigType &min,
const ServerType *) { min = ConfigType::__getMin__(); }
94 static void getMax(ConfigType &max,
const ServerType *) { max = ConfigType::__getMax__(); }
102 static void getDefault(ConfigType &dflt,
const ServerType *) { dflt = ConfigType::__getDefault__(); }
110 static dynamic_reconfigure::ConfigDescriptionPtr
getDescriptionMessage(
const ServerType *) {
return dynamic_reconfigure::ConfigDescriptionPtr(
new dynamic_reconfigure::ConfigDescription(ConfigType::__getDescriptionMessage__())); }
115 static const bool canRefresh =
false;
131 static void toMessage(ConfigType &config, dynamic_reconfigure::Config &message,
const ServerType *) { config.__toMessage__(message); }
140 static void fromMessage(ConfigType &config, dynamic_reconfigure::Config &message,
const ServerType *) { config.__fromMessage__(message); }
148 static void clamp(ConfigType &config,
const ServerType *) { config.__clamp__(); }
165 struct null_deleter {
166 void operator()(
const void *)
const {}
171 class DynamicReconfigureTestComponent;
177 template <
class ConfigType>
212 :
RTT::Service(name, owner)
214 , update_callback_default_impl_(
"updateProperties", &
Server<ConfigType>::updatePropertiesDefaultImpl, this,
RTT::OwnThread, owner->engine())
225 :
RTT::Service(
"reconfigure", owner)
227 , update_callback_default_impl_(
"updateProperties", &
Server<ConfigType>::updatePropertiesDefaultImpl, this,
RTT::OwnThread, owner->engine())
254 updateConfigInternal(config);
337 PublishDescription();
348 PublishDescription();
359 PublishDescription();
368 dynamic_reconfigure::ConfigDescriptionPtr description_message = traits::getDescriptionMessage(
this);
371 traits::toMessage(max_, description_message->max,
this);
372 traits::toMessage(min_, description_message->min,
this);
373 traits::toMessage(default_, description_message->dflt,
this);
375 return description_message;
391 if (getOwner()->getName() ==
"Deployer")
394 ns =
"~" + getOwner()->getName();
402 descr_pub_ = node_handle_->
advertise<dynamic_reconfigure::ConfigDescription>(
"parameter_descriptions", 1,
true);
403 update_pub_ = node_handle_->
advertise<dynamic_reconfigure::Config>(
"parameter_updates", 1,
true);
406 PublishDescription();
407 updateConfigInternal(config_);
430 ConfigType new_config = config_;
432 updateConfig(new_config);
445 traits::refreshDescription(
this);
448 traits::getMin(min_,
this);
449 traits::getMax(max_,
this);
450 traits::getDefault(default_,
this);
453 PublishDescription();
456 this->properties()->remove(this->properties()->
getProperty(
"min"));
457 this->properties()->remove(this->properties()->
getProperty(
"max"));
458 this->properties()->remove(this->properties()->
getProperty(
"default"));
459 this->properties()->ownProperty(
new RTT::Property<RTT::PropertyBag>(
"min",
"Minimum values as published to dynamic_reconfigure clients", traits::getDataSource(min_,
this)));
460 this->properties()->ownProperty(
new RTT::Property<RTT::PropertyBag>(
"max",
"Maximum values as published to dynamic_reconfigure clients", traits::getDataSource(max_,
this)));
461 this->properties()->ownProperty(
new RTT::Property<RTT::PropertyBag>(
"default",
"Default values as published to dynamic_reconfigure clients", traits::getDataSource(default_,
this)));
464 config_ = ConfigType();
465 traits::getDefault(config_,
this);
466 updater()->configFromProperties(config_, *(getOwner()->properties()));
468 config_.__fromServer__(*node_handle_);
469 traits::clamp(config_,
this);
473 updater()->propertiesFromConfig(config_, ~0, init_config);
476 #if !RTT_VERSION_GTE(2,8,99) 488 if (update_callback_impl) {
493 update_callback_impl.reset(update_callback_impl->cloneI(engine));
494 update_callback_impl->call(init_config, ~0);
496 update_callback_(init_config, ~0);
500 if (update_callback_const_impl) {
505 update_callback_const_impl.reset(update_callback_const_impl->cloneI(engine));
506 update_callback_const_impl->call(init_config, ~0);
508 update_callback_const_(init_config, ~0);
512 if (notify_callback_impl) {
517 notify_callback_impl.reset(notify_callback_impl->cloneI(engine));
518 notify_callback_impl->call(~0);
520 notify_callback_(~0);
524 if (update_callback_.
ready()) {
525 update_callback_(init_config, ~0);
526 }
else if (update_callback_const_.
ready()) {
527 update_callback_const_(init_config, ~0);
529 if (notify_callback_.
ready()) {
530 notify_callback_(~0);
534 updateConfigInternal(config_);
544 if (!updater_) updater_.reset(
new UpdaterType());
545 return updater_.get();
555 updater_.reset(updater, null_deleter());
565 update_callback_ = impl;
575 notify_callback_ = impl;
582 .doc(
"Advertise this dynamic_reconfigure server at the master.")
583 .arg(
"namespace",
"The namespace this server should be advertised in. Defaults to ~component.");
585 .doc(
"Shutdown this dynamic_reconfigure server.");
587 .doc(
"Notify the dynamic_reconfigure server that properties have been updated. This will update the GUI.");
590 .doc(
"Rediscover the owner's properties or update advertised min/max/default values. Call this operation after having added properties.");
593 UpdaterType *updater =
dynamic_cast<UpdaterType *
>(getOwner());
594 if (updater) setUpdater(updater);
597 if (getOwner() && getOwner()->provides()->hasMember(
"updateProperties")) {
600 update_callback_ = op;
602 update_callback_const_ = op;
609 if (getOwner() && getOwner()->provides()->hasMember(
"notifyPropertiesUpdate")) {
610 notify_callback_ = getOwner()->provides()->getPart(
"notifyPropertiesUpdate");
623 if (!descr_pub_)
return;
624 descr_pub_.
publish(getDescriptionMessage());
627 friend class DynamicReconfigureTestComponent;
629 dynamic_reconfigure::Reconfigure::Response &rsp)
633 ConfigType new_config = config_;
634 traits::fromMessage(new_config, req.config,
this);
635 traits::clamp(new_config,
this);
636 uint32_t level = config_.__level__(new_config);
640 if (update_callback_.
ready()) {
641 if (!update_callback_(bag, level))
return false;
642 updater()->configFromProperties(new_config, bag);
643 }
else if (update_callback_const_.
ready()) {
644 if (!update_callback_const_(bag, level))
return false;
648 if (notify_callback_.
ready()) notify_callback_(level);
650 updateConfigInternal(new_config);
651 new_config.__toMessage__(rsp.config);
660 config_.__toServer__(*node_handle_);
661 dynamic_reconfigure::Config msg;
662 config_.__toMessage__(msg);
684 template <
typename T,
typename ValueType>
697 if (boost::is_same<T,ValueType>::value) {
717 template <
typename T,
typename ValueType>
732 #define RTT_DYNAMIC_RECONFIGURE_SERVICE_PLUGIN(CONFIG, NAME) \ 734 RTT_EXPORT bool loadRTTPlugin(RTT::TaskContext* tc); \ 735 bool loadRTTPlugin(RTT::TaskContext* tc) { \ 736 if (tc == 0) return true; \ 737 RTT::Service::shared_ptr sp( new rtt_dynamic_reconfigure::Server<CONFIG>(NAME, tc ) ); \ 738 return tc->provides()->addService( sp ); \ 740 RTT_EXPORT RTT::Service::shared_ptr createService(); \ 741 RTT::Service::shared_ptr createService() { \ 742 RTT::Service::shared_ptr sp( new rtt_dynamic_reconfigure::Server<CONFIG>( 0 ) ); \ 745 RTT_EXPORT std::string getRTTPluginName(); \ 746 std::string getRTTPluginName() { \ 749 RTT_EXPORT std::string getRTTTargetName(); \ 750 std::string getRTTTargetName() { \ 751 return OROCOS_TARGET_NAME; \ 755 #endif // RTT_DYNAMIC_RECONFIGURE_SERVER_H
virtual os::ThreadInterface * getThread() const
virtual base::OperationCallerBase< Signature >::shared_ptr getOperationCaller()
Property< T > * getPropertyType(const std::string &name) const
void publish(const boost::shared_ptr< M > &message) const
bool updateProperties(PropertyBag &target, const PropertyBag &source)
const OperationCallerBasePtr getOperationCallerImpl() const
Variable opBinary s not applicable to op
virtual void set(param_t t)=0
std::string getName(void *handle)
ServiceServer advertiseService(const std::string &service, bool(T::*srv_func)(MReq &, MRes &), T *obj)
const_reference_t rvalue() const
void setCaller(ExecutionEngine *caller)
bool ownProperty(base::PropertyBase *p)
Property< T > & addProperty(const std::string &name, T &attr)
static RTT_API ExecutionEngine * Instance()
Publisher advertise(const std::string &topic, uint32_t queue_size, bool latch=false)
boost::intrusive_ptr< AssignableDataSource< T > > shared_ptr
virtual RTT_API boost::shared_ptr< base::DisposableInterface > getLocalOperation() const
base::PropertyBase * getProperty(const std::string &name) const
ROSCONSOLE_DECL void shutdown()
static Logger::LogFunction endlog()