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());
|
this->cost() + other.cost());
|
||||||
}
|
}
|
||||||
inline bool operator<(const Priority& other) const {
|
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())
|
if (this->depth() == other.depth())
|
||||||
return this->cost() < other.cost();
|
return this->cost() < other.cost();
|
||||||
else
|
else
|
||||||
@ -155,7 +163,7 @@ public:
|
|||||||
/// remove a state from the interface and return it as a one-element list
|
/// remove a state from the interface and return it as a one-element list
|
||||||
container_type remove(iterator it);
|
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);
|
void updatePriority(InterfaceState *state, const InterfaceState::Priority &priority);
|
||||||
|
|
||||||
private:
|
private:
|
||||||
|
|||||||
@ -106,11 +106,13 @@ 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()) {
|
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; });
|
auto it = std::find_if(begin(), end(), [state](const InterfaceState& other) { return state == &other; });
|
||||||
// state should be part of the interface
|
// state should be part of the interface
|
||||||
assert(it != end());
|
assert(it != end());
|
||||||
state->priority_ = priority;
|
state->priority_ = priority;
|
||||||
|
update(it);
|
||||||
if (notify_) notify_(it, true);
|
if (notify_) notify_(it, true);
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
|||||||
Loading…
Reference in New Issue
Block a user