diff --git a/core/test/test_fallback.cpp b/core/test/test_fallback.cpp index 87f9368f7..c5b7eb90b 100644 --- a/core/test/test_fallback.cpp +++ b/core/test/test_fallback.cpp @@ -1,7 +1,8 @@ #include #include #include -#include +#include +#include #include #include "stage_mockups.h" @@ -159,6 +160,26 @@ TEST_F(FallbacksFixturePropagate, activeChildReset) { EXPECT_COSTS(fwd2->solutions(), testing::IsEmpty()); } +// https://github.com/moveit/moveit_task_constructor/issues/581#issuecomment-2147985474 +TEST_F(FallbacksFixturePropagate, filterPropagatesFailures) { + t.add(std::make_unique(PredefinedCosts::single(0.0))); + + auto fallbacks = std::make_unique("Fallbacks"); + auto add_filtered_fwd = [&fallbacks](double cost, bool accept) { + auto fwd = std::make_unique(PredefinedCosts::constant(cost)); + auto filter = std::make_unique("filter", std::move(fwd)); + filter->setPredicate([accept](const SolutionBase& /*solution*/, std::string& /*comment*/) { return accept; }); + fallbacks->add(std::move(filter)); + }; + add_filtered_fwd(INF, false); // Propagate fails, filter rejects + add_filtered_fwd(2.0, true); // Propagate succeeds, filter accepts + fallbacks->add(std::make_unique()); + t.add(std::move(fallbacks)); + + EXPECT_TRUE(t.plan()); + EXPECT_COSTS(t.solutions(), testing::ElementsAre(2.)); +} + using FallbacksFixtureConnect = TaskTestBase; TEST_F(FallbacksFixtureConnect, connectStageInsideFallbacks) {