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 {
|
class Property {
|
||||||
friend class PropertyMap;
|
friend class PropertyMap;
|
||||||
|
|
||||||
|
Property(const std::type_index &type_index, const std::string &description, const boost::any &default_value);
|
||||||
|
|
||||||
public:
|
public:
|
||||||
typedef int SourceId;
|
typedef int SourceId;
|
||||||
|
/// 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;
|
||||||
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
|
/// set current value and default value
|
||||||
void setValue(const boost::any& value);
|
void setValue(const boost::any& value);
|
||||||
@ -97,7 +97,7 @@ public:
|
|||||||
/// 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(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);
|
void performInitFrom(SourceId source, const PropertyMap& other);
|
||||||
|
|
||||||
private:
|
private:
|
||||||
@ -105,7 +105,10 @@ private:
|
|||||||
std::type_index type_index_;
|
std::type_index type_index_;
|
||||||
boost::any default_;
|
boost::any default_;
|
||||||
boost::any value_;
|
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
|
/// implementation of declare methods
|
||||||
Property& declare(const std::string& name, const std::type_info& type,
|
Property& declare(const std::string& name, const std::type_info& type,
|
||||||
const std::string& description = "",
|
const std::string& description,
|
||||||
const boost::any& default_value = boost::any());
|
const boost::any& default_value);
|
||||||
public:
|
public:
|
||||||
/// declare a property for future use
|
/// declare a property for future use
|
||||||
template<typename T>
|
template<typename T>
|
||||||
Property& declare(const std::string& name, const std::string& description = "") {
|
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
|
/// declare a property with default value
|
||||||
template<typename T>
|
template<typename T>
|
||||||
|
|||||||
@ -158,8 +158,6 @@ protected:
|
|||||||
// get informed when new start or end state becomes available
|
// get informed when new start or end state becomes available
|
||||||
void newStartState(const std::list<InterfaceState>::iterator& it);
|
void newStartState(const std::list<InterfaceState>::iterator& it);
|
||||||
void newEndState(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_start_state_;
|
||||||
Interface::const_iterator next_end_state_;
|
Interface::const_iterator next_end_state_;
|
||||||
@ -187,10 +185,6 @@ public:
|
|||||||
|
|
||||||
bool canCompute() const override;
|
bool canCompute() const override;
|
||||||
bool compute() override;
|
bool compute() override;
|
||||||
|
|
||||||
private:
|
|
||||||
// initialize properties from parent
|
|
||||||
void initProperties();
|
|
||||||
};
|
};
|
||||||
PIMPL_FUNCTIONS(Generator)
|
PIMPL_FUNCTIONS(Generator)
|
||||||
|
|
||||||
@ -211,8 +205,6 @@ private:
|
|||||||
// get informed when new start or end state becomes available
|
// get informed when new start or end state becomes available
|
||||||
void newStartState(const std::list<InterfaceState>::iterator& it);
|
void newStartState(const std::list<InterfaceState>::iterator& it);
|
||||||
void newEndState(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_;
|
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);
|
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
|
// we need to have some children to do the actual work
|
||||||
if (children.empty()) {
|
if (children.empty()) {
|
||||||
errors.push_back(*this, "no children");
|
errors.push_back(*this, "no children");
|
||||||
|
|||||||
@ -84,22 +84,23 @@ void Property::reset()
|
|||||||
|
|
||||||
Property& Property::configureInitFrom(SourceId source, const Property::InitializerFunction &f)
|
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;
|
return *this;
|
||||||
}
|
}
|
||||||
|
|
||||||
Property &Property::configureInitFrom(SourceId source, const std::string &name)
|
Property &Property::configureInitFrom(SourceId source, const std::string &name)
|
||||||
{
|
{
|
||||||
initializers_[source] = [name](const PropertyMap& other) { return fromName(other, name); };
|
return configureInitFrom(source, [name](const PropertyMap& other) { return fromName(other, name); });
|
||||||
return *this;
|
|
||||||
}
|
}
|
||||||
|
|
||||||
void Property::performInitFrom(SourceId source, const PropertyMap &other)
|
void Property::performInitFrom(SourceId source, const PropertyMap &other)
|
||||||
{
|
{
|
||||||
auto it = initializers_.find(source);
|
if (source_id_ != source || !initializer_) return; // source ids not matching
|
||||||
if (it == initializers_.end()) return;
|
setCurrentValue(initializer_(other));
|
||||||
|
|
||||||
setCurrentValue(it->second(other));
|
|
||||||
}
|
}
|
||||||
|
|
||||||
|
|
||||||
|
|||||||
@ -140,6 +140,11 @@ void Stage::reset()
|
|||||||
|
|
||||||
void Stage::init(const planning_scene::PlanningSceneConstPtr &scene)
|
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 {
|
const ContainerBase *Stage::parent() const {
|
||||||
@ -278,16 +283,6 @@ const InterfaceState& PropagatingEitherWayPrivate::fetchEndState(){
|
|||||||
return state;
|
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
|
bool PropagatingEitherWayPrivate::canCompute() const
|
||||||
{
|
{
|
||||||
if ((dir & PropagatingEitherWay::FORWARD) && hasStartState())
|
if ((dir & PropagatingEitherWay::FORWARD) && hasStartState())
|
||||||
@ -304,13 +299,15 @@ bool PropagatingEitherWayPrivate::compute()
|
|||||||
bool result = false;
|
bool result = false;
|
||||||
if ((dir & PropagatingEitherWay::FORWARD) && hasStartState()) {
|
if ((dir & PropagatingEitherWay::FORWARD) && hasStartState()) {
|
||||||
const InterfaceState& state = fetchStartState();
|
const InterfaceState& state = fetchStartState();
|
||||||
initProperties(state);
|
// enforce property initialization from INTERFACE
|
||||||
|
properties_.performInitFrom(Stage::INTERFACE, state.properties(), true);
|
||||||
if (me->computeForward(state))
|
if (me->computeForward(state))
|
||||||
result |= true;
|
result |= true;
|
||||||
}
|
}
|
||||||
if ((dir & PropagatingEitherWay::BACKWARD) && hasEndState()) {
|
if ((dir & PropagatingEitherWay::BACKWARD) && hasEndState()) {
|
||||||
const InterfaceState& state = fetchEndState();
|
const InterfaceState& state = fetchEndState();
|
||||||
initProperties(state);
|
// enforce property initialization from INTERFACE
|
||||||
|
properties_.performInitFrom(Stage::INTERFACE, state.properties(), true);
|
||||||
if (me->computeBackward(state))
|
if (me->computeBackward(state))
|
||||||
result |= true;
|
result |= true;
|
||||||
}
|
}
|
||||||
@ -470,18 +467,9 @@ bool GeneratorPrivate::canCompute() const {
|
|||||||
}
|
}
|
||||||
|
|
||||||
bool GeneratorPrivate::compute() {
|
bool GeneratorPrivate::compute() {
|
||||||
initProperties();
|
|
||||||
return static_cast<Generator*>(me_)->compute();
|
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)
|
Generator::Generator(const std::string &name)
|
||||||
: ComputeBase(new GeneratorPrivate(this, name))
|
: ComputeBase(new GeneratorPrivate(this, name))
|
||||||
@ -525,14 +513,6 @@ void ConnectingPrivate::newEndState(const Interface::iterator& it)
|
|||||||
--it_pairs_.second;
|
--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{
|
bool ConnectingPrivate::canCompute() const{
|
||||||
// TODO: implement this properly
|
// TODO: implement this properly
|
||||||
return it_pairs_.first != starts_->end() &&
|
return it_pairs_.first != starts_->end() &&
|
||||||
@ -543,7 +523,6 @@ bool ConnectingPrivate::compute() {
|
|||||||
// TODO: implement this properly
|
// TODO: implement this properly
|
||||||
const InterfaceState& from = *it_pairs_.first;
|
const InterfaceState& from = *it_pairs_.first;
|
||||||
const InterfaceState& to = *(it_pairs_.second++);
|
const InterfaceState& to = *(it_pairs_.second++);
|
||||||
initProperties(from, to);
|
|
||||||
return static_cast<Connecting*>(me_)->compute(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);
|
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) {
|
TEST_F(InitFromTest, otherName) {
|
||||||
slave.property("double1").configureInitFrom(0, "double2"); // init double1 from double2
|
slave.property("double1").configureInitFrom(0, "double2"); // init double1 from double2
|
||||||
slave.performInitFrom(0, master);
|
slave.performInitFrom(0, master);
|
||||||
|
|||||||
Loading…
Reference in New Issue
Block a user