Merge different mockup implementations

Co-authored-by: Jascha Kühn <57101356+j-kuehn@users.noreply.github.com>
This commit is contained in:
v4hn 2021-07-04 23:30:49 +02:00 committed by Michael Görner
parent 4b1f240c21
commit 74d33c4ec0
7 changed files with 369 additions and 350 deletions

View File

@ -6,7 +6,7 @@
if (CATKIN_ENABLE_TESTING) if (CATKIN_ENABLE_TESTING)
find_package(rostest REQUIRED) find_package(rostest REQUIRED)
add_library(gtest_utils gtest_value_printers.cpp models.cpp) add_library(gtest_utils gtest_value_printers.cpp models.cpp stage_mockups.cpp)
target_link_libraries(gtest_utils ${PROJECT_NAME}) target_link_libraries(gtest_utils ${PROJECT_NAME})
catkin_add_gtest(${PROJECT_NAME}-test-stage test_stage.cpp) catkin_add_gtest(${PROJECT_NAME}-test-stage test_stage.cpp)

125
core/test/stage_mockups.cpp Normal file
View File

@ -0,0 +1,125 @@
#include <moveit/task_constructor/stage_p.h>
#include <moveit/planning_scene/planning_scene.h>
#include "stage_mockups.h"
#include <gtest/gtest.h>
namespace moveit {
namespace task_constructor {
unsigned int GeneratorMockup::id_ = 0;
unsigned int MonitoringGeneratorMockup::id_ = 0;
unsigned int ConnectMockup::id_ = 0;
unsigned int PropagatorMockup::id_ = 0;
unsigned int ForwardMockup::id_ = 0;
unsigned int BackwardMockup::id_ = 0;
void resetMockupIds() {
GeneratorMockup::id_ = 0;
MonitoringGeneratorMockup::id_ = 0;
ConnectMockup::id_ = 0;
PropagatorMockup::id_ = 0;
ForwardMockup::id_ = 0;
BackwardMockup::id_ = 0;
}
PredefinedCosts::PredefinedCosts(std::list<double>&& costs, bool finite)
: costs_{ std::move(costs) }, finite_{ finite } {}
bool PredefinedCosts::exhausted() const {
return costs_.empty();
}
double PredefinedCosts::cost() const {
// keep the tests going, but this should not happen
EXPECT_GT(costs_.size(), 0u);
if (costs_.empty())
return 0.0;
double c{ costs_.front() };
if (finite_ || costs_.size() > 1) {
costs_.pop_front();
}
return c;
}
GeneratorMockup::GeneratorMockup(PredefinedCosts&& costs)
: Generator{ "GEN" + std::to_string(++id_) }, costs_{ std::move(costs) } {}
void GeneratorMockup::init(const moveit::core::RobotModelConstPtr& robot_model) {
ps_.reset(new planning_scene::PlanningScene(robot_model));
Generator::init(robot_model);
}
bool GeneratorMockup::canCompute() const {
// check if runs are being used and if there are still runs left (costs are then never exhausted) or if costs
// are being used and they are not exhausted yet
return !costs_.exhausted();
}
void GeneratorMockup::compute() {
++runs_;
spawn(InterfaceState(ps_), costs_.cost());
}
MonitoringGeneratorMockup::MonitoringGeneratorMockup(Stage* monitored, PredefinedCosts&& costs)
: MonitoringGenerator{ "MON" + std::to_string(++id_), monitored }, costs_{ std::move(costs) } {}
void MonitoringGeneratorMockup::onNewSolution(const SolutionBase& s) {
++runs_;
spawn(InterfaceState{ s.end()->scene()->diff() }, SubTrajectory{});
}
ConnectMockup::ConnectMockup(PredefinedCosts&& costs)
: Connecting{ "CON" + std::to_string(++id_) }, costs_{ std::move(costs) } {}
void ConnectMockup::compute(const InterfaceState& from, const InterfaceState& to) {
++runs_;
auto solution{ std::make_shared<SubTrajectory>() };
solution->setCost(costs_.cost());
connect(from, to, solution);
}
PropagatorMockup::PropagatorMockup(PredefinedCosts&& costs, std::size_t solutions_per_compute)
: PropagatingEitherWay{ "PRO" + std::to_string(++id_) }
, costs_{ std::move(costs) }
, solutions_per_compute_{ solutions_per_compute } {}
void PropagatorMockup::computeForward(const InterfaceState& from) {
++runs_;
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 PropagatorMockup::computeBackward(const InterfaceState& to) {
++runs_;
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));
}
}
ForwardMockup::ForwardMockup(PredefinedCosts&& costs, std::size_t solutions_per_compute)
: PropagatorMockup{ std::move(costs), solutions_per_compute } {
restrictDirection(FORWARD);
setName("FWD" + std::to_string(++id_));
}
BackwardMockup::BackwardMockup(PredefinedCosts&& costs, std::size_t solutions_per_compute)
: PropagatorMockup{ std::move(costs), solutions_per_compute } {
restrictDirection(BACKWARD);
setName("BWD" + std::to_string(++id_));
}
} // namespace task_constructor
} // namespace moveit

117
core/test/stage_mockups.h Normal file
View File

