mirror of
https://github.com/moveit/moveit_task_constructor.git
synced 2025-11-04 14:49:57 +08:00
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.
This commit is contained in:
parent
97c2130404
commit
1ddf7dd3f0
@ -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;
|
||||
}
|
||||
};
|
||||
|
||||
|
||||
@ -712,12 +712,7 @@ ConnectingPrivate::StatePair ConnectingPrivate::make_pair<Interface::FORWARD>(In
|
||||
template <Interface::Direction other>
|
||||
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<opposite<other>()>(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;
|
||||
|
||||
@ -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));
|
||||
}
|
||||
}
|
||||
|
||||
Loading…
Reference in New Issue
Block a user