From 05932ad5f16a40b9e6f2a3cf1968d15b7a58deb0 Mon Sep 17 00:00:00 2001 From: v4hn Date: Tue, 1 Sep 2020 14:43:25 +0200 Subject: [PATCH] enable implicit construction of CostTerm from lambdas without the need for explicit casts to the correct signature. --- .../moveit/task_constructor/cost_terms.h | 19 ++++++++++-- core/src/cost_terms.cpp | 21 +++++++------- core/test/test_cost_terms.cpp | 29 +++++++++++++++++-- 3 files changed, 53 insertions(+), 16 deletions(-) diff --git a/core/include/moveit/task_constructor/cost_terms.h b/core/include/moveit/task_constructor/cost_terms.h index 82c07920..f1dd680c 100644 --- a/core/include/moveit/task_constructor/cost_terms.h +++ b/core/include/moveit/task_constructor/cost_terms.h @@ -47,8 +47,15 @@ namespace task_constructor { class CostTerm { public: - CostTerm(std::function); - CostTerm(std::function); + using SubTrajectorySig = std::function; + using SubTrajectoryShortSig = std::function; + + // accept lambdas according to either signature above + template + CostTerm(const Term& t) : CostTerm{ decltype(sigMatcher(t)){ t } } {} + + CostTerm(const SubTrajectorySig&); + CostTerm(const SubTrajectoryShortSig&); CostTerm(std::nullptr_t); CostTerm(); @@ -72,10 +79,16 @@ public: Flags supports() const; protected: - CostTerm(std::function, Flags); + CostTerm(const std::function&, Flags); std::function term_; Flags supports_; + +private: + template + auto sigMatcher(const T& t) -> decltype(t(SubTrajectory{}), SubTrajectoryShortSig{}); + template + auto sigMatcher(const T& t) -> decltype(t(SubTrajectory{}, std::string{}), SubTrajectorySig{}); }; namespace cost { diff --git a/core/src/cost_terms.cpp b/core/src/cost_terms.cpp index 15a43bbe..612ff7b3 100644 --- a/core/src/cost_terms.cpp +++ b/core/src/cost_terms.cpp @@ -1,4 +1,4 @@ -/********************************************************************* +/********************************************************************* * Software License Agreement (BSD License) * * Copyright (c) 2020, Hamburg University @@ -53,15 +53,15 @@ CostTerm::CostTerm() : term_{ nullptr }, supports_{ SolutionType::NONE } {} CostTerm::CostTerm(std::nullptr_t) : CostTerm{} {} -CostTerm::CostTerm(std::function t) - : term_{ [t](auto&& s, auto&& c) { return t(static_cast(s), c); } } - , supports_{ SolutionType::TRAJECTORY } {} +CostTerm::CostTerm(const SubTrajectorySig& term) + : CostTerm{ [term](const SolutionBase& s, std::string& c) { return term(static_cast(s), c); }, + SolutionType::TRAJECTORY } {} -CostTerm::CostTerm(std::function t) - : term_{ [t](auto&& s, auto&&) { return t(static_cast(s)); } } - , supports_{ SolutionType::TRAJECTORY } {} +CostTerm::CostTerm(const SubTrajectoryShortSig& term) + : CostTerm{ [term](const SolutionBase& s, std::string&) { return term(static_cast(s)); }, + SolutionType::TRAJECTORY } {} -CostTerm::CostTerm(std::function term, Flags flags) +CostTerm::CostTerm(const std::function& term, Flags flags) : term_{ term }, supports_{ flags } {} CostTerm::operator bool() const { @@ -76,7 +76,7 @@ double CostTerm::operator()(const SolutionBase& s, std::string& comment) const { namespace cost { Constant::Constant(double c) - : cost{ c }, CostTerm{ [this](auto&&, auto&&) { return this->cost; }, SolutionType::ALL } {} + : CostTerm{ [this](auto&&, auto&&) { return this->cost; }, SolutionType::ALL }, cost{ c } {} namespace { @@ -100,8 +100,7 @@ TrajectoryDuration::TrajectoryDuration() : CostTerm{ [](auto&& s, auto&&) { return s.trajectory() ? s.trajectory()->getDuration() : 0.0; } } {} LinkMotion::LinkMotion(std::string link) - : CostTerm{ [this](const SubTrajectory& s, std::string& c) { return this->compute(s, c); } } - , link_name{ std::move(link) } {} + : CostTerm{ [this](auto&& s, auto&& c) { return this->compute(s, c); } }, link_name{ std::move(link) } {} double LinkMotion::compute(const SubTrajectory& s, std::string& comment) const { const auto& traj{ s.trajectory() }; diff --git a/core/test/test_cost_terms.cpp b/core/test/test_cost_terms.cpp index 9d09a868..80488f30 100644 --- a/core/test/test_cost_terms.cpp +++ b/core/test/test_cost_terms.cpp @@ -173,6 +173,32 @@ public: } }; +TEST(CostTerm, SetLambdaCostTerm) { + ros::console::set_logger_level(ROSCONSOLE_ROOT_LOGGER_NAME, ros::console::levels::Fatal); + const moveit::core::RobotModelConstPtr robot{ getModel() }; + + Standalone container(robot); + auto stage{ std::make_unique() }; + stage->setCostTerm([](auto&&) { 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(); + stage->setCostTerm([](auto&&, auto&&) { 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&&, auto&& comment) { + comment = "I want the user to see this"; + 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"; +} + TEST(CostTerm, CostOverwrite) { ros::console::set_logger_level(ROSCONSOLE_ROOT_LOGGER_NAME, ros::console::levels::Fatal); const moveit::core::RobotModelConstPtr robot{ getModel() }; @@ -254,8 +280,7 @@ TEST(CostTerm, ForwardCanModifyCost) { auto stage{ std::make_unique() }; cost::Constant constant{ 8.0 }; stage->setCostTerm(constant); - container.setCostTerm(static_cast>( - [](const SubTrajectory& s) { return s.cost() * s.cost(); })); + container.setCostTerm([](auto&& s) { return s.cost() * s.cost(); }); container.computeWithStages({ std::move(stage) }); auto& wrapped{ dynamic_cast(*container.solutions().front()) };