mirror of
https://github.com/moveit/moveit_task_constructor.git
synced 2025-11-04 14:49:57 +08:00
inf cost states always go last
Also update sorted interface when state becomes inf or get's new cost.
This commit is contained in:
parent
728c1b40e2
commit
bbb64f2f8c
@ -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:
|
||||
|
||||
@ -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);
|
||||
}
|
||||
}
|
||||
|
||||
Loading…
Reference in New Issue
Block a user