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
|
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 {
|
||||||
|
|||||||
@ -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() };
|
||||||
|
|||||||
@ -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()) };
|
||||||
|
|||||||
Loading…
Reference in New Issue
Block a user