diff --git a/core/include/moveit/task_constructor/properties.h b/core/include/moveit/task_constructor/properties.h index 6fce3790..250d389b 100644 --- a/core/include/moveit/task_constructor/properties.h +++ b/core/include/moveit/task_constructor/properties.h @@ -107,7 +107,7 @@ public: /// exception thrown when trying to set a value not matching the declared type class type_error; - typedef int SourceId; + typedef uint SourceFlags; /// function callback used to initialize property value from another PropertyMap typedef std::function InitializerFunction; /// function callback used to signal value setting to external components @@ -138,14 +138,11 @@ public: std::string typeName() const { return type_index_.name(); } /// return true, if property initialized from given SourceId - bool initsFrom(SourceId source) const; + bool initsFrom(SourceFlags source) const; /// configure initialization from source using an arbitrary function - Property &configureInitFrom(SourceId source, const InitializerFunction& f); + Property &configureInitFrom(SourceFlags source, const InitializerFunction& f); /// configure initialization from source using given other property name - Property &configureInitFrom(SourceId source, const std::string& name); - - /// set current value using matching configured initializers - void performInitFrom(SourceId source, const PropertyMap& other); + Property &configureInitFrom(SourceFlags source, const std::string& name); /// define a function callback to be called on each value update /// note, that boost::any doesn't allow for change detection @@ -158,7 +155,8 @@ private: boost::any value_; /// used for external initialization - SourceId source_id_ = 0; + SourceFlags source_flags_ = 0; + SourceFlags initialized_from_; InitializerFunction initializer_; SignalFunction signaller_; SerializeFunction serialize_; @@ -237,7 +235,7 @@ public: const_iterator end() const { return props_.end(); } /// allow initialization from given source for listed properties - always using the same name - void configureInitFrom(Property::SourceId source, const std::set &properties = {}); + void configureInitFrom(Property::SourceFlags source, const std::set &properties = {}); /// set (and, if neccessary, declare) the value of a property template @@ -282,7 +280,7 @@ public: void reset(); /// perform initialization of still undefined properties using configured initializers - void performInitFrom(Property::SourceId source, const PropertyMap& other, bool enforce = false); + void performInitFrom(Property::SourceFlags source, const PropertyMap& other); }; // boost::any needs a specialization to avoid recursion diff --git a/core/include/moveit/task_constructor/stage.h b/core/include/moveit/task_constructor/stage.h index ef56e267..d16a6b26 100644 --- a/core/include/moveit/task_constructor/stage.h +++ b/core/include/moveit/task_constructor/stage.h @@ -132,9 +132,11 @@ public: * * INTERFACE takes precedence over PARENT. */ - enum PropertyInitializerSource { - PARENT, - INTERFACE, + enum PropertyInitializerSource { // TODO: move to property.cpp + DEFAULT = 0, + MANUAL = 1, + PARENT = 2, + INTERFACE = 4, }; virtual ~Stage(); diff --git a/core/src/properties.cpp b/core/src/properties.cpp index 75f6693e..53233109 100644 --- a/core/src/properties.cpp +++ b/core/src/properties.cpp @@ -49,15 +49,18 @@ Property::Property(const std::type_index& type_index, const std::string& descrip , type_index_(type_index) , default_(default_value) , value_() + , initialized_from_(-1) , serialize_(serialize) { // default value's type should match declared type by construction assert(default_.empty() || std::type_index(default_.type()) == type_index_); + reset(); } void Property::setValue(const boost::any &value) { setCurrentValue(value); default_ = value_; + initialized_from_ = 0; } void Property::setCurrentValue(const boost::any &value) @@ -66,13 +69,18 @@ void Property::setCurrentValue(const boost::any &value) throw Property::type_error(value.type().name(), type_index_.name()); value_ = value; + initialized_from_ = 1; // manually initialized TODO: use enums + if (signaller_) signaller_(this); } void Property::reset() { + if (initialized_from_ == 0) // TODO: use enum + return; // keep manually set values boost::any().swap(value_); + initialized_from_ = -1; // set to max value } std::string Property::serialize() const { @@ -80,32 +88,26 @@ std::string Property::serialize() const { return serialize_(value()); } -bool Property::initsFrom(Property::SourceId source) const +bool Property::initsFrom(Property::SourceFlags source) const { - return (source == source_id_ && initializer_); + return source & source_flags_; } -Property& Property::configureInitFrom(SourceId source, const Property::InitializerFunction &f) +Property& Property::configureInitFrom(SourceFlags source, const Property::InitializerFunction &f) { - if (source != source_id_ && initializer_) + if (source != source_flags_ && initializer_) throw error("Property was already configured for initialization from another source id"); - source_id_ = source; + source_flags_ = f ? source : SourceFlags(); initializer_ = f; return *this; } -Property &Property::configureInitFrom(SourceId source, const std::string &name) +Property &Property::configureInitFrom(SourceFlags source, const std::string &name) { return configureInitFrom(source, [name](const PropertyMap& other) { return fromName(other, name); }); } -void Property::performInitFrom(SourceId source, const PropertyMap &other) -{ - if (source_id_ != source || !initializer_) return; // source ids not matching - setCurrentValue(initializer_(other)); -} - Property& PropertyMap::declare(const std::string &name, const std::type_index &type_index, const std::string &description, const boost::any &default_value, @@ -143,7 +145,7 @@ void PropertyMap::exposeTo(PropertyMap& other, const std::string& name, const st other.declare(other_name, p.type_index_, p.description_, p.default_, p.serialize_); } -void PropertyMap::configureInitFrom(Property::SourceId source, const std::set &properties) +void PropertyMap::configureInitFrom(Property::SourceFlags source, const std::set &properties) { for (auto &pair : props_) { if (properties.empty() || properties.count(pair.first)) @@ -194,23 +196,37 @@ void PropertyMap::reset() pair.second.reset(); } -void PropertyMap::performInitFrom(Property::SourceId source, const PropertyMap &other, bool enforce) +void PropertyMap::performInitFrom(Property::SourceFlags source, const PropertyMap &other) { for (auto& pair : props_) { Property &p = pair.second; - if (enforce || !p.defined()) - p.performInitFrom(source, other); + + // don't override value previously set by higher-priority source + // MANUAL > CURRENT > PARENT > INTERFACE + if (p.initialized_from_ < source && p.defined()) + continue; + // is the property configured for initialization from this source? + if (!p.initsFrom(source)) + continue; + + boost::any value; + try { + value = p.initializer_(other); + } catch (const Property::undeclared&) { + // ignore undeclared + continue; + } catch (const Property::undefined&) { + } + + p.setCurrentValue(value); + p.initialized_from_ = source; } } boost::any fromName(const PropertyMap& other, const std::string& other_name) { - try { - return other.get(other_name); - } catch (const std::runtime_error &e) { - return boost::any(); - } + return other.get(other_name); } diff --git a/core/src/stage.cpp b/core/src/stage.cpp index 58e08960..6c320600 100644 --- a/core/src/stage.cpp +++ b/core/src/stage.cpp @@ -440,13 +440,13 @@ void PropagatingEitherWayPrivate::compute() if (hasStartState()) { const InterfaceState& state = fetchStartState(); // enforce property initialization from INTERFACE - properties_.performInitFrom(Stage::INTERFACE, state.properties(), true); + properties_.performInitFrom(Stage::INTERFACE, state.properties()); me->computeForward(state); } if (hasEndState()) { const InterfaceState& state = fetchEndState(); // enforce property initialization from INTERFACE - properties_.performInitFrom(Stage::INTERFACE, state.properties(), true); + properties_.performInitFrom(Stage::INTERFACE, state.properties()); me->computeBackward(state); } } diff --git a/core/src/stages/compute_ik.cpp b/core/src/stages/compute_ik.cpp index 8f2a4828..87ae14ff 100644 --- a/core/src/stages/compute_ik.cpp +++ b/core/src/stages/compute_ik.cpp @@ -212,7 +212,7 @@ void ComputeIK::onNewSolution(const SolutionBase &s) // -1 TODO: this should not be necessary in my opinion: Why do you think so? // It is, because the properties on the interface might change from call to call... // enforced initialization from interface ensures that new target_pose is read - properties().performInitFrom(INTERFACE, s.start()->properties(), true); + properties().performInitFrom(INTERFACE, s.start()->properties()); const auto& props = properties(); const bool ignore_collisions = props.get("ignore_collisions"); diff --git a/core/test/test_properties.cpp b/core/test/test_properties.cpp index 67e9f576..17226ed3 100644 --- a/core/test/test_properties.cpp +++ b/core/test/test_properties.cpp @@ -105,10 +105,10 @@ protected: }; TEST_F(InitFromTest, standard) { - slave.configureInitFrom(0); // init all matching vars + slave.configureInitFrom(1); // init all matching vars ASSERT_FALSE(slave.property("double1").defined()); - slave.performInitFrom(0, master); + slave.performInitFrom(1, master); EXPECT_EQ(slave.get("double1"), 1.0); EXPECT_EQ(slave.get("double2"), 2.0); EXPECT_FALSE(slave.property("double3").defined()); @@ -116,8 +116,8 @@ TEST_F(InitFromTest, standard) { } TEST_F(InitFromTest, limited) { - slave.configureInitFrom(0, {"double1"}); // limit init to listed props - slave.performInitFrom(0, master); + slave.configureInitFrom(1, {"double1"}); // limit init to listed props + slave.performInitFrom(1, master); EXPECT_EQ(slave.get("double1"), 1.0); EXPECT_FALSE(slave.property("double2").defined()); EXPECT_FALSE(slave.property("double3").defined()); @@ -125,8 +125,8 @@ TEST_F(InitFromTest, limited) { } TEST_F(InitFromTest, sourceId) { - slave.configureInitFrom(0); // init all matching vars - slave.performInitFrom(1, master); // init with wrong sourceId -> no effect + slave.configureInitFrom(1); // init all matching vars + slave.performInitFrom(2, master); // init with wrong sourceId -> no effect EXPECT_FALSE(slave.property("double1").defined()); EXPECT_FALSE(slave.property("double2").defined()); EXPECT_FALSE(slave.property("double3").defined()); @@ -134,14 +134,14 @@ TEST_F(InitFromTest, sourceId) { } 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 + slave.configureInitFrom(1); + slave.configureInitFrom(1); // init is allowed second time with same id + EXPECT_THROW(slave.configureInitFrom(2), 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); + slave.property("double1").configureInitFrom(1, "double2"); // init double1 from double2 + slave.performInitFrom(1, master); EXPECT_EQ(slave.get("double1"), 2.0); EXPECT_FALSE(slave.property("double2").defined()); EXPECT_FALSE(slave.property("double3").defined()); @@ -149,9 +149,9 @@ TEST_F(InitFromTest, otherName) { } TEST_F(InitFromTest, function) { - slave.property("double3").configureInitFrom(0, [](const PropertyMap& other) -> boost::any { + slave.property("double3").configureInitFrom(1, [](const PropertyMap& other) -> boost::any { return other.get("double1") + other.get("double2"); }); - slave.performInitFrom(0, master); + slave.performInitFrom(1, master); EXPECT_EQ(slave.get("double3"), 3.0); }