mirror of
https://github.com/moveit/moveit_task_constructor.git
synced 2025-11-04 14:49:57 +08:00
allow only a single inititialization source
This commit is contained in:
parent
e9363919e8
commit
c3eead0115
@ -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<boost::any(const PropertyMap& other)> InitializerFunction;
|
||||
typedef std::map<SourceId, InitializerFunction> 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<typename T>
|
||||
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<typename T>
|
||||
|
||||
@ -158,8 +158,6 @@ protected:
|
||||
// get informed when new start or end state becomes available
|
||||
void newStartState(const std::list<InterfaceState>::iterator& it);
|
||||
void newEndState(const std::list<InterfaceState>::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<InterfaceState>::iterator& it);
|
||||
void newEndState(const std::list<InterfaceState>::iterator& it);
|
||||
// initialize properties from parent and/or interface states
|
||||
void initProperties(const InterfaceState &start, const InterfaceState &end);
|
||||
|
||||
std::pair<Interface::const_iterator, Interface::const_iterator> it_pairs_;
|
||||
};
|
||||
|
||||
@ -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");
|
||||
|
||||
@ -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));
|
||||
}
|
||||
|
||||
|
||||
|
||||
@ -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<Generator*>(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<Connecting*>(me_)->compute(from, to);
|
||||
}
|
||||
|
||||
|
||||
@ -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);
|
||||
|
||||
Loading…
Reference in New Issue
Block a user