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
{
public:
CostTerm(std::function<double(const SubTrajectory&)>);
CostTerm(std::function<double(const SubTrajectory&, std::string&)>);
using SubTrajectorySig = 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();
@ -72,10 +79,16 @@ public:
Flags<SolutionType> supports() const;
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_;
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 {

View File

@ -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<double(const SubTrajectory&, std::string&)> t)
: term_{ [t](auto&& s, auto&& c) { return t(static_cast<const SubTrajectory&>(s), c); } }
, supports_{ SolutionType::TRAJECTORY } {}
CostTerm::CostTerm(const SubTrajectorySig& term)
: CostTerm{ [term](const SolutionBase& s, std::string& c) { return term(static_cast<const SubTrajectory&>(s), c); },
SolutionType::TRAJECTORY } {}
CostTerm::CostTerm(std::function<double(const SubTrajectory&)> t)
: term_{ [t](auto&& s, auto&&) { return t(static_cast<const SubTrajectory&>(s)); } }
, supports_{ SolutionType::TRAJECTORY } {}
CostTerm::CostTerm(const SubTrajectoryShortSig& term)
: CostTerm{ [term](const SolutionBase& s, std::string&) { return term(static_cast<const SubTrajectory&>(s)); },
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 } {}
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() };

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) {
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<BackwardMockup>() };
cost::Constant constant{ 8.0 };
stage->setCostTerm(constant);
container.setCostTerm(static_cast<std::function<double(const SubTrajectory&)>>(
[](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<const WrappedSolution&>(*container.solutions().front()) };