diff --git a/core/include/moveit/task_constructor/container.h b/core/include/moveit/task_constructor/container.h index 27b2fde0..8aa557a9 100644 --- a/core/include/moveit/task_constructor/container.h +++ b/core/include/moveit/task_constructor/container.h @@ -80,6 +80,9 @@ public: protected: ContainerBase(ContainerBasePrivate* impl); + +private: + using Stage::setCostTerm; }; std::ostream& operator<<(std::ostream& os, const ContainerBase& stage); diff --git a/core/include/moveit/task_constructor/stage.h b/core/include/moveit/task_constructor/stage.h index 4b4fcd30..bcb9f1f3 100644 --- a/core/include/moveit/task_constructor/stage.h +++ b/core/include/moveit/task_constructor/stage.h @@ -159,6 +159,7 @@ public: PARENT = 2, INTERFACE = 4, }; + virtual ~Stage(); /// auto-convert Stage to StagePrivate* when needed @@ -212,6 +213,9 @@ public: /// remove function callback void removeSolutionCallback(SolutionCallbackList::const_iterator which); + using CostTerm = std::function; + void setCostTerm(const CostTerm&); + const ordered& solutions() const; const std::list& failures() const; size_t numFailures() const; @@ -227,8 +231,10 @@ public: void setProperty(const std::string& name, const boost::any& value); /// overload: const char* values are stored as std::string inline void setProperty(const std::string& name, const char* value) { setProperty(name, std::string(value)); } + /// analyze source of error and report accordingly [[noreturn]] void reportPropertyError(const Property::error& e); + double getTotalComputeTime() const; protected: diff --git a/core/include/moveit/task_constructor/stage_p.h b/core/include/moveit/task_constructor/stage_p.h index 150080f5..3fbabfa0 100644 --- a/core/include/moveit/task_constructor/stage_p.h +++ b/core/include/moveit/task_constructor/stage_p.h @@ -154,8 +154,16 @@ public: total_compute_time_ += compute_stop_time - compute_start_time; } + /** compute cost for solution through configured CostTerm + * + * @return true if solution remains feasible (is no failure) + */ + bool addCost(SolutionBase& solution); + protected: - Stage* me_; // associated/owning Stage instance + // associated/owning Stage instance + Stage* me_; + std::string name_; PropertyMap properties_; @@ -163,6 +171,9 @@ protected: InterfacePtr starts_; InterfacePtr ends_; + // user-configurable cost estimator + Stage::CostTerm cost_term_; + // The total compute time std::chrono::duration total_compute_time_; diff --git a/core/src/stage.cpp b/core/src/stage.cpp index 38761006..45932615 100644 --- a/core/src/stage.cpp +++ b/core/src/stage.cpp @@ -151,8 +151,9 @@ void StagePrivate::sendForward(const InterfaceState& from, InterfaceState&& to, solution->setStartState(from); solution->setEndState(*to_it); - if (!solution->isFailure()) + if (!solution->isFailure() && addCost(*solution)) { nextStarts()->add(*to_it); + } newSolution(solution); } @@ -168,7 +169,7 @@ void StagePrivate::sendBackward(InterfaceState&& from, const InterfaceState& to, solution->setStartState(*from_it); solution->setEndState(to); - if (!solution->isFailure()) + if (!solution->isFailure() && addCost(*solution)) prevEnds()->add(*from_it); newSolution(solution); @@ -185,7 +186,7 @@ void StagePrivate::spawn(InterfaceState&& state, const SolutionBasePtr& solution solution->setStartState(*from); solution->setEndState(*to); - if (!solution->isFailure()) { + if (!solution->isFailure() && addCost(*solution)) { prevEnds()->add(*from); nextStarts()->add(*to); } @@ -200,6 +201,9 @@ void StagePrivate::connect(const InterfaceState& from, const InterfaceState& to, solution->setStartState(from); solution->setEndState(to); + if (!solution->isFailure()) + addCost(*solution); + newSolution(solution); } @@ -212,6 +216,13 @@ void StagePrivate::newSolution(const SolutionBasePtr& solution) { parent()->onNewSolution(*solution); } +bool StagePrivate::addCost(SolutionBase& solution) { + if (cost_term_) + solution.setCost(cost_term_(solution)); + + return !solution.isFailure(); +} + Stage::Stage(StagePrivate* impl) : pimpl_(impl) { assert(impl); auto& p = properties(); @@ -308,6 +319,10 @@ void Stage::removeSolutionCallback(SolutionCallbackList::const_iterator which) { pimpl()->solution_cbs_.erase(which); } +void Stage::setCostTerm(const CostTerm& cost_term) { + pimpl()->cost_term_ = cost_term; +} + const ordered& Stage::solutions() const { return pimpl()->solutions_; }