From 74d33c4ec0bbdb17c02870dd8e29f04d27cd4fa6 Mon Sep 17 00:00:00 2001 From: v4hn Date: Sun, 4 Jul 2021 23:30:49 +0200 Subject: [PATCH] Merge different mockup implementations MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Co-authored-by: Jascha Kühn <57101356+j-kuehn@users.noreply.github.com> --- core/test/CMakeLists.txt | 2 +- core/test/stage_mockups.cpp | 125 +++++++++++++++++++++++++ core/test/stage_mockups.h | 117 ++++++++++++++++++++++++ core/test/test_container.cpp | 146 +++++++----------------------- core/test/test_cost_terms.cpp | 166 ++++++++++++++-------------------- core/test/test_serial.cpp | 129 +++----------------------- core/test/test_stage.cpp | 34 ++----- 7 files changed, 369 insertions(+), 350 deletions(-) create mode 100644 core/test/stage_mockups.cpp create mode 100644 core/test/stage_mockups.h diff --git a/core/test/CMakeLists.txt b/core/test/CMakeLists.txt index 99836d13..3d7777c4 100644 --- a/core/test/CMakeLists.txt +++ b/core/test/CMakeLists.txt @@ -6,7 +6,7 @@ if (CATKIN_ENABLE_TESTING) 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}) catkin_add_gtest(${PROJECT_NAME}-test-stage test_stage.cpp) diff --git a/core/test/stage_mockups.cpp b/core/test/stage_mockups.cpp new file mode 100644 index 00000000..6f3ddd5b --- /dev/null +++ b/core/test/stage_mockups.cpp @@ -0,0 +1,125 @@ +#include +#include + +#include "stage_mockups.h" + +#include + +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&& 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() }; + 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 diff --git a/core/test/stage_mockups.h b/core/test/stage_mockups.h new file mode 100644 index 00000000..fdab8afa --- /dev/null +++ b/core/test/stage_mockups.h @@ -0,0 +1,117 @@ +#pragma once + +#include +#include + +namespace moveit { +namespace task_constructor { + +MOVEIT_STRUCT_FORWARD(PredefinedCosts); +struct PredefinedCosts : CostTerm +{ + mutable std::list costs_; // list of costs to assign + mutable bool finite_; + + PredefinedCosts(std::list&& costs, bool finite = true); + + static PredefinedCosts constant(double c) { return PredefinedCosts{ std::list{ c }, false }; } + static PredefinedCosts single(double c) { return PredefinedCosts{ std::list{ 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{ 0.0 }, true }); + GeneratorMockup(std::initializer_list costs) + : GeneratorMockup{ PredefinedCosts{ std::list{ 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 costs) + : MonitoringGeneratorMockup{ monitored, PredefinedCosts{ std::list{ 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 costs) + : ConnectMockup{ PredefinedCosts{ std::list{ 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 costs) + : PropagatorMockup{ PredefinedCosts{ std::list{ 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 costs) + : ForwardMockup{ PredefinedCosts{ std::list{ 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 costs) + : BackwardMockup{ PredefinedCosts{ std::list{ costs }, true } } {} +}; + +// reset ids of all Mockup types (used to generate unique stage names) +void resetMockupIds(); + +} // namespace task_constructor +} // namespace moveit diff --git a/core/test/test_container.cpp b/core/test/test_container.cpp index 50f91936..168f5ac0 100644 --- a/core/test/test_container.cpp +++ b/core/test/test_container.cpp @@ -4,8 +4,10 @@ #include #include +#include "stage_mockups.h" #include "models.h" #include "gtest_value_printers.h" + #include #include #include @@ -13,101 +15,6 @@ 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(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()); - } -}; - enum StageType { GEN, @@ -116,23 +23,23 @@ enum StageType ANY, CONN }; -void append(ContainerBase& c, const std::initializer_list& types, int runs = 0) { +void append(ContainerBase& c, const std::initializer_list& types) { for (StageType t : types) { switch (t) { case GEN: - c.add(std::make_unique(runs)); + c.add(std::make_unique()); break; case FW: - c.add(std::make_unique(runs)); + c.add(std::make_unique()); break; case BW: - c.add(std::make_unique(runs)); + c.add(std::make_unique()); break; case ANY: - c.add(std::make_unique(runs, runs)); + c.add(std::make_unique()); break; case CONN: - c.add(std::make_unique(runs)); + c.add(std::make_unique()); break; } } @@ -215,7 +122,7 @@ protected: } void initContainer(const std::initializer_list& types = {}) { container.clear(); - MOCK_ID = 0; + resetMockupIds(); append(container, types); } @@ -505,7 +412,7 @@ TEST_F(SerialTest, interface_detection) { } template -Stage::pointer make(const std::string& name, const std::initializer_list& children, int runs = 0) { +Stage::pointer make(const std::string& name, const std::initializer_list& children) { auto container = new T(name); append(*container, children); return Stage::pointer(container); @@ -669,13 +576,13 @@ TEST_F(ParallelTest, init_any) { } TEST(Task, move) { - MOCK_ID = 0; + resetMockupIds(); Task t1("foo"); t1.add(std::make_unique()); t1.add(std::make_unique()); EXPECT_EQ(t1.stages()->numChildren(), 2u); - MOCK_ID = 0; + resetMockupIds(); Task t2 = std::move(t1); EXPECT_EQ(t2.stages()->numChildren(), 2u); EXPECT_EQ(t1.stages()->numChildren(), 0u); // NOLINT(clang-analyzer-cplusplus.Move) @@ -692,13 +599,13 @@ TEST(Task, reuse) { t.setRobotModel(robot_model); auto configure = [](Task& t) { - MOCK_ID = 0; + resetMockupIds(); auto ref = new stages::FixedState("fixed"); auto scene = std::make_shared(t.getRobotModel()); ref->setState(scene); t.add(Stage::pointer(ref)); - t.add(std::make_unique(2)); + t.add(std::make_unique()); t.add(std::make_unique(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) { - MOCK_ID = 0; + resetMockupIds(); Task t; t.setRobotModel(getModel()); auto timeout = std::chrono::milliseconds(10); - t.add(std::make_unique(100)); // allow up to 100 solutions spawned + t.add(std::make_unique(PredefinedCosts::constant(0.0))); t.add(std::make_unique(timeout)); // no timeout, but limited number of solutions @@ -751,15 +671,15 @@ TEST(Task, timeout) { } TEST(Fallback, failing) { - MOCK_ID = 0; + resetMockupIds(); Task t; t.setRobotModel(getModel()); - t.add(std::make_unique()); + t.add(std::make_unique(PredefinedCosts::single(0.0))); auto fallback = std::make_unique("Fallbacks"); - fallback->add(std::make_unique()); - fallback->add(std::make_unique()); + fallback->add(std::make_unique(PredefinedCosts::constant(0.0), 0)); + fallback->add(std::make_unique(PredefinedCosts::constant(0.0), 0)); t.add(std::move(fallback)); EXPECT_FALSE(t.plan()); diff --git a/core/test/test_cost_terms.cpp b/core/test/test_cost_terms.cpp index 2f1cd1af..a610ca04 100644 --- a/core/test/test_cost_terms.cpp +++ b/core/test/test_cost_terms.cpp @@ -11,88 +11,58 @@ #include +#include "stage_mockups.h" + using namespace moveit::task_constructor; 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 }; -class GeneratorMockup : public Generator +struct GeneratorCostMockup : public GeneratorMockup { - PlanningScenePtr ps; - 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); - } + GeneratorCostMockup() : GeneratorMockup{ PredefinedCosts{ { STAGE_COST }, true } } {} }; -class ConnectMockup : public Connecting +struct ConnectCostMockup : public ConnectMockup { - using Connecting::Connecting; - - void compute(const InterfaceState& from, const InterfaceState& to) override { - auto solution{ std::make_shared() }; - solution->setCost(STAGE_COST); - connect(from, to, solution); - } + ConnectCostMockup() : ConnectMockup{ PredefinedCosts::constant(STAGE_COST) } {} }; -class ForwardMockup : public PropagatingForward +struct ForwardCostMockup : public ForwardMockup { - PlanningScenePtr ps; + ForwardCostMockup() : ForwardMockup{ PredefinedCosts::constant(STAGE_COST) } {} +}; -public: - using PropagatingForward::PropagatingForward; +struct BackwardCostMockup : public BackwardMockup +{ + BackwardCostMockup() : BackwardMockup{ PredefinedCosts::constant(STAGE_COST) } {} +}; - void init(const moveit::core::RobotModelConstPtr& robot_model) override { - ps.reset(new PlanningScene(robot_model)); - PropagatingForward::init(robot_model); - } +struct ForwardTrajectoryMockup : public ForwardMockup +{ + 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 { - SubTrajectory solution; - auto traj{ std::make_shared(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); + ++runs_; - 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(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 -{ - 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)); - }; + sendForward(from, std::move(to), std::move(solution)); + } + } }; template @@ -105,7 +75,9 @@ public: InterfaceStatePtr state_start, state_end; Standalone(const moveit::core::RobotModelConstPtr& robot) - : T(), robot(robot), dummy(std::make_shared()), ps(new planning_scene::PlanningScene(robot)) {} + : T(), robot(robot), dummy(std::make_shared()), ps(new planning_scene::PlanningScene(robot)) { + resetMockupIds(); + } // reset and prepare for a compute step void prepare() { @@ -173,7 +145,7 @@ TEST(CostTerm, SolutionConnected) { const moveit::core::RobotModelConstPtr robot{ getModel() }; Standalone container(robot); - auto stage{ std::make_unique() }; + auto stage{ std::make_unique() }; // custom CostTerm to verify SubTrajectory is hooked up to its states & creator class VerifySolutionCostTerm : public TrajectoryCostTerm @@ -193,38 +165,40 @@ TEST(CostTerm, SolutionConnected) { const_cast(container_.pimpl())->internalToExternalMap().at(s.end())) << "SubTrajectory is not connected to its expected end InterfaceState"; EXPECT_EQ(s.creator(), creator_); - return 1.0; + return TERM_COST; } }; stage->setCostTerm(std::make_unique(container, &*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) { const moveit::core::RobotModelConstPtr robot{ getModel() }; Standalone container(robot); - auto stage{ std::make_unique() }; - 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(); + auto stage{ std::make_unique() }; + 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(); stage->setCostTerm([](auto&& /*s*/, auto&& /*comment*/) { return 1.0; }); container.computeWithStages({ std::move(stage) }); EXPECT_EQ(container.solutions().front()->cost(), 1.0) << "can use full lambda signature"; - stage = std::make_unique(); - stage->setCostTerm([](auto&& /*s*/, auto&& comment) { - comment = "I want the user to see this"; + const std::string cost_term_comment{ "I want the user to see this" }; + stage = std::make_unique(); + stage->setCostTerm([&](auto&& /*s*/, auto&& comment) { + comment = cost_term_comment; return 1.0; }); container.computeWithStages({ std::move(stage) }); auto sol = std::dynamic_pointer_cast(container.solutions().front()); 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) { @@ -232,14 +206,14 @@ TEST(CostTerm, CostOverwrite) { Standalone container(robot); - container.computeWithStages({ std::make_unique() }); + container.computeWithStages({ std::make_unique() }); EXPECT_EQ(container.solutions().front()->cost(), STAGE_COST) << "return cost of stage by default"; - container.computeWithStageCost({ std::make_unique() }, nullptr); + container.computeWithStageCost({ std::make_unique() }, nullptr); EXPECT_EQ(container.solutions().front()->cost(), STAGE_COST) << "nullptr cost term forwards cost"; auto constant_cost{ std::make_shared(1.0) }; - container.computeWithStageCost({ std::make_unique() }, constant_cost); + container.computeWithStageCost({ std::make_unique() }, constant_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(1.0) }; // already tested above - // cont.computeWithStageCost(std::make_unique(), constant); + // cont.computeWithStageCost(std::make_unique(), constant); // EXPECT_EQ(cont.solutions().front()->cost(), constant.cost); - container.computeWithStageCost({ std::make_unique() }, constant); + container.computeWithStageCost({ std::make_unique() }, constant); EXPECT_EQ(container.solutions().front()->cost(), constant->cost) << "custom cost works for connect"; - container.computeWithStageCost({ std::make_unique() }, constant); + container.computeWithStageCost({ std::make_unique() }, constant); EXPECT_EQ(container.solutions().front()->cost(), constant->cost) << "custom cost works for forward propagator"; - container.computeWithStageCost({ std::make_unique() }, constant); + container.computeWithStageCost({ std::make_unique() }, constant); 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() }; Standalone container(robot); - auto stage{ std::make_unique() }; + auto stage{ std::make_unique() }; auto constant{ std::make_shared(84.0) }; stage->setCostTerm(constant); @@ -283,7 +257,7 @@ TEST(CostTerm, PassThroughOverwritesCost) { moveit::core::RobotModelPtr robot{ getModel() }; Standalone container(robot); - auto stage{ std::make_unique() }; + auto stage{ std::make_unique() }; auto constant_inner{ std::make_shared(84.0) }; stage->setCostTerm(constant_inner); @@ -300,7 +274,7 @@ TEST(CostTerm, PassThroughCanModifyCost) { moveit::core::RobotModelPtr robot{ getModel() }; Standalone container(robot); - auto stage{ std::make_unique() }; + auto stage{ std::make_unique() }; auto constant{ std::make_shared(8.0) }; stage->setCostTerm(constant); container.setCostTerm([](auto&& s) { return s.cost() * s.cost(); }); @@ -315,18 +289,18 @@ TEST(CostTerm, CompositeSolutions) { Standalone container{ getModel() }; { - auto s1{ std::make_unique() }; - auto s2{ std::make_unique() }; + auto s1{ std::make_unique() }; + auto s2{ std::make_unique() }; container.computeWithStages({ std::move(s1), std::move(s2) }); EXPECT_EQ(container.solutions().front()->cost(), 2 * STAGE_COST) << "by default stage costs are added"; } { - auto s1{ std::make_unique() }; + auto s1{ std::make_unique() }; auto constant{ std::make_shared(1.0) }; s1->setCostTerm(constant); - auto s2{ std::make_unique() }; + auto s2{ std::make_unique() }; container.computeWithStages({ std::move(s1), std::move(s2) }); EXPECT_EQ(container.solutions().front()->cost(), STAGE_COST + constant->cost) @@ -334,8 +308,8 @@ TEST(CostTerm, CompositeSolutions) { } { - auto s1{ std::make_unique() }; - auto s2{ std::make_unique() }; + auto s1{ std::make_unique() }; + auto s2{ std::make_unique() }; auto c1{ std::make_unique() }; auto constant1{ std::make_shared(1.0) }; s1->setCostTerm(constant1); @@ -343,7 +317,7 @@ TEST(CostTerm, CompositeSolutions) { c1->add(std::move(s1)); c1->add(std::move(s2)); - auto s3{ std::make_unique() }; + auto s3{ std::make_unique() }; auto constant2{ std::make_shared(9.0) }; s3->setCostTerm(constant2); @@ -356,15 +330,15 @@ TEST(CostTerm, CompositeSolutions) { TEST(CostTerm, CompositeSolutionsContainerCost) { Standalone container{ getModel() }; - auto s1{ std::make_unique() }; + auto s1{ std::make_unique() }; auto s1_ptr{ s1.get() }; - auto s2{ std::make_unique() }; + auto s2{ std::make_unique() }; auto c1{ std::make_unique() }; c1->add(std::move(s1)); c1->add(std::move(s2)); - auto s3{ std::make_unique() }; + auto s3{ std::make_unique() }; container.setCostTerm(std::make_unique()); container.computeWithStages({ std::move(c1), std::move(s3) }); diff --git a/core/test/test_serial.cpp b/core/test/test_serial.cpp index 424292c1..8112d40a 100644 --- a/core/test/test_serial.cpp +++ b/core/test/test_serial.cpp @@ -5,6 +5,7 @@ #include #include +#include "stage_mockups.h" #include "models.h" #include #include @@ -13,105 +14,11 @@ using namespace moveit::task_constructor; using namespace planning_scene; -MOVEIT_STRUCT_FORWARD(PredefinedCosts); -struct PredefinedCosts : CostTerm -{ - mutable std::list 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&& 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 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 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 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 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 */ struct Connect : stages::Connect { - PlanningScenePtr ps_; PredefinedCostsPtr costs_; - unsigned int calls_ = 0; + unsigned int calls_{ 0 }; static unsigned int id_; static GroupPlannerVector getPlanners() { @@ -119,9 +26,9 @@ struct Connect : stages::Connect return { { "group", planner }, { "eef_group", planner } }; } - Connect(std::initializer_list costs = {}, bool enforce_sequential = false) + Connect(std::initializer_list costs = { 0.0 }, bool enforce_sequential = false) : stages::Connect("CON" + std::to_string(++id_), getPlanners()) { - costs_ = std::make_shared(false, costs); + costs_ = std::make_shared(costs, false); setCostTerm(costs_); if (enforce_sequential) setProperty("merge_mode", SEQUENTIAL); @@ -133,25 +40,17 @@ struct Connect : stages::Connect }; constexpr double INF = std::numeric_limits::infinity(); -unsigned int GeneratorMockup::id_ = 0; -unsigned int ForwardMockup::id_ = 0; -unsigned int BackwardMockup::id_ = 0; unsigned int Connect::id_ = 0; struct TestBase : public testing::Test { Task task; TestBase() { - resetIds(); + resetMockupIds(); + Connect::id_ = 0; task.setRobotModel(getModel()); } - void resetIds() { - GeneratorMockup::id_ = 0; - ForwardMockup::id_ = 0; - BackwardMockup::id_ = 0; - Connect::id_ = 0; - } template auto add(C& container, S* stage) -> S* { container.add(Stage::pointer(stage)); @@ -162,11 +61,11 @@ struct TestBase : public testing::Test using ConnectConnect = TestBase; // https://github.com/ros-planning/moveit_task_constructor/issues/182 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 GeneratorMockup({ 10, 20 })); + add(task, new GeneratorMockup({ 10.0, 20.0 })); add(task, new Connect()); - add(task, new GeneratorMockup()); + add(task, new GeneratorMockup({ 0.0 })); EXPECT_TRUE(task.plan()); ASSERT_EQ(task.solutions().size(), 3u * 2u); @@ -185,7 +84,7 @@ TEST_F(ConnectConnect, FailSucc) { add(task, new GeneratorMockup()); add(task, new Connect()); add(task, new GeneratorMockup()); - add(task, new ForwardDummy()); + add(task, new ForwardMockup(PredefinedCosts::constant(0.0), 0)); EXPECT_FALSE(task.plan()); } @@ -199,7 +98,7 @@ TEST_F(Pruning, PropagatorFailure) { EXPECT_FALSE(task.plan()); ASSERT_EQ(task.solutions().size(), 0u); // ForwardMockup fails, so the backward stage should never compute - EXPECT_EQ(back->calls_, 0u); + EXPECT_EQ(back->runs_, 0u); } TEST_F(Pruning, PruningMultiForward) { @@ -207,7 +106,7 @@ TEST_F(Pruning, PruningMultiForward) { add(task, new BackwardMockup()); add(task, new GeneratorMockup()); // 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 add(task, new ForwardMockup({ 0, INF })); @@ -288,7 +187,7 @@ TEST_F(Pruning, PropagateFromContainerPull) { EXPECT_FALSE(task.plan()); // the failure inside the container should prune computing of back - EXPECT_EQ(back->calls_, 0u); + EXPECT_EQ(back->runs_, 0u); } TEST_F(Pruning, PropagateFromContainerPush) { @@ -318,5 +217,5 @@ TEST_F(Pruning, PropagateFromParallelContainerMultiplePaths) { EXPECT_TRUE(task.plan()); // the failure in one branch of Alternatives must not prune computing back - EXPECT_EQ(back->calls_, 1u); + EXPECT_EQ(back->runs_, 1u); } diff --git a/core/test/test_stage.cpp b/core/test/test_stage.cpp index 2dd7b2df..180bc70d 100644 --- a/core/test/test_stage.cpp +++ b/core/test/test_stage.cpp @@ -6,52 +6,36 @@ #include #include +#include "stage_mockups.h" #include #include using namespace moveit::task_constructor; using namespace planning_scene; -class GeneratorMockup : public Generator +struct StandaloneGeneratorMockup : public GeneratorMockup { - PlanningScenePtr ps; InterfacePtr prev; InterfacePtr next; -public: - GeneratorMockup() : Generator("generator") { + StandaloneGeneratorMockup(std::initializer_list&& costs) + : StandaloneGeneratorMockup{ PredefinedCosts{ std::move(costs), true } } {} + + StandaloneGeneratorMockup(PredefinedCosts&& costs = PredefinedCosts{ { 0.0 }, true }) + : GeneratorMockup{ std::move(costs) } { 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); - 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) { - GeneratorMockup g; + StandaloneGeneratorMockup g{ PredefinedCosts::constant(0.0) }; g.init(getModel()); uint called = 0; - auto cb = [&called](const SolutionBase& s) { + auto cb = [&called](const SolutionBase& /* s */) { ++called; return true; };