diff --git a/core/include/moveit/task_constructor/storage.h b/core/include/moveit/task_constructor/storage.h index 7a8e56a7..508ec2e4 100644 --- a/core/include/moveit/task_constructor/storage.h +++ b/core/include/moveit/task_constructor/storage.h @@ -94,6 +94,14 @@ public: this->cost() + other.cost()); } inline bool operator<(const Priority& other) const { + /* infinite cost should always be 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; + if (this->depth() == other.depth()) return this->cost() < other.cost(); else @@ -155,7 +163,7 @@ public: /// remove a state from the interface and return it as a one-element list container_type remove(iterator it); - /// update state's priority if new priority is smaller and call notify_ + /// update state's priority if new priority is smaller or became infeasible and call notify_ void updatePriority(InterfaceState *state, const InterfaceState::Priority &priority); private: diff --git a/core/src/storage.cpp b/core/src/storage.cpp index baadd92b..6f0e85e5 100644 --- a/core/src/storage.cpp +++ b/core/src/storage.cpp @@ -106,11 +106,13 @@ Interface::container_type Interface::remove(iterator it) void Interface::updatePriority(InterfaceState *state, const InterfaceState::Priority& priority) { - if (priority < state->priority()) { + double old_cost = state->priority_.cost(); + if (priority < state->priority() || std::isinf(priority.cost())) { auto it = std::find_if(begin(), end(), [state](const InterfaceState& other) { return state == &other; }); // state should be part of the interface assert(it != end()); state->priority_ = priority; + update(it); if (notify_) notify_(it, true); } }