diff --git a/core/include/moveit/task_constructor/properties.h b/core/include/moveit/task_constructor/properties.h index 8555fd32..cbc4e79e 100644 --- a/core/include/moveit/task_constructor/properties.h +++ b/core/include/moveit/task_constructor/properties.h @@ -66,12 +66,12 @@ boost::any fromName(const PropertyMap& other, const std::string& other_name); class Property { friend class PropertyMap; + Property(const std::type_index &type_index, const std::string &description, const boost::any &default_value); + public: typedef int SourceId; + /// function callback used to initialize property value from another PropertyMap typedef std::function InitializerFunction; - typedef std::map InitializerMap; - - Property(const std::type_index& type_index, const std::string& description, const boost::any& default_value); /// set current value and default value void setValue(const boost::any& value); @@ -97,7 +97,7 @@ public: /// configure initialization from source using given other property name Property &configureInitFrom(SourceId source, const std::string& name); - /// set current value using configured initializers + /// set current value using matching configured initializers void performInitFrom(SourceId source, const PropertyMap& other); private: @@ -105,7 +105,10 @@ private: std::type_index type_index_; boost::any default_; boost::any value_; - InitializerMap initializers_; + + /// used for external initialization + SourceId source_id_ = 0; + InitializerFunction initializer_; }; @@ -120,13 +123,13 @@ class PropertyMap /// implementation of declare methods Property& declare(const std::string& name, const std::type_info& type, - const std::string& description = "", - const boost::any& default_value = boost::any()); + const std::string& description, + const boost::any& default_value); public: /// declare a property for future use template Property& declare(const std::string& name, const std::string& description = "") { - return declare(name, typeid(T), description); + return declare(name, typeid(T), description, boost::any()); } /// declare a property with default value template diff --git a/core/include/moveit/task_constructor/stage_p.h b/core/include/moveit/task_constructor/stage_p.h index c6a5c428..50dd5e07 100644 --- a/core/include/moveit/task_constructor/stage_p.h +++ b/core/include/moveit/task_constructor/stage_p.h @@ -158,8 +158,6 @@ protected: // get informed when new start or end state becomes available void newStartState(const std::list::iterator& it); void newEndState(const std::list::iterator& it); - // initialize properties from parent and/or state - void initProperties(const InterfaceState &state); Interface::const_iterator next_start_state_; Interface::const_iterator next_end_state_; @@ -187,10 +185,6 @@ public: bool canCompute() const override; bool compute() override; - -private: - // initialize properties from parent - void initProperties(); }; PIMPL_FUNCTIONS(Generator) @@ -211,8 +205,6 @@ private: // get informed when new start or end state becomes available void newStartState(const std::list::iterator& it); void newEndState(const std::list::iterator& it); - // initialize properties from parent and/or interface states - void initProperties(const InterfaceState &start, const InterfaceState &end); std::pair it_pairs_; }; diff --git a/core/src/container.cpp b/core/src/container.cpp index 7d329616..f15bce86 100644 --- a/core/src/container.cpp +++ b/core/src/container.cpp @@ -168,11 +168,6 @@ void ContainerBase::init(const planning_scene::PlanningSceneConstPtr &scene) Stage::init(scene); - // containers don't need to reset and init their properties on each execution - impl->properties_.reset(); - if (impl->parent()) - impl->properties_.performInitFrom(PARENT, impl->parent()->properties()); - // we need to have some children to do the actual work if (children.empty()) { errors.push_back(*this, "no children"); diff --git a/core/src/properties.cpp b/core/src/properties.cpp index 7e714d58..524a9c61 100644 --- a/core/src/properties.cpp +++ b/core/src/properties.cpp @@ -84,22 +84,23 @@ void Property::reset() Property& Property::configureInitFrom(SourceId source, const Property::InitializerFunction &f) { - initializers_[source] = f; + if (source != source_id_ && initializer_) + throw std::runtime_error("Property was already configured for initialization from another source id"); + + source_id_ = source; + initializer_ = f; return *this; } Property &Property::configureInitFrom(SourceId source, const std::string &name) { - initializers_[source] = [name](const PropertyMap& other) { return fromName(other, name); }; - return *this; + return configureInitFrom(source, [name](const PropertyMap& other) { return fromName(other, name); }); } void Property::performInitFrom(SourceId source, const PropertyMap &other) { - auto it = initializers_.find(source); - if (it == initializers_.end()) return; - - setCurrentValue(it->second(other)); + if (source_id_ != source || !initializer_) return; // source ids not matching + setCurrentValue(initializer_(other)); } diff --git a/core/src/stage.cpp b/core/src/stage.cpp index ad1adbf2..b619d1df 100644 --- a/core/src/stage.cpp +++ b/core/src/stage.cpp @@ -140,6 +140,11 @@ void Stage::reset() void Stage::init(const planning_scene::PlanningSceneConstPtr &scene) { + // init properties once from parent + auto impl = pimpl(); + impl->properties_.reset(); + if (impl->parent()) + impl->properties_.performInitFrom(PARENT, impl->parent()->properties()); } const ContainerBase *Stage::parent() const { @@ -278,16 +283,6 @@ const InterfaceState& PropagatingEitherWayPrivate::fetchEndState(){ return state; } -void PropagatingEitherWayPrivate::initProperties(const InterfaceState& state) -{ - // reset properties to their defaults - properties_.reset(); - // first init from INTERFACE - properties_.performInitFrom(Stage::INTERFACE, state.properties()); - // then init from PARENT - properties_.performInitFrom(Stage::PARENT, parent()->properties()); -} - bool PropagatingEitherWayPrivate::canCompute() const { if ((dir & PropagatingEitherWay::FORWARD) && hasStartState()) @@ -304,13 +299,15 @@ bool PropagatingEitherWayPrivate::compute() bool result = false; if ((dir & PropagatingEitherWay::FORWARD) && hasStartState()) { const InterfaceState& state = fetchStartState(); - initProperties(state); + // enforce property initialization from INTERFACE + properties_.performInitFrom(Stage::INTERFACE, state.properties(), true); if (me->computeForward(state)) result |= true; } if ((dir & PropagatingEitherWay::BACKWARD) && hasEndState()) { const InterfaceState& state = fetchEndState(); - initProperties(state); + // enforce property initialization from INTERFACE + properties_.performInitFrom(Stage::INTERFACE, state.properties(), true); if (me->computeBackward(state)) result |= true; } @@ -470,18 +467,9 @@ bool GeneratorPrivate::canCompute() const { } bool GeneratorPrivate::compute() { - initProperties(); return static_cast(me_)->compute(); } -void GeneratorPrivate::initProperties() -{ - // reset properties to their defaults - properties_.reset(); - // then init from PARENT - properties_.performInitFrom(Stage::PARENT, parent()->properties()); -} - Generator::Generator(const std::string &name) : ComputeBase(new GeneratorPrivate(this, name)) @@ -525,14 +513,6 @@ void ConnectingPrivate::newEndState(const Interface::iterator& it) --it_pairs_.second; } -void ConnectingPrivate::initProperties(const InterfaceState &start, const InterfaceState &end) -{ - // reset properties to their defaults - properties_.reset(); - // then init from PARENT - properties_.performInitFrom(Stage::PARENT, parent()->properties()); -} - bool ConnectingPrivate::canCompute() const{ // TODO: implement this properly return it_pairs_.first != starts_->end() && @@ -543,7 +523,6 @@ bool ConnectingPrivate::compute() { // TODO: implement this properly const InterfaceState& from = *it_pairs_.first; const InterfaceState& to = *(it_pairs_.second++); - initProperties(from, to); return static_cast(me_)->compute(from, to); } diff --git a/core/test/test_properties.cpp b/core/test/test_properties.cpp index 9f885a2e..0e1f4b68 100644 --- a/core/test/test_properties.cpp +++ b/core/test/test_properties.cpp @@ -109,6 +109,12 @@ TEST_F(InitFromTest, sourceId) { EXPECT_THROW(slave.property("double4"), std::runtime_error); } +TEST_F(InitFromTest, multipleSourceIds) { + slave.configureInitFrom(0); + slave.configureInitFrom(0); // init is allowed second time with same id + EXPECT_THROW(slave.configureInitFrom(1), std::runtime_error); // but not with other id +} + TEST_F(InitFromTest, otherName) { slave.property("double1").configureInitFrom(0, "double2"); // init double1 from double2 slave.performInitFrom(0, master);