diff --git a/core/test/CMakeLists.txt b/core/test/CMakeLists.txt index 9125df027..b9709651f 100644 --- a/core/test/CMakeLists.txt +++ b/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) diff --git a/core/test/stage_mockups.cpp b/core/test/stage_mockups.cpp index 463ca3609..b084faf3b 100644 --- a/core/test/stage_mockups.cpp +++ b/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) } diff --git a/core/test/stage_mockups.h b/core/test/stage_mockups.h index f7bec9a09..fc75c633e 100644 --- a/core/test/stage_mockups.h +++ b/core/test/stage_mockups.h @@ -34,6 +34,18 @@ struct PredefinedCosts : CostTerm constexpr double INF{ std::numeric_limits::infinity() }; +/* wrapper stage to delay solutions by a given number of steps */ +struct DelayingWrapper : public WrapperBase +{ + std::list delay_; + /* delay list specifies the number of steps each received solution should be delayed */ + DelayingWrapper(std::list 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{ 0.0 }, true }, std::size_t solutions_per_compute = 1); - GeneratorMockup(std::initializer_list costs) - : GeneratorMockup{ PredefinedCosts{ std::list{ costs }, true } } {} + GeneratorMockup(std::initializer_list costs, std::size_t solutions_per_compute = 1) + : GeneratorMockup{ PredefinedCosts{ std::list{ costs }, true }, solutions_per_compute } {} void init(const moveit::core::RobotModelConstPtr& robot_model) override; bool canCompute() const override; diff --git a/core/test/test_container.cpp b/core/test/test_container.cpp index 1a7a9de4c..876a1e6d3 100644 --- a/core/test/test_container.cpp +++ b/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() } {} void pushInterface(bool start = true, bool end = true) { // pretend, that the container is connected diff --git a/core/test/test_mockups.cpp b/core/test/test_mockups.cpp new file mode 100644 index 000000000..7726a3c3e --- /dev/null +++ b/core/test/test_mockups.cpp @@ -0,0 +1,74 @@ +#include "stage_mockups.h" +#include + +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(); + prev_ends_ = std::make_shared(); + + 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{ __VA_ARGS__ }.size(); \ + EXPECT_EQ(runs, num_elements); \ + } + +TEST(GeneratorMockup, delayed) { + auto gen = std::make_unique(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(); + auto next_starts = std::make_shared(); + + 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