server.h
Go to the documentation of this file.
00001 /*********************************************************************
00002 * Software License Agreement (BSD License)
00003 *
00004 *  Copyright (c) 2009-2010, Willow Garage, Inc.
00005 *  Copyright (c) 2014, Intermodalics BVBA
00006 *  All rights reserved.
00007 *
00008 *  Redistribution and use in source and binary forms, with or without
00009 *  modification, are permitted provided that the following conditions
00010 *  are met:
00011 *
00012 *   * Redistributions of source code must retain the above copyright
00013 *     notice, this list of conditions and the following disclaimer.
00014 *   * Redistributions in binary form must reproduce the above
00015 *     copyright notice, this list of conditions and the following
00016 *     disclaimer in the documentation and/or other materials provided
00017 *     with the distribution.
00018 *   * Neither the name of the Willow Garage nor the names of its
00019 *     contributors may be used to endorse or promote products derived
00020 *     from this software without specific prior written permission.
00021 *
00022 *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
00023 *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
00024 *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
00025 *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
00026 *  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
00027 *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
00028 *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
00029 *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
00030 *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
00031 *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
00032 *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
00033 *  POSSIBILITY OF SUCH DAMAGE.
00034 *********************************************************************/
00035 
00036 #ifndef RTT_DYNAMIC_RECONFIGURE_SERVER_H
00037 #define RTT_DYNAMIC_RECONFIGURE_SERVER_H
00038 
00039 #include <rtt/Service.hpp>
00040 #include <rtt/TaskContext.hpp>
00041 #include <rtt/os/Mutex.hpp>
00042 #include <rtt/Logger.hpp>
00043 #include <rtt/Operation.hpp>
00044 #include <rtt/OperationCaller.hpp>
00045 
00046 #include <rtt/internal/DataSources.hpp>
00047 #include <rtt/internal/GlobalEngine.hpp>
00048 
00049 #include <ros/ros.h>
00050 
00051 #include <dynamic_reconfigure/Reconfigure.h>
00052 #include <dynamic_reconfigure/Config.h>
00053 #include <dynamic_reconfigure/ConfigDescription.h>
00054 
00055 #include <rtt/rtt-config.h>
00056 #if !defined(RTT_VERSION_GTE)
00057     #define RTT_VERSION_GTE(major,minor,patch) \
00058         ((RTT_VERSION_MAJOR > major) || (RTT_VERSION_MAJOR == major && \
00059          (RTT_VERSION_MINOR > minor) || (RTT_VERSION_MINOR == minor && \
00060          (RTT_VERSION_PATCH >= patch))))
00061 #endif
00062 
00063 namespace rtt_dynamic_reconfigure {
00064 
00065 template <class ConfigType> class Server;
00066 typedef bool (UpdateCallbackSignature)(RTT::PropertyBag &bag, uint32_t level);
00067 typedef bool (UpdateCallbackConstSignature)(const RTT::PropertyBag &bag, uint32_t level);
00068 typedef void (NotifyCallbackSignature)(uint32_t level);
00069 
00075 template <class ConfigType>
00076 struct Updater {
00077     virtual bool propertiesFromConfig(ConfigType &config, uint32_t level, RTT::PropertyBag &) { return false; }
00078     virtual bool configFromProperties(ConfigType &config, const RTT::PropertyBag &) { return false; }
00079 };
00080 
00081 template <class ConfigType>
00082 struct dynamic_reconfigure_traits {
00083     typedef Server<ConfigType> ServerType;
00084 
00091     static void getMin(ConfigType &min, const ServerType *) { min = ConfigType::__getMin__(); }
00092 
00099     static void getMax(ConfigType &max, const ServerType *) { max = ConfigType::__getMax__(); }
00100 
00107     static void getDefault(ConfigType &dflt, const ServerType *) { dflt = ConfigType::__getDefault__(); }
00108 
00115     static dynamic_reconfigure::ConfigDescriptionPtr getDescriptionMessage(const ServerType *) { return dynamic_reconfigure::ConfigDescriptionPtr(new dynamic_reconfigure::ConfigDescription(ConfigType::__getDescriptionMessage__())); }
00116 
00120     static const bool canRefresh = false;
00121 
00127     static void refreshDescription(const ServerType *) {}
00128 
00136     static void toMessage(ConfigType &config, dynamic_reconfigure::Config &message, const ServerType *) { config.__toMessage__(message); }
00137 
00145     static void fromMessage(ConfigType &config, dynamic_reconfigure::Config &message, const ServerType *) { config.__fromMessage__(message); }
00146 
00153     static void clamp(ConfigType &config, const ServerType *) { config.__clamp__(); }
00154 
00161     static RTT::internal::AssignableDataSource<RTT::PropertyBag>::shared_ptr getDataSource(ConfigType &config, const ServerType *server) {
00162         RTT::internal::AssignableDataSource<RTT::PropertyBag>::shared_ptr ds(new RTT::internal::ValueDataSource<RTT::PropertyBag>());
00163         if (!server->updater()->propertiesFromConfig(config, ~0, ds->set()))
00164             ds.reset();
00165         return ds;
00166     }
00167 };
00168 
00169 namespace {
00170   struct null_deleter {
00171       void operator()(const void *) const {}
00172   };
00173 }
00174 
00175 // Forward declaration of class DynamicReconfigureTestComponent
00176 class DynamicReconfigureTestComponent;
00177 
00182 template <class ConfigType>
00183 class Server : public RTT::Service
00184 {
00185 private:
00186     typedef Updater<ConfigType> UpdaterType;
00187     typedef dynamic_reconfigure_traits<ConfigType> traits;
00188     typedef boost::shared_ptr< Server<ConfigType> > shared_ptr;
00189 
00190     RTT::os::MutexRecursive mutex_;
00191     ros::NodeHandle *node_handle_;
00192     ros::ServiceServer set_service_;
00193     ros::Publisher update_pub_;
00194     ros::Publisher descr_pub_;
00195 
00196     ConfigType config_;
00197     ConfigType min_;
00198     ConfigType max_;
00199     ConfigType default_;
00200 
00201     mutable boost::shared_ptr<UpdaterType> updater_;
00202     bool initialized_;
00203 
00204     RTT::OperationCaller<UpdateCallbackSignature> update_callback_;
00205     RTT::OperationCaller<UpdateCallbackConstSignature> update_callback_const_;
00206     RTT::Operation<UpdateCallbackConstSignature> update_callback_default_impl_;
00207     RTT::OperationCaller<NotifyCallbackSignature> notify_callback_;
00208 
00209 public:
00216     Server(const std::string &name, RTT::TaskContext *owner)
00217         : RTT::Service(name, owner)
00218         , node_handle_(0)
00219         , update_callback_default_impl_("updateProperties", &Server<ConfigType>::updatePropertiesDefaultImpl, this, RTT::OwnThread, owner->engine())
00220     {
00221         construct();
00222     }
00223 
00229     Server(RTT::TaskContext *owner)
00230         : RTT::Service("reconfigure", owner)
00231         , node_handle_(0)
00232         , update_callback_default_impl_("updateProperties", &Server<ConfigType>::updatePropertiesDefaultImpl, this, RTT::OwnThread, owner->engine())
00233     {
00234         construct();
00235     }
00236 
00241     virtual ~Server() {
00242         shutdown();
00243     }
00244 
00250     const ConfigType &getConfig() const { return config_; }
00251 
00257     void updateConfig(const ConfigType &config)
00258     {
00259         updateConfigInternal(config);
00260     }
00261 
00267     void getConfigMax(ConfigType &max) const
00268     {
00269         max = max_;
00270     }
00271 
00277     const ConfigType &getConfigMax() const { return max_; }
00278 
00284     ConfigType &getConfigMax() { return max_; }
00285 
00291     void getConfigMin(ConfigType &min) const
00292     {
00293         min = min_;
00294     }
00295 
00301     const ConfigType &getConfigMin() const { return min_; }
00302 
00308     ConfigType &getConfigMin() { return min_; }
00309 
00315     void getConfigDefault(ConfigType &config) const
00316     {
00317         config = default_;
00318     }
00319 
00325     const ConfigType &getConfigDefault() const { return default_; }
00326 
00332     ConfigType &getConfigDefault() { return default_; }
00333 
00339     void setConfigMax(const ConfigType &config)
00340     {
00341         max_ = config;
00342         PublishDescription();
00343     }
00344 
00350     void setConfigMin(const ConfigType &config)
00351     {
00352         min_ = config;
00353         PublishDescription();
00354     }
00355 
00361     void setConfigDefault(const ConfigType &config)
00362     {
00363         default_ = config;
00364         PublishDescription();
00365     }
00366 
00372     dynamic_reconfigure::ConfigDescriptionPtr getDescriptionMessage() {
00373         dynamic_reconfigure::ConfigDescriptionPtr description_message = traits::getDescriptionMessage(this);
00374 
00375         // Copy over min_ max_ default_
00376         traits::toMessage(max_, description_message->max, this);
00377         traits::toMessage(min_, description_message->min, this);
00378         traits::toMessage(default_, description_message->dflt, this);
00379 
00380         return description_message;
00381     }
00382 
00389     void advertise(std::string ns = std::string())
00390     {
00391         // shutdown publishers/service servers from previous runs
00392         shutdown();
00393 
00394         // set default namespace
00395         if (ns.empty()) {
00396             if (getOwner()->getName() == "Deployer")
00397                 ns = "~";
00398             else
00399                 ns = "~" + getOwner()->getName();
00400         }
00401 
00402         // create NodeHandle
00403         node_handle_ = new ros::NodeHandle(ns);
00404 
00405         // advertise service server and publishers
00406         set_service_ = node_handle_->advertiseService("set_parameters", &Server<ConfigType>::setConfigCallback, this);
00407         descr_pub_ = node_handle_->advertise<dynamic_reconfigure::ConfigDescription>("parameter_descriptions", 1, true);
00408         update_pub_ = node_handle_->advertise<dynamic_reconfigure::Config>("parameter_updates", 1, true);
00409 
00410         // publish update once
00411         PublishDescription();
00412         updateConfigInternal(config_);
00413     }
00414 
00419     void shutdown()
00420     {
00421         if (node_handle_) {
00422             node_handle_->shutdown();
00423             delete node_handle_;
00424             node_handle_ = 0;
00425         }
00426     }
00427 
00433     bool updated()
00434     {
00435         ConfigType new_config = config_;
00436         if (!updater()->configFromProperties(new_config, *(getOwner()->properties()))) return false;
00437         updateConfig(new_config);
00438         return true;
00439     }
00440 
00445     void refresh()
00446     {
00447         RTT::os::MutexLock lock(mutex_);
00448 
00449         // Refresh config description (no-op unless for AutoConfig)
00450         traits::refreshDescription(this);
00451 
00452         // Grab copys of the data from the config files.  These are declared in the generated config file.
00453         traits::getMin(min_, this);
00454         traits::getMax(max_, this);
00455         traits::getDefault(default_, this);
00456 
00457         // Publish the description
00458         PublishDescription();
00459 
00460         // Add/replace min/max/default properties
00461         this->properties()->remove(this->properties()->getProperty("min"));
00462         this->properties()->remove(this->properties()->getProperty("max"));
00463         this->properties()->remove(this->properties()->getProperty("default"));
00464         this->properties()->ownProperty(new RTT::Property<RTT::PropertyBag>("min", "Minimum values as published to dynamic_reconfigure clients", traits::getDataSource(min_, this)));
00465         this->properties()->ownProperty(new RTT::Property<RTT::PropertyBag>("max", "Maximum values as published to dynamic_reconfigure clients", traits::getDataSource(max_, this)));
00466         this->properties()->ownProperty(new RTT::Property<RTT::PropertyBag>("default", "Default values as published to dynamic_reconfigure clients", traits::getDataSource(default_, this)));
00467 
00468         // Get initial values from current property settings
00469         config_ = ConfigType();
00470         traits::getDefault(config_, this);
00471         updater()->configFromProperties(config_, *(getOwner()->properties()));
00472         if (node_handle_)
00473             config_.__fromServer__(*node_handle_);
00474         traits::clamp(config_, this);
00475 
00476         // At startup we need to load the configuration with all level bits set (everything has changed).
00477         RTT::PropertyBag init_config;
00478         updater()->propertiesFromConfig(config_, ~0, init_config);
00479 
00480         // Invoke update and notification callback
00481 #if !RTT_VERSION_GTE(2,8,99)
00482         // Additional check for RTT < 2.9:
00483         // ===============================
00484         // We do not know for sure which thread is calling this method/operation, but we can check if the current
00485         // thread is the same as the thread that will process the update/notify operation. If yes, we clone the
00486         // underlying OperationCaller implementation and set the caller to the processing engine. In this case
00487         // RTT < 2.9 should always call the operation directly as if it would be a ClientThread operation:
00488         // https://github.com/orocos-toolchain/rtt/blob/toolchain-2.8/rtt/base/OperationCallerInterface.hpp#L79
00489         //
00490         // RTT 2.9 and above already checks the caller thread internally and therefore does not require this hack.
00491         //
00492         RTT::base::OperationCallerBase<UpdateCallbackSignature>::shared_ptr update_callback_impl = update_callback_.getOperationCallerImpl();
00493         if (update_callback_impl) {
00494             RTT::ExecutionEngine *engine = update_callback_impl->getMessageProcessor();
00495             if (update_callback_impl->isSend() && engine && engine->getThread() && engine->getThread()->isSelf()) {
00496                 RTT::Logger::In in(this->getOwner()->getName() + "." + this->getName());
00497                 RTT::log(RTT::Debug) << "calling my own updateProperties operation from refresh()" << RTT::endlog();
00498                 update_callback_impl.reset(update_callback_impl->cloneI(engine));
00499                 update_callback_impl->call(init_config, ~0);
00500             } else {
00501                 update_callback_(init_config, ~0);
00502             }
00503         }
00504         RTT::base::OperationCallerBase<UpdateCallbackConstSignature>::shared_ptr update_callback_const_impl = update_callback_const_.getOperationCallerImpl();
00505         if (update_callback_const_impl) {
00506             RTT::ExecutionEngine *engine = update_callback_const_impl->getMessageProcessor();
00507             if (update_callback_const_impl->isSend() && engine && engine->getThread() && engine->getThread()->isSelf()) {
00508                 RTT::Logger::In in(this->getOwner()->getName() + "." + this->getName());
00509                 RTT::log(RTT::Debug) << "calling my own updateProperties operation from refresh()" << RTT::endlog();
00510                 update_callback_const_impl.reset(update_callback_const_impl->cloneI(engine));
00511                 update_callback_const_impl->call(init_config, ~0);
00512             } else {
00513                 update_callback_const_(init_config, ~0);
00514             }
00515         }
00516         RTT::base::OperationCallerBase<NotifyCallbackSignature>::shared_ptr notify_callback_impl = notify_callback_.getOperationCallerImpl();
00517         if (notify_callback_impl) {
00518             RTT::ExecutionEngine *engine = notify_callback_impl->getMessageProcessor();
00519             if (notify_callback_impl->isSend() && engine && engine->getThread() && engine->getThread()->isSelf()) {
00520                 RTT::Logger::In in(this->getOwner()->getName() + "." + this->getName());
00521                 RTT::log(RTT::Debug) << "calling my own notifyPropertiesUpdate operation from refresh()" << RTT::endlog();
00522                 notify_callback_impl.reset(notify_callback_impl->cloneI(engine));
00523                 notify_callback_impl->call(~0);
00524             } else {
00525                 notify_callback_(~0);
00526             }
00527         }
00528 #else
00529         if (update_callback_.ready()) {
00530             update_callback_(init_config, ~0);
00531         } else if (update_callback_const_.ready()) {
00532             update_callback_const_(init_config, ~0);
00533         }
00534         if (notify_callback_.ready()) {
00535             notify_callback_(~0);
00536         }
00537 #endif
00538 
00539         updateConfigInternal(config_);
00540     }
00541 
00547     UpdaterType *updater() const
00548     {
00549         if (!updater_) updater_.reset(new UpdaterType());
00550         return updater_.get();
00551     }
00552 
00558     void setUpdater(UpdaterType *updater)
00559     {
00560         updater_.reset(updater, null_deleter());
00561     }
00562 
00568     void setUpdateCallback(RTT::OperationInterfacePart *impl)
00569     {
00570         update_callback_ = impl;
00571     }
00572 
00578     void setNotificationCallback(RTT::OperationInterfacePart *impl)
00579     {
00580         notify_callback_ = impl;
00581     }
00582 
00583 private:
00584     void construct()
00585     {
00586         this->addOperation("advertise", &Server<ConfigType>::advertise, this)
00587             .doc("Advertise this dynamic_reconfigure server at the master.")
00588             .arg("namespace", "The namespace this server should be advertised in. Defaults to ~component.");
00589         this->addOperation("shutdown", &Server<ConfigType>::shutdown, this)
00590             .doc("Shutdown this dynamic_reconfigure server.");
00591         this->addOperation("updated", &Server<ConfigType>::updated, this)
00592             .doc("Notify the dynamic_reconfigure server that properties have been updated. This will update the GUI.");
00593 
00594         this->addOperation("refresh", &Server<ConfigType>::refresh, this)
00595             .doc("Rediscover the owner's properties or update advertised min/max/default values. Call this operation after having added properties.");
00596 
00597         // check if owner implements the Updater interface
00598         UpdaterType *updater = dynamic_cast<UpdaterType *>(getOwner());
00599         if (updater) setUpdater(updater);
00600 
00601         // check if owner provides the updateProperties operation
00602         if (getOwner() && getOwner()->provides()->hasMember("updateProperties")) {
00603             RTT::OperationInterfacePart *op = getOwner()->provides()->getPart("updateProperties");
00604             if (boost::dynamic_pointer_cast< RTT::base::OperationCallerBase<UpdateCallbackSignature> >(op->getLocalOperation())) {
00605                 update_callback_ = op;
00606             } else {
00607                 update_callback_const_ = op;
00608             }
00609         } else {
00610             update_callback_const_ = update_callback_default_impl_.getOperationCaller();
00611         }
00612 
00613         // check if owner provides the notifyPropertiesUpdate operation
00614         if (getOwner() && getOwner()->provides()->hasMember("notifyPropertiesUpdate")) {
00615             notify_callback_ = getOwner()->provides()->getPart("notifyPropertiesUpdate");
00616         }
00617 
00618         // update_callback_ and notify_callback_ are called from the ROS spinner thread -> set GlobalEngine as caller engine
00619         update_callback_.setCaller(RTT::internal::GlobalEngine::Instance());
00620         update_callback_const_.setCaller(RTT::internal::GlobalEngine::Instance());
00621         notify_callback_.setCaller(RTT::internal::GlobalEngine::Instance());
00622 
00623         // refresh once
00624         refresh();
00625     }
00626 
00627     void PublishDescription() {
00628         if (!descr_pub_) return;
00629         descr_pub_.publish(getDescriptionMessage());
00630     }
00631 
00632     friend class DynamicReconfigureTestComponent;
00633     bool setConfigCallback(dynamic_reconfigure::Reconfigure::Request &req,
00634                            dynamic_reconfigure::Reconfigure::Response &rsp)
00635     {
00636         RTT::os::MutexLock lock(mutex_);
00637 
00638         ConfigType new_config = config_;
00639         traits::fromMessage(new_config, req.config, this);
00640         traits::clamp(new_config, this);
00641         uint32_t level = config_.__level__(new_config);
00642 
00643         RTT::PropertyBag bag;
00644         if (!updater()->propertiesFromConfig(new_config, level, bag)) return false;
00645         if (update_callback_.ready()) {
00646             if (!update_callback_(bag, level)) return false;
00647             updater()->configFromProperties(new_config, bag);
00648         } else if (update_callback_const_.ready()) {
00649             if (!update_callback_const_(bag, level)) return false;
00650         } else {
00651             return false;
00652         }
00653         if (notify_callback_.ready()) notify_callback_(level);
00654 
00655         updateConfigInternal(new_config);
00656         new_config.__toMessage__(rsp.config);
00657         return true;
00658     }
00659 
00660     void updateConfigInternal(const ConfigType &config)
00661     {
00662         RTT::os::MutexLock lock(mutex_);
00663         config_ = config;
00664         if (node_handle_)
00665             config_.__toServer__(*node_handle_);
00666         dynamic_reconfigure::Config msg;
00667         config_.__toMessage__(msg);
00668 
00669         if (update_pub_)
00670             update_pub_.publish(msg);
00671     }
00672 
00673     bool updatePropertiesDefaultImpl(const RTT::PropertyBag &bag, uint32_t)
00674     {
00675         return RTT::updateProperties(*(getOwner()->properties()), bag);
00676     }
00677 };
00678 
00689 template <typename T, typename ValueType>
00690 bool setProperty(const std::string &name, RTT::PropertyBag &bag, ValueType &value)
00691 {
00692     if (bag.getProperty(name)) {
00693         RTT::Property<T> *prop = bag.getPropertyType<T>(name);
00694         if (!prop) {
00695             RTT::log(RTT::Error) << "Could not assign property '" << name << "': Property exists with a different type." << RTT::endlog();
00696             return false;
00697         }
00698 
00699         prop->set() = value;
00700 
00701     } else {
00702         if (boost::is_same<T,ValueType>::value) {
00703             bag.addProperty(name, value);
00704         } else {
00705             bag.ownProperty(new RTT::Property<T>(name, std::string(), value));
00706         }
00707     }
00708 
00709     return true;
00710 }
00711 
00722 template <typename T, typename ValueType>
00723 bool getProperty(const std::string &name, const RTT::PropertyBag &bag, ValueType &value)
00724 {
00725     RTT::Property<T> *prop = bag.getPropertyType<T>(name);
00726     if (!prop) {
00727         RTT::log(RTT::Error) << "Could not get property '" << name << "': No such property in the bag." << RTT::endlog();
00728         return false;
00729     }
00730 
00731     value = prop->rvalue();
00732     return true;
00733 }
00734 
00735 } // namespace rtt_dynamic_reconfigure
00736 
00737 #define RTT_DYNAMIC_RECONFIGURE_SERVICE_PLUGIN(CONFIG, NAME) \
00738     extern "C" {\
00739         RTT_EXPORT bool loadRTTPlugin(RTT::TaskContext* tc);  \
00740         bool loadRTTPlugin(RTT::TaskContext* tc) {    \
00741             if (tc == 0) return true; \
00742             RTT::Service::shared_ptr sp( new rtt_dynamic_reconfigure::Server<CONFIG>(NAME, tc ) ); \
00743             return tc->provides()->addService( sp ); \
00744         } \
00745         RTT_EXPORT RTT::Service::shared_ptr createService();  \
00746         RTT::Service::shared_ptr createService() {    \
00747             RTT::Service::shared_ptr sp( new rtt_dynamic_reconfigure::Server<CONFIG>( 0 ) ); \
00748             return sp; \
00749         } \
00750         RTT_EXPORT std::string getRTTPluginName(); \
00751         std::string getRTTPluginName() { \
00752             return NAME; \
00753         } \
00754         RTT_EXPORT std::string getRTTTargetName(); \
00755         std::string getRTTTargetName() { \
00756             return OROCOS_TARGET_NAME; \
00757         } \
00758     }
00759 
00760 #endif // RTT_DYNAMIC_RECONFIGURE_SERVER_H


rtt_dynamic_reconfigure
Author(s): Johannes Meyer
autogenerated on Mon Apr 3 2017 03:34:31