diff --git a/core/include/moveit/task_constructor/container_p.h b/core/include/moveit/task_constructor/container_p.h index 45858028..af204e00 100644 --- a/core/include/moveit/task_constructor/container_p.h +++ b/core/include/moveit/task_constructor/container_p.h @@ -105,7 +105,7 @@ public: void validateConnectivity() const override; - // containers derive their required interface from their children + // Containers derive their required interface from their children // UNKNOWN until pruneInterface was called InterfaceFlags requiredInterface() const override { return required_interface_; } @@ -124,12 +124,12 @@ protected: // If, during pruneInterface() later, we notice that it's not needed, prune there. inline void setChildsPushBackwardInterface(Stage& child) { InterfaceFlags required = child.pimpl()->requiredInterface(); - bool allowed = (required & WRITES_PREV_END) || (required == UNKNOWN); + bool allowed = (required & WRITES_PREV_END); child.pimpl()->setPrevEnds(allowed ? pending_backward_ : InterfacePtr()); } inline void setChildsPushForwardInterface(Stage& child) { InterfaceFlags required = child.pimpl()->requiredInterface(); - bool allowed = (required & WRITES_NEXT_START) || (required == UNKNOWN); + bool allowed = (required & WRITES_NEXT_START); child.pimpl()->setNextStarts(allowed ? pending_forward_ : InterfacePtr()); } // report error about mismatching interface (start or end as determined by mask) @@ -144,7 +144,7 @@ protected: auto& internalToExternalMap() { return internal_to_external_; } const auto& internalToExternalMap() const { return internal_to_external_; } - // set by containers in pruneInterface() + // set in pruneInterface() InterfaceFlags required_interface_; private: @@ -249,7 +249,7 @@ public: typedef std::function Spawner; MergerPrivate(Merger* me, const std::string& name); - InterfaceFlags requiredInterface() const override; + void pruneInterface(InterfaceFlags accepted) override; void onNewPropagateSolution(const SolutionBase& s); void onNewGeneratorSolution(const SolutionBase& s); diff --git a/core/include/moveit/task_constructor/stage_p.h b/core/include/moveit/task_constructor/stage_p.h index e2386267..572bc2d0 100644 --- a/core/include/moveit/task_constructor/stage_p.h +++ b/core/include/moveit/task_constructor/stage_p.h @@ -76,11 +76,10 @@ public: * If the interface is unknown (because it is auto-detected from context), return UNKNOWN */ virtual InterfaceFlags requiredInterface() const = 0; - /** validate correct interface usage and resolve ambiguous interfaces */ - virtual void pruneInterface(InterfaceFlags accepted); - - void pruneStartInterface(InterfaceFlags start_flags) { pruneInterface(start_flags & START_IF_MASK); } - void pruneEndInterface(InterfaceFlags end_flags) { pruneInterface(end_flags & END_IF_MASK); } + /** Prune interface to comply with the given propagation direction + * + * PropagatingEitherWay uses this in restrictDirection() */ + virtual void pruneInterface(InterfaceFlags /* accepted */) {} /// validate connectivity of children (after init() was done) virtual void validateConnectivity() const; @@ -212,10 +211,10 @@ class PropagatingEitherWayPrivate : public ComputeBasePrivate friend class PropagatingEitherWay; public: - PropagatingEitherWay::Direction required_interface_dirs_; + PropagatingEitherWay::Direction configured_dir_; + InterfaceFlags required_interface_; - inline PropagatingEitherWayPrivate(PropagatingEitherWay* me, - PropagatingEitherWay::Direction required_interface_dirs_, + inline PropagatingEitherWayPrivate(PropagatingEitherWay* me, PropagatingEitherWay::Direction configured_dir_, const std::string& name); InterfaceFlags requiredInterface() const override; diff --git a/core/src/container.cpp b/core/src/container.cpp index a21e83b5..4bad4efd 100644 --- a/core/src/container.cpp +++ b/core/src/container.cpp @@ -425,51 +425,39 @@ void SerialContainerPrivate::connect(StagePrivate& stage1, StagePrivate& stage2) // called by parent asking for pruning of this' interface void SerialContainerPrivate::pruneInterface(InterfaceFlags accepted) { + // we need to have some children to do the actual work if (children().empty()) - throw InitStageException(*me(), "container is empty"); + throw InitStageException(*me(), "no children"); - // TODO(v4hn): if ever there is a use case to start pruning - // with a specified end interface, this would need to be extended - if (!(accepted & (READS_START | WRITES_PREV_END))) - return; // The start interface direction is not decided + if (!(accepted & START_IF_MASK)) + throw InitStageException(*me(), "unknown start interface"); Stage& first = *children().front(); Stage& last = *children().back(); - // sweep through children once: infer and connect interfaces - first.pimpl()->pruneStartInterface(accepted); + // FIRST child + first.pimpl()->pruneInterface(accepted & START_IF_MASK); + // connect first child's (start) push interface setChildsPushBackwardInterface(first); - - for (auto it = ++children().begin(), previous_it = children().begin(); it != children().end(); ++it, ++previous_it) { - StagePrivate* child_impl = (**it).pimpl(); - StagePrivate* previous_impl = (**previous_it).pimpl(); - child_impl->pruneStartInterface(invert(previous_impl->requiredInterface())); - connect(*previous_impl, *child_impl); - } - - // potentially connect outmost push interface to pending_ buffer - setChildsPushForwardInterface(last); - - if ((accepted & END_IF_MASK) != UNKNOWN && - (last.pimpl()->requiredInterface() & END_IF_MASK) != (accepted & END_IF_MASK)) { - boost::format desc( - "requested end interface for container (%1%) does not agree with inferred end interface of last child (%2%)"); - desc % flowSymbol(accepted) % flowSymbol(last.pimpl()->requiredInterface()); - throw InitStageException(*me(), desc.str()); - } - - // if first/last pull, this needs to pull to and forward to the children + // connect first child's (start) pull interface if (const InterfacePtr& target = first.pimpl()->starts()) starts_.reset(new Interface( [this, target](Interface::iterator it, bool updated) { this->copyState(it, target, updated); })); - else - starts_.reset(); + // process all children and connect them + for (auto it = ++children().begin(), previous_it = children().begin(); it != children().end(); ++it, ++previous_it) { + StagePrivate* child_impl = (**it).pimpl(); + StagePrivate* previous_impl = (**previous_it).pimpl(); + child_impl->pruneInterface(invert(previous_impl->requiredInterface()) & START_IF_MASK); + connect(*previous_impl, *child_impl); + } + + // connect last child's (end) push interface + setChildsPushForwardInterface(last); + // connect last child's (end) pull interface if (const InterfacePtr& target = last.pimpl()->ends()) ends_.reset(new Interface( [this, target](Interface::iterator it, bool updated) { this->copyState(it, target, updated); })); - else - ends_.reset(); required_interface_ = first.pimpl()->interfaceFlags() & START_IF_MASK | last.pimpl()->interfaceFlags() & END_IF_MASK; } @@ -582,11 +570,9 @@ ParallelContainerBasePrivate::ParallelContainerBasePrivate(ParallelContainerBase : ContainerBasePrivate(me, name) {} void ParallelContainerBasePrivate::pruneInterface(InterfaceFlags accepted) { + // we need to have some children to do the actual work if (children().empty()) - throw InitStageException(*me(), "trying to prune empty container"); - - if (accepted == UNKNOWN) - return; // nothing to prune + throw InitStageException(*me(), "no children"); InitStageException exceptions; InterfaceFlags interface; @@ -789,24 +775,14 @@ void Fallbacks::onNewSolution(const SolutionBase& s) { MergerPrivate::MergerPrivate(Merger* me, const std::string& name) : ParallelContainerBasePrivate(me, name) {} -InterfaceFlags MergerPrivate::requiredInterface() const { - if (children().size() < 2) - throw InitStageException(*me_, "Need 2 children at least."); - - InterfaceFlags required = ParallelContainerBasePrivate::requiredInterface(); - - // all children need to share a common interface - for (const Stage::pointer& stage : children()) { - InterfaceFlags current = stage->pimpl()->requiredInterface(); - if (current != required) - throw InitStageException(*stage, "Interface does not match the common one."); - } - - switch (required) { +void MergerPrivate::pruneInterface(InterfaceFlags accepted) { + ContainerBasePrivate::pruneInterface(accepted); + switch (interfaceFlags()) { case PROPAGATE_FORWARDS: case PROPAGATE_BACKWARDS: case UNKNOWN: break; // these are supported + case GENERATE: throw InitStageException(*me_, "Generator stages not yet supported."); case CONNECT: @@ -814,7 +790,6 @@ InterfaceFlags MergerPrivate::requiredInterface() const { default: throw InitStageException(*me_, "Children's interface not supported."); } - return required; } Merger::Merger(const std::string& name) : Merger(new MergerPrivate(this, name)) {} diff --git a/core/src/stage.cpp b/core/src/stage.cpp index 3a65f9a2..0d001c85 100644 --- a/core/src/stage.cpp +++ b/core/src/stage.cpp @@ -112,28 +112,6 @@ InterfaceFlags StagePrivate::interfaceFlags() const { return f; } -void StagePrivate::pruneInterface(InterfaceFlags accepted) { - const InterfaceFlags accepted_start{ accepted & START_IF_MASK }; - const InterfaceFlags accepted_end{ accepted & END_IF_MASK }; - - const InterfaceFlags required{ requiredInterface() }; - const InterfaceFlags required_start{ required & START_IF_MASK }; - const InterfaceFlags required_end{ required & END_IF_MASK }; - - if (required == UNKNOWN) - throw std::logic_error("stage type does not specify interface, but also does not override pruneInterface. " - "Who sets up its interfaces?"); - - // only one side needs to be specified but the value has to be compatible with this stage - if (((accepted_start != UNKNOWN) && (accepted_start & required_start) != required_start) || - ((accepted_end != UNKNOWN) && (accepted_end & required_end) != required_end)) { - boost::format desc("interface %1% %2% does not match inferred interface %3% %4%"); - desc % flowSymbol(required_start) % flowSymbol(required_end); - desc % flowSymbol(accepted_start) % flowSymbol(accepted_end); - throw InitStageException(*me(), desc.str()); - } -} - void StagePrivate::validateConnectivity() const { // check that the required interface is provided InterfaceFlags required = requiredInterface(); @@ -416,47 +394,53 @@ ComputeBase::ComputeBase(ComputeBasePrivate* impl) : Stage(impl) {} PropagatingEitherWayPrivate::PropagatingEitherWayPrivate(PropagatingEitherWay* me, PropagatingEitherWay::Direction dir, const std::string& name) - : ComputeBasePrivate(me, name), required_interface_dirs_(dir) {} + : ComputeBasePrivate(me, name), configured_dir_(dir) { + initInterface(dir); +} // initialize pull interfaces to match requested propagation directions void PropagatingEitherWayPrivate::initInterface(PropagatingEitherWay::Direction dir) { - if (required_interface_dirs_ != PropagatingEitherWay::AUTO && dir != required_interface_dirs_) { - auto flow = [](PropagatingEitherWay::Direction dir) -> const char* { - switch (dir) { - case PropagatingEitherWay::AUTO: - return flowSymbol(InterfaceFlags{ READS_START, WRITES_PREV_END }); - case PropagatingEitherWay::FORWARD: - return flowSymbol(InterfaceFlags{ READS_START }); - case PropagatingEitherWay::BACKWARD: - return flowSymbol(InterfaceFlags{ WRITES_PREV_END }); - } - }; - - boost::format desc("propagation direction %1% incompatible with inferred direction %2%"); - desc % flow(required_interface_dirs_) % flow(dir); - throw InitStageException(*me(), desc.str()); + switch (dir) { + case PropagatingEitherWay::FORWARD: + required_interface_ = PROPAGATE_FORWARDS; + if (!starts_) // keep existing interface if possible + starts_.reset( + new Interface([this](Interface::iterator it, bool /*updated*/) { this->dropFailedStarts(it); })); + ends_.reset(); + return; + case PropagatingEitherWay::BACKWARD: + required_interface_ = PROPAGATE_BACKWARDS; + starts_.reset(); + if (!ends_) // keep existing interface if possible + ends_.reset(new Interface([this](Interface::iterator it, bool /*updated*/) { this->dropFailedEnds(it); })); + return; + case PropagatingEitherWay::AUTO: + required_interface_ = UNKNOWN; + return; } - - if (dir == PropagatingEitherWay::FORWARD) - starts_.reset(new Interface([this](Interface::iterator it, bool /* updated */) { this->dropFailedStarts(it); })); - else if (dir == PropagatingEitherWay::BACKWARD) - ends_.reset(new Interface([this](Interface::iterator it, bool /* updated */) { this->dropFailedEnds(it); })); - - required_interface_dirs_ = dir; } void PropagatingEitherWayPrivate::pruneInterface(InterfaceFlags accepted) { if (accepted == UNKNOWN) - throw InitStageException(*me(), "cannot prune to unknown interface"); + throw InitStageException(*me(), "cannot initialize to unknown interface"); + + auto dir = PropagatingEitherWay::AUTO; if ((accepted & START_IF_MASK) == READS_START || (accepted & END_IF_MASK) == WRITES_NEXT_START) - initInterface(PropagatingEitherWay::FORWARD); + dir = PropagatingEitherWay::FORWARD; else if ((accepted & START_IF_MASK) == WRITES_PREV_END || (accepted & END_IF_MASK) == READS_END) - initInterface(PropagatingEitherWay::BACKWARD); + dir = PropagatingEitherWay::BACKWARD; else { - boost::format desc("propagator cannot act with interfaces start(%1%), end(%2%)"); + boost::format desc("propagator cannot handle external interface %1% %2%"); desc % flowSymbol(accepted) % flowSymbol(accepted); throw InitStageException(*me(), desc.str()); } + if (configured_dir_ != PropagatingEitherWay::AUTO && dir != configured_dir_) { + boost::format desc("configured interface (%1% %2%) does not match external one (%3% %4%)"); + desc % flowSymbol(required_interface_) % flowSymbol(required_interface_); + desc % flowSymbol(accepted) % flowSymbol(accepted); + throw InitStageException(*me(), desc.str()); + } + initInterface(dir); } void PropagatingEitherWayPrivate::validateConnectivity() const { @@ -475,14 +459,7 @@ void PropagatingEitherWayPrivate::validateConnectivity() const { } InterfaceFlags PropagatingEitherWayPrivate::requiredInterface() const { - switch (required_interface_dirs_) { - case PropagatingEitherWay::FORWARD: - return PROPAGATE_FORWARDS; - case PropagatingEitherWay::BACKWARD: - return PROPAGATE_BACKWARDS; - case PropagatingEitherWay::AUTO: - return UNKNOWN; - } + return required_interface_; } void PropagatingEitherWayPrivate::dropFailedStarts(Interface::iterator state) { @@ -540,9 +517,11 @@ PropagatingEitherWay::PropagatingEitherWay(PropagatingEitherWayPrivate* impl) : void PropagatingEitherWay::restrictDirection(PropagatingEitherWay::Direction dir) { auto impl = pimpl(); - if (impl->required_interface_dirs_ != AUTO) + if (impl->configured_dir_ == dir) + return; + if (impl->configured_dir_ != AUTO) throw std::runtime_error("Cannot change direction after being connected"); - + impl->configured_dir_ = dir; impl->initInterface(dir); }