Unit tests for moveit#485

ubi-agni · Oct 20, 2023 · 68de335 · 68de335
1 parent 4a3b938
commit 68de335
Show file tree

Hide file tree

Showing 2 changed files with 31 additions and 0 deletions.
diff --git a/core/test/test_pruning.cpp b/core/test/test_pruning.cpp
@@ -192,3 +192,18 @@ TEST_F(Pruning, TwoConnects) {
 
 	EXPECT_FALSE(t.plan());
 }
+
+TEST_F(Pruning, BackPropagateFailure) {
+	add(t, new GeneratorMockup({ 1.0 }));
+	auto con1 = add(t, new ConnectMockup());
+	add(t, new GeneratorMockup({ 10.0, 20.0 }, 2));  // create all solutions on first run
+	auto con2 = add(t, new ConnectMockup());
+	add(t, new GeneratorMockup({ 100.0, 200.0 }, 2));  // create all solutions on first run
+	// delay failure (INF) until CON2 has found first solution
+	add(t, new DelayingWrapper({ 1 }, std::make_unique<ForwardMockup>(PredefinedCosts({ INF, 2000 }))));
+
+	EXPECT_TRUE(t.plan());
+	EXPECT_COSTS(t.solutions(), ::testing::ElementsAre(2211, 2221));
+	EXPECT_EQ(con1->runs_, 2u);
+	EXPECT_EQ(con2->runs_, 3u);  // 100 - 20 is pruned
+}
diff --git a/core/test/test_serial.cpp b/core/test/test_serial.cpp
@@ -69,3 +69,19 @@ TEST_F(ConnectConnect, FailSucc) {
 
 	EXPECT_FALSE(t.plan());
 }
+
+// https://github.com/ros-planning/moveit_task_constructor/issues/485#issuecomment-1760606116
+// TODO: Solutions are enumerated multiple times
+// TODO: invalid solutions leak into enumeration
+TEST_F(ConnectConnect, UniqueEnumeration) {
+	add(t, new GeneratorMockup({ 1.0, 2.0, 3.0 }));
+	auto con1 = add(t, new ConnectMockup());
+	add(t, new GeneratorMockup({ 10.0, 20.0 }));
+	auto con2 = add(t, new ConnectMockup({ INF, 0.0, 0.0, 0.0 }));
+	add(t, new GeneratorMockup({ 100.0, 200.0 }));
+
+	EXPECT_TRUE(t.plan());
+	EXPECT_COSTS(t.solutions(), ::testing::ElementsAre(121, 122, 123, 211, 212, 213, 221, 222, 223));
+	EXPECT_EQ(con1->runs_, 3u * 2u);
+	EXPECT_EQ(con2->runs_, 2u * 2u);
+}