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:
|
protected:
|
||||||
ContainerBase(ContainerBasePrivate* impl);
|
ContainerBase(ContainerBasePrivate* impl);
|
||||||
|
|
||||||
|
private:
|
||||||
|
using Stage::setCostTerm;
|
||||||
};
|
};
|
||||||
std::ostream& operator<<(std::ostream& os, const ContainerBase& stage);
|
std::ostream& operator<<(std::ostream& os, const ContainerBase& stage);
|
||||||
|
|
||||||
|
|||||||
@ -159,6 +159,7 @@ public:
|
|||||||
PARENT = 2,
|
PARENT = 2,
|
||||||
INTERFACE = 4,
|
INTERFACE = 4,
|
||||||
};
|
};
|
||||||
|
|
||||||
virtual ~Stage();
|
virtual ~Stage();
|
||||||
|
|
||||||
/// auto-convert Stage to StagePrivate* when needed
|
/// auto-convert Stage to StagePrivate* when needed
|
||||||
@ -212,6 +213,9 @@ public:
|
|||||||
/// remove function callback
|
/// remove function callback
|
||||||
void removeSolutionCallback(SolutionCallbackList::const_iterator which);
|
void removeSolutionCallback(SolutionCallbackList::const_iterator which);
|
||||||
|
|
||||||
|
using CostTerm = std::function<double(const SolutionBase&)>;
|
||||||
|
void setCostTerm(const CostTerm&);
|
||||||
|
|
||||||
const ordered<SolutionBaseConstPtr>& solutions() const;
|
const ordered<SolutionBaseConstPtr>& solutions() const;
|
||||||
const std::list<SolutionBaseConstPtr>& failures() const;
|
const std::list<SolutionBaseConstPtr>& failures() const;
|
||||||
size_t numFailures() const;
|
size_t numFailures() const;
|
||||||
@ -227,8 +231,10 @@ public:
|
|||||||
void setProperty(const std::string& name, const boost::any& value);
|
void setProperty(const std::string& name, const boost::any& value);
|
||||||
/// overload: const char* values are stored as std::string
|
/// overload: const char* values are stored as std::string
|
||||||
inline void setProperty(const std::string& name, const char* value) { setProperty(name, std::string(value)); }
|
inline void setProperty(const std::string& name, const char* value) { setProperty(name, std::string(value)); }
|
||||||
|
|
||||||
/// analyze source of error and report accordingly
|
/// analyze source of error and report accordingly
|
||||||
[[noreturn]] void reportPropertyError(const Property::error& e);
|
[[noreturn]] void reportPropertyError(const Property::error& e);
|
||||||
|
|
||||||
double getTotalComputeTime() const;
|
double getTotalComputeTime() const;
|
||||||
|
|
||||||
protected:
|
protected:
|
||||||
|
|||||||
@ -154,8 +154,16 @@ public:
|
|||||||
total_compute_time_ += compute_stop_time - compute_start_time;
|
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:
|
protected:
|
||||||
Stage* me_; // associated/owning Stage instance
|
// associated/owning Stage instance
|
||||||
|
Stage* me_;
|
||||||
|
|
||||||
std::string name_;
|
std::string name_;
|
||||||
PropertyMap properties_;
|
PropertyMap properties_;
|
||||||
|
|
||||||
@ -163,6 +171,9 @@ protected:
|
|||||||
InterfacePtr starts_;
|
InterfacePtr starts_;
|
||||||
InterfacePtr ends_;
|
InterfacePtr ends_;
|
||||||
|
|
||||||
|
// user-configurable cost estimator
|
||||||
|
Stage::CostTerm cost_term_;
|
||||||
|
|
||||||
// The total compute time
|
// The total compute time
|
||||||
std::chrono::duration<double> 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->setStartState(from);
|
||||||
solution->setEndState(*to_it);
|
solution->setEndState(*to_it);
|
||||||
|
|
||||||
if (!solution->isFailure())
|
if (!solution->isFailure() && addCost(*solution)) {
|
||||||
nextStarts()->add(*to_it);
|
nextStarts()->add(*to_it);
|
||||||
|
}
|
||||||
|
|
||||||
newSolution(solution);
|
newSolution(solution);
|
||||||
}
|
}
|
||||||
@ -168,7 +169,7 @@ void StagePrivate::sendBackward(InterfaceState&& from, const InterfaceState& to,
|
|||||||
solution->setStartState(*from_it);
|
solution->setStartState(*from_it);
|
||||||
solution->setEndState(to);
|
solution->setEndState(to);
|
||||||
|
|
||||||
if (!solution->isFailure())
|
if (!solution->isFailure() && addCost(*solution))
|
||||||
prevEnds()->add(*from_it);
|
prevEnds()->add(*from_it);
|
||||||
|
|
||||||
newSolution(solution);
|
newSolution(solution);
|
||||||
@ -185,7 +186,7 @@ void StagePrivate::spawn(InterfaceState&& state, const SolutionBasePtr& solution
|
|||||||
solution->setStartState(*from);
|
solution->setStartState(*from);
|
||||||
solution->setEndState(*to);
|
solution->setEndState(*to);
|
||||||
|
|
||||||
if (!solution->isFailure()) {
|
if (!solution->isFailure() && addCost(*solution)) {
|
||||||
prevEnds()->add(*from);
|
prevEnds()->add(*from);
|
||||||
nextStarts()->add(*to);
|
nextStarts()->add(*to);
|
||||||
}
|
}
|
||||||
@ -200,6 +201,9 @@ void StagePrivate::connect(const InterfaceState& from, const InterfaceState& to,
|
|||||||
solution->setStartState(from);
|
solution->setStartState(from);
|
||||||
solution->setEndState(to);
|
solution->setEndState(to);
|
||||||
|
|
||||||
|
if (!solution->isFailure())
|
||||||
|
addCost(*solution);
|
||||||
|
|
||||||
newSolution(solution);
|
newSolution(solution);
|
||||||
}
|
}
|
||||||
|
|
||||||
@ -212,6 +216,13 @@ void StagePrivate::newSolution(const SolutionBasePtr& solution) {
|
|||||||
parent()->onNewSolution(*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) {
|
Stage::Stage(StagePrivate* impl) : pimpl_(impl) {
|
||||||
assert(impl);
|
assert(impl);
|
||||||
auto& p = properties();
|
auto& p = properties();
|
||||||
@ -308,6 +319,10 @@ void Stage::removeSolutionCallback(SolutionCallbackList::const_iterator which) {
|
|||||||
pimpl()->solution_cbs_.erase(which);
|
pimpl()->solution_cbs_.erase(which);
|
||||||
}
|
}
|
||||||
|
|
||||||
|
void Stage::setCostTerm(const CostTerm& cost_term) {
|
||||||
|
pimpl()->cost_term_ = cost_term;
|
||||||
|
}
|
||||||
|
|
||||||
const ordered<SolutionBaseConstPtr>& Stage::solutions() const {
|
const ordered<SolutionBaseConstPtr>& Stage::solutions() const {
|
||||||
return pimpl()->solutions_;
|
return pimpl()->solutions_;
|
||||||
}
|
}
|
||||||
|
|||||||
Loading…
Reference in New Issue
Block a user