mirror of
https://github.com/moveit/moveit_task_constructor.git
synced 2025-11-04 14:49:57 +08:00
add CostTerm interface
basic stages can now be configured with arbitrary cost terms.
This commit is contained in:
parent
1a2eafa256
commit
9337743de2
@ -80,6 +80,9 @@ public:
|
||||
|
||||
protected:
|
||||
ContainerBase(ContainerBasePrivate* impl);
|
||||
|
||||
private:
|
||||
using Stage::setCostTerm;
|
||||
};
|
||||
std::ostream& operator<<(std::ostream& os, const ContainerBase& stage);
|
||||
|
||||
|
||||
@ -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<double(const SolutionBase&)>;
|
||||
void setCostTerm(const CostTerm&);
|
||||
|
||||
const ordered<SolutionBaseConstPtr>& solutions() const;
|
||||
const std::list<SolutionBaseConstPtr>& 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:
|
||||
|
||||
@ -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<double> total_compute_time_;
|
||||
|
||||
|
||||
@ -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<SolutionBaseConstPtr>& Stage::solutions() const {
|
||||
return pimpl()->solutions_;
|
||||
}
|
||||
|
||||
Loading…
Reference in New Issue
Block a user