DelayingWrapper stage to delay solution shipping in unit tests

core/test/CMakeLists.txt

@@ -34,6 +34,7 @@ if (CATKIN_ENABLE_TESTING)
 		mtc_add_test("gmock" ${ARGN})
 	endmacro()
 
+	mtc_add_gmock(test_mockups.cpp)
 	mtc_add_gtest(test_stage.cpp)
 	mtc_add_gtest(test_container.cpp)
 	mtc_add_gmock(test_serial.cpp)

core/test/stage_mockups.cpp

@@ -46,6 +46,19 @@ double PredefinedCosts::cost() const {
 	return c;
 }
 
+void DelayingWrapper::compute() {
+	if (!delay_.empty()) {  // if empty, we don't delay
+		if (delay_.front() == 0)
+			delay_.pop_front();  // we can compute() now
+		else {
+			--delay_.front();  // continue delaying
+			return;
+		}
+	}
+	// forward to child, eventually generating multiple solutions at once
+	WrapperBase::compute();
+}
+
 GeneratorMockup::GeneratorMockup(PredefinedCosts&& costs, std::size_t solutions_per_compute)
   : Generator{ "GEN" + std::to_string(++id_) }
   , costs_{ std::move(costs) }

core/test/stage_mockups.h

@@ -34,6 +34,18 @@ struct PredefinedCosts : CostTerm
 
 constexpr double INF{ std::numeric_limits<double>::infinity() };
 
+/* wrapper stage to delay solutions by a given number of steps */
+struct DelayingWrapper : public WrapperBase
+{
+	std::list<unsigned int> delay_;
+	/* delay list specifies the number of steps each received solution should be delayed */
+	DelayingWrapper(std::list<unsigned int> delay, Stage::pointer&& child)
+	  : WrapperBase("delayer", std::move(child)), delay_{ std::move(delay) } {}
+
+	void compute() override;
+	void onNewSolution(const SolutionBase& s) override { liftSolution(s); }
+};
+
 struct GeneratorMockup : public Generator
 {
 	planning_scene::PlanningScenePtr ps_;
@@ -47,8 +59,8 @@ struct GeneratorMockup : public Generator
 	// default to one solution to avoid infinity loops
 	GeneratorMockup(PredefinedCosts&& costs = PredefinedCosts{ std::list<double>{ 0.0 }, true },
 	                std::size_t solutions_per_compute = 1);
-	GeneratorMockup(std::initializer_list<double> costs)
-	  : GeneratorMockup{ PredefinedCosts{ std::list<double>{ costs }, true } } {}
+	GeneratorMockup(std::initializer_list<double> costs, std::size_t solutions_per_compute = 1)
+	  : GeneratorMockup{ PredefinedCosts{ std::list<double>{ costs }, true }, solutions_per_compute } {}
 
 	void init(const moveit::core::RobotModelConstPtr& robot_model) override;
 	bool canCompute() const override;

core/test/test_container.cpp

@@ -110,7 +110,7 @@ class InitTest : public ::testing::Test
 	Container container;
 	InterfacePtr dummy;
 
-	InitTest() : ::testing::Test{}, robot_model{ getModel() }, dummy{ new Interface } {}
+	InitTest() : ::testing::Test{}, robot_model{ getModel() }, dummy{ std::make_shared<Interface>() } {}
 
 	void pushInterface(bool start = true, bool end = true) {
 		// pretend, that the container is connected

core/test/test_mockups.cpp

@@ -0,0 +1,74 @@
+#include "stage_mockups.h"
+#include <moveit/task_constructor/container_p.h>
+
+using namespace moveit::task_constructor;
+
+struct TestGeneratorMockup : public GeneratorMockup
+{
+	InterfacePtr next_starts_;
+	InterfacePtr prev_ends_;
+
+	using GeneratorMockup::GeneratorMockup;
+	void init() {
+		next_starts_ = std::make_shared<Interface>();
+		prev_ends_ = std::make_shared<Interface>();
+
+		GeneratorMockup::reset();
+		GeneratorMockup::init(getModel());
+
+		GeneratorPrivate* impl = pimpl();
+		impl->setNextStarts(next_starts_);
+		impl->setPrevEnds(prev_ends_);
+	}
+};
+
+TEST(GeneratorMockup, compute) {
+	TestGeneratorMockup gen({ 1.0, 2.0, 3.0 });
+	gen.init();
+
+	for (int i = 0; i < 3; ++i) {
+		ASSERT_TRUE(gen.canCompute());
+		gen.compute();
+	}
+	EXPECT_FALSE(gen.canCompute());
+
+	EXPECT_COSTS(gen.solutions(), ::testing::ElementsAre(1, 2, 3));
+}
+
+#define COMPUTE_EXPECT_COSTS(stage, ...)                                                 \
+	{                                                                                     \
+		EXPECT_TRUE(stage.canCompute());                                                   \
+		stage.compute();                                                                   \
+		EXPECT_COSTS(stage.solutions(), ::testing::ElementsAre(__VA_ARGS__));              \
+		constexpr auto num_elements = std::initializer_list<double>{ __VA_ARGS__ }.size(); \
+		EXPECT_EQ(runs, num_elements);                                                     \
+	}
+
+TEST(GeneratorMockup, delayed) {
+	auto gen = std::make_unique<TestGeneratorMockup>(PredefinedCosts({ 1.0, 2.0, 3.0 }));
+	gen->init();
+	auto& runs = gen->runs_;
+
+	DelayingWrapper w({ 1, 0, 2 }, std::move(gen));
+	auto prev_ends = std::make_shared<Interface>();
+	auto next_starts = std::make_shared<Interface>();
+
+	WrapperBasePrivate* impl = w.pimpl();
+	impl->setPrevEnds(prev_ends);
+	impl->setNextStarts(next_starts);
+
+	// 1st compute() is delayed by 1
+	COMPUTE_EXPECT_COSTS(w);
+	COMPUTE_EXPECT_COSTS(w, 1);
+
+	// 2nd compute() is not delayed
+	COMPUTE_EXPECT_COSTS(w, 1, 2);
+
+	// 1st compute() is delayed by 2
+	COMPUTE_EXPECT_COSTS(w, 1, 2);
+	COMPUTE_EXPECT_COSTS(w, 1, 2);
+	COMPUTE_EXPECT_COSTS(w, 1, 2, 3);
+
+	EXPECT_FALSE(w.canCompute());
+}
+#undef COMPUTE_EXPECT_COSTS