auto_config.cpp
Go to the documentation of this file.
00001 /*********************************************************************
00002 * Software License Agreement (BSD License)
00003 *
00004 *  Copyright (c) 2014, Intermodalics BVBA
00005 *  All rights reserved.
00006 *
00007 *  Redistribution and use in source and binary forms, with or without
00008 *  modification, are permitted provided that the following conditions
00009 *  are met:
00010 *
00011 *   * Redistributions of source code must retain the above copyright
00012 *     notice, this list of conditions and the following disclaimer.
00013 *   * Redistributions in binary form must reproduce the above
00014 *     copyright notice, this list of conditions and the following
00015 *     disclaimer in the documentation and/or other materials provided
00016 *     with the distribution.
00017 *   * Neither the name of the Intermodalics BVBA nor the names of its
00018 *     contributors may be used to endorse or promote products derived
00019 *     from this software without specific prior written permission.
00020 *
00021 *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
00022 *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
00023 *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
00024 *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
00025 *  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
00026 *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
00027 *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
00028 *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
00029 *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
00030 *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
00031 *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
00032 *  POSSIBILITY OF SUCH DAMAGE.
00033 *********************************************************************/
00034 
00035 #include <rtt_dynamic_reconfigure/auto_config.h>
00036 #include <rtt/Property.hpp>
00037 #include <rtt/internal/DataSources.hpp>
00038 #include <rtt/types/PropertyComposition.hpp>
00039 
00040 #include <cassert>
00041 #include <climits>
00042 #include <cfloat>
00043 
00044 #include <boost/thread/locks.hpp>
00045 
00046 #include <dynamic_reconfigure/config_tools.h>
00047 
00048 namespace rtt_dynamic_reconfigure {
00049 
00050 using namespace dynamic_reconfigure;
00051 
00057 class AutoConfigDataSource
00058     : public RTT::internal::AssignableDataSource<RTT::PropertyBag>
00059 {
00060 protected:
00061     AutoConfig mdata;
00062 
00063 public:
00064     ~AutoConfigDataSource() {}
00065 
00066     typedef boost::intrusive_ptr<AutoConfigDataSource> shared_ptr;
00067 
00068     AutoConfigDataSource(const AutoConfig &data) : mdata(data) {}
00069     AutoConfigDataSource() {}
00070 
00071     RTT::internal::DataSource<RTT::PropertyBag>::result_t get() const { return mdata; }
00072     RTT::internal::DataSource<RTT::PropertyBag>::result_t value() const { return mdata; }
00073 
00074     void set( typename AssignableDataSource<RTT::PropertyBag>::param_t t ) { mdata = t; }
00075     AutoConfig& set() { return mdata; }
00076     const AutoConfig& rvalue() const { return mdata; }
00077 
00078     virtual AutoConfigDataSource* clone() const { return new AutoConfigDataSource(mdata); }
00079 
00080     /* copied from ValueDataSource<T>::copy() in DataSources.inl */
00081     virtual AutoConfigDataSource* copy( std::map<const RTT::base::DataSourceBase*, RTT::base::DataSourceBase*>& replace ) const
00082     {
00083         // if somehow a copy exists, return the copy, otherwise return this (see Attribute copy)
00084         if ( replace[this] != 0 ) {
00085             assert ( dynamic_cast<AutoConfigDataSource*>( replace[this] ) == static_cast<AutoConfigDataSource*>( replace[this] ) );
00086             return static_cast<AutoConfigDataSource*>( replace[this] );
00087         }
00088         // Other pieces in the code rely on insertion in the map :
00089         replace[this] = const_cast<AutoConfigDataSource*>(this);
00090         // return this instead of a copy.
00091         return const_cast<AutoConfigDataSource*>(this);
00092     }
00093 
00098     static AutoConfigDataSource* narrow(RTT::base::DataSourceBase* dsb) {
00099         AutoConfigDataSource* ret = dynamic_cast< AutoConfigDataSource* >( dsb );
00100         return ret;
00101     }
00102 };
00103 
00104 AutoConfig::AutoConfig()
00105     : parent(), id(), state()
00106 {
00107 }
00108 
00109 AutoConfig::AutoConfig(const RTT::PropertyBag &bag)
00110     : parent(), id(), state()
00111 {
00112     this->fromProperties(bag);
00113 }
00114 
00115 AutoConfig::~AutoConfig()
00116 {
00117 }
00118 
00119 // default type
00120 template <typename T> struct PropertyTypeInfo {
00121     typedef std::string dynamic_reconfigure_type;
00122     static std::string getType() { return "str"; }
00123     static bool hasLimits() { return false; }
00124     static T getMin() { return std::numeric_limits<T>::lowest(); }
00125     static T getMax() { return std::numeric_limits<T>::max(); }
00126 };
00127 
00128 template <> struct PropertyTypeInfo<bool>
00129 {
00130     typedef bool dynamic_reconfigure_type;
00131     static std::string getType() { return "bool"; }
00132     static bool hasLimits() { return false; }
00133     static bool getMin() { return false; }
00134     static bool getMax() { return true; }
00135 };
00136 
00137 template <> struct PropertyTypeInfo<int>
00138 {
00139     typedef int dynamic_reconfigure_type;
00140     static std::string getType() { return "int"; }
00141     static bool hasLimits() { return true; }
00142     static int getMin() { return INT_MIN; }
00143     static int getMax() { return INT_MAX; }
00144 };
00145 
00146 template <> struct PropertyTypeInfo<unsigned int>
00147 {
00148     typedef int dynamic_reconfigure_type;
00149     static std::string getType() { return "int"; }
00150     static bool hasLimits() { return true; }
00151     static int getMin() { return 0; }
00152     static int getMax() { return INT_MAX; }
00153 };
00154 
00155 template <> struct PropertyTypeInfo<std::string>
00156 {
00157     typedef std::string dynamic_reconfigure_type;
00158     static std::string getType() { return "str"; }
00159     static bool hasLimits() { return false; }
00160     static std::string getMin() { return ""; }
00161     static std::string getMax() { return ""; }
00162 };
00163 
00164 template <> struct PropertyTypeInfo<double>
00165 {
00166     typedef double dynamic_reconfigure_type;
00167     static std::string getType() { return "double"; }
00168     static bool hasLimits() { return true; }
00169     static double getMin() { return -DBL_MAX; }
00170     static double getMax() { return  DBL_MAX; }
00171 };
00172 
00173 template <> struct PropertyTypeInfo<float>
00174 {
00175     typedef double dynamic_reconfigure_type;
00176     static std::string getType() { return "double"; }
00177     static bool hasLimits() { return true; }
00178     static double getMin() { return -FLT_MAX; }
00179     static double getMax() { return  FLT_MAX; }
00180 };
00181 
00182 static AutoConfig *getAutoConfigFromProperty(const RTT::base::PropertyBase *pb)
00183 {
00184     const RTT::Property<RTT::PropertyBag> *prop = dynamic_cast<const RTT::Property<RTT::PropertyBag> *>(pb);
00185     if (!prop) return 0;
00186     AutoConfigDataSource *ds = AutoConfigDataSource::narrow(prop->getDataSource().get());
00187     if (!ds) return 0;
00188     return &(ds->set());
00189 }
00190 
00191 template <typename T>
00192 static bool propertyFromMessage(AutoConfig &config, Config &msg, const RTT::base::PropertyBase *sample, const std::string &param_name)
00193 {
00194     const RTT::Property<T> *sample_prop = dynamic_cast<const RTT::Property<T> *>(sample);
00195     if (!sample_prop) return false;
00196 
00197     typename PropertyTypeInfo<T>::dynamic_reconfigure_type value;
00198     if (!ConfigTools::getParameter(msg, param_name, value)) return false;
00199 
00200     RTT::Property<T> *prop = config.getPropertyType<T>(sample->getName());
00201     if (!prop) {
00202         prop = sample_prop->create();
00203         config.ownProperty(prop);
00204     }
00205     prop->set(value);
00206     return true;
00207 }
00208 
00209 bool AutoConfig::__fromMessage__(Config &msg, const AutoConfig &sample)
00210 {
00211     return __fromMessage__(*this, msg, sample);
00212 }
00213 
00214 bool AutoConfig::__fromMessage__(AutoConfig &config, Config &msg, const AutoConfig &sample)
00215 {
00216     // get group state
00217     config.prefix_ = sample.prefix_;
00218     config.name = sample.name;
00219     config.id = sample.id;
00220     config.parent = sample.parent;
00221     dynamic_reconfigure::ConfigTools::getGroupState(msg, config.name, config);
00222 
00223     // iterate over all properties in sample
00224     bool result = true;
00225     for(RTT::PropertyBag::const_iterator i = sample.begin(); i != sample.end(); ++i) {
00226         std::string param_name = config.prefix_ + (*i)->getName();
00227 
00228         // For sub groups, add a sub config to *this and recurse...
00229         const AutoConfig *sample_sub = getAutoConfigFromProperty(*i);
00230         if (sample_sub) {
00231             RTT::Property<RTT::PropertyBag> *sub = config.getPropertyType<RTT::PropertyBag>((*i)->getName());
00232             AutoConfigDataSource *ds;
00233             if (sub) {
00234                 ds = AutoConfigDataSource::narrow(sub->getDataSource().get());
00235                 assert(ds->rvalue().getType() == sample_sub->getType());
00236             } else {
00237                 ds = new AutoConfigDataSource();
00238                 ds->set().setType(sample_sub->getType());
00239             }
00240 
00241             if (ds && __fromMessage__(ds->set(), msg, *sample_sub)) {
00242                 if (!sub) {
00243                     // new AutoConfigDataSource
00244                     if (!ds->rvalue().empty()) {
00245                         sub = new RTT::Property<RTT::PropertyBag>((*i)->getName(), (*i)->getDescription(), AutoConfigDataSource::shared_ptr(ds));
00246                         config.ownProperty(sub);
00247                     } else {
00248                         delete ds;
00249                     }
00250                 }
00251                 continue;
00252             }
00253         }
00254 
00255         // search parameter in Config message
00256         bool param_found = false;
00257         for(Config::_bools_type::const_iterator n = msg.bools.begin(); n != msg.bools.end(); ++n) {
00258             if (n->name == param_name) param_found = true;
00259         }
00260         for(Config::_ints_type::const_iterator n = msg.ints.begin(); n != msg.ints.end(); ++n) {
00261             if (n->name == param_name) param_found = true;
00262         }
00263         for(Config::_strs_type::const_iterator n = msg.strs.begin(); n != msg.strs.end(); ++n) {
00264             if (n->name == param_name) param_found = true;
00265         }
00266         for(Config::_doubles_type::const_iterator n = msg.doubles.begin(); n != msg.doubles.end(); ++n) {
00267             if (n->name == param_name) param_found = true;
00268         }
00269         if (!param_found) continue;
00270 
00271         // get parameter value from Config message
00272         if (
00273             propertyFromMessage<bool>(config, msg, *i, param_name) ||
00274             propertyFromMessage<int>(config, msg, *i, param_name) ||
00275             propertyFromMessage<unsigned int>(config, msg, *i, param_name) ||
00276             propertyFromMessage<std::string>(config, msg, *i, param_name) ||
00277             propertyFromMessage<double>(config, msg, *i, param_name) ||
00278             propertyFromMessage<float>(config, msg, *i, param_name)
00279            ) continue;
00280 
00281         result = false;
00282     }
00283 
00284     return result;
00285 }
00286 
00287 template <typename T>
00288 static bool propertyToMessage(Config &msg, const RTT::base::PropertyBase *pb, const std::string &_prefix)
00289 {
00290     const RTT::Property<T> *prop = dynamic_cast<const RTT::Property<T> *>(pb);
00291     if (!prop) return false;
00292 
00293     typename PropertyTypeInfo<T>::dynamic_reconfigure_type value = prop->get();
00294     ConfigTools::appendParameter(msg, _prefix + pb->getName(), value);
00295     return true;
00296 }
00297 
00298 void AutoConfig::__toMessage__(Config &msg) const
00299 {
00300     __toMessage__(*this, msg);
00301 }
00302 
00303 void AutoConfig::__toMessage__(const AutoConfig &config, Config &msg)
00304 {
00305     // add group state
00306     dynamic_reconfigure::ConfigTools::appendGroup(msg, config.name, config.id, config.parent, config);
00307 
00308     // iterate over all properties
00309     bool result = true;
00310     for(RTT::PropertyBag::const_iterator i = config.begin(); i != config.end(); ++i) {
00311         if (propertyToMessage<bool>(msg, *i, config.prefix_) ||
00312             propertyToMessage<int>(msg, *i, config.prefix_) ||
00313             propertyToMessage<unsigned int>(msg, *i, config.prefix_) ||
00314             propertyToMessage<std::string>(msg, *i, config.prefix_) ||
00315             propertyToMessage<double>(msg, *i, config.prefix_) ||
00316             propertyToMessage<float>(msg, *i, config.prefix_)
00317            ) continue;
00318 
00319         // test if *i has type AutoConfig
00320         const AutoConfig *sub = getAutoConfigFromProperty(*i);
00321         if (sub) {
00322             __toMessage__(*sub, msg);
00323             continue;
00324         }
00325 
00326         result = false;
00327     }
00328 }
00329 
00330 void AutoConfig::__toServer__(const ros::NodeHandle &nh) const
00331 {
00332 
00333 }
00334 
00335 void AutoConfig::__fromServer__(const ros::NodeHandle &nh)
00336 {
00337 
00338 }
00339 
00340 void AutoConfig::__clamp__(const ServerType *server)
00341 {
00342     const AutoConfig &min = server->getConfigMin();
00343     const AutoConfig &max = server->getConfigMax();
00344 
00345     // TODO: clamp values
00346 }
00347 
00348 uint32_t AutoConfig::__level__(const AutoConfig &config) const
00349 {
00350     return 0;
00351 }
00352 
00353 bool AutoConfig::updateProperties(RTT::PropertyBag &target) const
00354 {
00355     RTT::PropertyBag composed;
00356     if (!RTT::types::composePropertyBag(*this, composed)) return false;
00357     return RTT::updateProperties(target, composed);
00358 }
00359 
00360 bool AutoConfig::fromProperties(const RTT::PropertyBag &source)
00361 {
00362     RTT::PropertyBag decomposed;
00363     if (!RTT::types::decomposePropertyBag(source, decomposed)) return false;
00364 
00365     for(RTT::PropertyBag::const_iterator i = decomposed.begin(); i != decomposed.end(); ++i) {
00366         RTT::base::PropertyBase *pb = this->getProperty((*i)->getName());
00367         if (pb) {
00368             pb->update(*i);
00369             continue;
00370         }
00371 
00372         RTT::Property<RTT::PropertyBag> *sub = dynamic_cast<RTT::Property<RTT::PropertyBag> *>(*i);
00373         if (sub) {
00374             AutoConfigDataSource *ds = new AutoConfigDataSource(sub->rvalue());
00375             ds->set().setType(sub->rvalue().getType());
00376             this->ownProperty(new RTT::Property<RTT::PropertyBag>(sub->getName(), sub->getDescription(), ds));
00377             continue;
00378         } else {
00379             this->ownProperty((*i)->clone());
00380         }
00381     }
00382 
00383     return true;
00384 }
00385 
00386 template <typename T>
00387 static bool buildParamDescription(const RTT::base::PropertyBase *pb, const std::string &prefix, Group::_parameters_type& params, AutoConfig& dflt, AutoConfig& min, AutoConfig& max)
00388 {
00389     const RTT::Property<T> *prop = dynamic_cast<const RTT::Property<T> *>(pb);
00390     if (!prop) return false;
00391 
00392     ParamDescription param;
00393     param.name = prefix + pb->getName();
00394     param.type = PropertyTypeInfo<T>::getType();
00395     param.description = pb->getDescription();
00396     params.push_back(param);
00397 
00398     // get current value as default
00399     if (!dflt.getProperty(pb->getName())) {
00400         RTT::Property<T> *dflt_prop = prop->create();
00401         dflt_prop->set(prop->get());
00402         dflt.ownProperty(dflt_prop);
00403     }
00404 
00405     // get minimum/maximum value
00406     if (!min.getProperty(pb->getName())) {
00407         RTT::Property<T> *min_prop = prop->create();
00408         min_prop->set(PropertyTypeInfo<T>::getMin());
00409         min.ownProperty(min_prop);
00410     }
00411     if (!max.getProperty(pb->getName())) {
00412         RTT::Property<T> *max_prop = prop->create();
00413         max_prop->set(PropertyTypeInfo<T>::getMax());
00414         max.ownProperty(max_prop);
00415     }
00416 
00417     return true;
00418 }
00419 
00420 static void buildGroupDescription(RTT::TaskContext *owner, const RTT::PropertyBag &bag, ConfigDescription& config_description, AutoConfig& dflt, AutoConfig& min, AutoConfig& max, const std::string &prefix, const std::string &name, const std::string &type, int32_t parent, int32_t id)
00421 {
00422     std::size_t group_index = config_description.groups.size();
00423     config_description.groups.push_back(Group());
00424 
00425     Group &group = config_description.groups[group_index];
00426     group.name = name.empty() ? "Default" : name;
00427     group.type = type;
00428     group.parent = parent;
00429     group.id = id;
00430 
00431     dflt.prefix_ = prefix;
00432     dflt.name = group.name;
00433     dflt.type = group.type;
00434     dflt.parent = group.parent;
00435     dflt.id = group.id;
00436     dflt.state = true;
00437 
00438     min.prefix_ = prefix;
00439     min.name = group.name;
00440     min.type = group.type;
00441     min.parent = group.parent;
00442     min.id = group.id;
00443     min.state = true;
00444 
00445     max.prefix_ = prefix;
00446     max.name = group.name;
00447     max.type = group.type;
00448     max.parent = group.parent;
00449     max.id = group.id;
00450     max.state = true;
00451 
00452     // for loop might invalidate group reference -> use index group_index instead
00453     for(RTT::PropertyBag::const_iterator i = bag.begin(); i != bag.end(); ++i) {
00454         if (buildParamDescription<bool>(*i, prefix, config_description.groups[group_index].parameters, dflt, min, max) ||
00455             buildParamDescription<int>(*i, prefix, config_description.groups[group_index].parameters, dflt, min, max) ||
00456             buildParamDescription<unsigned int>(*i, prefix, config_description.groups[group_index].parameters, dflt, min, max) ||
00457             buildParamDescription<std::string>(*i, prefix, config_description.groups[group_index].parameters, dflt, min, max) ||
00458             buildParamDescription<double>(*i, prefix, config_description.groups[group_index].parameters, dflt, min, max) ||
00459             buildParamDescription<float>(*i, prefix, config_description.groups[group_index].parameters, dflt, min, max)
00460            ) continue;
00461 
00462         const RTT::Property<RTT::PropertyBag> *sub = dynamic_cast<RTT::Property<RTT::PropertyBag> *>(*i);
00463         if (sub) {
00464             AutoConfig *sub_dflt = getAutoConfigFromProperty(dflt.getProperty(sub->getName()));
00465             if (!sub_dflt) {
00466                 AutoConfigDataSource *ds = new AutoConfigDataSource();
00467                 sub_dflt = &(ds->set());
00468                 sub_dflt->setType(sub->rvalue().getType());
00469                 dflt.ownProperty(new RTT::Property<RTT::PropertyBag>(sub->getName(), sub->getDescription(), ds));
00470             }
00471 
00472             AutoConfig *sub_min = getAutoConfigFromProperty(min.getProperty(sub->getName()));
00473             if (!sub_min) {
00474                 AutoConfigDataSource *ds = new AutoConfigDataSource();
00475                 sub_min = &(ds->set());
00476                 sub_min->setType(sub->rvalue().getType());
00477                 min.ownProperty(new RTT::Property<RTT::PropertyBag>(sub->getName(), sub->getDescription(), ds));
00478             }
00479 
00480             AutoConfig *sub_max = getAutoConfigFromProperty(max.getProperty(sub->getName()));
00481             if (!sub_max) {
00482                 AutoConfigDataSource *ds = new AutoConfigDataSource();
00483                 sub_max = &(ds->set());
00484                 sub_max->setType(sub->rvalue().getType());
00485                 max.ownProperty(new RTT::Property<RTT::PropertyBag>(sub->getName(), sub->getDescription(), ds));
00486             }
00487 
00488             buildGroupDescription(owner, sub->rvalue(), config_description, *sub_dflt, *sub_min, *sub_max, prefix + sub->getName() + "__", prefix + sub->getName(), "", config_description.groups[group_index].id, ++id);
00489         }
00490     }
00491 }
00492 
00493 std::map<const AutoConfig::ServerType *, AutoConfig::CachePtr> AutoConfig::cache_;
00494 boost::shared_mutex AutoConfig::cache_mutex_;
00495 
00496 void AutoConfig::buildCache(const ServerType *server, RTT::TaskContext *owner)
00497 {
00498     RTT::PropertyBag decomposed;
00499     if (!RTT::types::decomposePropertyBag(*(owner->properties()), decomposed)) {
00500         RTT::log(RTT::Error) << "Failed to decompose properties of '" << owner->getName() << "' for dynamic_reconfigure. Properties with custom types will not be available for reconfiguration." << RTT::endlog();
00501         decomposed = *(owner->properties());
00502     }
00503 
00504     boost::upgrade_lock<boost::shared_mutex> upgrade_lock(cache_mutex_);
00505     if (upgrade_lock.owns_lock())
00506     {
00507         boost::upgrade_to_unique_lock<boost::shared_mutex> unique_lock(upgrade_lock);
00508         CachePtr& cache = cache_[server];
00509         if (!cache) cache.reset(new Cache());
00510         cache->description_message_.reset(new ConfigDescription);
00511         buildGroupDescription(owner, decomposed, *(cache->description_message_), cache->default_, cache->min_, cache->max_, "", "", "", 0, 0);
00512     }
00513 }
00514 
00515 dynamic_reconfigure::ConfigDescriptionPtr AutoConfig::__getDescriptionMessage__(const ServerType *server)
00516 {
00517     boost::shared_lock<boost::shared_mutex> lock(cache_mutex_);
00518     if (!cache_.count(server)) buildCache(server, server->getOwner());
00519     return cache_.at(server)->description_message_;
00520 }
00521 
00522 const AutoConfig &AutoConfig::__getDefault__(const ServerType *server)
00523 {
00524     boost::shared_lock<boost::shared_mutex> lock(cache_mutex_);
00525     if (!cache_.count(server)) buildCache(server, server->getOwner());
00526     return cache_.at(server)->default_;
00527 }
00528 
00529 const AutoConfig &AutoConfig::__getMax__(const ServerType *server)
00530 {
00531     boost::shared_lock<boost::shared_mutex> lock(cache_mutex_);
00532     if (!cache_.count(server)) buildCache(server, server->getOwner());
00533     return cache_.at(server)->max_;
00534 }
00535 
00536 const AutoConfig &AutoConfig::__getMin__(const ServerType *server)
00537 {
00538     boost::shared_lock<boost::shared_mutex> lock(cache_mutex_);
00539     if (!cache_.count(server)) buildCache(server, server->getOwner());
00540     return cache_.at(server)->min_;
00541 }
00542 
00543 void AutoConfig::__refreshDescription__(const ServerType *server)
00544 {
00545     buildCache(server, server->getOwner());
00546 }
00547 
00548 } // namespace rtt_dynamic_reconfigure


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