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,
|
static inline bool less(const InterfaceState::Priority& lhsA, const InterfaceState::Priority& lhsB,
|
||||||
const InterfaceState::Priority& rhsA, const InterfaceState::Priority& rhsB) {
|
const InterfaceState::Priority& rhsA, const InterfaceState::Priority& rhsB) {
|
||||||
unsigned char lhs = (lhsA.enabled() << 1) | lhsB.enabled(); // combine bits into two-digit binary number
|
bool lhs = lhsA.enabled() && lhsB.enabled();
|
||||||
unsigned char rhs = (rhsA.enabled() << 1) | rhsB.enabled();
|
bool rhs = rhsA.enabled() && rhsB.enabled();
|
||||||
|
|
||||||
if (lhs == rhs) // if enabled status is identical
|
if (lhs == rhs) // if enabled status is identical
|
||||||
return lhsA + lhsB < rhsA + rhsB; // compare the sums of both contributions
|
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);
|
// sort both-enabled pairs first
|
||||||
// both states valid (b11)
|
static_assert(true > false, "Comparing enabled states requires true > false");
|
||||||
if (lhs == 0b11)
|
return lhs > rhs;
|
||||||
return true;
|
|
||||||
if (rhs == 0b11)
|
|
||||||
return false;
|
|
||||||
return lhs < rhs; // disabled states in 1st component go before disabled states in 2nd component
|
|
||||||
}
|
}
|
||||||
};
|
};
|
||||||
|
|
||||||
|
|||||||
@ -712,11 +712,6 @@ ConnectingPrivate::StatePair ConnectingPrivate::make_pair<Interface::FORWARD>(In
|
|||||||
template <Interface::Direction other>
|
template <Interface::Direction other>
|
||||||
void ConnectingPrivate::newState(Interface::iterator it, bool updated) {
|
void ConnectingPrivate::newState(Interface::iterator it, bool updated) {
|
||||||
if (updated) { // many pairs might be affected: resort
|
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
|
} else { // new state: insert all pairs with other interface
|
||||||
assert(it->priority().enabled()); // new solutions are feasible, aren't they?
|
assert(it->priority().enabled()); // new solutions are feasible, aren't they?
|
||||||
@ -752,7 +747,7 @@ inline bool ConnectingPrivate::hasPendingOpposites(const InterfaceState* source)
|
|||||||
return true;
|
return true;
|
||||||
|
|
||||||
// early stopping when only infeasible pairs are to come
|
// 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;
|
break;
|
||||||
}
|
}
|
||||||
return false;
|
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)));
|
EXPECT_TRUE(pair(Prio(1, 1), Prio(1, 1)) < pair(Prio(1, 0), Prio(0, 0)));
|
||||||
|
|
||||||
auto good = InterfaceState::Status::ENABLED;
|
auto good = InterfaceState::Status::ENABLED;
|
||||||
auto bad = InterfaceState::Status::FAILED;
|
auto good_good = pair(Prio(0, 10, good), Prio(0, 0, good));
|
||||||
EXPECT_TRUE(pair(good, good) < pair(good, bad));
|
ASSERT_TRUE(good_good > pair(good, good)); // a bad status should reverse this relation
|
||||||
EXPECT_TRUE(pair(good, good) < pair(bad, good));
|
for (auto bad : { InterfaceState::Status::FAILED, InterfaceState::Status::PRUNED }) {
|
||||||
EXPECT_TRUE(pair(bad, good) < pair(good, bad));
|
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