diff --git a/core/test/test_cost_terms.cpp b/core/test/test_cost_terms.cpp index 0598ca31..0a3d24af 100644 --- a/core/test/test_cost_terms.cpp +++ b/core/test/test_cost_terms.cpp @@ -16,6 +16,8 @@ using namespace planning_scene; const double STAGE_COST{ 7.0 }; +const double TRAJECTORY_DURATION{ 9.0 }; + class GeneratorMockup : public Generator { PlanningScenePtr ps; @@ -30,7 +32,10 @@ public: pimpl()->setNextStarts(next); } - void init(const moveit::core::RobotModelConstPtr& robot_model) override { ps.reset(new PlanningScene(robot_model)); } + 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; } @@ -39,7 +44,6 @@ public: spawn(std::move(state), STAGE_COST); } }; -MOVEIT_CLASS_FORWARD(GeneratorMockup); class ConnectMockup : public Connecting { @@ -51,21 +55,32 @@ class ConnectMockup : public Connecting connect(from, to, solution); } }; -MOVEIT_CLASS_FORWARD(GeneratorMockup); class ForwardMockup : public PropagatingForward { + PlanningScenePtr ps; + +public: using PropagatingForward::PropagatingForward; + void init(const moveit::core::RobotModelConstPtr& robot_model) override { + ps.reset(new PlanningScene(robot_model)); + PropagatingForward::init(robot_model); + } + 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); sendForward(from, std::move(to), std::move(solution)); }; }; -MOVEIT_CLASS_FORWARD(GeneratorMockup); class BackwardMockup : public PropagatingBackward { @@ -79,9 +94,9 @@ class BackwardMockup : public PropagatingBackward sendBackward(std::move(from), to, std::move(solution)); }; }; -MOVEIT_CLASS_FORWARD(GeneratorMockup); -class SerialContainerStandalone : public SerialContainer +template +class Standalone : public T { moveit::core::RobotModelConstPtr robot; InterfacePtr dummy; @@ -89,15 +104,13 @@ class SerialContainerStandalone : public SerialContainer InterfaceStatePtr state_start, state_end; public: - SerialContainerStandalone(const moveit::core::RobotModelConstPtr& robot) - : SerialContainer() - , robot(robot) - , dummy(std::make_shared()) - , ps(new planning_scene::PlanningScene(robot)) {} + Standalone(const moveit::core::RobotModelConstPtr& robot) + : T(), robot(robot), dummy(std::make_shared()), ps(new planning_scene::PlanningScene(robot)) {} // reset and prepare for a compute step void prepare() { - reset(); + auto impl{ this->pimpl() }; + this->reset(); state_start.reset(new InterfaceState(ps, InterfaceState::Priority(1, 0.0))); state_end.reset(new InterfaceState(ps, InterfaceState::Priority(1, 0.0))); @@ -106,46 +119,55 @@ public: Stage* s{ this }; ContainerBase* b; - while ((b = dynamic_cast(s))) { + while ((b = dynamic_cast(s))) s = &(*b->pimpl()->children().front()); - } InterfaceFlags start_flags{ s->pimpl()->requiredInterface() & START_IF_MASK }; s = this; - while ((b = dynamic_cast(s))) { + while ((b = dynamic_cast(s))) s = &(*b->pimpl()->children().back()); - } InterfaceFlags end_flags{ s->pimpl()->requiredInterface() & END_IF_MASK }; InterfaceFlags flags{ start_flags | end_flags }; - pimpl()->setPrevEnds(flags & WRITES_PREV_END ? dummy : nullptr); - pimpl()->setNextStarts(flags & WRITES_NEXT_START ? dummy : nullptr); - SerialContainer::init(robot); - pimpl()->resolveInterface(flags); + impl->setPrevEnds(flags & WRITES_PREV_END ? dummy : nullptr); + impl->setNextStarts(flags & WRITES_NEXT_START ? dummy : nullptr); + T::init(robot); + impl->resolveInterface(flags); // feed interfaces as required for one computation if (flags & READS_START) { - pimpl()->starts()->add(*state_start); + impl->starts()->add(*state_start); } if (flags & READS_END) { - pimpl()->ends()->add(*state_end); + impl->ends()->add(*state_end); } } - void runCompute() { pimpl()->runCompute(); } + void runCompute() { this->pimpl()->runCompute(); } - void computeWithStage(StageUniquePtr&& stage) { - clear(); - add(std::move(stage)); + void computeWithStages(std::initializer_list stages) { + this->clear(); + + // initializer_list offers only const access (a known shortcoming in C++) + for (const auto& stage : stages) { + this->add(const_cast(stage)); + } prepare(); runCompute(); } - void computeWithStageCost(StageUniquePtr&& stage, const CostTerm& cost_term) { - stage->setCostTerm(cost_term); - computeWithStage(std::move(stage)); + void computeWithStageCost(std::initializer_list stages, const CostTerm& cost_term) { + this->setCostTerm(nullptr); + for (const auto& stage : stages) + const_cast(stage)->setCostTerm(cost_term); + computeWithStages(stages); + } + + void computeWithContainerCost(std::initializer_list stages, const CostTerm& cost_term) { + this->setCostTerm(cost_term); + computeWithStages(stages); } }; @@ -153,28 +175,24 @@ TEST(CostTerm, CostOverwrite) { ros::console::set_logger_level(ROSCONSOLE_ROOT_LOGGER_NAME, ros::console::levels::Fatal); const moveit::core::RobotModelConstPtr robot{ getModel() }; - SerialContainerStandalone container(robot); + Standalone container(robot); - // by default return the cost the stage declared - container.computeWithStage(std::make_unique()); - EXPECT_EQ(container.solutions().front()->cost(), STAGE_COST); + container.computeWithStages({ std::make_unique() }); + EXPECT_EQ(container.solutions().front()->cost(), STAGE_COST) << "return cost of stage by default"; - // nullptr is the explicit default - container.computeWithStageCost(std::make_unique(), nullptr); - EXPECT_EQ(container.solutions().front()->cost(), STAGE_COST); + container.computeWithStageCost({ std::make_unique() }, nullptr); + EXPECT_EQ(container.solutions().front()->cost(), STAGE_COST) << "nullptr cost term forwards cost"; - // custom cost overwrites the stage cost cost::Constant constant_cost{ 1.0 }; - container.computeWithStageCost(std::make_unique(), constant_cost); - EXPECT_EQ(container.solutions().front()->cost(), constant_cost.cost); + container.computeWithStageCost({ std::make_unique() }, constant_cost); + EXPECT_EQ(container.solutions().front()->cost(), constant_cost.cost) << "custom cost overwrites stage cost"; } -// test all primitive stage types TEST(CostTerm, StageTypes) { ros::console::set_logger_level(ROSCONSOLE_ROOT_LOGGER_NAME, ros::console::levels::Fatal); moveit::core::RobotModelPtr robot{ getModel() }; - SerialContainerStandalone container(robot); + Standalone container(robot); const cost::Constant constant{ 1.0 }; @@ -182,72 +200,127 @@ TEST(CostTerm, StageTypes) { // cont.computeWithStageCost(std::make_unique(), constant); // EXPECT_EQ(cont.solutions().front()->cost(), constant.cost); - container.computeWithStageCost(std::make_unique(), constant); - EXPECT_EQ(container.solutions().front()->cost(), constant.cost); + 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); - EXPECT_EQ(container.solutions().front()->cost(), constant.cost); + 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); - EXPECT_EQ(container.solutions().front()->cost(), constant.cost); + container.computeWithStageCost({ std::make_unique() }, constant); + EXPECT_EQ(container.solutions().front()->cost(), constant.cost) << "custom cost works for backward propagator"; } -// Forward uses cost from solution TEST(CostTerm, ForwardUsesCost) { + ros::console::set_logger_level(ROSCONSOLE_ROOT_LOGGER_NAME, ros::console::levels::Fatal); moveit::core::RobotModelPtr robot{ getModel() }; - SerialContainerStandalone container(robot); + Standalone container(robot); auto stage{ std::make_unique() }; cost::Constant constant_stage{ 84.0 }; stage->setCostTerm(constant_stage); - auto forward{ std::make_unique("forward", std::move(stage)) }; - auto* forward_ptr{ forward.get() }; - container.computeWithStage(std::move(forward)); + container.computeWithStages({ std::move(stage) }); - ASSERT_NE(nullptr, dynamic_cast(&(*forward_ptr->solutions().front()))); - - auto& wrapped{ dynamic_cast(*forward_ptr->solutions().front()) }; - EXPECT_EQ(wrapped.cost(), constant_stage.cost); - EXPECT_EQ(wrapped.wrapped()->cost(), constant_stage.cost); + auto& wrapped{ dynamic_cast(*container.solutions().front()) }; + EXPECT_EQ(wrapped.cost(), constant_stage.cost) << "Forward forwards children's cost"; + EXPECT_EQ(wrapped.wrapped()->cost(), constant_stage.cost) << "Child cost equals Forward cost"; } -// Forward can overwrite cost from solution TEST(CostTerm, ForwardOverwritesCost) { + ros::console::set_logger_level(ROSCONSOLE_ROOT_LOGGER_NAME, ros::console::levels::Fatal); moveit::core::RobotModelPtr robot{ getModel() }; - SerialContainerStandalone container(robot); + Standalone container(robot); auto stage{ std::make_unique() }; cost::Constant constant_stage{ 84.0 }; stage->setCostTerm(constant_stage); - auto forward{ std::make_unique("forward", std::move(stage)) }; - auto* forward_ptr{ forward.get() }; cost::Constant constant_forward{ 72.0 }; - forward->setCostTerm(constant_forward); + container.setCostTerm(constant_forward); - container.computeWithStage(std::move(forward)); - auto& wrapped{ dynamic_cast(*forward_ptr->solutions().front()) }; - EXPECT_EQ(wrapped.cost(), constant_forward.cost); - EXPECT_EQ(wrapped.wrapped()->cost(), constant_stage.cost); - - // Forward can modify cost from solution + container.computeWithStages({ std::move(stage) }); + auto& wrapped{ dynamic_cast(*container.solutions().front()) }; + EXPECT_EQ(wrapped.cost(), constant_forward.cost) << "Forward can apply custom cost"; + EXPECT_EQ(wrapped.wrapped()->cost(), constant_stage.cost) << "child's cost is not affected"; } -// Forward can modify cost from solution TEST(CostTerm, ForwardCanModifyCost) { + ros::console::set_logger_level(ROSCONSOLE_ROOT_LOGGER_NAME, ros::console::levels::Fatal); moveit::core::RobotModelPtr robot{ getModel() }; - SerialContainerStandalone container(robot); + Standalone container(robot); auto stage{ std::make_unique() }; cost::Constant constant{ 8.0 }; stage->setCostTerm(constant); - auto forward{ std::make_unique("forward", std::move(stage)) }; - auto* forward_ptr{ forward.get() }; - forward->setCostTerm([](auto&& s) { return s.cost() * s.cost(); }); + container.setCostTerm([](auto&& s) { return s.cost() * s.cost(); }); - container.computeWithStage(std::move(forward)); - auto& wrapped{ dynamic_cast(*forward_ptr->solutions().front()) }; - EXPECT_EQ(wrapped.cost(), constant.cost * constant.cost); - EXPECT_EQ(wrapped.wrapped()->cost(), constant.cost); + container.computeWithStages({ std::move(stage) }); + auto& wrapped{ dynamic_cast(*container.solutions().front()) }; + EXPECT_EQ(wrapped.cost(), constant.cost * constant.cost) << "Forward can compute cost based on child"; + EXPECT_EQ(wrapped.wrapped()->cost(), constant.cost) << "child's cost is not affected"; +} + +TEST(CostTerm, CompositeSolutions) { + ros::console::set_logger_level(ROSCONSOLE_ROOT_LOGGER_NAME, ros::console::levels::Fatal); + Standalone container{ getModel() }; + + { + 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() }; + cost::Constant constant{ 1.0 }; + s1->setCostTerm(constant); + auto s2{ std::make_unique() }; + + container.computeWithStages({ std::move(s1), std::move(s2) }); + EXPECT_EQ(container.solutions().front()->cost(), STAGE_COST + constant.cost) + << "mix of explicit and implicit cost terms works"; + } + + { + auto s1{ std::make_unique() }; + auto s2{ std::make_unique() }; + auto c1{ std::make_unique() }; + cost::Constant constant1{ 1.0 }; + s1->setCostTerm(constant1); + s2->setCostTerm(constant1); + c1->add(std::move(s1)); + c1->add(std::move(s2)); + + auto s3{ std::make_unique() }; + cost::Constant constant2{ 9.0 }; + s3->setCostTerm(constant2); + + container.computeWithStages({ std::move(c1), std::move(s3) }); + EXPECT_EQ(container.solutions().front()->cost(), 2 * constant1.cost + constant2.cost) + << "cost aggregation works across multiple levels"; + } +} + +TEST(CostTerm, CompositeSolutionsContainerCost) { + ros::console::set_logger_level(ROSCONSOLE_ROOT_LOGGER_NAME, ros::console::levels::Fatal); + Standalone container{ getModel() }; + + auto s1{ std::make_unique() }; + auto s1_ptr{ s1.get() }; + 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() }; + + container.setCostTerm(cost::TrajectoryDuration); + container.computeWithStages({ std::move(c1), std::move(s3) }); + EXPECT_EQ(container.solutions().front()->cost(), 3 * TRAJECTORY_DURATION) + << "container cost term overwrites stage costs"; + EXPECT_EQ(s1_ptr->solutions().front()->cost(), STAGE_COST) << "child cost is not affected"; +} }