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
|
/// exception thrown when trying to set a value not matching the declared type
|
||||||
class type_error;
|
class type_error;
|
||||||
|
|
||||||
typedef int SourceId;
|
typedef uint SourceFlags;
|
||||||
/// function callback used to initialize property value from another PropertyMap
|
/// function callback used to initialize property value from another PropertyMap
|
||||||
typedef std::function<boost::any(const PropertyMap& other)> InitializerFunction;
|
typedef std::function<boost::any(const PropertyMap& other)> InitializerFunction;
|
||||||
/// function callback used to signal value setting to external components
|
/// function callback used to signal value setting to external components
|
||||||
@ -138,14 +138,11 @@ public:
|
|||||||
std::string typeName() const { return type_index_.name(); }
|
std::string typeName() const { return type_index_.name(); }
|
||||||
|
|
||||||
/// return true, if property initialized from given SourceId
|
/// 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
|
/// 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
|
/// configure initialization from source using given other property name
|
||||||
Property &configureInitFrom(SourceId source, const std::string& name);
|
Property &configureInitFrom(SourceFlags source, const std::string& name);
|
||||||
|
|
||||||
/// set current value using matching configured initializers
|
|
||||||
void performInitFrom(SourceId source, const PropertyMap& other);
|
|
||||||
|
|
||||||
/// define a function callback to be called on each value update
|
/// define a function callback to be called on each value update
|
||||||
/// note, that boost::any doesn't allow for change detection
|
/// note, that boost::any doesn't allow for change detection
|
||||||
@ -158,7 +155,8 @@ private:
|
|||||||
boost::any value_;
|
boost::any value_;
|
||||||
|
|
||||||
/// used for external initialization
|
/// used for external initialization
|
||||||
SourceId source_id_ = 0;
|
SourceFlags source_flags_ = 0;
|
||||||
|
SourceFlags initialized_from_;
|
||||||
InitializerFunction initializer_;
|
InitializerFunction initializer_;
|
||||||
SignalFunction signaller_;
|
SignalFunction signaller_;
|
||||||
SerializeFunction serialize_;
|
SerializeFunction serialize_;
|
||||||
@ -237,7 +235,7 @@ public:
|
|||||||
const_iterator end() const { return props_.end(); }
|
const_iterator end() const { return props_.end(); }
|
||||||
|
|
||||||
/// allow initialization from given source for listed properties - always using the same name
|
/// 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
|
/// set (and, if neccessary, declare) the value of a property
|
||||||
template <typename T>
|
template <typename T>
|
||||||
@ -282,7 +280,7 @@ public:
|
|||||||
void reset();
|
void reset();
|
||||||
|
|
||||||
/// perform initialization of still undefined properties using configured initializers
|
/// 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
|
// boost::any needs a specialization to avoid recursion
|
||||||
|
|||||||
@ -132,9 +132,11 @@ public:
|
|||||||
*
|
*
|
||||||
* INTERFACE takes precedence over PARENT.
|
* INTERFACE takes precedence over PARENT.
|
||||||
*/
|
*/
|
||||||
enum PropertyInitializerSource {
|
enum PropertyInitializerSource { // TODO: move to property.cpp
|
||||||
PARENT,
|
DEFAULT = 0,
|
||||||
INTERFACE,
|
MANUAL = 1,
|
||||||
|
PARENT = 2,
|
||||||
|
INTERFACE = 4,
|
||||||
};
|
};
|
||||||
virtual ~Stage();
|
virtual ~Stage();
|
||||||
|
|
||||||
|
|||||||
@ -49,15 +49,18 @@ Property::Property(const std::type_index& type_index, const std::string& descrip
|
|||||||
, type_index_(type_index)
|
, type_index_(type_index)
|
||||||
, default_(default_value)
|
, default_(default_value)
|
||||||
, value_()
|
, value_()
|
||||||
|
, initialized_from_(-1)
|
||||||
, serialize_(serialize)
|
, serialize_(serialize)
|
||||||
{
|
{
|
||||||
// default value's type should match declared type by construction
|
// default value's type should match declared type by construction
|
||||||
assert(default_.empty() || std::type_index(default_.type()) == type_index_);
|
assert(default_.empty() || std::type_index(default_.type()) == type_index_);
|
||||||
|
reset();
|
||||||
}
|
}
|
||||||
|
|
||||||
void Property::setValue(const boost::any &value) {
|
void Property::setValue(const boost::any &value) {
|
||||||
setCurrentValue(value);
|
setCurrentValue(value);
|
||||||
default_ = value_;
|
default_ = value_;
|
||||||
|
initialized_from_ = 0;
|
||||||
}
|
}
|
||||||
|
|
||||||
void Property::setCurrentValue(const boost::any &value)
|
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());
|
throw Property::type_error(value.type().name(), type_index_.name());
|
||||||
|
|
||||||
value_ = value;
|
value_ = value;
|
||||||
|
initialized_from_ = 1; // manually initialized TODO: use enums
|
||||||
|
|
||||||
if (signaller_)
|
if (signaller_)
|
||||||
signaller_(this);
|
signaller_(this);
|
||||||
}
|
}
|
||||||
|
|
||||||
void Property::reset()
|
void Property::reset()
|
||||||
{
|
{
|
||||||
|
if (initialized_from_ == 0) // TODO: use enum
|
||||||
|
return; // keep manually set values
|
||||||
boost::any().swap(value_);
|
boost::any().swap(value_);
|
||||||
|
initialized_from_ = -1; // set to max value
|
||||||
}
|
}
|
||||||
|
|
||||||
std::string Property::serialize() const {
|
std::string Property::serialize() const {
|
||||||
@ -80,32 +88,26 @@ std::string Property::serialize() const {
|
|||||||
return serialize_(value());
|
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");
|
throw error("Property was already configured for initialization from another source id");
|
||||||
|
|
||||||
source_id_ = source;
|
source_flags_ = f ? source : SourceFlags();
|
||||||
initializer_ = f;
|
initializer_ = f;
|
||||||
return *this;
|
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); });
|
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,
|
Property& PropertyMap::declare(const std::string &name, const std::type_index &type_index,
|
||||||
const std::string &description, const boost::any &default_value,
|
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_);
|
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_) {
|
for (auto &pair : props_) {
|
||||||
if (properties.empty() || properties.count(pair.first))
|
if (properties.empty() || properties.count(pair.first))
|
||||||
@ -194,23 +196,37 @@ void PropertyMap::reset()
|
|||||||
pair.second.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_) {
|
for (auto& pair : props_) {
|
||||||
Property &p = pair.second;
|
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)
|
boost::any fromName(const PropertyMap& other, const std::string& other_name)
|
||||||
{
|
{
|
||||||
try {
|
|
||||||
return other.get(other_name);
|
return other.get(other_name);
|
||||||
} catch (const std::runtime_error &e) {
|
|
||||||
return boost::any();
|
|
||||||
}
|
|
||||||
}
|
}
|
||||||
|
|
||||||
|
|
||||||
|
|||||||
@ -440,13 +440,13 @@ void PropagatingEitherWayPrivate::compute()
|
|||||||
if (hasStartState()) {
|
if (hasStartState()) {
|
||||||
const InterfaceState& state = fetchStartState();
|
const InterfaceState& state = fetchStartState();
|
||||||
// enforce property initialization from INTERFACE
|
// enforce property initialization from INTERFACE
|
||||||
properties_.performInitFrom(Stage::INTERFACE, state.properties(), true);
|
properties_.performInitFrom(Stage::INTERFACE, state.properties());
|
||||||
me->computeForward(state);
|
me->computeForward(state);
|
||||||
}
|
}
|
||||||
if (hasEndState()) {
|
if (hasEndState()) {
|
||||||
const InterfaceState& state = fetchEndState();
|
const InterfaceState& state = fetchEndState();
|
||||||
// enforce property initialization from INTERFACE
|
// enforce property initialization from INTERFACE
|
||||||
properties_.performInitFrom(Stage::INTERFACE, state.properties(), true);
|
properties_.performInitFrom(Stage::INTERFACE, state.properties());
|
||||||
me->computeBackward(state);
|
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?
|
// -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...
|
// 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
|
// 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 auto& props = properties();
|
||||||
|
|
||||||
const bool ignore_collisions = props.get<bool>("ignore_collisions");
|
const bool ignore_collisions = props.get<bool>("ignore_collisions");
|
||||||
|
|||||||
@ -105,10 +105,10 @@ protected:
|
|||||||
};
|
};
|
||||||
|
|
||||||
TEST_F(InitFromTest, standard) {
|
TEST_F(InitFromTest, standard) {
|
||||||
slave.configureInitFrom(0); // init all matching vars
|
slave.configureInitFrom(1); // init all matching vars
|
||||||
ASSERT_FALSE(slave.property("double1").defined());
|
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>("double1"), 1.0);
|
||||||
EXPECT_EQ(slave.get<double>("double2"), 2.0);
|
EXPECT_EQ(slave.get<double>("double2"), 2.0);
|
||||||
EXPECT_FALSE(slave.property("double3").defined());
|
EXPECT_FALSE(slave.property("double3").defined());
|
||||||
@ -116,8 +116,8 @@ TEST_F(InitFromTest, standard) {
|
|||||||
}
|
}
|
||||||
|
|
||||||
TEST_F(InitFromTest, limited) {
|
TEST_F(InitFromTest, limited) {
|
||||||
slave.configureInitFrom(0, {"double1"}); // limit init to listed props
|
slave.configureInitFrom(1, {"double1"}); // limit init to listed props
|
||||||
slave.performInitFrom(0, master);
|
slave.performInitFrom(1, master);
|
||||||
EXPECT_EQ(slave.get<double>("double1"), 1.0);
|
EXPECT_EQ(slave.get<double>("double1"), 1.0);
|
||||||
EXPECT_FALSE(slave.property("double2").defined());
|
EXPECT_FALSE(slave.property("double2").defined());
|
||||||
EXPECT_FALSE(slave.property("double3").defined());
|
EXPECT_FALSE(slave.property("double3").defined());
|
||||||
@ -125,8 +125,8 @@ TEST_F(InitFromTest, limited) {
|
|||||||
}
|
}
|
||||||
|
|
||||||
TEST_F(InitFromTest, sourceId) {
|
TEST_F(InitFromTest, sourceId) {
|
||||||
slave.configureInitFrom(0); // init all matching vars
|
slave.configureInitFrom(1); // init all matching vars
|
||||||
slave.performInitFrom(1, master); // init with wrong sourceId -> no effect
|
slave.performInitFrom(2, master); // init with wrong sourceId -> no effect
|
||||||
EXPECT_FALSE(slave.property("double1").defined());
|
EXPECT_FALSE(slave.property("double1").defined());
|
||||||
EXPECT_FALSE(slave.property("double2").defined());
|
EXPECT_FALSE(slave.property("double2").defined());
|
||||||
EXPECT_FALSE(slave.property("double3").defined());
|
EXPECT_FALSE(slave.property("double3").defined());
|
||||||
@ -134,14 +134,14 @@ TEST_F(InitFromTest, sourceId) {
|
|||||||
}
|
}
|
||||||
|
|
||||||
TEST_F(InitFromTest, multipleSourceIds) {
|
TEST_F(InitFromTest, multipleSourceIds) {
|
||||||
slave.configureInitFrom(0);
|
slave.configureInitFrom(1);
|
||||||
slave.configureInitFrom(0); // init is allowed second time with same id
|
slave.configureInitFrom(1); // init is allowed second time with same id
|
||||||
EXPECT_THROW(slave.configureInitFrom(1), std::runtime_error); // but not with other id
|
EXPECT_THROW(slave.configureInitFrom(2), std::runtime_error); // but not with other id
|
||||||
}
|
}
|
||||||
|
|
||||||
TEST_F(InitFromTest, otherName) {
|
TEST_F(InitFromTest, otherName) {
|
||||||
slave.property("double1").configureInitFrom(0, "double2"); // init double1 from double2
|
slave.property("double1").configureInitFrom(1, "double2"); // init double1 from double2
|
||||||
slave.performInitFrom(0, master);
|
slave.performInitFrom(1, master);
|
||||||
EXPECT_EQ(slave.get<double>("double1"), 2.0);
|
EXPECT_EQ(slave.get<double>("double1"), 2.0);
|
||||||
EXPECT_FALSE(slave.property("double2").defined());
|
EXPECT_FALSE(slave.property("double2").defined());
|
||||||
EXPECT_FALSE(slave.property("double3").defined());
|
EXPECT_FALSE(slave.property("double3").defined());
|
||||||
@ -149,9 +149,9 @@ TEST_F(InitFromTest, otherName) {
|
|||||||
}
|
}
|
||||||
|
|
||||||
TEST_F(InitFromTest, function) {
|
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");
|
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);
|
EXPECT_EQ(slave.get<double>("double3"), 3.0);
|
||||||
}
|
}
|
||||||
|
|||||||
Loading…
Reference in New Issue
Block a user