Skip to content

Commit

Permalink
Merge pull request #39 from orocos/rtt_dynamic_reconfigure-property-c…
Browse files Browse the repository at this point in the history 
…omposition

rtt_dynamic_reconfigure: added support for Property composition and decomposition and fixed reconfiguration of nested properties
  • Loading branch information
Ruben Smits committed Jun 12, 2015
2 parents 7855b3f + 05d889e commit 03b680c
Show file tree
Hide file tree
Showing 8 changed files with 203 additions and 86 deletions.
9 changes: 6 additions & 3 deletions rtt_dynamic_reconfigure/include/orocos/rtt_dynamic_reconfigure/auto_config.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -76,6 +76,9 @@ class AutoConfig : public RTT::PropertyBag

static void __refreshDescription__(const ServerType *server);

bool updateProperties(RTT::PropertyBag &) const;
bool fromProperties(const RTT::PropertyBag &);

private:
struct Cache;
typedef boost::shared_ptr<Cache> CachePtr;
Expand All @@ -100,8 +103,8 @@ namespace rtt_dynamic_reconfigure {

template <>
struct Updater<AutoConfig> {
static bool propertiesFromConfig(AutoConfig &config, uint32_t, RTT::PropertyBag &bag) { return RTT::updateProperties(bag, config); }
static bool configFromProperties(AutoConfig &config, const RTT::PropertyBag &bag) { return RTT::updateProperties(config, bag); }
static bool propertiesFromConfig(AutoConfig &config, uint32_t, RTT::PropertyBag &bag) { return config.updateProperties(bag); }
static bool configFromProperties(AutoConfig &config, const RTT::PropertyBag &bag) { return config.fromProperties(bag); }
};

template <>
Expand All @@ -120,7 +123,7 @@ struct dynamic_reconfigure_traits<AutoConfig> {
static void fromMessage(AutoConfig &config, dynamic_reconfigure::Config &message, const ServerType *server) { config.__fromMessage__(message, config); }
static void clamp(AutoConfig &config, const ServerType *server) { config.__clamp__(server); }

static RTT::internal::AssignableDataSource<RTT::PropertyBag>::shared_ptr toPropertyBag(AutoConfig &config, const ServerType *) {
static RTT::internal::AssignableDataSource<RTT::PropertyBag>::shared_ptr getDataSource(AutoConfig &config, const ServerType *) {
return RTT::internal::AssignableDataSource<RTT::PropertyBag>::shared_ptr(new RTT::internal::ReferenceDataSource<RTT::PropertyBag>(config));
}
};
Expand Down
10 changes: 5 additions & 5 deletions rtt_dynamic_reconfigure/include/orocos/rtt_dynamic_reconfigure/server.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -142,12 +142,12 @@ struct dynamic_reconfigure_traits {
static void clamp(ConfigType &config, const ServerType *) { config.__clamp__(); }

/**
* Create a new RTT::internal::ValueDataSource<RTT::PropertyBag> filled with properties from a ConfigType instance.
* Creates a new RTT::internal::AssignableDataSource<RTT::PropertyBag> filled with properties from a ConfigType instance.
*
* \param config referencte to the ConfigType instance to be read
* \param server pointer to the rtt_dynamic_reconfigure server instance whose updater is going to be used
*/
static RTT::internal::AssignableDataSource<RTT::PropertyBag>::shared_ptr toPropertyBag(ConfigType &config, const ServerType *server) {
static RTT::internal::AssignableDataSource<RTT::PropertyBag>::shared_ptr getDataSource(ConfigType &config, const ServerType *server) {
RTT::internal::AssignableDataSource<RTT::PropertyBag>::shared_ptr ds(new RTT::internal::ValueDataSource<RTT::PropertyBag>());
if (!server->updater()->propertiesFromConfig(config, ~0, ds->set()))
ds.reset();
Expand Down Expand Up @@ -439,9 +439,9 @@ class Server : public RTT::Service
this->properties()->remove(this->properties()->getProperty("min"));
this->properties()->remove(this->properties()->getProperty("max"));
this->properties()->remove(this->properties()->getProperty("default"));
this->properties()->ownProperty(new RTT::Property<RTT::PropertyBag>("min", "Minimum values as published to dynamic_reconfigure clients", traits::toPropertyBag(min_, this)));
this->properties()->ownProperty(new RTT::Property<RTT::PropertyBag>("max", "Maximum values as published to dynamic_reconfigure clients", traits::toPropertyBag(max_, this)));
this->properties()->ownProperty(new RTT::Property<RTT::PropertyBag>("default", "Default values as published to dynamic_reconfigure clients", traits::toPropertyBag(default_, this)));
this->properties()->ownProperty(new RTT::Property<RTT::PropertyBag>("min", "Minimum values as published to dynamic_reconfigure clients", traits::getDataSource(min_, this)));
this->properties()->ownProperty(new RTT::Property<RTT::PropertyBag>("max", "Maximum values as published to dynamic_reconfigure clients", traits::getDataSource(max_, this)));
this->properties()->ownProperty(new RTT::Property<RTT::PropertyBag>("default", "Default values as published to dynamic_reconfigure clients", traits::getDataSource(default_, this)));

// Get initial values from current property settings
config_ = ConfigType();
Expand Down
87 changes: 65 additions & 22 deletions rtt_dynamic_reconfigure/src/auto_config.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -35,6 +35,7 @@
#include <rtt_dynamic_reconfigure/auto_config.h>
#include <rtt/Property.hpp>
#include <rtt/internal/DataSources.hpp>
#include <rtt/types/PropertyComposition.hpp>

#include <climits>
#include <cfloat>
Expand Down Expand Up @@ -104,8 +105,8 @@ AutoConfig::AutoConfig()
}

AutoConfig::AutoConfig(const RTT::PropertyBag &bag)
: RTT::PropertyBag(bag)
{
this->fromProperties(bag);
}

AutoConfig::~AutoConfig()
Expand Down Expand Up @@ -222,6 +223,23 @@ bool AutoConfig::__fromMessage__(AutoConfig &config, Config &msg, const AutoConf
RTT::base::PropertyBase *pb = config.getProperty((*i)->getName());
std::string param_name = config.prefix_ + (*i)->getName();

// For sub groups, add a sub config to *this and recurse...
const AutoConfig *sample_sub = getAutoConfigFromProperty(*i);
if (sample_sub) {
RTT::Property<RTT::PropertyBag> *sub = config.getPropertyType<RTT::PropertyBag>((*i)->getName());
AutoConfigDataSource *ds;
if (sub) {
ds = AutoConfigDataSource::narrow(sub->getDataSource().get());
} else {
ds = new AutoConfigDataSource();
sub = new RTT::Property<RTT::PropertyBag>((*i)->getName(), (*i)->getDescription(), AutoConfigDataSource::shared_ptr(ds));
config.ownProperty(sub);
}

if (ds && __fromMessage__(ds->set(), msg, *sample_sub))
continue;
}

// search parameter in Config message
bool param_found = false;
for(Config::_bools_type::const_iterator n = msg.bools.begin(); n != msg.bools.end(); ++n) {
Expand All @@ -248,23 +266,6 @@ bool AutoConfig::__fromMessage__(AutoConfig &config, Config &msg, const AutoConf
propertyFromMessage<float>(config, msg, *i, param_name)
) continue;

// For sub groups, add a sub config to *this and recurse...
const AutoConfig *sample_sub = getAutoConfigFromProperty(*i);
if (sample_sub) {
RTT::Property<RTT::PropertyBag> *sub = config.getPropertyType<RTT::PropertyBag>((*i)->getName());
AutoConfigDataSource *ds;
if (sub) {
ds = AutoConfigDataSource::narrow(sub->getDataSource().get());
} else {
ds = new AutoConfigDataSource();
sub = new RTT::Property<RTT::PropertyBag>((*i)->getName(), (*i)->getDescription(), AutoConfigDataSource::shared_ptr(ds));
config.ownProperty(sub);
}

if (ds && __fromMessage__(ds->set(), msg, *sample_sub))
continue;
}

result = false;
}

Expand Down Expand Up @@ -337,6 +338,39 @@ uint32_t AutoConfig::__level__(const AutoConfig &config) const
return 0;
}

bool AutoConfig::updateProperties(RTT::PropertyBag &target) const
{
RTT::PropertyBag composed;
if (!RTT::types::composePropertyBag(*this, composed)) return false;
return RTT::updateProperties(target, composed);
}

bool AutoConfig::fromProperties(const RTT::PropertyBag &source)
{
RTT::PropertyBag decomposed;
if (!RTT::types::decomposePropertyBag(source, decomposed)) return false;

for(RTT::PropertyBag::const_iterator i = decomposed.begin(); i != decomposed.end(); ++i) {
RTT::base::PropertyBase *pb = this->getProperty((*i)->getName());
if (pb) {
pb->update(*i);
continue;
}

RTT::Property<RTT::PropertyBag> *sub = dynamic_cast<RTT::Property<RTT::PropertyBag> *>(*i);
if (sub) {
AutoConfigDataSource *ds = new AutoConfigDataSource(sub->rvalue());
ds->set().setType(sub->rvalue().getType());
this->ownProperty(new RTT::Property<RTT::PropertyBag>(sub->getName(), sub->getDescription(), ds));
continue;
} else {
this->ownProperty((*i)->clone());
}
}

return true;
}

template <typename T>
static bool buildParamDescription(const RTT::base::PropertyBase *pb, const std::string &prefix, Group::_parameters_type& params, AutoConfig& dflt, AutoConfig& min, AutoConfig& max)
{
Expand Down Expand Up @@ -371,7 +405,7 @@ static bool buildParamDescription(const RTT::base::PropertyBase *pb, const std::
return true;
}

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)
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)
{
std::size_t group_index = config_description.groups.size();
config_description.groups.push_back(Group());
Expand Down Expand Up @@ -404,7 +438,7 @@ static void buildGroupDescription(RTT::TaskContext *owner, const RTT::PropertyBa
max.state = true;

// for loop might invalidate group reference -> use index group_index instead
for(RTT::PropertyBag::const_iterator i = bag->begin(); i != bag->end(); ++i) {
for(RTT::PropertyBag::const_iterator i = bag.begin(); i != bag.end(); ++i) {
if (buildParamDescription<bool>(*i, prefix, config_description.groups[group_index].parameters, dflt, min, max) ||
buildParamDescription<int>(*i, prefix, config_description.groups[group_index].parameters, dflt, min, max) ||
buildParamDescription<unsigned int>(*i, prefix, config_description.groups[group_index].parameters, dflt, min, max) ||
Expand All @@ -419,24 +453,27 @@ static void buildGroupDescription(RTT::TaskContext *owner, const RTT::PropertyBa
if (!sub_dflt) {
AutoConfigDataSource *ds = new AutoConfigDataSource();
sub_dflt = &(ds->set());
sub_dflt->setType(sub->rvalue().getType());
dflt.ownProperty(new RTT::Property<RTT::PropertyBag>(sub->getName(), sub->getDescription(), ds));
}

AutoConfig *sub_min = getAutoConfigFromProperty(min.getProperty(sub->getName()));
if (!sub_min) {
AutoConfigDataSource *ds = new AutoConfigDataSource();
sub_min = &(ds->set());
sub_min->setType(sub->rvalue().getType());
min.ownProperty(new RTT::Property<RTT::PropertyBag>(sub->getName(), sub->getDescription(), ds));
}

AutoConfig *sub_max = getAutoConfigFromProperty(max.getProperty(sub->getName()));
if (!sub_max) {
AutoConfigDataSource *ds = new AutoConfigDataSource();
sub_max = &(ds->set());
sub_max->setType(sub->rvalue().getType());
max.ownProperty(new RTT::Property<RTT::PropertyBag>(sub->getName(), sub->getDescription(), ds));
}

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);
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);
}
}
}
Expand All @@ -446,14 +483,20 @@ boost::shared_mutex AutoConfig::cache_mutex_;

void AutoConfig::buildCache(const ServerType *server, RTT::TaskContext *owner)
{
RTT::PropertyBag decomposed;
if (!RTT::types::decomposePropertyBag(*(owner->properties()), decomposed)) {
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();
decomposed = *(owner->properties());
}

boost::upgrade_lock<boost::shared_mutex> upgrade_lock(cache_mutex_);
if (upgrade_lock.owns_lock())
{
boost::upgrade_to_unique_lock<boost::shared_mutex> unique_lock(upgrade_lock);
CachePtr& cache = cache_[server];
if (!cache) cache.reset(new Cache());
cache->description_message_.reset(new ConfigDescription);
buildGroupDescription(owner, owner->properties(), *(cache->description_message_), cache->default_, cache->min_, cache->max_, "", "", "", 0, 0);
buildGroupDescription(owner, decomposed, *(cache->description_message_), cache->default_, cache->min_, cache->max_, "", "", "", 0, 0);
}
}

Expand Down
6 changes: 5 additions & 1 deletion tests/rtt_dynamic_reconfigure_tests/CMakeLists.txt
View file
Original file line number Diff line number Diff line change
Expand Up @@ -12,13 +12,17 @@ if(CATKIN_ENABLE_TESTING)
orocos_plugin(rtt_dynamic_reconfigure_tests_service test/service.cpp)
add_dependencies(rtt_dynamic_reconfigure_tests_service ${PROJECT_NAME}_gencfg)

orocos_component(rtt_dynamic_reconfigure_tests_component test/test_component.cpp)

catkin_add_gtest(rtt_dynamic_reconfigure_tests test/rtt_dynamic_reconfigure_tests.cpp)
add_dependencies(rtt_dynamic_reconfigure_tests rtt_dynamic_reconfigure_tests_service)
target_link_libraries(rtt_dynamic_reconfigure_tests
${catkin_LIBRARIES}
${USE_OROCOS_LIBRARIES}
${OROCOS-RTT_LIBRARIES}
${OROCOS-RTT_RTT-SCRIPTING_LIBRARY} )
${OROCOS-RTT_RTT-SCRIPTING_LIBRARY}
rtt_dynamic_reconfigure_tests_component
)

orocos_generate_package()

Expand Down
3 changes: 3 additions & 0 deletions tests/rtt_dynamic_reconfigure_tests/package.xml
View file
Original file line number Diff line number Diff line change
Expand Up @@ -13,14 +13,17 @@

<build_depend>rtt_ros</build_depend>
<build_depend>rtt_dynamic_reconfigure</build_depend>
<build_depend>rtt_geometry_msgs</build_depend>

<run_depend>rtt_ros</run_depend>
<run_depend>rtt_dynamic_reconfigure</run_depend>
<run_depend>rtt_geometry_msgs</run_depend>

<!-- The export tag contains other, unspecified, tags -->
<export>
<rtt_ros>
<plugin_depend>rtt_dynamic_reconfigure</plugin_depend>
<plugin_depend>rtt_geometry_msgs</plugin_depend>
</rtt_ros>
</export>

Expand Down
Loading

0 comments on commit 03b680c

Please sign in to comment.