mirror of
https://github.com/moveit/moveit_task_constructor.git
synced 2025-11-04 14:49:57 +08:00
fixup! SerialContainer: don't consider failures for solutions
This commit is contained in:
parent
862ac37cd6
commit
c2590636b1
@ -314,10 +314,6 @@ void SerialContainer::onNewSolution(const SolutionBase ¤t)
|
|||||||
const StagePrivate *creator = current.creator();
|
const StagePrivate *creator = current.creator();
|
||||||
auto& children = impl->children();
|
auto& children = impl->children();
|
||||||
|
|
||||||
if (current.isFailure()) {
|
|
||||||
return; // don't consider failures
|
|
||||||
}
|
|
||||||
|
|
||||||
// find number of stages before and after creator stage
|
// find number of stages before and after creator stage
|
||||||
size_t num_before = 0, num_after = 0;
|
size_t num_before = 0, num_after = 0;
|
||||||
for (auto it = children.begin(), end = children.end(); it != end; ++it, ++num_before)
|
for (auto it = children.begin(), end = children.end(); it != end; ++it, ++num_before)
|
||||||
@ -346,6 +342,8 @@ void SerialContainer::onNewSolution(const SolutionBase ¤t)
|
|||||||
in.second + current.cost() + out.second);
|
in.second + current.cost() + out.second);
|
||||||
// found a complete solution path connecting start to end?
|
// found a complete solution path connecting start to end?
|
||||||
if (prio.depth() == children.size()) {
|
if (prio.depth() == children.size()) {
|
||||||
|
if (std::isinf(prio.cost()))
|
||||||
|
continue; // don't propagate failures
|
||||||
assert(solution.empty());
|
assert(solution.empty());
|
||||||
// insert incoming solutions in reverse order
|
// insert incoming solutions in reverse order
|
||||||
solution.insert(solution.end(), in.first.rbegin(), in.first.rend());
|
solution.insert(solution.end(), in.first.rbegin(), in.first.rend());
|
||||||
|
|||||||
Loading…
Reference in New Issue
Block a user