Fix leaking of failures into enumerated solutions

This commit is contained in:
Robert Haschke 2023-10-20 06:55:38 +02:00
parent 7b965863e3
commit 13264ba21d
2 changed files with 2 additions and 2 deletions

View File

@ -487,7 +487,8 @@ struct SolutionCollector
solutions.emplace_back(std::make_pair(trace, prio)); solutions.emplace_back(std::make_pair(trace, prio));
} else { } else {
for (SolutionBase* successor : next) { for (SolutionBase* successor : next) {
assert(!successor->isFailure()); // We shouldn't have invalid solutions if (successor->isFailure())
continue; // skip failures
trace.push_back(successor); trace.push_back(successor);
traverse(*successor, prio + InterfaceState::Priority(1, successor->cost())); traverse(*successor, prio + InterfaceState::Priority(1, successor->cost()));
trace.pop_back(); trace.pop_back();

View File

@ -72,7 +72,6 @@ TEST_F(ConnectConnect, FailSucc) {
// https://github.com/ros-planning/moveit_task_constructor/issues/485#issuecomment-1760606116 // https://github.com/ros-planning/moveit_task_constructor/issues/485#issuecomment-1760606116
// TODO: Solutions are enumerated multiple times // TODO: Solutions are enumerated multiple times
// TODO: invalid solutions leak into enumeration
TEST_F(ConnectConnect, UniqueEnumeration) { TEST_F(ConnectConnect, UniqueEnumeration) {
add(t, new GeneratorMockup({ 1.0, 2.0, 3.0 })); add(t, new GeneratorMockup({ 1.0, 2.0, 3.0 }));
auto con1 = add(t, new ConnectMockup()); auto con1 = add(t, new ConnectMockup());