From 6c18e2b4823a20944a28cae3db1c766bb5240737 Mon Sep 17 00:00:00 2001 From: Robert Haschke Date: Sun, 6 Dec 2020 02:01:24 +0100 Subject: [PATCH] test comparison of ConnectingPrivate's StatePairs --- .../include/moveit/task_constructor/stage_p.h | 34 ++++++++++++++----- .../include/moveit/task_constructor/storage.h | 5 +-- core/test/test_interface_state.cpp | 22 +++++++++++- 3 files changed, 49 insertions(+), 12 deletions(-) diff --git a/core/include/moveit/task_constructor/stage_p.h b/core/include/moveit/task_constructor/stage_p.h index 17fa0fd2..43e53174 100644 --- a/core/include/moveit/task_constructor/stage_p.h +++ b/core/include/moveit/task_constructor/stage_p.h @@ -294,18 +294,30 @@ class ConnectingPrivate : public ComputeBasePrivate { friend class Connecting; - using StatePair = std::pair; - struct StatePairLess +public: + struct StatePair : std::pair { - 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::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 - 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 + inline StatePair make_pair(Interface::const_iterator first, Interface::const_iterator second); + // get informed when new start or end state becomes available template void newState(Interface::iterator it, bool updated); // ordered list of pending state pairs - ordered pending; + ordered pending; }; PIMPL_FUNCTIONS(Connecting) } // namespace task_constructor diff --git a/core/include/moveit/task_constructor/storage.h b/core/include/moveit/task_constructor/storage.h index deca3360..5f817b2d 100644 --- a/core/include/moveit/task_constructor/storage.h +++ b/core/include/moveit/task_constructor/storage.h @@ -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*(); } }; diff --git a/core/test/test_interface_state.cpp b/core/test/test_interface_state.cpp index 54b26777..e86640bf 100644 --- a/core/test/test_interface_state.cpp +++ b/core/test/test_interface_state.cpp @@ -11,8 +11,9 @@ #include 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; +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)); +}