Add unittest for moveit#581

ubi-agni · Jul 17, 2024 · 7631486 · 7631486
1 parent cd28bdc
commit 7631486
Showing 1 changed file with 22 additions and 1 deletion.
diff --git a/core/test/test_fallback.cpp b/core/test/test_fallback.cpp
@@ -1,7 +1,8 @@
 #include <moveit/task_constructor/container_p.h>
 #include <moveit/task_constructor/stage_p.h>
 #include <moveit/task_constructor/task_p.h>
-#include <moveit/task_constructor/stages/fixed_state.h>
+#include <moveit/task_constructor/stages/predicate_filter.h>
+#include <moveit/task_constructor/stages/noop.h>
 #include <moveit/planning_scene/planning_scene.h>
 
 #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<GeneratorMockup>(PredefinedCosts::single(0.0)));
+
+	auto fallbacks = std::make_unique<Fallbacks>("Fallbacks");
+	auto add_filtered_fwd = [&fallbacks](double cost, bool accept) {
+		auto fwd = std::make_unique<ForwardMockup>(PredefinedCosts::constant(cost));
+		auto filter = std::make_unique<stages::PredicateFilter>("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<stages::NoOp>());
+	t.add(std::move(fallbacks));
+
+	EXPECT_TRUE(t.plan());
+	EXPECT_COSTS(t.solutions(), testing::ElementsAre(2.));
+}
+
 using FallbacksFixtureConnect = TaskTestBase;
 
 TEST_F(FallbacksFixtureConnect, connectStageInsideFallbacks) {