diff --git a/core/include/moveit/task_constructor/storage.h b/core/include/moveit/task_constructor/storage.h index 5832e250..deca3360 100644 --- a/core/include/moveit/task_constructor/storage.h +++ b/core/include/moveit/task_constructor/storage.h @@ -81,18 +81,28 @@ public: * Depth of interlinked trajectory parts and accumulated trajectory costs along that path. * Preference ordering considers high-depth first and within same depth, minimal cost paths. */ - struct Priority : public std::pair + struct Priority : std::tuple { - Priority(unsigned int depth, double cost) : std::pair(depth, cost) {} + Priority(unsigned int depth, double cost, bool enabled = true) + : std::tuple(enabled, depth, cost) { + assert(std::isfinite(cost)); + } + // Constructor copying depth and cost, but modifying enabled status + Priority(const Priority& other, bool enabled) : Priority(other.depth(), other.cost(), enabled) {} - inline unsigned int depth() const { return this->first; } - inline double cost() const { return this->second; } + inline bool enabled() const { return std::get<0>(*this); } + inline unsigned int depth() const { return std::get<1>(*this); } + inline double cost() const { return std::get<2>(*this); } // add priorities Priority operator+(const Priority& other) const { - return Priority(this->depth() + other.depth(), this->cost() + other.cost()); + return Priority(depth() + other.depth(), cost() + other.cost(), enabled() || other.enabled()); } - bool operator<(const Priority& other) const; + // comparison operators + bool operator<(const Priority& rhs) const; + inline bool operator>(const Priority& rhs) const { return rhs < *this; } + inline bool operator<=(const Priority& rhs) const { return !(rhs < *this); } + inline bool operator>=(const Priority& rhs) const { return !(*this < rhs); } }; using Solutions = std::deque; diff --git a/core/src/container.cpp b/core/src/container.cpp index f66314e4..14a4f1c7 100644 --- a/core/src/container.cpp +++ b/core/src/container.cpp @@ -298,7 +298,7 @@ struct SolutionCollector solutions.emplace_back(std::make_pair(trace, prio)); } else { for (SolutionBase* successor : next) { - if (successor->isFailure()) + if (successor->isFailure()) // skip failure "solutions" continue; trace.push_back(successor); @@ -345,7 +345,7 @@ void SerialContainer::onNewSolution(const SolutionBase& current) { InterfaceState::Priority prio = in.second + InterfaceState::Priority(1u, current.cost()) + out.second; // found a complete solution path connecting start to end? if (prio.depth() == children.size()) { - assert(std::isfinite(prio.cost())); + assert(prio.enabled()); assert(solution.empty()); // insert incoming solutions in reverse order solution.insert(solution.end(), in.first.rbegin(), in.first.rend()); diff --git a/core/src/stage.cpp b/core/src/stage.cpp index aa3cdec6..2f071763 100644 --- a/core/src/stage.cpp +++ b/core/src/stage.cpp @@ -701,16 +701,12 @@ ConnectingPrivate::StatePair ConnectingPrivate::make_pair(In template void ConnectingPrivate::newState(Interface::iterator it, bool updated) { - if (!std::isfinite(it->priority().cost())) - return; if (updated) { // many pairs might be affected: sort pending.sort(); } else { // new state: insert all pairs with other interface InterfacePtr other_interface = pullInterface(other); for (Interface::iterator oit = other_interface->begin(), oend = other_interface->end(); oit != oend; ++oit) { - if (!std::isfinite(oit->priority().cost())) - break; if (static_cast(me_)->compatible(*it, *oit)) pending.insert(make_pair(it, oit)); } @@ -725,7 +721,7 @@ void ConnectingPrivate::compute() { const StatePair& top = pending.pop(); const InterfaceState& from = *top.first; const InterfaceState& to = *top.second; - assert(std::isfinite((from.priority() + to.priority()).cost())); + assert(from.priority().enabled() && to.priority().enabled()); static_cast(me_)->compute(from, to); } diff --git a/core/src/storage.cpp b/core/src/storage.cpp index ad5e8314..f79ff86b 100644 --- a/core/src/storage.cpp +++ b/core/src/storage.cpp @@ -70,18 +70,15 @@ InterfaceState::InterfaceState(const InterfaceState& other) : scene_(other.scene_), properties_(other.properties_), priority_(other.priority_) {} bool InterfaceState::Priority::operator<(const InterfaceState::Priority& other) const { - // infinite costs go always last - if (std::isinf(this->cost()) && std::isinf(other.cost())) - return this->depth() > other.depth(); - else if (std::isinf(this->cost())) - return false; - else if (std::isinf(other.cost())) - return true; + // disabled states always after enabled ones + if (enabled() != other.enabled()) + return enabled(); - if (this->depth() == other.depth()) - return this->cost() < other.cost(); + // both prios have same enabled_ state + if (depth() == other.depth()) + return cost() < other.cost(); else - return this->depth() > other.depth(); + return depth() > other.depth(); // larger depth = smaller prio! } std::ostream& operator<<(std::ostream& os, const InterfaceState::Priority& p) { @@ -113,7 +110,7 @@ void Interface::add(InterfaceState& state) { else if (!state.outgoingTrajectories().empty()) it->priority_ = InterfaceState::Priority(1, state.outgoingTrajectories().front()->cost()); else // otherwise, assume priority was well defined before - assert(it->priority_ >= InterfaceState::Priority(1, 0.0)); + assert(it->priority_.enabled() && it->priority_.depth() >= 1u); // move list node into interface's state list (sorted by priority) moveFrom(it, container); diff --git a/core/test/test_interface_state.cpp b/core/test/test_interface_state.cpp index 5aa887fc..54b26777 100644 --- a/core/test/test_interface_state.cpp +++ b/core/test/test_interface_state.cpp @@ -13,20 +13,26 @@ using namespace moveit::task_constructor; TEST(InterfaceStatePriority, compare) { using Prio = InterfaceState::Priority; - constexpr double inf = std::numeric_limits::infinity(); - EXPECT_TRUE(Prio(0, 0) == Prio(0, 0)); - EXPECT_TRUE(Prio(0, inf) == Prio(0, inf)); - EXPECT_TRUE(Prio(1, 0) < Prio(0, 0)); // higher depth is smaller EXPECT_TRUE(Prio(1, 42) < Prio(0, 0)); - EXPECT_TRUE(Prio(1, inf) > Prio(0, 0)); // infinite costs are always largest - EXPECT_TRUE(Prio(1, inf) < Prio(0, inf)); + EXPECT_TRUE(Prio(0, 0) < Prio(0, 42)); // at same depth, higher cost is larger - EXPECT_TRUE(Prio(0, 0) < Prio(0, 42)); // higher cost is larger - EXPECT_TRUE(Prio(0, 0) < Prio(0, inf)); - EXPECT_TRUE(Prio(0, 42) > Prio(0, 0)); - EXPECT_TRUE(Prio(0, inf) > Prio(0, 0)); + EXPECT_TRUE(Prio(0, 0, false) == Prio(0, 0, false)); + EXPECT_TRUE(Prio(1, 0, false) < Prio(0, 0, false)); + EXPECT_TRUE(Prio(1, 42, false) < Prio(0, 0, false)); + EXPECT_TRUE(Prio(0, 0, false) < Prio(0, 42, false)); + + // disabled prios are always larger than enabled ones + EXPECT_TRUE(Prio(0, 42) < Prio(1, 0, false)); + EXPECT_TRUE(Prio(1, 0) < Prio(0, 42, false)); + + // other comparison operators + EXPECT_TRUE(Prio(0, 0) <= Prio(0, 0)); + EXPECT_TRUE(Prio(0, 0) <= Prio(0, 1)); + EXPECT_TRUE(Prio(0, 0) > Prio(1, 10)); + EXPECT_TRUE(Prio(0, 0) >= Prio(0, 0)); + EXPECT_TRUE(Prio(0, 10) >= Prio(0, 0)); } using Prio = InterfaceState::Priority; @@ -60,6 +66,6 @@ TEST(Interface, update) { i.updatePriority(*i.rbegin(), Prio(5, 0.0)); EXPECT_THAT(i.depths(), ::testing::ElementsAreArray({ 5, 3 })); - i.updatePriority(*i.begin(), Prio(6, std::numeric_limits::infinity())); + i.updatePriority(*i.begin(), Prio(6, 0, false)); EXPECT_THAT(i.depths(), ::testing::ElementsAreArray({ 5, 3 })); // larger priority is ignored }