enable implicit construction of CostTerm from lambdas

without the need for explicit casts to the correct signature.
This commit is contained in:
v4hn 2020-09-01 14:43:25 +02:00
parent 034a155315
commit 05932ad5f1
3 changed files with 53 additions and 16 deletions

View File

@ -47,8 +47,15 @@ namespace task_constructor {
class CostTerm class CostTerm
{ {
public: public:
CostTerm(std::function<double(const SubTrajectory&)>); using SubTrajectorySig = std::function<double(const SubTrajectory&, std::string&)>;
CostTerm(std::function<double(const SubTrajectory&, std::string&)>); using SubTrajectoryShortSig = std::function<double(const SubTrajectory&)>;
// accept lambdas according to either signature above
template <typename Term>
CostTerm(const Term& t) : CostTerm{ decltype(sigMatcher(t)){ t } } {}
CostTerm(const SubTrajectorySig&);
CostTerm(const SubTrajectoryShortSig&);
CostTerm(std::nullptr_t); CostTerm(std::nullptr_t);
CostTerm(); CostTerm();
@ -72,10 +79,16 @@ public:
Flags<SolutionType> supports() const; Flags<SolutionType> supports() const;
protected: protected:
CostTerm(std::function<double(const SolutionBase&, std::string&)>, Flags<SolutionType>); CostTerm(const std::function<double(const SolutionBase&, std::string&)>&, Flags<SolutionType>);
std::function<double(const SolutionBase&, std::string&)> term_; std::function<double(const SolutionBase&, std::string&)> term_;
Flags<SolutionType> supports_; Flags<SolutionType> supports_;
private:
template <typename T>
auto sigMatcher(const T& t) -> decltype(t(SubTrajectory{}), SubTrajectoryShortSig{});
template <typename T>
auto sigMatcher(const T& t) -> decltype(t(SubTrajectory{}, std::string{}), SubTrajectorySig{});
}; };
namespace cost { namespace cost {

View File

@ -1,4 +1,4 @@
/********************************************************************* /*********************************************************************
* Software License Agreement (BSD License) * Software License Agreement (BSD License)
* *
* Copyright (c) 2020, Hamburg University * 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::nullptr_t) : CostTerm{} {}
CostTerm::CostTerm(std::function<double(const SubTrajectory&, std::string&)> t) CostTerm::CostTerm(const SubTrajectorySig& term)
: term_{ [t](auto&& s, auto&& c) { return t(static_cast<const SubTrajectory&>(s), c); } } : CostTerm{ [term](const SolutionBase& s, std::string& c) { return term(static_cast<const SubTrajectory&>(s), c); },
, supports_{ SolutionType::TRAJECTORY } {} SolutionType::TRAJECTORY } {}
CostTerm::CostTerm(std::function<double(const SubTrajectory&)> t) CostTerm::CostTerm(const SubTrajectoryShortSig& term)
: term_{ [t](auto&& s, auto&&) { return t(static_cast<const SubTrajectory&>(s)); } } : CostTerm{ [term](const SolutionBase& s, std::string&) { return term(static_cast<const SubTrajectory&>(s)); },
, supports_{ SolutionType::TRAJECTORY } {} SolutionType::TRAJECTORY } {}
CostTerm::CostTerm(std::function<double(const SolutionBase&, std::string&)> term, Flags<SolutionType> flags) CostTerm::CostTerm(const std::function<double(const SolutionBase&, std::string&)>& term, Flags<SolutionType> flags)
: term_{ term }, supports_{ flags } {} : term_{ term }, supports_{ flags } {}
CostTerm::operator bool() const { CostTerm::operator bool() const {
@ -76,7 +76,7 @@ double CostTerm::operator()(const SolutionBase& s, std::string& comment) const {
namespace cost { namespace cost {
Constant::Constant(double c) 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 { namespace {
@ -100,8 +100,7 @@ TrajectoryDuration::TrajectoryDuration()
: CostTerm{ [](auto&& s, auto&&) { return s.trajectory() ? s.trajectory()->getDuration() : 0.0; } } {} : CostTerm{ [](auto&& s, auto&&) { return s.trajectory() ? s.trajectory()->getDuration() : 0.0; } } {}
LinkMotion::LinkMotion(std::string link) LinkMotion::LinkMotion(std::string link)
: CostTerm{ [this](const SubTrajectory& s, std::string& c) { return this->compute(s, c); } } : CostTerm{ [this](auto&& s, auto&& c) { return this->compute(s, c); } }, link_name{ std::move(link) } {}
, link_name{ std::move(link) } {}
double LinkMotion::compute(const SubTrajectory& s, std::string& comment) const { double LinkMotion::compute(const SubTrajectory& s, std::string& comment) const {
const auto& traj{ s.trajectory() }; const auto& traj{ s.trajectory() };

View File

@ -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<SerialContainer> container(robot);
auto stage{ std::make_unique<GeneratorMockup>() };
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<GeneratorMockup>();
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<GeneratorMockup>();
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<const SolutionSequence>(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) { TEST(CostTerm, CostOverwrite) {
ros::console::set_logger_level(ROSCONSOLE_ROOT_LOGGER_NAME, ros::console::levels::Fatal); ros::console::set_logger_level(ROSCONSOLE_ROOT_LOGGER_NAME, ros::console::levels::Fatal);
const moveit::core::RobotModelConstPtr robot{ getModel() }; const moveit::core::RobotModelConstPtr robot{ getModel() };
@ -254,8 +280,7 @@ TEST(CostTerm, ForwardCanModifyCost) {
auto stage{ std::make_unique<BackwardMockup>() }; auto stage{ std::make_unique<BackwardMockup>() };
cost::Constant constant{ 8.0 }; cost::Constant constant{ 8.0 };
stage->setCostTerm(constant); stage->setCostTerm(constant);
container.setCostTerm(static_cast<std::function<double(const SubTrajectory&)>>( container.setCostTerm([](auto&& s) { return s.cost() * s.cost(); });
[](const SubTrajectory& s) { return s.cost() * s.cost(); }));
container.computeWithStages({ std::move(stage) }); container.computeWithStages({ std::move(stage) });
auto& wrapped{ dynamic_cast<const WrappedSolution&>(*container.solutions().front()) }; auto& wrapped{ dynamic_cast<const WrappedSolution&>(*container.solutions().front()) };