mirror of
https://github.com/moveit/moveit_task_constructor.git
synced 2025-11-04 14:49:57 +08:00
update priorities of all interface states along a (partial) solution path
if a parallel container is involved somewhere in the middle, it will again access these states, e.g. planning alternative solutions
This commit is contained in:
parent
1eacea6b35
commit
373d7f204f
@ -235,10 +235,25 @@ struct SolutionCollector {
|
||||
solutions.emplace_back(std::make_pair(trace, cost));
|
||||
}
|
||||
|
||||
std::list<std::pair<SerialContainer::solution_container, double>> solutions;
|
||||
typedef std::list<std::pair<SerialContainer::solution_container, double>> SolutionCostPairs;
|
||||
SolutionCostPairs solutions;
|
||||
const size_t max_depth;
|
||||
};
|
||||
|
||||
void updateStateCosts(const SerialContainer::solution_container &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());
|
||||
if (state->owner()) state->owner()->updatePriority(*state, prio);
|
||||
}
|
||||
|
||||
void SerialContainer::onNewSolution(const SolutionBase ¤t)
|
||||
{
|
||||
auto impl = pimpl();
|
||||
@ -283,11 +298,10 @@ void SerialContainer::onNewSolution(const SolutionBase ¤t)
|
||||
// store solution in sorted list
|
||||
sorted.insert(SerialSolution(impl, std::move(solution), prio.cost()));
|
||||
} else if (prio.depth() > 1) {
|
||||
// update state priority at both ends of the partial solution path
|
||||
const InterfaceState* start = (in.first.empty() ? current : *in.first.back()).start();
|
||||
if (start->owner()) start->owner()->updatePriority(*const_cast<InterfaceState*>(start), prio);
|
||||
const InterfaceState* end = (out.first.empty() ? current : *out.first.back()).end();
|
||||
if (end->owner()) end->owner()->updatePriority(*const_cast<InterfaceState*>(end), prio);
|
||||
// update state priorities along the whole partial solution path
|
||||
updateStateCosts(in.first, prio);
|
||||
updateStateCosts({¤t}, prio);
|
||||
updateStateCosts(out.first, prio);
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
Loading…
Reference in New Issue
Block a user