From 35f15403696b65266ce944d27682deb19f484019 Mon Sep 17 00:00:00 2001 From: Robert Haschke Date: Sun, 18 Feb 2018 20:57:14 +0100 Subject: [PATCH] recursive interface auto-detection trigger auto-detection from top (task) level: only there we now for sure the accepted interface --- .../moveit/task_constructor/container_p.h | 7 ++- .../include/moveit/task_constructor/stage_p.h | 6 +- core/src/container.cpp | 55 ++++++++++++++----- core/src/stage.cpp | 15 ++++- core/src/task.cpp | 2 +- core/test/test_container.cpp | 38 ++++++++++--- 6 files changed, 91 insertions(+), 32 deletions(-) diff --git a/core/include/moveit/task_constructor/container_p.h b/core/include/moveit/task_constructor/container_p.h index 120669a4..6dc02c8c 100644 --- a/core/include/moveit/task_constructor/container_p.h +++ b/core/include/moveit/task_constructor/container_p.h @@ -172,19 +172,20 @@ class SerialContainerPrivate : public ContainerBasePrivate { public: SerialContainerPrivate(SerialContainer* me, const std::string &name); + // called by parent asking for pruning of this' interface + void pruneInterface(InterfaceFlags accepted) override; + private: // connect cur stage to its predecessor and successor bool connect(container_type::const_iterator cur); - // called by parent asking for pruning of this' interface - void pruneInterface(PropagatingEitherWay::Direction dir) override; // called by init() to prune interfaces for children in range [first, last) void pruneInterfaces(container_type::const_iterator first, container_type::const_iterator end); // prune interface for children in range [first, last) to given direction void pruneInterfaces(container_type::const_iterator first, container_type::const_iterator end, - PropagatingEitherWay::Direction dir); + InterfaceFlags accepted); // set of all solutions ordered solutions_; diff --git a/core/include/moveit/task_constructor/stage_p.h b/core/include/moveit/task_constructor/stage_p.h index aaddb8b3..b061b1b4 100644 --- a/core/include/moveit/task_constructor/stage_p.h +++ b/core/include/moveit/task_constructor/stage_p.h @@ -64,7 +64,7 @@ public: // if this is unknown (because interface is auto-detected from context), return 0 virtual InterfaceFlags requiredInterface() const = 0; // prune interface to the given propagation direction - virtual void pruneInterface(PropagatingEitherWay::Direction direction) {} + virtual void pruneInterface(InterfaceFlags accepted) {} virtual bool canCompute() const = 0; virtual bool compute() = 0; @@ -169,9 +169,7 @@ public: // initialize pull interfaces for given propagation directions void initInterface(PropagatingEitherWay::Direction dir); // prune interface to the given propagation direction - void pruneInterface(PropagatingEitherWay::Direction direction) override { - initInterface(direction); - } + void pruneInterface(InterfaceFlags accepted) override; bool canCompute() const override; bool compute() override; diff --git a/core/src/container.cpp b/core/src/container.cpp index 7bf56c7f..f955ffef 100644 --- a/core/src/container.cpp +++ b/core/src/container.cpp @@ -427,8 +427,26 @@ void SerialContainer::init(const planning_scene::PlanningSceneConstPtr &scene) } // called by parent asking for pruning of this' interface -void SerialContainerPrivate::pruneInterface(PropagatingEitherWay::Direction dir) { - pruneInterfaces(children().begin(), children().end(), dir); +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. + if (accepted != BOTHWAYS && // will interface be restricted at all? + children().front()->pimpl()->interfaceFlags() == BOTHWAYS) // still undecided? + { + pruneInterfaces(children().begin(), children().end(), accepted); + + // reset my pull interfaces, if first/last child don't pull anymore + if (!children().front()->pimpl()->starts()) + starts_.reset(); + 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. } // called by init() to prune interfaces for children in range [first, last) @@ -438,40 +456,51 @@ void SerialContainerPrivate::pruneInterfaces(container_type::const_iterator firs { if (first == end) return; // nothing to do in this case - // determine feasible propagation directions from available push interfaces - int dir = 0; + // determine accepted interface from available push interfaces + InterfaceFlags accepted; // if previous stage pushes forward, we accept forward propagation if (first != children().begin()) { auto prev = first; --prev; // pointer to previous stage if ((*prev)->pimpl()->requiredInterface() & WRITES_NEXT_START) - dir |= PropagatingEitherWay::FORWARD; + accepted |= WRITES_NEXT_START; } // else: for first child we cannot determine the interface yet // if end stage pushes backward, we accept backward propagation if (end != children().end()) { if ((*end)->pimpl()->requiredInterface() & WRITES_PREV_END) - dir |= PropagatingEitherWay::BACKWARD; + accepted |= WRITES_PREV_END; } // else: for last child we cannot determine the interface yet // nothing to do if: - // - dir == 0: [first, last) covers all children, cannot determine interface - // - dir == PropagatingEitherWay::BOTHWAY: nothing changed - if (dir != 0 && dir != PropagatingEitherWay::BOTHWAY) - pruneInterfaces(first, end, PropagatingEitherWay::Direction(dir)); + // - accepted == 0: interface still unknown + // - accepted == PROPAGATE_FORWARDS | PROPAGATE_BACKWARDS: no change + if (accepted != 0 && accepted != InterfaceFlags({PROPAGATE_FORWARDS, PROPAGATE_BACKWARDS})) + pruneInterfaces(first, end, accepted); } // prune interface for children in range [first, last) to given direction void SerialContainerPrivate::pruneInterfaces(container_type::const_iterator first, container_type::const_iterator end, - PropagatingEitherWay::Direction dir) + InterfaceFlags accepted) { + // 1st sweep: remove push interfaces for (auto it = first; it != end; ++it) { StagePrivate* impl = (*it)->pimpl(); // range should only contain stages with unknown required interface assert(impl->requiredInterface() == PropagatingEitherWay::AUTO); - // let the child do the actual work (recursively) - impl->pruneInterface(dir); + + // remove push interfaces + if (!(accepted & WRITES_PREV_END)) + impl->setPrevEnds(InterfacePtr()); + + if (!(accepted & WRITES_NEXT_START)) + impl->setNextStarts(InterfacePtr()); + } + // 2nd sweep: recursively prune children + for (auto it = first; it != end; ++it) { + StagePrivate* impl = (*it)->pimpl(); + impl->pruneInterface(accepted); } } diff --git a/core/src/stage.cpp b/core/src/stage.cpp index cd79296e..702adccf 100644 --- a/core/src/stage.cpp +++ b/core/src/stage.cpp @@ -283,13 +283,22 @@ void PropagatingEitherWayPrivate::initInterface(PropagatingEitherWay::Direction } } +void PropagatingEitherWayPrivate::pruneInterface(InterfaceFlags accepted) { + int dir = 0; + if (accepted & PROPAGATE_FORWARDS) + dir |= PropagatingEitherWay::FORWARD; + if (accepted & PROPAGATE_BACKWARDS) + dir |= PropagatingEitherWay::BACKWARD; + initInterface(PropagatingEitherWay::Direction(dir)); +} + InterfaceFlags PropagatingEitherWayPrivate::requiredInterface() const { InterfaceFlags f; if (required_interface_dirs_ & PropagatingEitherWay::FORWARD) - f |= InterfaceFlags({READS_START, WRITES_NEXT_START}); + f |= PROPAGATE_FORWARDS; if (required_interface_dirs_ & PropagatingEitherWay::BACKWARD) - f |= InterfaceFlags({READS_END, WRITES_PREV_END}); + f |= PROPAGATE_BACKWARDS; // if required_interface_dirs_ == ANYWAY, we don't require an interface // but the parent container auto-derives the propagation direction return f; @@ -495,7 +504,7 @@ ConnectingPrivate::ConnectingPrivate(Connecting *me, const std::string &name) } InterfaceFlags ConnectingPrivate::requiredInterface() const { - return InterfaceFlags( { READS_START, READS_END } ); + return InterfaceFlags(CONNECT); } void ConnectingPrivate::newStartState(Interface::iterator it, bool updated) diff --git a/core/src/task.cpp b/core/src/task.cpp index 9faba6dc..84920d66 100644 --- a/core/src/task.cpp +++ b/core/src/task.cpp @@ -195,7 +195,7 @@ void Task::init(const planning_scene::PlanningSceneConstPtr &scene) // and *afterwards* initialize all children recursively stages()->init(scene); // task expects its wrapped child to push to both ends, this triggers interface resolution - // TODO: stages()->pruneInterface(InterfaceFlags({WRITES_PREV_END, WRITES_NEXT_START})); + stages()->pimpl()->pruneInterface(InterfaceFlags({GENERATE})); // and *finally* validate connectivity stages()->validateConnectivity(); diff --git a/core/test/test_container.cpp b/core/test/test_container.cpp index 090da012..4a866af4 100644 --- a/core/test/test_container.cpp +++ b/core/test/test_container.cpp @@ -113,6 +113,11 @@ protected: append(serial, types); try { serial.init(scene); + // prune pull interfaces based on provided external interface (start, end) + InterfaceFlags accepted; + if (start) accepted |= WRITES_PREV_END; + if (end) accepted |= WRITES_NEXT_START; + serial.pimpl()->pruneInterface(accepted); serial.validateConnectivity(); if (!expect_failure) return; // as expected ADD_FAILURE() << "init() didn't recognize a failure condition as expected\n" << serial; @@ -188,11 +193,11 @@ TEST_F(SerialTest, init_empty) { TEST_F(SerialTest, init_connecting) { EXPECT_INIT_SUCCESS(false, false, CONN); - EXPECT_EQ(serial.pimpl()->interfaceFlags(), InterfaceFlags({READS_START, READS_END})); + EXPECT_EQ(serial.pimpl()->interfaceFlags(), InterfaceFlags(CONNECT)); EXPECT_EQ(serial.pimpl()->interfaceFlags(), serial.pimpl()->children().front()->pimpl()->interfaceFlags()); EXPECT_INIT_FAILURE(true, true, CONN); - EXPECT_EQ(serial.pimpl()->interfaceFlags(), InterfaceFlags({READS_START, READS_END, WRITES_NEXT_START, WRITES_PREV_END})); + EXPECT_EQ(serial.pimpl()->interfaceFlags(), InterfaceFlags({CONNECT, GENERATE})); EXPECT_INIT_FAILURE(false, false, CONN, CONN); // two connecting stages cannot be connected } @@ -253,11 +258,28 @@ TEST_F(SerialTest, interface_detection) { EXPECT_EQ((*++it)->pimpl()->interfaceFlags(), InterfaceFlags(PROPAGATE_FORWARDS)); EXPECT_EQ(serial.pimpl()->interfaceFlags(), InterfaceFlags(GENERATE)); -#if 0 // derive propagation direction from outer interface - EXPECT_INIT_SUCCESS(false, true, ANY); // -> -> - EXPECT_EQ(serial.pimpl()->interfaceFlags(), InterfaceFlags({READS_START, WRITES_NEXT_START})); - EXPECT_INIT_SUCCESS(false, true, ANY, ANY); // <- <- - EXPECT_EQ(serial.pimpl()->interfaceFlags(), InterfaceFlags({READS_START, WRITES_NEXT_START})); -#endif + EXPECT_INIT_SUCCESS(false, true, ANY, ANY); // -> -> + it = serial.pimpl()->children().begin(); + EXPECT_EQ( (*it)->pimpl()->interfaceFlags(), InterfaceFlags(PROPAGATE_FORWARDS)); + EXPECT_EQ((*++it)->pimpl()->interfaceFlags(), InterfaceFlags(PROPAGATE_FORWARDS)); + EXPECT_EQ(serial.pimpl()->interfaceFlags(), InterfaceFlags(PROPAGATE_FORWARDS)); + + EXPECT_INIT_SUCCESS(true, false, ANY, ANY); // <- <- + it = serial.pimpl()->children().begin(); + EXPECT_EQ( (*it)->pimpl()->interfaceFlags(), InterfaceFlags(PROPAGATE_BACKWARDS)); + EXPECT_EQ((*++it)->pimpl()->interfaceFlags(), InterfaceFlags(PROPAGATE_BACKWARDS)); + EXPECT_EQ(serial.pimpl()->interfaceFlags(), InterfaceFlags(PROPAGATE_BACKWARDS)); + + EXPECT_INIT_SUCCESS(true, true, ANY, ANY); // <> <> + it = serial.pimpl()->children().begin(); + EXPECT_EQ( (*it)->pimpl()->interfaceFlags(), InterfaceFlags({PROPAGATE_FORWARDS, PROPAGATE_BACKWARDS})); + EXPECT_EQ((*++it)->pimpl()->interfaceFlags(), InterfaceFlags({PROPAGATE_FORWARDS, PROPAGATE_BACKWARDS})); + EXPECT_EQ(serial.pimpl()->interfaceFlags(), InterfaceFlags({PROPAGATE_FORWARDS, PROPAGATE_BACKWARDS})); + + EXPECT_INIT_FAILURE(false, false, ANY, ANY); // -- -- + it = serial.pimpl()->children().begin(); + EXPECT_EQ( (*it)->pimpl()->interfaceFlags(), InterfaceFlags()); + EXPECT_EQ((*++it)->pimpl()->interfaceFlags(), InterfaceFlags()); + EXPECT_EQ(serial.pimpl()->interfaceFlags(), InterfaceFlags()); }