mirror of
https://github.com/moveit/moveit_task_constructor.git
synced 2025-11-04 14:49:57 +08:00
enable implicit construction of CostTerm from lambdas
without the need for explicit casts to the correct signature.
This commit is contained in:
parent
034a155315
commit
05932ad5f1
@ -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 {
|
||||
|
||||
@ -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() };
|
||||
|
||||
@ -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()) };
|
||||
|
||||
Loading…
Reference in New Issue
Block a user