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;
|
friend class Connecting;
|
||||||
|
|
||||||
using StatePair = std::pair<Interface::const_iterator, Interface::const_iterator>;
|
public:
|
||||||
struct StatePairLess
|
struct StatePair : std::pair<Interface::const_iterator, Interface::const_iterator>
|
||||||
{
|
{
|
||||||
bool operator()(const StatePair& x, const StatePair& y) const {
|
using std::pair<Interface::const_iterator, Interface::const_iterator>::pair; // inherit base constructors
|
||||||
return x.first->priority() + x.second->priority() < y.first->priority() + y.second->priority();
|
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);
|
inline ConnectingPrivate(Connecting* me, const std::string& name);
|
||||||
|
|
||||||
InterfaceFlags requiredInterface() const override;
|
InterfaceFlags requiredInterface() const override;
|
||||||
@ -313,12 +325,16 @@ public:
|
|||||||
void compute() override;
|
void compute() override;
|
||||||
|
|
||||||
private:
|
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
|
// get informed when new start or end state becomes available
|
||||||
template <Interface::Direction other>
|
template <Interface::Direction other>
|
||||||
void newState(Interface::iterator it, bool updated);
|
void newState(Interface::iterator it, bool updated);
|
||||||
|
|
||||||
// ordered list of pending state pairs
|
// ordered list of pending state pairs
|
||||||
ordered<StatePair, StatePairLess> pending;
|
ordered<StatePair> pending;
|
||||||
};
|
};
|
||||||
PIMPL_FUNCTIONS(Connecting)
|
PIMPL_FUNCTIONS(Connecting)
|
||||||
} // namespace task_constructor
|
} // namespace task_constructor
|
||||||
|
|||||||
@ -157,19 +157,20 @@ public:
|
|||||||
class iterator : public base_type::iterator
|
class iterator : public base_type::iterator
|
||||||
{
|
{
|
||||||
public:
|
public:
|
||||||
using base_type::iterator::iterator; // inherit base constructors
|
|
||||||
iterator(base_type::iterator other) : base_type::iterator(other) {}
|
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*(); }
|
||||||
|
|
||||||
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
|
class const_iterator : public base_type::const_iterator
|
||||||
{
|
{
|
||||||
public:
|
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::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*(); }
|
||||||
|
|
||||||
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>
|
#include <gmock/gmock-matchers.h>
|
||||||
|
|
||||||
using namespace moveit::task_constructor;
|
using namespace moveit::task_constructor;
|
||||||
|
using Prio = InterfaceState::Priority;
|
||||||
|
|
||||||
TEST(InterfaceStatePriority, compare) {
|
TEST(InterfaceStatePriority, compare) {
|
||||||
using Prio = InterfaceState::Priority;
|
|
||||||
EXPECT_TRUE(Prio(0, 0) == Prio(0, 0));
|
EXPECT_TRUE(Prio(0, 0) == Prio(0, 0));
|
||||||
EXPECT_TRUE(Prio(1, 0) < Prio(0, 0)); // higher depth is smaller
|
EXPECT_TRUE(Prio(1, 0) < Prio(0, 0)); // higher depth is smaller
|
||||||
EXPECT_TRUE(Prio(1, 42) < Prio(0, 0));
|
EXPECT_TRUE(Prio(1, 42) < Prio(0, 0));
|
||||||
@ -69,3 +70,22 @@ TEST(Interface, update) {
|
|||||||
i.updatePriority(*i.begin(), Prio(6, 0, false));
|
i.updatePriority(*i.begin(), Prio(6, 0, false));
|
||||||
EXPECT_THAT(i.depths(), ::testing::ElementsAreArray({ 5, 3 })); // larger priority is ignored
|
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