diff --git a/core/src/stage.cpp b/core/src/stage.cpp index bf78c0e9..18286837 100644 --- a/core/src/stage.cpp +++ b/core/src/stage.cpp @@ -693,13 +693,8 @@ ConnectingPrivate::StatePair ConnectingPrivate::make_pair(In template void ConnectingPrivate::newState(Interface::iterator it, bool updated) { - // TODO: only consider interface states with priority depth > threshold - if (!std::isfinite(it->priority().cost())) { - // remove pending pairs, if cost updated to infinity - if (updated) - pending.remove_if([it](const StatePair& p) { return p.first == it; }); + if (!std::isfinite(it->priority().cost())) return; - } if (updated) { // many pairs might be affected: sort pending.sort(); @@ -722,6 +717,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())); static_cast(me_)->compute(from, to); } diff --git a/core/src/storage.cpp b/core/src/storage.cpp index 93e2b9c6..e3bcba8b 100644 --- a/core/src/storage.cpp +++ b/core/src/storage.cpp @@ -125,7 +125,7 @@ Interface::container_type Interface::remove(iterator it) { } void Interface::updatePriority(InterfaceState* state, const InterfaceState::Priority& priority) { - if (priority != state->priority()) { + if (priority < state->priority()) { // only allow decreasing of priority (smaller is better) auto it = std::find(begin(), end(), state); // find iterator to state assert(it != end()); // state should be part of this interface state->priority_ = priority; // update priority diff --git a/core/test/test_interface_state.cpp b/core/test/test_interface_state.cpp index 60a71f02..0ea85747 100644 --- a/core/test/test_interface_state.cpp +++ b/core/test/test_interface_state.cpp @@ -12,16 +12,19 @@ using namespace moveit::task_constructor; TEST(InterfaceStatePriority, compare) { using Prio = InterfaceState::Priority; - const double inf = std::numeric_limits::infinity(); + 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, 1)); // 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)); } @@ -63,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(1, 0.0)); - EXPECT_THAT(i.depths(), ::testing::ElementsAreArray({ 3, 1 })); + i.updatePriority(*i.begin(), Prio(6, std::numeric_limits::infinity())); + EXPECT_THAT(i.depths(), ::testing::ElementsAreArray({ 5, 3 })); // larger priority is ignored } \ No newline at end of file