mirror of
https://github.com/moveit/moveit_task_constructor.git
synced 2025-11-04 14:49:57 +08:00
Interface::updatePriority(): only allow decrease
This commit is contained in:
parent
66fdff9711
commit
c73c0c3555
@ -693,13 +693,8 @@ ConnectingPrivate::StatePair ConnectingPrivate::make_pair<Interface::FORWARD>(In
|
|||||||
|
|
||||||
template <Interface::Direction other>
|
template <Interface::Direction other>
|
||||||
void ConnectingPrivate::newState(Interface::iterator it, bool updated) {
|
void ConnectingPrivate::newState(Interface::iterator it, bool updated) {
|
||||||
// TODO: only consider interface states with priority depth > threshold
|
if (!std::isfinite(it->priority().cost()))
|
||||||
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; });
|
|
||||||
return;
|
return;
|
||||||
}
|
|
||||||
if (updated) {
|
if (updated) {
|
||||||
// many pairs might be affected: sort
|
// many pairs might be affected: sort
|
||||||
pending.sort();
|
pending.sort();
|
||||||
@ -722,6 +717,7 @@ void ConnectingPrivate::compute() {
|
|||||||
const StatePair& top = pending.pop();
|
const StatePair& top = pending.pop();
|
||||||
const InterfaceState& from = *top.first;
|
const InterfaceState& from = *top.first;
|
||||||
const InterfaceState& to = *top.second;
|
const InterfaceState& to = *top.second;
|
||||||
|
assert(std::isfinite((from.priority() + to.priority()).cost()));
|
||||||
static_cast<Connecting*>(me_)->compute(from, to);
|
static_cast<Connecting*>(me_)->compute(from, to);
|
||||||
}
|
}
|
||||||
|
|
||||||
|
|||||||
@ -125,7 +125,7 @@ Interface::container_type Interface::remove(iterator it) {
|
|||||||
}
|
}
|
||||||
|
|
||||||
void Interface::updatePriority(InterfaceState* state, const InterfaceState::Priority& priority) {
|
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
|
auto it = std::find(begin(), end(), state); // find iterator to state
|
||||||
assert(it != end()); // state should be part of this interface
|
assert(it != end()); // state should be part of this interface
|
||||||
state->priority_ = priority; // update priority
|
state->priority_ = priority; // update priority
|
||||||
|
|||||||
@ -12,16 +12,19 @@
|
|||||||
using namespace moveit::task_constructor;
|
using namespace moveit::task_constructor;
|
||||||
TEST(InterfaceStatePriority, compare) {
|
TEST(InterfaceStatePriority, compare) {
|
||||||
using Prio = InterfaceState::Priority;
|
using Prio = InterfaceState::Priority;
|
||||||
const double inf = std::numeric_limits<double>::infinity();
|
constexpr double inf = std::numeric_limits<double>::infinity();
|
||||||
|
|
||||||
EXPECT_TRUE(Prio(0, 0) == Prio(0, 0));
|
EXPECT_TRUE(Prio(0, 0) == Prio(0, 0));
|
||||||
EXPECT_TRUE(Prio(0, inf) == Prio(0, inf));
|
EXPECT_TRUE(Prio(0, inf) == Prio(0, inf));
|
||||||
|
|
||||||
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, inf) > Prio(0, 0)); // infinite costs are always largest
|
||||||
EXPECT_TRUE(Prio(1, inf) < Prio(0, inf));
|
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, 0) < Prio(0, inf));
|
||||||
|
EXPECT_TRUE(Prio(0, 42) > Prio(0, 0));
|
||||||
EXPECT_TRUE(Prio(0, inf) > 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));
|
i.updatePriority(*i.rbegin(), Prio(5, 0.0));
|
||||||
EXPECT_THAT(i.depths(), ::testing::ElementsAreArray({ 5, 3 }));
|
EXPECT_THAT(i.depths(), ::testing::ElementsAreArray({ 5, 3 }));
|
||||||
|
|
||||||
i.updatePriority(*i.begin(), Prio(1, 0.0));
|
i.updatePriority(*i.begin(), Prio(6, std::numeric_limits<double>::infinity()));
|
||||||
EXPECT_THAT(i.depths(), ::testing::ElementsAreArray({ 3, 1 }));
|
EXPECT_THAT(i.depths(), ::testing::ElementsAreArray({ 5, 3 })); // larger priority is ignored
|
||||||
}
|
}
|
||||||
Loading…
Reference in New Issue
Block a user