diff --git a/core/test/stage_mockups.cpp b/core/test/stage_mockups.cpp index 6f3ddd5b..463ca360 100644 --- a/core/test/stage_mockups.cpp +++ b/core/test/stage_mockups.cpp @@ -46,8 +46,10 @@ double PredefinedCosts::cost() const { return c; } -GeneratorMockup::GeneratorMockup(PredefinedCosts&& costs) - : Generator{ "GEN" + std::to_string(++id_) }, costs_{ std::move(costs) } {} +GeneratorMockup::GeneratorMockup(PredefinedCosts&& costs, std::size_t solutions_per_compute) + : Generator{ "GEN" + std::to_string(++id_) } + , costs_{ std::move(costs) } + , solutions_per_compute_{ solutions_per_compute } {} void GeneratorMockup::init(const moveit::core::RobotModelConstPtr& robot_model) { ps_.reset(new planning_scene::PlanningScene(robot_model)); @@ -63,7 +65,8 @@ bool GeneratorMockup::canCompute() const { void GeneratorMockup::compute() { ++runs_; - spawn(InterfaceState(ps_), costs_.cost()); + for (std::size_t i = 0; canCompute() && i < solutions_per_compute_; ++i) + spawn(InterfaceState(ps_), costs_.cost()); } MonitoringGeneratorMockup::MonitoringGeneratorMockup(Stage* monitored, PredefinedCosts&& costs) diff --git a/core/test/stage_mockups.h b/core/test/stage_mockups.h index 1a4d7ff3..f7bec9a0 100644 --- a/core/test/stage_mockups.h +++ b/core/test/stage_mockups.h @@ -40,11 +40,13 @@ struct GeneratorMockup : public Generator PredefinedCosts costs_; size_t runs_{ 0 }; + std::size_t solutions_per_compute_; static unsigned int id_; // default to one solution to avoid infinity loops - GeneratorMockup(PredefinedCosts&& costs = PredefinedCosts{ std::list{ 0.0 }, true }); + 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 } } {}