mirror of
https://github.com/moveit/moveit_task_constructor.git
synced 2025-11-04 14:49:57 +08:00
test comparison of ConnectingPrivate's StatePairs
This commit is contained in:
parent
2a09daa42f
commit
6c18e2b482
@ -294,18 +294,30 @@ class ConnectingPrivate : public ComputeBasePrivate
|
||||
{
|
||||
friend class Connecting;
|
||||
|
||||
using StatePair = std::pair<Interface::const_iterator, Interface::const_iterator>;
|
||||
struct StatePairLess
|
||||
public:
|
||||
struct StatePair : std::pair<Interface::const_iterator, Interface::const_iterator>
|
||||
{
|
||||
bool operator()(const StatePair& x, const StatePair& y) const {
|
||||
return x.first->priority() + x.second->priority() < y.first->priority() + y.second->priority();
|
||||
using std::pair<Interface::const_iterator, Interface::const_iterator>::pair; // inherit base constructors
|
||||
bool operator<(const StatePair& rhs) const {
|
||||
return less(first->priority(), second->priority(), rhs.first->priority(), rhs.second->priority());
|
||||
}
|
||||
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();
|
||||
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
|
||||
}
|
||||
};
|
||||
|
||||
template <Interface::Direction other>
|
||||
inline StatePair make_pair(Interface::const_iterator first, Interface::const_iterator second);
|
||||
|
||||
public:
|
||||
inline ConnectingPrivate(Connecting* me, const std::string& name);
|
||||
|
||||
InterfaceFlags requiredInterface() const override;
|
||||
@ -313,12 +325,16 @@ public:
|
||||
void compute() override;
|
||||
|
||||
private:
|
||||
// Create a pair of Interface states for pending list, such that the order (start, end) is maintained
|
||||
template <Interface::Direction other>
|
||||
inline StatePair make_pair(Interface::const_iterator first, Interface::const_iterator second);
|
||||
|
||||
// get informed when new start or end state becomes available
|
||||
template <Interface::Direction other>
|
||||
void newState(Interface::iterator it, bool updated);
|
||||
|
||||
// ordered list of pending state pairs
|
||||
ordered<StatePair, StatePairLess> pending;
|
||||
ordered<StatePair> pending;
|
||||
};
|
||||
PIMPL_FUNCTIONS(Connecting)
|
||||
} // namespace task_constructor
|
||||
|
||||
@ -157,19 +157,20 @@ public:
|
||||
class iterator : public base_type::iterator
|
||||
{
|
||||
public:
|
||||
using base_type::iterator::iterator; // inherit base constructors
|
||||
iterator(base_type::iterator other) : base_type::iterator(other) {}
|
||||
|
||||
InterfaceState& operator*() const noexcept { return *base_type::iterator::operator*(); }
|
||||
|
||||
InterfaceState* operator->() const noexcept { return base_type::iterator::operator*(); }
|
||||
};
|
||||
class const_iterator : public base_type::const_iterator
|
||||
{
|
||||
public:
|
||||
using base_type::const_iterator::const_iterator; // inherit base constructors
|
||||
const_iterator(base_type::const_iterator other) : base_type::const_iterator(other) {}
|
||||
const_iterator(base_type::iterator other) : base_type::const_iterator(other) {}
|
||||
|
||||
const InterfaceState& operator*() const noexcept { return *base_type::const_iterator::operator*(); }
|
||||
|
||||
const InterfaceState* operator->() const noexcept { return base_type::const_iterator::operator*(); }
|
||||
};
|
||||
|
||||
|
||||
@ -11,8 +11,9 @@
|
||||
#include <gmock/gmock-matchers.h>
|
||||
|
||||
using namespace moveit::task_constructor;
|
||||
using Prio = InterfaceState::Priority;
|
||||
|
||||
TEST(InterfaceStatePriority, compare) {
|
||||
using Prio = InterfaceState::Priority;
|
||||
EXPECT_TRUE(Prio(0, 0) == Prio(0, 0));
|
||||
EXPECT_TRUE(Prio(1, 0) < Prio(0, 0)); // higher depth is smaller
|
||||
EXPECT_TRUE(Prio(1, 42) < Prio(0, 0));
|
||||
@ -69,3 +70,22 @@ TEST(Interface, update) {
|
||||
i.updatePriority(*i.begin(), Prio(6, 0, false));
|
||||
EXPECT_THAT(i.depths(), ::testing::ElementsAreArray({ 5, 3 })); // larger priority is ignored
|
||||
}
|
||||
|
||||
using PrioPair = std::pair<Prio, Prio>;
|
||||
inline bool operator<(const PrioPair& lhs, const PrioPair& rhs) {
|
||||
return ConnectingPrivate::StatePair::less(lhs.first, lhs.second, rhs.first, rhs.second);
|
||||
}
|
||||
PrioPair pair(Prio&& p1, Prio&& p2) {
|
||||
return std::make_pair(std::move(p1), std::move(p2));
|
||||
}
|
||||
PrioPair pair(bool e1, bool e2) {
|
||||
return pair(Prio(0, 0, e1), Prio(0, 0, e2));
|
||||
}
|
||||
TEST(StatePairs, compare) {
|
||||
EXPECT_TRUE(pair(Prio(1, 0), Prio(0, 1)) < pair(Prio(1, 1), Prio(0, 1)));
|
||||
EXPECT_TRUE(pair(Prio(1, 1), Prio(1, 1)) < pair(Prio(1, 0), Prio(0, 0)));
|
||||
|
||||
EXPECT_TRUE(pair(true, true) < pair(true, false));
|
||||
EXPECT_TRUE(pair(true, true) < pair(false, true));
|
||||
EXPECT_TRUE(pair(false, true) < pair(true, false));
|
||||
}
|
||||
|
||||
Loading…
Reference in New Issue
Block a user