From 1a0b9b36ee081f9027edf5e9543c363ee88066d0 Mon Sep 17 00:00:00 2001 From: Robert Haschke Date: Sat, 17 Feb 2018 01:28:28 +0100 Subject: [PATCH] fix connection creation Establishing the interface connections, we face a chicken-egg-problem: To establish a connection, a predecessors/successors pull interface is assigned to the current's stage push interface. However, propagating stages (in auto-detection mode) can only create their pull interfaces if the corresponding, opposite-side push interface is present already (because that's the mechanism to determine the supported propagation directions). Hence, we need to resolve this by performing two sweeps: - initialization, assuming both propagation directions should be supported, thus generating both pull interfaces, i.e. providing the egg - stripping down the interfaces to the actual context This context is provided by two stages pushing from both ends into a (potentially long) sequence of propagating stages (tbd). Contributions of this PR: - PropagatingEitherWay: explicitly distinguish AUTO from BOTHWAYS interface AUTO: auto-derive interface from provided push interfaces BOTHWAYS: explicitly require both directions - SerialContainer: (better, but not yet perfect) validation of connectivity - ParallelContainer: determine interface from what children offer --- core/demo/plan_pick_pa10.cpp | 1 + core/demo/plan_pick_trixi.cpp | 1 + core/demo/plan_pick_ur5.cpp | 1 + .../moveit/task_constructor/container.h | 1 + .../moveit/task_constructor/container_p.h | 25 ++- core/include/moveit/task_constructor/stage.h | 31 +-- .../include/moveit/task_constructor/stage_p.h | 12 +- core/src/container.cpp | 178 ++++++++++-------- core/src/stage.cpp | 106 ++++++----- core/src/task.cpp | 5 +- 10 files changed, 218 insertions(+), 143 deletions(-) diff --git a/core/demo/plan_pick_pa10.cpp b/core/demo/plan_pick_pa10.cpp index 3652f9d4..b68c3825 100644 --- a/core/demo/plan_pick_pa10.cpp +++ b/core/demo/plan_pick_pa10.cpp @@ -170,6 +170,7 @@ int main(int argc, char** argv){ } catch (const InitStageException &e) { std::cerr << e; + t.printState(); return EINVAL; } diff --git a/core/demo/plan_pick_trixi.cpp b/core/demo/plan_pick_trixi.cpp index 25f9dbbd..b4cacc14 100644 --- a/core/demo/plan_pick_trixi.cpp +++ b/core/demo/plan_pick_trixi.cpp @@ -118,6 +118,7 @@ int main(int argc, char** argv){ } catch (const InitStageException &e) { std::cerr << e; + t.printState(); return EINVAL; } diff --git a/core/demo/plan_pick_ur5.cpp b/core/demo/plan_pick_ur5.cpp index 221bfef6..396ba88c 100644 --- a/core/demo/plan_pick_ur5.cpp +++ b/core/demo/plan_pick_ur5.cpp @@ -118,6 +118,7 @@ int main(int argc, char** argv){ } catch (const InitStageException &e) { std::cerr << e; + t.printState(); return EINVAL; } diff --git a/core/include/moveit/task_constructor/container.h b/core/include/moveit/task_constructor/container.h index 6c42aeac..fb799a2a 100644 --- a/core/include/moveit/task_constructor/container.h +++ b/core/include/moveit/task_constructor/container.h @@ -89,6 +89,7 @@ public: void reset() override; void init(const planning_scene::PlanningSceneConstPtr &scene) override; + bool canCompute() const override; bool compute() override; diff --git a/core/include/moveit/task_constructor/container_p.h b/core/include/moveit/task_constructor/container_p.h index 9fbd339a..26619ae9 100644 --- a/core/include/moveit/task_constructor/container_p.h +++ b/core/include/moveit/task_constructor/container_p.h @@ -100,14 +100,18 @@ public: protected: ContainerBasePrivate(ContainerBase *me, const std::string &name); - // Get push interface to be used for children: If our own push interface is not set, - // don't set children's interface either: pushing is not supported. - // Otherwise return pending_* buffer. - InterfacePtr getPushBackwardInterface() { - return prevEnds() ? pending_backward_ : InterfacePtr(); + // 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 so + inline void setChildsPushBackwardInterface(Stage& child) { + bool allowed = (child.pimpl()->requiredInterface() & WRITES_PREV_END); + child.pimpl()->setPrevEnds(allowed ? pending_backward_ : InterfacePtr()); } - InterfacePtr getPushForwardInterface() { - return nextStarts() ? pending_forward_ : InterfacePtr(); + inline void setChildsPushForwardInterface(Stage& child) { + bool allowed = (child.pimpl()->requiredInterface() & WRITES_NEXT_START); + child.pimpl()->setNextStarts(allowed ? pending_forward_ : InterfacePtr()); } /// copy external_state to a child's interface and remember the link in internal_to map @@ -165,7 +169,12 @@ public: SerialContainerPrivate(SerialContainer* me, const std::string &name); private: - void connect(StagePrivate *prev, StagePrivate *next); + // connect cur stage to its predecessor and successor + bool connect(container_type::const_iterator cur, InitStageException& errors, + const planning_scene::PlanningSceneConstPtr& scene); + // restrict interfaces of auto-mode propagating stages + void stripInterfaces(std::vector& stages); + void validateConnectivity(InitStageException& errors) const; // set of all solutions ordered solutions_; diff --git a/core/include/moveit/task_constructor/stage.h b/core/include/moveit/task_constructor/stage.h index b9f51300..d14ad6ad 100644 --- a/core/include/moveit/task_constructor/stage.h +++ b/core/include/moveit/task_constructor/stage.h @@ -69,11 +69,6 @@ enum InterfaceFlag { READS_END = 0x02, WRITES_NEXT_START = 0x04, WRITES_PREV_END = 0x08, - - OWN_IF_MASK = READS_START | READS_END, - EXT_IF_MASK = WRITES_NEXT_START | WRITES_PREV_END, - INPUT_IF_MASK = READS_START | WRITES_PREV_END, - OUTPUT_IF_MASK = READS_END | WRITES_NEXT_START, }; typedef Flags InterfaceFlags; @@ -132,9 +127,17 @@ public: operator StagePrivate*(); operator const StagePrivate*() const; - /// reset stage, clearing all solutions, interfaces, etc. + /// reset stage, clearing all solutions, interfaces, inherited properties. virtual void reset(); - /// initialize stage once before planning + + /** initialize stage once before planning + * + * When called, properties configured for initialization from parent are already defined. + * Push interfaces are not yet defined! + * + * The planning scene is the initial planning scene of the task. Use it as is or + * to learn about the robot model. + */ virtual void init(const planning_scene::PlanningSceneConstPtr& scene); const ContainerBase* parent() const; @@ -198,11 +201,16 @@ public: PRIVATE_CLASS(PropagatingEitherWay) PropagatingEitherWay(const std::string& name); - enum Direction { FORWARD = 0x01, BACKWARD = 0x02, ANYWAY = FORWARD | BACKWARD}; + enum Direction { + AUTO = 0x00, // auto-derive direction from context + FORWARD = 0x01, // propagate forward only + BACKWARD = 0x02, // propagate backward only + BOTHWAY = FORWARD | BACKWARD, // propagate both ways + }; void restrictDirection(Direction dir); - virtual void reset() override; - virtual void init(const planning_scene::PlanningSceneConstPtr &scene) override; + void reset() override; + void init(const planning_scene::PlanningSceneConstPtr &scene) override; virtual bool computeForward(const InterfaceState& from) = 0; virtual bool computeBackward(const InterfaceState& to) = 0; @@ -232,7 +240,6 @@ public: protected: // constructor for use in derived classes PropagatingEitherWay(PropagatingEitherWayPrivate* impl); - void initInterface(); }; @@ -285,7 +292,7 @@ public: PRIVATE_CLASS(Connecting) Connecting(const std::string& name); - virtual void reset() override; + void reset() override; virtual bool compute(const InterfaceState& from, const InterfaceState& to) = 0; void connect(const InterfaceState& from, const InterfaceState& to, diff --git a/core/include/moveit/task_constructor/stage_p.h b/core/include/moveit/task_constructor/stage_p.h index 2f8b1de1..72762926 100644 --- a/core/include/moveit/task_constructor/stage_p.h +++ b/core/include/moveit/task_constructor/stage_p.h @@ -60,6 +60,7 @@ public: virtual ~StagePrivate() = default; InterfaceFlags interfaceFlags() const; + virtual InterfaceFlags requiredInterface() const = 0; virtual bool canCompute() const = 0; virtual bool compute() = 0; @@ -155,13 +156,14 @@ class PropagatingEitherWayPrivate : public ComputeBasePrivate { std::list processed; // already processed states public: - PropagatingEitherWay::Direction dir; + PropagatingEitherWay::Direction required_interface_dirs_; - inline PropagatingEitherWayPrivate(PropagatingEitherWay *me, PropagatingEitherWay::Direction dir, + inline PropagatingEitherWayPrivate(PropagatingEitherWay *me, PropagatingEitherWay::Direction required_interface_dirs_, const std::string &name); - // returns true if prevEnds() or nextStarts() are accessible - inline bool isConnected() const { return prevEnds() || nextStarts(); } + InterfaceFlags requiredInterface() const override; + // initialize pull interfaces for given propagation directions + void initInterface(PropagatingEitherWay::Direction dir); bool canCompute() const override; bool compute() override; @@ -198,6 +200,7 @@ class GeneratorPrivate : public ComputeBasePrivate { public: inline GeneratorPrivate(Generator *me, const std::string &name); + InterfaceFlags requiredInterface() const override; bool canCompute() const override; bool compute() override; }; @@ -216,6 +219,7 @@ class ConnectingPrivate : public ComputeBasePrivate { public: inline ConnectingPrivate(Connecting *me, const std::string &name); + InterfaceFlags requiredInterface() const override; bool canCompute() const override; bool compute() override; diff --git a/core/src/container.cpp b/core/src/container.cpp index bddb5187..af97d974 100644 --- a/core/src/container.cpp +++ b/core/src/container.cpp @@ -332,65 +332,106 @@ SerialContainerPrivate::SerialContainerPrivate(SerialContainer *me, const std::s : ContainerBasePrivate(me, name) {} -void SerialContainerPrivate::connect(StagePrivate* prev, StagePrivate* next) { - prev->setNextStarts(next->starts()); - next->setPrevEnds(prev->ends()); +// 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, InitStageException& errors, + const planning_scene::PlanningSceneConstPtr &scene) +{ + constexpr InterfaceFlags UNKNOWN; + StagePrivate* const cur_impl = **cur; + InterfaceFlags required = cur_impl->requiredInterface(); + + // get iterators to prev / next stage in sequence + auto prev = cur; --prev; + auto next = cur; ++next; + + // set push forward connection using next's starts + if ((required == UNKNOWN || required & WRITES_NEXT_START) + && next != children().end()) // last child has not a next one + cur_impl->setNextStarts((*next)->pimpl()->starts()); + + // set push backward connection using prev's ends + if ((required == UNKNOWN || required & WRITES_PREV_END) + && cur != children().begin()) // first child has not a previous one + cur_impl->setPrevEnds((*prev)->pimpl()->ends()); + + // schedule stage with unknown interface for 2nd sweep + return required == UNKNOWN; +} + +// restrict interfaces of propagating stages that have auto-mode enabled +void SerialContainerPrivate::stripInterfaces(std::vector& stages) +{ + for (auto it = stages.rbegin(); it != stages.rend(); ++it) { + } + stages.clear(); } void SerialContainer::init(const planning_scene::PlanningSceneConstPtr &scene) { - InitStageException errors; + // reset pull interfaces auto impl = pimpl(); + impl->starts_.reset(); + impl->ends_.reset(); + ContainerBase::init(scene); // throws if there are no children - // if there are no children, there is nothing to connect - if (!impl->children().empty()) { - /*** connect children ***/ - // first stage sends backward to pending_backward_ - auto start = impl->children().begin(); - (*start)->pimpl()->setPrevEnds(impl->getPushBackwardInterface()); + InitStageException errors; - // last stage sends forward to pending_forward_ - auto last = --impl->children().end(); - (*last)->pimpl()->setNextStarts(impl->getPushForwardInterface()); + auto start = impl->children().begin(); + auto last = --impl->children().end(); - auto cur = start; - auto prev = cur; ++cur; // prev points to 1st, cur points to 2nd stage - if (prev != last) {// we have more than one children - auto next = cur; ++next; // next points to 3rd stage (or end) - for (; cur != last; ++prev, ++cur, ++next) { - impl->connect(**prev, **cur); - impl->connect(**cur, **next); - } - // finally connect last == cur and prev stage - impl->connect(**prev, **cur); - } + // connect first / last child's push interfaces to our pending_* buffers + impl->setChildsPushBackwardInterface(**start); + impl->setChildsPushForwardInterface(**last); - // recursively init + validate all children - // this needs to be done *after* initializing the connections - ContainerBase::init(scene); + std::vector scheduled; // stages scheduled for 2n sweep - // initialize starts_ and ends_ interfaces - if (const InterfacePtr& target = (*start)->pimpl()->starts()) - impl->starts_.reset(new Interface(std::bind(&SerialContainerPrivate::copyState, impl, _1, std::cref(target), _2))); - if (const InterfacePtr& target = (*last)->pimpl()->ends()) - impl->ends_.reset(new Interface(std::bind(&SerialContainerPrivate::copyState, impl, _1, std::cref(target), _2))); - - // validate connectivity of this - if (!impl->nextStarts()) - errors.push_back(*this, "cannot sendForward()"); - if (!impl->prevEnds()) - errors.push_back(*this, "cannot sendBackward()"); - } else { - errors.push_back(*this, "no children"); - // no children -> no reading - impl->starts_.reset(); - impl->ends_.reset(); + // initialize and connect remaining children in 2 sweeps + // to allow for interface auto-detection for propagating stages + for (auto cur = start, end = impl->children().end(); cur != end; ++cur) { + // 1st sweep: process forward connections + if (impl->connect(cur, errors, scene)) + scheduled.push_back(cur); + else if (!scheduled.empty()) // reached a stage with known interface + impl->stripInterfaces(scheduled); } + impl->stripInterfaces(scheduled); // process stages accumulated up to end + + // initialize pull interfaces if first/last child pulls + if (const InterfacePtr& target = (*start)->pimpl()->starts()) + impl->starts_.reset(new Interface(std::bind(&SerialContainerPrivate::copyState, impl, _1, std::cref(target), _2))); + if (const InterfacePtr& target = (*last)->pimpl()->ends()) + impl->ends_.reset(new Interface(std::bind(&SerialContainerPrivate::copyState, impl, _1, std::cref(target), _2))); + + // finally validate connectivity + impl->validateConnectivity(errors); if (errors) throw errors; } +void SerialContainerPrivate::validateConnectivity(InitStageException& errors) const +{ + // validate propagation from children to this and vice versa + if (!children().empty()) { + const StagePrivate* start = children().front()->pimpl(); + if (bool(start->prevEnds()) ^ bool(prevEnds())) + errors.push_back(*me(), "cannot propagate backward pushes of first child"); + + const StagePrivate* last = children().back()->pimpl(); + if (bool(last->nextStarts()) ^ bool(nextStarts())) + errors.push_back(*me(), "cannot propagate forward pushes of last child"); + } + + // validate connectivity of children + for (auto& child : children()) { + InterfaceFlags required = child->pimpl()->requiredInterface(); + InterfaceFlags actual = child->pimpl()->interfaceFlags(); + if ((required & actual) != required) + errors.push_back(*child, "required interface doesn't match actual"); + } +} + bool SerialContainer::canCompute() const { return !pimpl()->children().empty(); @@ -505,42 +546,31 @@ void ParallelContainerBase::reset() */ void ParallelContainerBase::init(const planning_scene::PlanningSceneConstPtr &scene) { - InitStageException errors; + // recursively init children + ContainerBase::init(scene); auto impl = pimpl(); - // initialize push connections of children - InterfacePtr push_prev = impl->getPushBackwardInterface(); - InterfacePtr push_next = impl->getPushForwardInterface(); - for (const Stage::pointer& stage : impl->children()) { - StagePrivate *child = stage->pimpl(); - child->setPrevEnds(push_prev); - child->setNextStarts(push_next); - } - // recursively init + validate all children - // this needs to be done *after* initializing push connections - ContainerBase::init(scene); - - bool pulls[2]; - for (const Stage::pointer& stage : impl->children()) { - StagePrivate *child = stage->pimpl(); - // is there any child reading from starts() or ends() ? - pulls[Interface::START] |= bool(child->pullInterface(Interface::START)); - pulls[Interface::END] |= bool(child->pullInterface(Interface::END)); - } + // determine the union of interfaces required by children + // TODO: should we better use the least common interface? + InterfaceFlags required; + for (const Stage::pointer& stage : impl->children()) + required |= stage->pimpl()->requiredInterface(); // initialize this' pull connections - for (Interface::Direction dir : { Interface::START, Interface::END }) { - if (pulls[dir]) - impl->pullInterface(dir).reset(new Interface(std::bind(&ParallelContainerBasePrivate::onNewExternalState, impl, dir, _1, _2))); - else - impl->pullInterface(dir).reset(); + impl->starts().reset(required & READS_START + ? new Interface(std::bind(&ParallelContainerBasePrivate::onNewExternalState, + impl, Interface::FORWARD, _1, _2)) + : nullptr); + impl->ends().reset(required & READS_END + ? new Interface(std::bind(&ParallelContainerBasePrivate::onNewExternalState, + impl, Interface::BACKWARD, _1, _2)) + : nullptr); + + // initialize push connections of children according to their demands + for (const Stage::pointer& stage : impl->children()) { + impl->setChildsPushForwardInterface(*stage); + impl->setChildsPushBackwardInterface(*stage); } - - if (impl->children().empty()) - errors.push_back(*this, "no children"); - - if (errors) - throw errors; } size_t ParallelContainerBase::numSolutions() const diff --git a/core/src/stage.cpp b/core/src/stage.cpp index 3f86dfe6..a304ccdc 100644 --- a/core/src/stage.cpp +++ b/core/src/stage.cpp @@ -118,10 +118,14 @@ Stage::operator const StagePrivate*() const { void Stage::reset() { auto impl = pimpl(); + // clear pull interfaces if (impl->starts_) impl->starts_->clear(); if (impl->ends_) impl->ends_->clear(); + // reset push interfaces impl->prev_ends_.reset(); impl->next_starts_.reset(); + // reset inherited properties + impl->properties_.reset(); } void Stage::init(const planning_scene::PlanningSceneConstPtr &scene) @@ -173,7 +177,9 @@ 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; bool other_if = f & other; bool reverse = own & INPUT_IF_MASK; @@ -254,8 +260,39 @@ void ComputeBase::reset() { PropagatingEitherWayPrivate::PropagatingEitherWayPrivate(PropagatingEitherWay *me, PropagatingEitherWay::Direction dir, const std::string &name) - : ComputeBasePrivate(me, name), dir(dir) + : ComputeBasePrivate(me, name), required_interface_dirs_(dir) { + initInterface(required_interface_dirs_); +} + +// initialize pull interfaces to match requested propagation directions +void PropagatingEitherWayPrivate::initInterface(PropagatingEitherWay::Direction dir) +{ + if (dir & PropagatingEitherWay::FORWARD) { + if (!starts_) // keep existing interface if possible + starts_.reset(new Interface(std::bind(&PropagatingEitherWayPrivate::dropFailedStarts, this, _1))); + } else { + starts_.reset(); + } + + if (dir & PropagatingEitherWay::BACKWARD) { + if (!ends_) // keep existing interface if possible + ends_.reset(new Interface(std::bind(&PropagatingEitherWayPrivate::dropFailedEnds, this, _1))); + } else { + ends_.reset(); + } +} + +InterfaceFlags PropagatingEitherWayPrivate::requiredInterface() const +{ + InterfaceFlags f; + if (required_interface_dirs_ & PropagatingEitherWay::FORWARD) + f |= InterfaceFlags({READS_START, WRITES_NEXT_START}); + if (required_interface_dirs_ & PropagatingEitherWay::BACKWARD) + f |= InterfaceFlags({READS_END, WRITES_PREV_END}); + // If required_interface_dirs_ == ANYWAY, we don't require an interface, + // but auto-derive - in init() - from the provided push interfaces. + return f; } void PropagatingEitherWayPrivate::dropFailedStarts(Interface::iterator state) { @@ -291,11 +328,7 @@ const InterfaceState& PropagatingEitherWayPrivate::fetchEndState(){ bool PropagatingEitherWayPrivate::canCompute() const { - if ((dir & PropagatingEitherWay::FORWARD) && hasStartState()) - return true; - if ((dir & PropagatingEitherWay::BACKWARD) && hasEndState()) - return true; - return false; + return hasStartState() || hasEndState(); } bool PropagatingEitherWayPrivate::compute() @@ -303,14 +336,14 @@ bool PropagatingEitherWayPrivate::compute() PropagatingEitherWay* me = static_cast(me_); bool result = false; - if ((dir & PropagatingEitherWay::FORWARD) && hasStartState()) { + if (hasStartState()) { const InterfaceState& state = fetchStartState(); // enforce property initialization from INTERFACE properties_.performInitFrom(Stage::INTERFACE, state.properties(), true); if (countFailures(me->computeForward(state))) result |= true; } - if ((dir & PropagatingEitherWay::BACKWARD) && hasEndState()) { + if (hasEndState()) { const InterfaceState& state = fetchEndState(); // enforce property initialization from INTERFACE properties_.performInitFrom(Stage::INTERFACE, state.properties(), true); @@ -322,42 +355,23 @@ bool PropagatingEitherWayPrivate::compute() PropagatingEitherWay::PropagatingEitherWay(const std::string &name) - : PropagatingEitherWay(new PropagatingEitherWayPrivate(this, ANYWAY, name)) + : PropagatingEitherWay(new PropagatingEitherWayPrivate(this, AUTO, name)) { } PropagatingEitherWay::PropagatingEitherWay(PropagatingEitherWayPrivate *impl) : ComputeBase(impl) { - initInterface(); -} - -void PropagatingEitherWay::initInterface() -{ - auto impl = pimpl(); - if (impl->dir & PropagatingEitherWay::FORWARD) { - if (!impl->starts_) // keep existing interface if possible - impl->starts_.reset(new Interface(std::bind(&PropagatingEitherWayPrivate::dropFailedStarts, impl, _1))); - } else { - impl->starts_.reset(); - } - - if (impl->dir & PropagatingEitherWay::BACKWARD) { - if (!impl->ends_) // keep existing interface if possible - impl->ends_.reset(new Interface(std::bind(&PropagatingEitherWayPrivate::dropFailedEnds, impl, _1))); - } else { - impl->ends_.reset(); - } } void PropagatingEitherWay::restrictDirection(PropagatingEitherWay::Direction dir) { auto impl = pimpl(); - if (impl->dir == dir) return; - if (impl->isConnected()) + if (impl->required_interface_dirs_ == dir) return; + if (impl->prevEnds() || impl->nextStarts()) throw std::runtime_error("Cannot change direction after being connected"); - impl->dir = dir; - initInterface(); + impl->required_interface_dirs_ = dir; + impl->initInterface(impl->required_interface_dirs_); } void PropagatingEitherWay::reset() @@ -366,19 +380,19 @@ void PropagatingEitherWay::reset() ComputeBase::reset(); } -void PropagatingEitherWay::init(const planning_scene::PlanningSceneConstPtr &scene) +void PropagatingEitherWay::init(const planning_scene::PlanningSceneConstPtr& scene) { - ComputeBase::init(scene); - + Stage::init(scene); auto impl = pimpl(); - // after being connected, restrict actual interface directions - if (!impl->nextStarts()) - impl->starts_.reset(); - if (!impl->prevEnds()) - impl->ends_.reset(); - if (!impl->isConnected()) - throw InitStageException(*this, "can neither send forwards nor backwards"); + // In AUTO-mode, i.e. when auto-detecting direction of propagation from context, + // pretend that we offer both interface directions during init(). + // This is needed due to a chicken-egg-problem: interface auto-detection requires + // the context (external pushing interfaces prevEnds, nextStarts) to be set, + // while the are ony set if we detected the correct interface... + if (impl->required_interface_dirs_ == AUTO) + impl->initInterface(BOTHWAY); + // otherwise the interface is already fixed and well-defined } void PropagatingEitherWay::sendForward(const InterfaceState& from, @@ -442,6 +456,10 @@ GeneratorPrivate::GeneratorPrivate(Generator *me, const std::string &name) : ComputeBasePrivate(me, name) {} +InterfaceFlags GeneratorPrivate::requiredInterface() const { + return InterfaceFlags({WRITES_NEXT_START, WRITES_PREV_END}); +} + bool GeneratorPrivate::canCompute() const { return static_cast(me_)->canCompute(); } @@ -476,6 +494,10 @@ ConnectingPrivate::ConnectingPrivate(Connecting *me, const std::string &name) ends_.reset(new Interface(std::bind(&ConnectingPrivate::newEndState, this, _1, _2))); } +InterfaceFlags ConnectingPrivate::requiredInterface() const { + return InterfaceFlags( { READS_START, READS_END } ); +} + void ConnectingPrivate::newStartState(Interface::iterator it, bool updated) { // TODO: only consider interface states with priority depth > threshold diff --git a/core/src/task.cpp b/core/src/task.cpp index f2065b24..7a8348ed 100644 --- a/core/src/task.cpp +++ b/core/src/task.cpp @@ -192,9 +192,8 @@ void Task::init(const planning_scene::PlanningSceneConstPtr &scene) child->setPrevEnds(impl->pendingBackward()); child->setNextStarts(impl->pendingForward()); - // explicitly call ContainerBase::init(), not WrapperBase::init() - // to keep the just set push interface for the wrapped child - ContainerBase::init(scene); + // and *afterwards* initialize all children + wrapped()->init(scene); // provide introspection instance to all stages impl->setIntrospection(introspection_.get());