mirror of
https://github.com/moveit/moveit_task_constructor.git
synced 2025-11-04 14:49:57 +08:00
property inheritance: both from PARENT and INTERFACE
- source_id -> source_flags: bits indicating configured paths - initializers, e.g. fromName(), should throw - ignore undeclared errors during inheritance - on undefined error, reset the value to None - override value only if previously set by lower-priority source MANUAL > CURRENT > PARENT > INTERFACE
This commit is contained in:
parent
2e9932cfbc
commit
cbb2cd69f7
@ -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<boost::any(const PropertyMap& other)> 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<std::string> &properties = {});
|
||||
void configureInitFrom(Property::SourceFlags source, const std::set<std::string> &properties = {});
|
||||
|
||||
/// set (and, if neccessary, declare) the value of a property
|
||||
template <typename T>
|
||||
@ -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
|
||||
|
||||
@ -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();
|
||||
|
||||
|
||||
@ -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<std::string> &properties)
|
||||
void PropertyMap::configureInitFrom(Property::SourceFlags source, const std::set<std::string> &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);
|
||||
}
|
||||
|
||||
|
||||
|
||||
@ -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);
|
||||
}
|
||||
}
|
||||
|
||||
@ -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<bool>("ignore_collisions");
|
||||
|
||||
@ -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<double>("double1"), 1.0);
|
||||
EXPECT_EQ(slave.get<double>("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<double>("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<double>("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<double>("double1") + other.get<double>("double2");
|
||||
});
|
||||
slave.performInitFrom(0, master);
|
||||
slave.performInitFrom(1, master);
|
||||
EXPECT_EQ(slave.get<double>("double3"), 3.0);
|
||||
}
|
||||
|
||||
Loading…
Reference in New Issue
Block a user