diff --git a/core/test/test_serial.cpp b/core/test/test_serial.cpp index bbffcaca..d2284d6b 100644 --- a/core/test/test_serial.cpp +++ b/core/test/test_serial.cpp @@ -57,17 +57,23 @@ struct GeneratorMockup : Generator class PropagatorMockup : public PropagatingEitherWay { PredefinedCosts costs_; + std::size_t solutions_per_compute_; public: - PropagatorMockup(std::initializer_list costs = { 0.0 }) : PropagatingEitherWay(), costs_(false, costs) {} + PropagatorMockup(std::initializer_list costs = { 0.0 }, std::size_t solutions_per_compute = 1) + : PropagatingEitherWay(), costs_(false, costs), solutions_per_compute_(solutions_per_compute) {} void computeForward(const InterfaceState& from) override { - SubTrajectory solution(robot_trajectory::RobotTrajectoryConstPtr(), costs_.cost()); - sendForward(from, InterfaceState(from.scene()->diff()), std::move(solution)); + for (std::size_t i = 0; i < solutions_per_compute_; ++i) { + SubTrajectory solution(robot_trajectory::RobotTrajectoryConstPtr(), costs_.cost()); + sendForward(from, InterfaceState(from.scene()->diff()), std::move(solution)); + } } void computeBackward(const InterfaceState& to) override { - SubTrajectory solution(robot_trajectory::RobotTrajectoryConstPtr(), costs_.cost()); - sendBackward(InterfaceState(to.scene()->diff()), to, std::move(solution)); + for (std::size_t i = 0; i < solutions_per_compute_; ++i) { + SubTrajectory solution(robot_trajectory::RobotTrajectoryConstPtr(), costs_.cost()); + sendBackward(InterfaceState(to.scene()->diff()), to, std::move(solution)); + } } }; class ForwardMockup : public PropagatorMockup @@ -75,7 +81,8 @@ class ForwardMockup : public PropagatorMockup static unsigned int id_; public: - ForwardMockup(std::initializer_list costs = { 0.0 }) : PropagatorMockup(costs) { + ForwardMockup(std::initializer_list costs = { 0.0 }, std::size_t solutions_per_compute = 1) + : PropagatorMockup(costs, solutions_per_compute) { restrictDirection(FORWARD); setName("FW" + std::to_string(++id_)); } @@ -95,7 +102,7 @@ public: struct ForwardDummy : PropagatingForward { using PropagatingForward::PropagatingForward; - void computeForward(const InterfaceState& from) override {} + void computeForward(const InterfaceState& /*from*/) override {} }; /* Connect creating solutions with given costs */ @@ -176,6 +183,28 @@ TEST(ConnectConnect, PruningForward) { EXPECT_EQ(c2->calls_, 6u); } +TEST(ConnectConnect, PruningMultiForward) { + GeneratorMockup::id_ = Connect::id_ = 0; // reset IDs + Task t; + + t.setRobotModel(getModel()); + t.add(Stage::pointer(new GeneratorMockup())); + t.add(Stage::pointer(new Connect())); + t.add(Stage::pointer(new BackwardMockup())); + /* TODO(v4hn): + * t.add(Stage::pointer(new GeneratorMockup())); + * should be enough instead of the above, but propagators do not even respect ENABLED/DISABLED yet + */ + + t.add(Stage::pointer(new GeneratorMockup())); + t.add(Stage::pointer(new ForwardMockup({ 0, 0 }, 2))); // spawn two solutions for the only incoming state + t.add(Stage::pointer(new ForwardMockup({ 0, inf }))); + t.plan(); + + ASSERT_EQ(t.solutions().size(), 1); + EXPECT_EQ((*t.solutions().begin())->cost(), 0u); +} + TEST(ConnectConnect, PruningBackward) { GeneratorMockup::id_ = Connect::id_ = 0; // reset IDs Task t;