From 1ddf7dd3f051a33733b1efe321e8ff5e368a96a3 Mon Sep 17 00:00:00 2001 From: Robert Haschke Date: Fri, 19 Nov 2021 06:59:09 +0100 Subject: [PATCH] Never remove pending CONNECT pairs Both, failed and pruned states might get re-enabled later! This also required rework (simplification) of the sorting function for pending pairs. --- core/include/moveit/task_constructor/stage_p.h | 17 +++++++---------- core/src/stage.cpp | 9 ++------- core/test/test_interface_state.cpp | 11 +++++++---- 3 files changed, 16 insertions(+), 21 deletions(-) diff --git a/core/include/moveit/task_constructor/stage_p.h b/core/include/moveit/task_constructor/stage_p.h index 87ed7b27..883a5f29 100644 --- a/core/include/moveit/task_constructor/stage_p.h +++ b/core/include/moveit/task_constructor/stage_p.h @@ -310,18 +310,15 @@ public: } static inline bool less(const InterfaceState::Priority& lhsA, const InterfaceState::Priority& lhsB, const InterfaceState::Priority& rhsA, const InterfaceState::Priority& rhsB) { - unsigned char lhs = (lhsA.enabled() << 1) | lhsB.enabled(); // combine bits into two-digit binary number - unsigned char rhs = (rhsA.enabled() << 1) | rhsB.enabled(); + bool lhs = lhsA.enabled() && lhsB.enabled(); + bool rhs = rhsA.enabled() && rhsB.enabled(); + if (lhs == rhs) // if enabled status is identical return lhsA + lhsB < rhsA + rhsB; // compare the sums of both contributions - // one of the states in each pair should be enabled - assert(lhs != 0b00 && rhs != 0b00); - // both states valid (b11) - if (lhs == 0b11) - return true; - if (rhs == 0b11) - return false; - return lhs < rhs; // disabled states in 1st component go before disabled states in 2nd component + + // sort both-enabled pairs first + static_assert(true > false, "Comparing enabled states requires true > false"); + return lhs > rhs; } }; diff --git a/core/src/stage.cpp b/core/src/stage.cpp index 0de28926..4140922a 100644 --- a/core/src/stage.cpp +++ b/core/src/stage.cpp @@ -712,12 +712,7 @@ ConnectingPrivate::StatePair ConnectingPrivate::make_pair(In template void ConnectingPrivate::newState(Interface::iterator it, bool updated) { if (updated) { // many pairs might be affected: resort - if (it->priority().pruned()) - // remove all pending pairs involving this state - pending.remove_if([it](const StatePair& p) { return std::get()>(p) == it; }); - else - // TODO(v4hn): If a state becomes reenabled, this skips all previously removed pairs, right? - pending.sort(); + pending.sort(); } else { // new state: insert all pairs with other interface assert(it->priority().enabled()); // new solutions are feasible, aren't they? InterfacePtr other_interface = pullInterface(other); @@ -752,7 +747,7 @@ inline bool ConnectingPrivate::hasPendingOpposites(const InterfaceState* source) return true; // early stopping when only infeasible pairs are to come - if (!std::get<0>(candidate)->priority().enabled()) + if (!std::get<0>(candidate)->priority().enabled() || !std::get<1>(candidate)->priority().enabled()) break; } return false; diff --git a/core/test/test_interface_state.cpp b/core/test/test_interface_state.cpp index 9ca6af53..ec4261a2 100644 --- a/core/test/test_interface_state.cpp +++ b/core/test/test_interface_state.cpp @@ -87,8 +87,11 @@ TEST(StatePairs, compare) { EXPECT_TRUE(pair(Prio(1, 1), Prio(1, 1)) < pair(Prio(1, 0), Prio(0, 0))); auto good = InterfaceState::Status::ENABLED; - auto bad = InterfaceState::Status::FAILED; - EXPECT_TRUE(pair(good, good) < pair(good, bad)); - EXPECT_TRUE(pair(good, good) < pair(bad, good)); - EXPECT_TRUE(pair(bad, good) < pair(good, bad)); + auto good_good = pair(Prio(0, 10, good), Prio(0, 0, good)); + ASSERT_TRUE(good_good > pair(good, good)); // a bad status should reverse this relation + for (auto bad : { InterfaceState::Status::FAILED, InterfaceState::Status::PRUNED }) { + EXPECT_TRUE(good_good < pair(bad, good)); + EXPECT_TRUE(good_good < pair(good, bad)); + EXPECT_TRUE(good_good < pair(bad, bad)); + } }