diff --git a/core/include/moveit/task_constructor/container_p.h b/core/include/moveit/task_constructor/container_p.h index 6dc02c8c..1af29b5a 100644 --- a/core/include/moveit/task_constructor/container_p.h +++ b/core/include/moveit/task_constructor/container_p.h @@ -100,10 +100,6 @@ public: protected: ContainerBasePrivate(ContainerBase *me, const std::string &name); - // containers don't required a specific interface on their own - // their interface is derived from their children - InterfaceFlags requiredInterface() const override { return InterfaceFlags(); } - // Set child's push interfaces: allow pushing if child requires it or // if the interface is unknown: in this case greedily assume a push interface. // If, during pruneInterface() later, we notice that it's not needed, prune there. @@ -172,6 +168,9 @@ class SerialContainerPrivate : public ContainerBasePrivate { public: SerialContainerPrivate(SerialContainer* me, const std::string &name); + // containers derive their required interface from their children + InterfaceFlags requiredInterface() const override; + // called by parent asking for pruning of this' interface void pruneInterface(InterfaceFlags accepted) override; @@ -220,6 +219,12 @@ class ParallelContainerBasePrivate : public ContainerBasePrivate { public: ParallelContainerBasePrivate(ParallelContainerBase* me, const std::string &name); + // containers derive their required interface from their children + InterfaceFlags requiredInterface() const override; + + // called by parent asking for pruning of this' interface + void pruneInterface(InterfaceFlags accepted) override; + private: /// callback for new externally received states void onNewExternalState(Interface::Direction dir, Interface::iterator external, bool updated); diff --git a/core/include/moveit/task_constructor/stage.h b/core/include/moveit/task_constructor/stage.h index 5038aee0..91e5254f 100644 --- a/core/include/moveit/task_constructor/stage.h +++ b/core/include/moveit/task_constructor/stage.h @@ -77,6 +77,11 @@ enum InterfaceFlag { }; typedef Flags InterfaceFlags; +// some useful constants +constexpr InterfaceFlags UNKNOWN; +constexpr InterfaceFlags INPUT_IF_MASK({READS_START, WRITES_PREV_END}); +constexpr InterfaceFlags OUTPUT_IF_MASK({READS_END, WRITES_NEXT_START}); + MOVEIT_CLASS_FORWARD(Interface) MOVEIT_CLASS_FORWARD(Stage) class InterfaceState; diff --git a/core/src/container.cpp b/core/src/container.cpp index f955ffef..f73dcf27 100644 --- a/core/src/container.cpp +++ b/core/src/container.cpp @@ -344,11 +344,20 @@ SerialContainerPrivate::SerialContainerPrivate(SerialContainer *me, const std::s : ContainerBasePrivate(me, name) {} +// a serial container's required interface is derived from the required input interface +// of the first child and the required output interface of the last child +InterfaceFlags SerialContainerPrivate::requiredInterface() const +{ + if (children().empty()) + return UNKNOWN; + return (children().front()->pimpl()->requiredInterface() & INPUT_IF_MASK) + | (children().back()->pimpl()->requiredInterface() & OUTPUT_IF_MASK); +} + // connect cur stage to its predecessor and successor by setting the push interface pointers // return true if cur stage should be scheduled for a second sweep bool SerialContainerPrivate::connect(container_type::const_iterator cur) { - constexpr InterfaceFlags UNKNOWN; StagePrivate* const cur_impl = **cur; InterfaceFlags required = cur_impl->requiredInterface(); @@ -431,7 +440,7 @@ void SerialContainerPrivate::pruneInterface(InterfaceFlags accepted) { if (children().empty()) return; constexpr InterfaceFlags BOTHWAYS({PROPAGATE_FORWARDS, PROPAGATE_BACKWARDS}); - // I think, we only need to deal with the special case of the whole sequence to be pruned. + // We only need to deal with the special case of the whole sequence to be pruned. if (accepted != BOTHWAYS && // will interface be restricted at all? children().front()->pimpl()->interfaceFlags() == BOTHWAYS) // still undecided? { @@ -443,10 +452,8 @@ void SerialContainerPrivate::pruneInterface(InterfaceFlags accepted) { if (!children().back()->pimpl()->ends()) ends_.reset(); } - - // If my assumption doesn't hold, we would need to prune partial sequences of stages - // with unknown-only interfaces at the *beginning* and *end* of the whole sequence. - // Reuse code from init() in that case. + if (interfaceFlags() == UNKNOWN) + throw InitStageException(*me(), "failed to derive propagation direction"); } // called by init() to prune interfaces for children in range [first, last) @@ -509,17 +516,14 @@ void SerialContainer::validateConnectivity() const auto impl = pimpl(); InitStageException errors; - constexpr InterfaceFlags INP_IF_MASK({READS_START, WRITES_PREV_END}); - constexpr InterfaceFlags OUT_IF_MASK({READS_END, WRITES_NEXT_START}); - // check that input / output interface of first / last child matches this' resp. interface if (!impl->children().empty()) { const StagePrivate* start = impl->children().front()->pimpl(); - if ((start->interfaceFlags() & INP_IF_MASK) != (this->pimpl()->interfaceFlags() & INP_IF_MASK)) + if ((start->interfaceFlags() & INPUT_IF_MASK) != (this->pimpl()->interfaceFlags() & INPUT_IF_MASK)) errors.push_back(*this, "input interface of '" + start->name() + "' doesn't match mine"); const StagePrivate* last = impl->children().back()->pimpl(); - if ((last->interfaceFlags() & OUT_IF_MASK) != (this->pimpl()->interfaceFlags() & OUT_IF_MASK)) + if ((last->interfaceFlags() & OUTPUT_IF_MASK) != (this->pimpl()->interfaceFlags() & OUTPUT_IF_MASK)) errors.push_back(*this, "output interface of '" + last->name() + "' doesn't match mine"); } @@ -635,6 +639,27 @@ ParallelContainerBasePrivate::ParallelContainerBasePrivate(ParallelContainerBase { } +// A parallel container's required interface is derived from the required interfaces of all of its children. +// They must not conflict to each other. Otherwise an InitStageException is thrown. +InterfaceFlags ParallelContainerBasePrivate::requiredInterface() const +{ + if (children().empty()) + return UNKNOWN; + /* TODO: replace this with a proper check for consistency of interfaces. Allowed combinations are: + * ❘ ❘ = ❘ (connecting stages) + * ↑ ↑ = ↑ (backward propagating) + * ↓ ↓ = ↓ (forward propagating) + * ↑ ↓ = ⇅ = ⇅ ↑ = ⇅ ↓ (propagating in both directions) + * ↕ ↕ = ↕ (generating) + */ + return children().front()->pimpl()->requiredInterface(); +} + +void ParallelContainerBasePrivate::pruneInterface(InterfaceFlags accepted) +{ + // TODO: forward pruning to all children with UNKNOWN required interface +} + void ParallelContainerBasePrivate::onNewExternalState(Interface::Direction dir, Interface::iterator external, bool updated) { for (const Stage::pointer& stage : children()) copyState(external, stage->pimpl()->pullInterface(dir), updated); @@ -695,9 +720,6 @@ void ParallelContainerBase::init(const planning_scene::PlanningSceneConstPtr &sc void ParallelContainerBase::validateConnectivity() const { - constexpr InterfaceFlags INP_IF_MASK({READS_START, WRITES_PREV_END}); - constexpr InterfaceFlags OUT_IF_MASK({READS_END, WRITES_NEXT_START}); - InitStageException errors; auto impl = pimpl(); InterfaceFlags my_interface = impl->interfaceFlags(); diff --git a/core/src/stage.cpp b/core/src/stage.cpp index 702adccf..c4b02a11 100644 --- a/core/src/stage.cpp +++ b/core/src/stage.cpp @@ -177,7 +177,6 @@ void Stage::setProperty(const std::string& name, const boost::any& value) { template const char* direction(const StagePrivate& stage) { - static constexpr InterfaceFlags INPUT_IF_MASK({ READS_START | WRITES_PREV_END }); InterfaceFlags f = stage.interfaceFlags(); bool own_if = f & own;