@ -0,0 +1,117 @@
#pragma once
#include <moveit/task_constructor/stage_p.h>
#include <moveit/planning_scene/planning_scene.h>
namespace moveit {
namespace task_constructor {
MOVEIT_STRUCT_FORWARD(PredefinedCosts);
struct PredefinedCosts : CostTerm
{
mutable std::list<double> costs_; // list of costs to assign
mutable bool finite_;
PredefinedCosts(std::list<double>&& costs, bool finite = true);
static PredefinedCosts constant(double c) { return PredefinedCosts{ std::list<double>{ c }, false }; }
static PredefinedCosts single(double c) { return PredefinedCosts{ std::list<double>{ c }, true }; }
bool exhausted() const;
double cost() const;
double operator()(const SubTrajectory& /*s*/, std::string& /*comment*/) const override { return cost(); }
double operator()(const SolutionSequence& /*s*/, std::string& /*comment*/) const override { return cost(); }
double operator()(const WrappedSolution& /*s*/, std::string& /*comment*/) const override { return cost(); }
};
struct GeneratorMockup : public Generator
{
planning_scene::PlanningScenePtr ps_;
PredefinedCosts costs_;
size_t runs_{ 0 };
static unsigned int id_;
// default to one solution to avoid infinity loops
GeneratorMockup(PredefinedCosts&& costs = PredefinedCosts{ std::list<double>{ 0.0 }, true });
GeneratorMockup(std::initializer_list<double> costs)
: GeneratorMockup{ PredefinedCosts{ std::list<double>{ costs }, true } } {}
void init(const moveit::core::RobotModelConstPtr& robot_model) override;
bool canCompute() const override;
void compute() override;
};
struct MonitoringGeneratorMockup : public MonitoringGenerator
{
PredefinedCosts costs_;
size_t runs_{ 0 };
static unsigned int id_;
MonitoringGeneratorMockup(Stage* monitored, PredefinedCosts&& costs = PredefinedCosts::constant(0.0));
MonitoringGeneratorMockup(Stage* monitored, std::initializer_list<double> costs)
: MonitoringGeneratorMockup{ monitored, PredefinedCosts{ std::list<double>{ costs }, true } } {}
bool canCompute() const override { return false; }
void compute() override {}
void onNewSolution(const SolutionBase& s) override;
};
struct ConnectMockup : public Connecting
{
PredefinedCosts costs_;
size_t runs_{ 0 };
static unsigned int id_;
ConnectMockup(PredefinedCosts&& costs = PredefinedCosts::constant(0.0));
ConnectMockup(std::initializer_list<double> costs)
: ConnectMockup{ PredefinedCosts{ std::list<double>{ costs }, true } } {}
using Connecting::compatible; // make this accessible for testing
void compute(const InterfaceState& from, const InterfaceState& to) override;
};
struct PropagatorMockup : public PropagatingEitherWay
{
PredefinedCosts costs_;
size_t runs_{ 0 };
std::size_t solutions_per_compute_;
static unsigned int id_;
PropagatorMockup(PredefinedCosts&& costs = PredefinedCosts::constant(0.0), std::size_t solutions_per_compute = 1);
PropagatorMockup(std::initializer_list<double> costs)
: PropagatorMockup{ PredefinedCosts{ std::list<double>{ costs }, true } } {}
void computeForward(const InterfaceState& from) override;
void computeBackward(const InterfaceState& to) override;
};
struct ForwardMockup : public PropagatorMockup
{
static unsigned int id_;
ForwardMockup(PredefinedCosts&& costs = PredefinedCosts::constant(0.0), std::size_t solutions_per_compute = 1);
ForwardMockup(std::initializer_list<double> costs)
: ForwardMockup{ PredefinedCosts{ std::list<double>{ costs }, true } } {}
};
struct BackwardMockup : public PropagatorMockup
{
static unsigned int id_;
BackwardMockup(PredefinedCosts&& costs = PredefinedCosts::constant(0.0), std::size_t solutions_per_compute = 1);
BackwardMockup(std::initializer_list<double> costs)
: BackwardMockup{ PredefinedCosts{ std::list<double>{ costs }, true } } {}
};
// reset ids of all Mockup types (used to generate unique stage names)
void resetMockupIds();
} // namespace task_constructor
} // namespace moveit

View File

