mirror of
https://github.com/moveit/moveit_task_constructor.git
synced 2025-11-04 14:49:57 +08:00
Simplify state priority updates
Only need to update the end points of a partial solution.
This commit is contained in:
parent
fe189cd692
commit
66fdff9711
@ -314,21 +314,9 @@ struct SolutionCollector
|
||||
const size_t max_depth;
|
||||
};
|
||||
|
||||
void updateStateCosts(const SolutionSequence::container_type& partial_solution_path,
|
||||
const InterfaceState::Priority& prio) {
|
||||
for (const SolutionBase* solution : partial_solution_path) {
|
||||
// here it suffices to update the start state, because the end state is the start state
|
||||
// of the next solution (they are all connected)
|
||||
InterfaceState* state = const_cast<InterfaceState*>(solution->start());
|
||||
if (state->owner())
|
||||
state->owner()->updatePriority(state, prio);
|
||||
}
|
||||
// finally update the end state of the last solution
|
||||
if (partial_solution_path.empty())
|
||||
return;
|
||||
InterfaceState* state = const_cast<InterfaceState*>(partial_solution_path.back()->end());
|
||||
inline void updateStatePrio(const InterfaceState* state, const InterfaceState::Priority& prio) {
|
||||
if (state->owner())
|
||||
state->owner()->updatePriority(state, prio);
|
||||
state->owner()->updatePriority(const_cast<InterfaceState*>(state), prio);
|
||||
}
|
||||
|
||||
void SerialContainer::onNewSolution(const SolutionBase& current) {
|
||||
@ -377,10 +365,9 @@ void SerialContainer::onNewSolution(const SolutionBase& current) {
|
||||
// store solution in sorted list
|
||||
sorted.insert(std::make_shared<SolutionSequence>(std::move(solution), prio.cost(), this));
|
||||
} else if (prio.depth() > 1) {
|
||||
// update state priorities along the whole partial solution path
|
||||
updateStateCosts(in.first, prio);
|
||||
updateStateCosts({ ¤t }, prio);
|
||||
updateStateCosts(out.first, prio);
|
||||
// update state priorities at both ends of newly created partial solution
|
||||
updateStatePrio((in.first.empty() ? current : *in.first.back()).start(), prio);
|
||||
updateStatePrio((out.first.empty() ? current : *out.first.back()).end(), prio);
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
@ -126,10 +126,9 @@ Interface::container_type Interface::remove(iterator it) {
|
||||
|
||||
void Interface::updatePriority(InterfaceState* state, const InterfaceState::Priority& priority) {
|
||||
if (priority != state->priority()) {
|
||||
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;
|
||||
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
|
||||
update(it);
|
||||
if (notify_)
|
||||
notify_(it, true);
|
||||
|
||||
Loading…
Reference in New Issue
Block a user