From a31e52dd53078741f2d35d690ad3845c3164541b Mon Sep 17 00:00:00 2001 From: Robert Haschke Date: Sun, 21 Nov 2021 06:19:13 +0100 Subject: [PATCH] Propagate status across Connecting gap Not only propagate updates along solution paths, but also bridge the gap of a `Connecting` stage. - If a state becomes enabled, re-enable opposite `ARMED` states as well. - If a state becomes pruned, also prune opposite states if they don't have alternatives. - Make sure that we don't run into a recursive update loop by disabling notify() callbacks. --- .../include/moveit/task_constructor/storage.h | 15 +++++++- core/src/stage.cpp | 35 +++++++++++++++++-- core/test/test_pruning.cpp | 2 ++ 3 files changed, 49 insertions(+), 3 deletions(-) diff --git a/core/include/moveit/task_constructor/storage.h b/core/include/moveit/task_constructor/storage.h index 72d8181f..28824d7b 100644 --- a/core/include/moveit/task_constructor/storage.h +++ b/core/include/moveit/task_constructor/storage.h @@ -200,6 +200,18 @@ public: BACKWARD, }; using NotifyFunction = std::function; + + class DisableNotify + { + Interface& if_; + Interface::NotifyFunction old_; + + public: + DisableNotify(Interface& i) : if_(i) { old_.swap(if_.notify_); } + ~DisableNotify() { old_.swap(if_.notify_); } + }; + friend class DisableNotify; + Interface(const NotifyFunction& notify = NotifyFunction()); /// add a new InterfaceState @@ -210,9 +222,10 @@ public: /// update state's priority (and call notify_ if it really has changed) void updatePriority(InterfaceState* state, const InterfaceState::Priority& priority); + inline bool notifyEnabled() const { return static_cast(notify_); } private: - const NotifyFunction notify_; + NotifyFunction notify_; // restrict access to some functions to ensure consistency // (we need to set/unset InterfaceState::owner_) diff --git a/core/src/stage.cpp b/core/src/stage.cpp index 010b712a..b63daf57 100644 --- a/core/src/stage.cpp +++ b/core/src/stage.cpp @@ -709,13 +709,44 @@ ConnectingPrivate::StatePair ConnectingPrivate::make_pair(In return StatePair(second, first); } +// TODO: bool updated -> uint_8 updated (bitfield of PRIORITY | STATUS) template void ConnectingPrivate::newState(Interface::iterator it, bool updated) { - if (updated) { // many pairs might be affected: resort + auto parent_pimpl = parent()->pimpl(); + Interface::DisableNotify disable_source_interface(*pullInterface(dir)); + if (updated) { + if (pullInterface(opposite())->notifyEnabled()) // suppress recursive loop + { + // If status has changed, propagate the update to the opposite side + auto status = it->priority().status(); + if (status == InterfaceState::Status::PRUNED) // PRUNED becomes ARMED on opposite side + status = InterfaceState::Status::ARMED; // (only for pending state pairs) + + for (const auto& candidate : this->pending) { + if (std::get()>(candidate) != it) // only consider pairs with source state == state + continue; + auto oit = std::get(candidate); // opposite target state + auto ostatus = oit->priority().status(); + if (ostatus != status) { + if (status != InterfaceState::Status::ENABLED) { + // quicker check for hasPendingOpposites(): search in it->owner() for an enabled alternative + bool cancel = false; // if found, cancel propagation of new status + for (const auto alternative : *it->owner()) + if ((cancel = alternative->priority().enabled())) + break; + if (cancel) + continue; + } + // pass creator=nullptr to skip hasPendingOpposites() check + parent_pimpl->setStatus()>(nullptr, nullptr, &*oit, status); + } + } + } + + // many pairs will have changed priorities: resort pending list pending.sort(); } else { // new state: insert all pairs with other interface assert(it->priority().enabled()); // new solutions are feasible, aren't they? - auto parent_pimpl = parent()->pimpl(); InterfacePtr other_interface = pullInterface(dir); bool have_enabled_opposites = false; for (Interface::iterator oit = other_interface->begin(), oend = other_interface->end(); oit != oend; ++oit) { diff --git a/core/test/test_pruning.cpp b/core/test/test_pruning.cpp index c82f9f35..d386f1a3 100644 --- a/core/test/test_pruning.cpp +++ b/core/test/test_pruning.cpp @@ -91,6 +91,7 @@ TYPED_TEST(PruningContainerTests, ConnectReactivatesPrunedPaths) { } TEST_F(Pruning, ConnectConnectForward) { + add(t, new BackwardMockup()); add(t, new GeneratorMockup()); auto c1 = add(t, new ConnectMockup({ INF, 0, 0 })); // 1st attempt is a failue add(t, new GeneratorMockup({ 0, 10, 20 })); @@ -112,6 +113,7 @@ TEST_F(Pruning, ConnectConnectForward) { } TEST_F(Pruning, ConnectConnectBackward) { + add(t, new BackwardMockup()); add(t, new GeneratorMockup({ 1, 2, 3 })); auto c1 = add(t, new ConnectMockup()); add(t, new BackwardMockup());