diff --git a/core/include/moveit/task_constructor/cost_terms.h b/core/include/moveit/task_constructor/cost_terms.h index 586797ea..a1ad49ed 100644 --- a/core/include/moveit/task_constructor/cost_terms.h +++ b/core/include/moveit/task_constructor/cost_terms.h @@ -7,21 +7,20 @@ namespace task_constructor { namespace cost { /// These structures all implement the Stage::CostTerm API and can be configured via Stage::setCostTerm() - /// add a constant cost to each solution class ConstantCost { public: ConstantCost(double cost) : cost_(cost) {} - double operator()(const SolutionBase&) { return cost_; } + double operator()(const SubTrajectory&) { return cost_; } private: double cost_; }; /// define cost as execution duration of the whole trajectory -double PathLengthCost(const SolutionBase& s); +double PathLengthCost(const SubTrajectory& s); } } } diff --git a/core/include/moveit/task_constructor/stage.h b/core/include/moveit/task_constructor/stage.h index bcb9f1f3..2336b633 100644 --- a/core/include/moveit/task_constructor/stage.h +++ b/core/include/moveit/task_constructor/stage.h @@ -213,8 +213,7 @@ public: /// remove function callback void removeSolutionCallback(SolutionCallbackList::const_iterator which); - using CostTerm = std::function; - void setCostTerm(const CostTerm&); + using CostTerm = std::function; const ordered& solutions() const; const std::list& failures() const; diff --git a/core/src/cost_terms.cpp b/core/src/cost_terms.cpp index cc0f3758..a508103a 100644 --- a/core/src/cost_terms.cpp +++ b/core/src/cost_terms.cpp @@ -41,20 +41,8 @@ namespace moveit { namespace task_constructor { namespace cost { -double PathLengthCost(const SolutionBase& s) { - auto* trajectory{ dynamic_cast(&s) }; - - if (trajectory) { - return trajectory->trajectory() ? trajectory->trajectory()->getDuration() : 0.0; - } else { - // there is no other alternative implementation of SolutionBase so static_cast is safe - auto& seq{ *static_cast(&s) }; - - double cost{ 0.0 }; - for (auto& sub : seq.solutions()) - cost += PathLengthCost(*sub); - return cost; - } +double PathLengthCost(const SubTrajectory& s) { + return s.trajectory() ? s.trajectory()->getDuration() : 0.0; } } } diff --git a/core/src/stage.cpp b/core/src/stage.cpp index 45932615..0e29e674 100644 --- a/core/src/stage.cpp +++ b/core/src/stage.cpp @@ -217,8 +217,9 @@ void StagePrivate::newSolution(const SolutionBasePtr& solution) { } bool StagePrivate::addCost(SolutionBase& solution) { - if (cost_term_) - solution.setCost(cost_term_(solution)); + auto* trajectory{ dynamic_cast(&solution) }; + if (trajectory && cost_term_) + solution.setCost(cost_term_(*trajectory)); return !solution.isFailure(); } @@ -319,8 +320,8 @@ void Stage::removeSolutionCallback(SolutionCallbackList::const_iterator which) { pimpl()->solution_cbs_.erase(which); } -void Stage::setCostTerm(const CostTerm& cost_term) { - pimpl()->cost_term_ = cost_term; +void Stage::setCostTerm(const CostTerm& term) { + pimpl()->cost_term_ = term; } const ordered& Stage::solutions() const {