@ -4,8 +4,10 @@
#include <moveit/task_constructor/stages/fixed_state.h> #include <moveit/task_constructor/stages/fixed_state.h>
#include <moveit/planning_scene/planning_scene.h> #include <moveit/planning_scene/planning_scene.h>
#include "stage_mockups.h"
#include "models.h" #include "models.h"
#include "gtest_value_printers.h" #include "gtest_value_printers.h"
#include <gtest/gtest.h> #include <gtest/gtest.h>
#include <initializer_list> #include <initializer_list>
#include <chrono> #include <chrono>
@ -13,101 +15,6 @@
using namespace moveit::task_constructor; using namespace moveit::task_constructor;
static unsigned int MOCK_ID = 0;
class GeneratorMockup : public Generator
{
moveit::core::RobotModelConstPtr robot;
int runs = 0;
public:
GeneratorMockup(int runs = 0) : Generator("generator " + std::to_string(++MOCK_ID)), runs(runs) {}
void init(const moveit::core::RobotModelConstPtr& robot_model) override { robot = robot_model; }
bool canCompute() const override { return runs > 0; }
void compute() override {
if (runs > 0) {
--runs;
spawn(InterfaceState(std::make_shared<planning_scene::PlanningScene>(robot)), SubTrajectory());
}
}
};
class MonitoringGeneratorMockup : public MonitoringGenerator
{
public:
MonitoringGeneratorMockup(Stage* monitored)
: MonitoringGenerator("monitoring generator " + std::to_string(++MOCK_ID), monitored) {}
bool canCompute() const override { return false; }
void compute() override {}
void onNewSolution(const SolutionBase& s) override {
spawn(InterfaceState(s.end()->scene()->diff()), SubTrajectory());
}
};
class PropagatorMockup : public PropagatingEitherWay
{
int fw_runs = 0;
int bw_runs = 0;
public:
PropagatorMockup(int fw = 0, int bw = 0)
: PropagatingEitherWay("propagate " + std::to_string(++MOCK_ID)), fw_runs(fw), bw_runs(bw) {}
void computeForward(const InterfaceState& from) override {
if (fw_runs > 0) {
--fw_runs;
sendForward(from, InterfaceState(from.scene()->diff()), SubTrajectory());
}
}
void computeBackward(const InterfaceState& to) override {
if (bw_runs > 0) {
--bw_runs;
sendBackward(InterfaceState(to.scene()->diff()), to, SubTrajectory());
}
}
};
class ForwardMockup : public PropagatorMockup
{
public:
ForwardMockup(int runs = 0) : PropagatorMockup(runs, 0) {
restrictDirection(FORWARD);
setName("forward " + std::to_string(MOCK_ID));
}
};
class BackwardMockup : public PropagatorMockup
{
public:
BackwardMockup(int runs = 0) : PropagatorMockup(0, runs) {
restrictDirection(BACKWARD);
setName("backward " + std::to_string(MOCK_ID));
}
};
// ForwardMockup that takes a while for its computation
class TimedForwardMockup : public ForwardMockup
{
std::chrono::milliseconds duration_;
public:
TimedForwardMockup(std::chrono::milliseconds duration) : ForwardMockup(1000), duration_(duration) {}
void computeForward(const InterfaceState& from) override {
std::this_thread::sleep_for(duration_);
ForwardMockup::computeForward(from);
}
};
class ConnectMockup : public Connecting
{
int runs = 0;
public:
ConnectMockup(int runs = 0) : Connecting("connect " + std::to_string(++MOCK_ID)), runs(runs) {}
void compute(const InterfaceState& from, const InterfaceState& to) override {
if (runs > 0)
--runs;
connect(from, to, std::make_shared<SubTrajectory>());
}
};
enum StageType enum StageType
{ {
GEN, GEN,
@ -116,23 +23,23 @@ enum StageType
ANY, ANY,
CONN CONN
}; };
void append(ContainerBase& c, const std::initializer_list<StageType>& types, int runs = 0) { void append(ContainerBase& c, const std::initializer_list<StageType>& types) {
for (StageType t : types) { for (StageType t : types) {
switch (t) { switch (t) {
case GEN: case GEN:
c.add(std::make_unique<GeneratorMockup>(runs)); c.add(std::make_unique<GeneratorMockup>());
break; break;
case FW: case FW:
c.add(std::make_unique<ForwardMockup>(runs)); c.add(std::make_unique<ForwardMockup>());
break; break;
case BW: case BW:
c.add(std::make_unique<BackwardMockup>(runs)); c.add(std::make_unique<BackwardMockup>());
break; break;
case ANY: case ANY:
c.add(std::make_unique<PropagatorMockup>(runs, runs)); c.add(std::make_unique<PropagatorMockup>());
break; break;
case CONN: case CONN:
c.add(std::make_unique<ConnectMockup>(runs)); c.add(std::make_unique<ConnectMockup>());
break; break;
} }
} }
@ -215,7 +122,7 @@ protected:
} }
void initContainer(const std::initializer_list<StageType>& types = {}) { void initContainer(const std::initializer_list<StageType>& types = {}) {
container.clear(); container.clear();
MOCK_ID = 0; resetMockupIds();
append(container, types); append(container, types);
} }
@ -505,7 +412,7 @@ TEST_F(SerialTest, interface_detection) {
} }
template <typename T> template <typename T>
Stage::pointer make(const std::string& name, const std::initializer_list<StageType>& children, int runs = 0) { Stage::pointer make(const std::string& name, const std::initializer_list<StageType>& children) {
auto container = new T(name); auto container = new T(name);
append(*container, children); append(*container, children);
return Stage::pointer(container); return Stage::pointer(container);
@ -669,13 +576,13 @@ TEST_F(ParallelTest, init_any) {
} }
TEST(Task, move) { TEST(Task, move) {
MOCK_ID = 0; resetMockupIds();
Task t1("foo"); Task t1("foo");
t1.add(std::make_unique<GeneratorMockup>()); t1.add(std::make_unique<GeneratorMockup>());
t1.add(std::make_unique<GeneratorMockup>()); t1.add(std::make_unique<GeneratorMockup>());
EXPECT_EQ(t1.stages()->numChildren(), 2u); EXPECT_EQ(t1.stages()->numChildren(), 2u);
MOCK_ID = 0; resetMockupIds();
Task t2 = std::move(t1); Task t2 = std::move(t1);
EXPECT_EQ(t2.stages()->numChildren(), 2u); EXPECT_EQ(t2.stages()->numChildren(), 2u);
EXPECT_EQ(t1.stages()->numChildren(), 0u); // NOLINT(clang-analyzer-cplusplus.Move) EXPECT_EQ(t1.stages()->numChildren(), 0u); // NOLINT(clang-analyzer-cplusplus.Move)
@ -692,13 +599,13 @@ TEST(Task, reuse) {
t.setRobotModel(robot_model); t.setRobotModel(robot_model);
auto configure = [](Task& t) { auto configure = [](Task& t) {
MOCK_ID = 0; resetMockupIds();
auto ref = new stages::FixedState("fixed"); auto ref = new stages::FixedState("fixed");
auto scene = std::make_shared<planning_scene::PlanningScene>(t.getRobotModel()); auto scene = std::make_shared<planning_scene::PlanningScene>(t.getRobotModel());
ref->setState(scene); ref->setState(scene);
t.add(Stage::pointer(ref)); t.add(Stage::pointer(ref));
t.add(std::make_unique<ConnectMockup>(2)); t.add(std::make_unique<ConnectMockup>());
t.add(std::make_unique<MonitoringGeneratorMockup>(ref)); t.add(std::make_unique<MonitoringGeneratorMockup>(ref));
}; };
@ -719,13 +626,26 @@ TEST(Task, reuse) {
} }
} }
// ForwardMockup that takes a while for its computation
class TimedForwardMockup : public ForwardMockup
{
std::chrono::milliseconds duration_;
public:
TimedForwardMockup(std::chrono::milliseconds duration) : ForwardMockup{}, duration_{ duration } {}
void computeForward(const InterfaceState& from) override {
std::this_thread::sleep_for(duration_);
ForwardMockup::computeForward(from);
}
};
TEST(Task, timeout) { TEST(Task, timeout) {
MOCK_ID = 0; resetMockupIds();
Task t; Task t;
t.setRobotModel(getModel()); t.setRobotModel(getModel());
auto timeout = std::chrono::milliseconds(10); auto timeout = std::chrono::milliseconds(10);
t.add(std::make_unique<GeneratorMockup>(100)); // allow up to 100 solutions spawned t.add(std::make_unique<GeneratorMockup>(PredefinedCosts::constant(0.0)));
t.add(std::make_unique<TimedForwardMockup>(timeout)); t.add(std::make_unique<TimedForwardMockup>(timeout));
// no timeout, but limited number of solutions // no timeout, but limited number of solutions
@ -751,15 +671,15 @@ TEST(Task, timeout) {
} }
TEST(Fallback, failing) { TEST(Fallback, failing) {
MOCK_ID = 0; resetMockupIds();
Task t; Task t;
t.setRobotModel(getModel()); t.setRobotModel(getModel());
t.add(std::make_unique<GeneratorMockup>()); t.add(std::make_unique<GeneratorMockup>(PredefinedCosts::single(0.0)));
auto fallback = std::make_unique<Fallbacks>("Fallbacks"); auto fallback = std::make_unique<Fallbacks>("Fallbacks");
fallback->add(std::make_unique<ForwardMockup>()); fallback->add(std::make_unique<ForwardMockup>(PredefinedCosts::constant(0.0), 0));
fallback->add(std::make_unique<ForwardMockup>()); fallback->add(std::make_unique<ForwardMockup>(PredefinedCosts::constant(0.0), 0));
t.add(std::move(fallback)); t.add(std::move(fallback));
EXPECT_FALSE(t.plan()); EXPECT_FALSE(t.plan());

View File

@ -11,88 +11,58 @@
#include <gtest/gtest.h> #include <gtest/gtest.h>
#include "stage_mockups.h"
using namespace moveit::task_constructor; using namespace moveit::task_constructor;
using namespace planning_scene; using namespace planning_scene;
const double STAGE_COST{ 7.0 }; constexpr double STAGE_COST{ 7.0 };
constexpr double TERM_COST{ 1.0 };
const double TRAJECTORY_DURATION{ 9.0 }; const double TRAJECTORY_DURATION{ 9.0 };
class GeneratorMockup : public Generator struct GeneratorCostMockup : public GeneratorMockup
{ {
PlanningScenePtr ps; GeneratorCostMockup() : GeneratorMockup{ PredefinedCosts{ { STAGE_COST }, true } } {}
InterfacePtr prev;
InterfacePtr next;
public:
GeneratorMockup() : Generator("generator") {
prev.reset(new Interface);
next.reset(new Interface);
pimpl()->setPrevEnds(prev);
pimpl()->setNextStarts(next);
}
void init(const moveit::core::RobotModelConstPtr& robot_model) override {
ps.reset(new PlanningScene(robot_model));
Generator::init(robot_model);
}
bool canCompute() const override { return true; }
void compute() override {
InterfaceState state(ps);
spawn(std::move(state), STAGE_COST);
}
}; };
class ConnectMockup : public Connecting struct ConnectCostMockup : public ConnectMockup
{ {
using Connecting::Connecting; ConnectCostMockup() : ConnectMockup{ PredefinedCosts::constant(STAGE_COST) } {}
void compute(const InterfaceState& from, const InterfaceState& to) override {
auto solution{ std::make_shared<SubTrajectory>() };
solution->setCost(STAGE_COST);
connect(from, to, solution);
}
}; };
class ForwardMockup : public PropagatingForward struct ForwardCostMockup : public ForwardMockup
{ {
PlanningScenePtr ps; ForwardCostMockup() : ForwardMockup{ PredefinedCosts::constant(STAGE_COST) } {}
};
public: struct BackwardCostMockup : public BackwardMockup
using PropagatingForward::PropagatingForward; {
BackwardCostMockup() : BackwardMockup{ PredefinedCosts::constant(STAGE_COST) } {}
};
void init(const moveit::core::RobotModelConstPtr& robot_model) override { struct ForwardTrajectoryMockup : public ForwardMockup
ps.reset(new PlanningScene(robot_model)); {
PropagatingForward::init(robot_model); ForwardTrajectoryMockup(PredefinedCosts&& costs = PredefinedCosts::constant(0.0),
} std::size_t solutions_per_compute = 1)
: ForwardMockup{ std::move(costs), solutions_per_compute } {}
void computeForward(const InterfaceState& from) override { void computeForward(const InterfaceState& from) override {
SubTrajectory solution; ++runs_;
auto traj{ std::make_shared<robot_trajectory::RobotTrajectory>(ps->getRobotModel(),
ps->getRobotModel()->getJointModelGroups()[0]) };
traj->addSuffixWayPoint(ps->getCurrentState(), 0.0);
traj->addSuffixWayPoint(ps->getCurrentState(), TRAJECTORY_DURATION);
solution.setTrajectory(traj);
solution.setCost(STAGE_COST);
InterfaceState to(from);
sendForward(from, std::move(to), std::move(solution)); for (size_t i = 0; i < solutions_per_compute_; ++i) {
}; SubTrajectory solution;
}; auto traj{ std::make_shared<robot_trajectory::RobotTrajectory>(from.scene()->getRobotModel(), nullptr) };
planning_scene::PlanningScenePtr ps{ from.scene()->diff() };
traj->addSuffixWayPoint(ps->getCurrentState(), 0.0);
traj->addSuffixWayPoint(ps->getCurrentState(), TRAJECTORY_DURATION);
solution.setTrajectory(traj);
solution.setCost(STAGE_COST);
InterfaceState to(from);
class BackwardMockup : public PropagatingBackward sendForward(from, std::move(to), std::move(solution));
{ }
using PropagatingBackward::PropagatingBackward; }
void computeBackward(const InterfaceState& to) override {
SubTrajectory solution;
solution.setCost(STAGE_COST);
InterfaceState from(to);
sendBackward(std::move(from), to, std::move(solution));
};
}; };
template <typename T> template <typename T>
@ -105,7 +75,9 @@ public:
InterfaceStatePtr state_start, state_end; InterfaceStatePtr state_start, state_end;
Standalone(const moveit::core::RobotModelConstPtr& robot) Standalone(const moveit::core::RobotModelConstPtr& robot)
: T(), robot(robot), dummy(std::make_shared<Interface>()), ps(new planning_scene::PlanningScene(robot)) {} : T(), robot(robot), dummy(std::make_shared<Interface>()), ps(new planning_scene::PlanningScene(robot)) {
resetMockupIds();
}
// reset and prepare for a compute step // reset and prepare for a compute step
void prepare() { void prepare() {
@ -173,7 +145,7 @@ TEST(CostTerm, SolutionConnected) {
const moveit::core::RobotModelConstPtr robot{ getModel() }; const moveit::core::RobotModelConstPtr robot{ getModel() };
Standalone<SerialContainer> container(robot); Standalone<SerialContainer> container(robot);
auto stage{ std::make_unique<ConnectMockup>() }; auto stage{ std::make_unique<ConnectCostMockup>() };
// custom CostTerm to verify SubTrajectory is hooked up to its states & creator // custom CostTerm to verify SubTrajectory is hooked up to its states & creator
class VerifySolutionCostTerm : public TrajectoryCostTerm class VerifySolutionCostTerm : public TrajectoryCostTerm
@ -193,38 +165,40 @@ TEST(CostTerm, SolutionConnected) {
const_cast<const SerialContainerPrivate*>(container_.pimpl())->internalToExternalMap().at(s.end())) const_cast<const SerialContainerPrivate*>(container_.pimpl())->internalToExternalMap().at(s.end()))
<< "SubTrajectory is not connected to its expected end InterfaceState"; << "SubTrajectory is not connected to its expected end InterfaceState";
EXPECT_EQ(s.creator(), creator_); EXPECT_EQ(s.creator(), creator_);
return 1.0; return TERM_COST;
} }
}; };
stage->setCostTerm(std::make_unique<VerifySolutionCostTerm>(container, &*stage)); stage->setCostTerm(std::make_unique<VerifySolutionCostTerm>(container, &*stage));
container.computeWithStages({ std::move(stage) }); container.computeWithStages({ std::move(stage) });
EXPECT_EQ(container.solutions().front()->cost(), 1.0) << "custom CostTerm overwrites stage cost"; EXPECT_EQ(container.solutions().front()->cost(), TERM_COST) << "custom CostTerm overwrites stage cost";
} }
TEST(CostTerm, SetLambdaCostTerm) { TEST(CostTerm, SetLambdaCostTerm) {
const moveit::core::RobotModelConstPtr robot{ getModel() }; const moveit::core::RobotModelConstPtr robot{ getModel() };
Standalone<SerialContainer> container(robot); Standalone<SerialContainer> container(robot);
auto stage{ std::make_unique<GeneratorMockup>() };
stage->setCostTerm([](auto&& /*s*/) { return 1.0; });
container.computeWithStages({ std::move(stage) });
EXPECT_EQ(container.solutions().front()->cost(), 1.0) << "can use simple lambda signature";
stage = std::make_unique<GeneratorMockup>(); auto stage{ std::make_unique<GeneratorCostMockup>() };
stage->setCostTerm([](auto&& /*s*/) { return TERM_COST; });
container.computeWithStages({ std::move(stage) });
EXPECT_EQ(container.solutions().front()->cost(), TERM_COST) << "can use simple lambda signature";
stage = std::make_unique<GeneratorCostMockup>();
stage->setCostTerm([](auto&& /*s*/, auto&& /*comment*/) { return 1.0; }); stage->setCostTerm([](auto&& /*s*/, auto&& /*comment*/) { return 1.0; });
container.computeWithStages({ std::move(stage) }); container.computeWithStages({ std::move(stage) });
EXPECT_EQ(container.solutions().front()->cost(), 1.0) << "can use full lambda signature"; EXPECT_EQ(container.solutions().front()->cost(), 1.0) << "can use full lambda signature";
stage = std::make_unique<GeneratorMockup>(); const std::string cost_term_comment{ "I want the user to see this" };
stage->setCostTerm([](auto&& /*s*/, auto&& comment) { stage = std::make_unique<GeneratorCostMockup>();
comment = "I want the user to see this"; stage->setCostTerm([&](auto&& /*s*/, auto&& comment) {
comment = cost_term_comment;
return 1.0; return 1.0;
}); });
container.computeWithStages({ std::move(stage) }); container.computeWithStages({ std::move(stage) });
auto sol = std::dynamic_pointer_cast<const SolutionSequence>(container.solutions().front()); auto sol = std::dynamic_pointer_cast<const SolutionSequence>(container.solutions().front());
EXPECT_EQ(sol->cost(), 1.0); EXPECT_EQ(sol->cost(), 1.0);
EXPECT_EQ(sol->solutions()[0]->comment(), "I want the user to see this") << "can write to comment"; EXPECT_EQ(sol->solutions()[0]->comment(), cost_term_comment) << "can write to comment";
} }
TEST(CostTerm, CostOverwrite) { TEST(CostTerm, CostOverwrite) {
@ -232,14 +206,14 @@ TEST(CostTerm, CostOverwrite) {
Standalone<SerialContainer> container(robot); Standalone<SerialContainer> container(robot);
container.computeWithStages({ std::make_unique<GeneratorMockup>() }); container.computeWithStages({ std::make_unique<GeneratorCostMockup>() });
EXPECT_EQ(container.solutions().front()->cost(), STAGE_COST) << "return cost of stage by default"; EXPECT_EQ(container.solutions().front()->cost(), STAGE_COST) << "return cost of stage by default";
container.computeWithStageCost({ std::make_unique<GeneratorMockup>() }, nullptr); container.computeWithStageCost({ std::make_unique<GeneratorCostMockup>() }, nullptr);
EXPECT_EQ(container.solutions().front()->cost(), STAGE_COST) << "nullptr cost term forwards cost"; EXPECT_EQ(container.solutions().front()->cost(), STAGE_COST) << "nullptr cost term forwards cost";
auto constant_cost{ std::make_shared<cost::Constant>(1.0) }; auto constant_cost{ std::make_shared<cost::Constant>(1.0) };
container.computeWithStageCost({ std::make_unique<GeneratorMockup>() }, constant_cost); container.computeWithStageCost({ std::make_unique<GeneratorCostMockup>() }, constant_cost);
EXPECT_EQ(container.solutions().front()->cost(), constant_cost->cost) << "custom cost overwrites stage cost"; EXPECT_EQ(container.solutions().front()->cost(), constant_cost->cost) << "custom cost overwrites stage cost";
} }
@ -251,16 +225,16 @@ TEST(CostTerm, StageTypes) {
auto constant{ std::make_shared<cost::Constant>(1.0) }; auto constant{ std::make_shared<cost::Constant>(1.0) };
// already tested above // already tested above
// cont.computeWithStageCost(std::make_unique<GeneratorMockup>(), constant); // cont.computeWithStageCost(std::make_unique<GeneratorMockupCost>(), constant);
// EXPECT_EQ(cont.solutions().front()->cost(), constant.cost); // EXPECT_EQ(cont.solutions().front()->cost(), constant.cost);
container.computeWithStageCost({ std::make_unique<ConnectMockup>() }, constant); container.computeWithStageCost({ std::make_unique<ConnectCostMockup>() }, constant);
EXPECT_EQ(container.solutions().front()->cost(), constant->cost) << "custom cost works for connect"; EXPECT_EQ(container.solutions().front()->cost(), constant->cost) << "custom cost works for connect";
container.computeWithStageCost({ std::make_unique<ForwardMockup>() }, constant); container.computeWithStageCost({ std::make_unique<ForwardCostMockup>() }, constant);
EXPECT_EQ(container.solutions().front()->cost(), constant->cost) << "custom cost works for forward propagator"; EXPECT_EQ(container.solutions().front()->cost(), constant->cost) << "custom cost works for forward propagator";
container.computeWithStageCost({ std::make_unique<BackwardMockup>() }, constant); container.computeWithStageCost({ std::make_unique<BackwardCostMockup>() }, constant);
EXPECT_EQ(container.solutions().front()->cost(), constant->cost) << "custom cost works for backward propagator"; EXPECT_EQ(container.solutions().front()->cost(), constant->cost) << "custom cost works for backward propagator";
} }
@ -268,7 +242,7 @@ TEST(CostTerm, PassThroughUsesCost) {
moveit::core::RobotModelPtr robot{ getModel() }; moveit::core::RobotModelPtr robot{ getModel() };
Standalone<stages::PassThrough> container(robot); Standalone<stages::PassThrough> container(robot);
auto stage{ std::make_unique<BackwardMockup>() }; auto stage{ std::make_unique<BackwardCostMockup>() };
auto constant{ std::make_shared<cost::Constant>(84.0) }; auto constant{ std::make_shared<cost::Constant>(84.0) };
stage->setCostTerm(constant); stage->setCostTerm(constant);
@ -283,7 +257,7 @@ TEST(CostTerm, PassThroughOverwritesCost) {
moveit::core::RobotModelPtr robot{ getModel() }; moveit::core::RobotModelPtr robot{ getModel() };
Standalone<stages::PassThrough> container(robot); Standalone<stages::PassThrough> container(robot);
auto stage{ std::make_unique<BackwardMockup>() }; auto stage{ std::make_unique<BackwardCostMockup>() };
auto constant_inner{ std::make_shared<cost::Constant>(84.0) }; auto constant_inner{ std::make_shared<cost::Constant>(84.0) };
stage->setCostTerm(constant_inner); stage->setCostTerm(constant_inner);
@ -300,7 +274,7 @@ TEST(CostTerm, PassThroughCanModifyCost) {
moveit::core::RobotModelPtr robot{ getModel() }; moveit::core::RobotModelPtr robot{ getModel() };
Standalone<stages::PassThrough> container(robot); Standalone<stages::PassThrough> container(robot);
auto stage{ std::make_unique<BackwardMockup>() }; auto stage{ std::make_unique<BackwardCostMockup>() };
auto constant{ std::make_shared<cost::Constant>(8.0) }; auto constant{ std::make_shared<cost::Constant>(8.0) };
stage->setCostTerm(constant); stage->setCostTerm(constant);
container.setCostTerm([](auto&& s) { return s.cost() * s.cost(); }); container.setCostTerm([](auto&& s) { return s.cost() * s.cost(); });
@ -315,18 +289,18 @@ TEST(CostTerm, CompositeSolutions) {
Standalone<SerialContainer> container{ getModel() }; Standalone<SerialContainer> container{ getModel() };
{ {
auto s1{ std::make_unique<ForwardMockup>() }; auto s1{ std::make_unique<ForwardCostMockup>() };
auto s2{ std::make_unique<ForwardMockup>() }; auto s2{ std::make_unique<ForwardCostMockup>() };
container.computeWithStages({ std::move(s1), std::move(s2) }); container.computeWithStages({ std::move(s1), std::move(s2) });
EXPECT_EQ(container.solutions().front()->cost(), 2 * STAGE_COST) << "by default stage costs are added"; EXPECT_EQ(container.solutions().front()->cost(), 2 * STAGE_COST) << "by default stage costs are added";
} }
{ {
auto s1{ std::make_unique<ForwardMockup>() }; auto s1{ std::make_unique<ForwardCostMockup>() };
auto constant{ std::make_shared<cost::Constant>(1.0) }; auto constant{ std::make_shared<cost::Constant>(1.0) };
s1->setCostTerm(constant); s1->setCostTerm(constant);
auto s2{ std::make_unique<ForwardMockup>() }; auto s2{ std::make_unique<ForwardCostMockup>() };
container.computeWithStages({ std::move(s1), std::move(s2) }); container.computeWithStages({ std::move(s1), std::move(s2) });
EXPECT_EQ(container.solutions().front()->cost(), STAGE_COST + constant->cost) EXPECT_EQ(container.solutions().front()->cost(), STAGE_COST + constant->cost)
@ -334,8 +308,8 @@ TEST(CostTerm, CompositeSolutions) {
} }
{ {
auto s1{ std::make_unique<ForwardMockup>() }; auto s1{ std::make_unique<ForwardCostMockup>() };
auto s2{ std::make_unique<ForwardMockup>() }; auto s2{ std::make_unique<ForwardCostMockup>() };
auto c1{ std::make_unique<SerialContainer>() }; auto c1{ std::make_unique<SerialContainer>() };
auto constant1{ std::make_shared<cost::Constant>(1.0) }; auto constant1{ std::make_shared<cost::Constant>(1.0) };
s1->setCostTerm(constant1); s1->setCostTerm(constant1);
@ -343,7 +317,7 @@ TEST(CostTerm, CompositeSolutions) {
c1->add(std::move(s1)); c1->add(std::move(s1));
c1->add(std::move(s2)); c1->add(std::move(s2));
auto s3{ std::make_unique<ForwardMockup>() }; auto s3{ std::make_unique<ForwardCostMockup>() };
auto constant2{ std::make_shared<cost::Constant>(9.0) }; auto constant2{ std::make_shared<cost::Constant>(9.0) };
s3->setCostTerm(constant2); s3->setCostTerm(constant2);
@ -356,15 +330,15 @@ TEST(CostTerm, CompositeSolutions) {
TEST(CostTerm, CompositeSolutionsContainerCost) { TEST(CostTerm, CompositeSolutionsContainerCost) {
Standalone<SerialContainer> container{ getModel() }; Standalone<SerialContainer> container{ getModel() };
auto s1{ std::make_unique<ForwardMockup>() }; auto s1{ std::make_unique<ForwardTrajectoryMockup>() };
auto s1_ptr{ s1.get() }; auto s1_ptr{ s1.get() };
auto s2{ std::make_unique<ForwardMockup>() }; auto s2{ std::make_unique<ForwardTrajectoryMockup>() };
auto c1{ std::make_unique<SerialContainer>() }; auto c1{ std::make_unique<SerialContainer>() };
c1->add(std::move(s1)); c1->add(std::move(s1));
c1->add(std::move(s2)); c1->add(std::move(s2));
auto s3{ std::make_unique<ForwardMockup>() }; auto s3{ std::make_unique<ForwardTrajectoryMockup>() };
container.setCostTerm(std::make_unique<cost::TrajectoryDuration>()); container.setCostTerm(std::make_unique<cost::TrajectoryDuration>());
container.computeWithStages({ std::move(c1), std::move(s3) }); container.computeWithStages({ std::move(c1), std::move(s3) });

View File

@ -5,6 +5,7 @@
#include <moveit/planning_scene/planning_scene.h> #include <moveit/planning_scene/planning_scene.h>
#include <moveit/utils/robot_model_test_utils.h> #include <moveit/utils/robot_model_test_utils.h>
#include "stage_mockups.h"
#include "models.h" #include "models.h"
#include <list> #include <list>
#include <memory> #include <memory>
@ -13,105 +14,11 @@
using namespace moveit::task_constructor; using namespace moveit::task_constructor;
using namespace planning_scene; using namespace planning_scene;
MOVEIT_STRUCT_FORWARD(PredefinedCosts);
struct PredefinedCosts : CostTerm
{
mutable std::list<double> costs_; // list of costs to assign
mutable double cost_ = 0.0; // last assigned cost
bool finite_; // finite number of compute() attempts?
PredefinedCosts(bool finite, std::list<double>&& costs) : costs_(std::move(costs)), finite_(finite) {}
bool exhausted() const { return finite_ && costs_.empty(); }
double cost() const {
if (!costs_.empty()) {
cost_ = costs_.front();
costs_.pop_front();
}
return cost_;
}
double operator()(const SubTrajectory& /*s*/, std::string& /*comment*/) const override { return cost(); }
double operator()(const SolutionSequence& /*s*/, std::string& /*comment*/) const override { return cost(); }
double operator()(const WrappedSolution& /*s*/, std::string& /*comment*/) const override { return cost(); }
};
/** Generator creating solutions with given costs */
struct GeneratorMockup : Generator
{
PlanningScenePtr ps_;
PredefinedCosts costs_;
static unsigned int id_;
GeneratorMockup(std::initializer_list<double> costs = { 0.0 })
: Generator("GEN" + std::to_string(++id_)), costs_(true, costs) {}
void init(const moveit::core::RobotModelConstPtr& robot_model) override {
ps_.reset(new PlanningScene(robot_model));
Generator::init(robot_model);
}
bool canCompute() const override { return !costs_.exhausted(); }
void compute() override { spawn(InterfaceState(ps_), costs_.cost()); }
};
struct PropagatorMockup : public PropagatingEitherWay
{
PredefinedCosts costs_;
std::size_t solutions_per_compute_;
unsigned int calls_ = 0;
PropagatorMockup(std::initializer_list<double> 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 {
++calls_;
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 {
++calls_;
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));
}
}
};
struct ForwardMockup : public PropagatorMockup
{
static unsigned int id_;
ForwardMockup(std::initializer_list<double> costs = { 0.0 }, std::size_t solutions_per_compute = 1)
: PropagatorMockup(costs, solutions_per_compute) {
restrictDirection(FORWARD);
setName("FW" + std::to_string(++id_));
}
};
struct BackwardMockup : public PropagatorMockup
{
static unsigned int id_;
BackwardMockup(std::initializer_list<double> costs = { 0.0 }) : PropagatorMockup(costs) {
restrictDirection(BACKWARD);
setName("BW" + std::to_string(++id_));
}
};
/* Forward propagator, contributing no solutions at all */
struct ForwardDummy : PropagatingForward
{
using PropagatingForward::PropagatingForward;
void computeForward(const InterfaceState& /*from*/) override {}
};
/* Connect creating solutions with given costs */ /* Connect creating solutions with given costs */
struct Connect : stages::Connect struct Connect : stages::Connect
{ {
PlanningScenePtr ps_;
PredefinedCostsPtr costs_; PredefinedCostsPtr costs_;
unsigned int calls_ = 0; unsigned int calls_{ 0 };
static unsigned int id_; static unsigned int id_;
static GroupPlannerVector getPlanners() { static GroupPlannerVector getPlanners() {
@ -119,9 +26,9 @@ struct Connect : stages::Connect
return { { "group", planner }, { "eef_group", planner } }; return { { "group", planner }, { "eef_group", planner } };
} }
Connect(std::initializer_list<double> costs = {}, bool enforce_sequential = false) Connect(std::initializer_list<double> costs = { 0.0 }, bool enforce_sequential = false)
: stages::Connect("CON" + std::to_string(++id_), getPlanners()) { : stages::Connect("CON" + std::to_string(++id_), getPlanners()) {
costs_ = std::make_shared<PredefinedCosts>(false, costs); costs_ = std::make_shared<PredefinedCosts>(costs, false);
setCostTerm(costs_); setCostTerm(costs_);
if (enforce_sequential) if (enforce_sequential)
setProperty("merge_mode", SEQUENTIAL); setProperty("merge_mode", SEQUENTIAL);
@ -133,25 +40,17 @@ struct Connect : stages::Connect
}; };
constexpr double INF = std::numeric_limits<double>::infinity(); constexpr double INF = std::numeric_limits<double>::infinity();
unsigned int GeneratorMockup::id_ = 0;
unsigned int ForwardMockup::id_ = 0;
unsigned int BackwardMockup::id_ = 0;
unsigned int Connect::id_ = 0; unsigned int Connect::id_ = 0;
struct TestBase : public testing::Test struct TestBase : public testing::Test
{ {
Task task; Task task;
TestBase() { TestBase() {
resetIds(); resetMockupIds();
Connect::id_ = 0;
task.setRobotModel(getModel()); task.setRobotModel(getModel());
} }
void resetIds() {
GeneratorMockup::id_ = 0;
ForwardMockup::id_ = 0;
BackwardMockup::id_ = 0;
Connect::id_ = 0;
}
template <typename C, typename S> template <typename C, typename S>
auto add(C& container, S* stage) -> S* { auto add(C& container, S* stage) -> S* {
container.add(Stage::pointer(stage)); container.add(Stage::pointer(stage));
@ -162,11 +61,11 @@ struct TestBase : public testing::Test
using ConnectConnect = TestBase; using ConnectConnect = TestBase;
// https://github.com/ros-planning/moveit_task_constructor/issues/182 // https://github.com/ros-planning/moveit_task_constructor/issues/182
TEST_F(ConnectConnect, SuccSucc) { TEST_F(ConnectConnect, SuccSucc) {
add(task, new GeneratorMockup({ 1, 2, 3 })); add(task, new GeneratorMockup({ 1.0, 2.0, 3.0 }));
add(task, new Connect()); add(task, new Connect());
add(task, new GeneratorMockup({ 10, 20 })); add(task, new GeneratorMockup({ 10.0, 20.0 }));
add(task, new Connect()); add(task, new Connect());
add(task, new GeneratorMockup()); add(task, new GeneratorMockup({ 0.0 }));
EXPECT_TRUE(task.plan()); EXPECT_TRUE(task.plan());
ASSERT_EQ(task.solutions().size(), 3u * 2u); ASSERT_EQ(task.solutions().size(), 3u * 2u);
@ -185,7 +84,7 @@ TEST_F(ConnectConnect, FailSucc) {
add(task, new GeneratorMockup()); add(task, new GeneratorMockup());
add(task, new Connect()); add(task, new Connect());
add(task, new GeneratorMockup()); add(task, new GeneratorMockup());
add(task, new ForwardDummy()); add(task, new ForwardMockup(PredefinedCosts::constant(0.0), 0));
EXPECT_FALSE(task.plan()); EXPECT_FALSE(task.plan());
} }
@ -199,7 +98,7 @@ TEST_F(Pruning, PropagatorFailure) {
EXPECT_FALSE(task.plan()); EXPECT_FALSE(task.plan());
ASSERT_EQ(task.solutions().size(), 0u); ASSERT_EQ(task.solutions().size(), 0u);
// ForwardMockup fails, so the backward stage should never compute // ForwardMockup fails, so the backward stage should never compute
EXPECT_EQ(back->calls_, 0u); EXPECT_EQ(back->runs_, 0u);
} }
TEST_F(Pruning, PruningMultiForward) { TEST_F(Pruning, PruningMultiForward) {
@ -207,7 +106,7 @@ TEST_F(Pruning, PruningMultiForward) {
add(task, new BackwardMockup()); add(task, new BackwardMockup());
add(task, new GeneratorMockup()); add(task, new GeneratorMockup());
// spawn two solutions for the only incoming state // spawn two solutions for the only incoming state
add(task, new ForwardMockup({ 0, 0 }, 2)); add(task, new ForwardMockup(PredefinedCosts{ { 0.0, 0.0 } }, 2));
// fail to extend the second solution // fail to extend the second solution
add(task, new ForwardMockup({ 0, INF })); add(task, new ForwardMockup({ 0, INF }));
@ -288,7 +187,7 @@ TEST_F(Pruning, PropagateFromContainerPull) {
EXPECT_FALSE(task.plan()); EXPECT_FALSE(task.plan());
// the failure inside the container should prune computing of back // the failure inside the container should prune computing of back
EXPECT_EQ(back->calls_, 0u); EXPECT_EQ(back->runs_, 0u);
} }
TEST_F(Pruning, PropagateFromContainerPush) { TEST_F(Pruning, PropagateFromContainerPush) {
@ -318,5 +217,5 @@ TEST_F(Pruning, PropagateFromParallelContainerMultiplePaths) {
EXPECT_TRUE(task.plan()); EXPECT_TRUE(task.plan());
// the failure in one branch of Alternatives must not prune computing back // the failure in one branch of Alternatives must not prune computing back
EXPECT_EQ(back->calls_, 1u); EXPECT_EQ(back->runs_, 1u);
} }

View File

@ -6,52 +6,36 @@
#include <moveit/planning_scene/planning_scene.h> #include <moveit/planning_scene/planning_scene.h>
#include <geometry_msgs/PoseStamped.h> #include <geometry_msgs/PoseStamped.h>
#include "stage_mockups.h"
#include <ros/console.h> #include <ros/console.h>
#include <gtest/gtest.h> #include <gtest/gtest.h>
using namespace moveit::task_constructor; using namespace moveit::task_constructor;
using namespace planning_scene; using namespace planning_scene;
class GeneratorMockup : public Generator struct StandaloneGeneratorMockup : public GeneratorMockup
{ {
PlanningScenePtr ps;
InterfacePtr prev; InterfacePtr prev;
InterfacePtr next; InterfacePtr next;
public: StandaloneGeneratorMockup(std::initializer_list<double>&& costs)
GeneratorMockup() : Generator("generator") { : StandaloneGeneratorMockup{ PredefinedCosts{ std::move(costs), true } } {}
StandaloneGeneratorMockup(PredefinedCosts&& costs = PredefinedCosts{ { 0.0 }, true })
: GeneratorMockup{ std::move(costs) } {
prev.reset(new Interface); prev.reset(new Interface);
next.reset(new Interface); next.reset(new Interface);
pimpl()->setPrevEnds(prev); pimpl()->setPrevEnds(prev);
pimpl()->setNextStarts(next); pimpl()->setNextStarts(next);
} }
void init(const moveit::core::RobotModelConstPtr& robot_model) override {
ps.reset((new PlanningScene(robot_model)));
Generator::init(robot_model);
}
bool canCompute() const override { return true; }
void compute() override {
InterfaceState state(ps);
state.properties().set("target_pose", geometry_msgs::PoseStamped());
spawn(std::move(state), 0.0);
}
};
class ConnectMockup : public Connecting
{
public:
using Connecting::compatible;
void compute(const InterfaceState& from, const InterfaceState& to) override {}
}; };
TEST(Stage, registerCallbacks) { TEST(Stage, registerCallbacks) {
GeneratorMockup g; StandaloneGeneratorMockup g{ PredefinedCosts::constant(0.0) };
g.init(getModel()); g.init(getModel());
uint called = 0; uint called = 0;
auto cb = [&called](const SolutionBase& s) { auto cb = [&called](const SolutionBase& /* s */) {
++called; ++called;
return true; return true;
}; };