mirror of
https://github.com/moveit/moveit_task_constructor.git
synced 2025-11-04 14:49:57 +08:00
move more overhead to computeCost
This commit is contained in:
parent
e53d943b75
commit
039b0a2c27
@ -154,11 +154,8 @@ 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
|
/** compute cost for solution through configured CostTerm */
|
||||||
*
|
void computeCost(const InterfaceState& from, const InterfaceState& to, SolutionBase& solution);
|
||||||
* @return true if solution remains feasible (is no failure)
|
|
||||||
*/
|
|
||||||
bool computeCost(SolutionBase& solution);
|
|
||||||
|
|
||||||
protected:
|
protected:
|
||||||
// associated/owning Stage instance
|
// associated/owning Stage instance
|
||||||
|
|||||||
@ -148,19 +148,11 @@ void StagePrivate::sendForward(const InterfaceState& from, InterfaceState&& to,
|
|||||||
|
|
||||||
solution->setCreator(me());
|
solution->setCreator(me());
|
||||||
|
|
||||||
if (!solution->isFailure()) {
|
computeCost(from, to, *solution);
|
||||||
// chicken-and-egg problem: we don't know whether/where we will store the solution,
|
|
||||||
// but need to store it properly to compute the cost with a clean interface:
|
|
||||||
// set state copies for solution before computing cost:
|
|
||||||
InterfaceState tmp_from{ from }, tmp_to{ to };
|
|
||||||
solution->setStartState(tmp_from);
|
|
||||||
solution->setEndState(tmp_to);
|
|
||||||
|
|
||||||
computeCost(*solution);
|
|
||||||
}
|
|
||||||
|
|
||||||
if (!storeSolution(solution))
|
if (!storeSolution(solution))
|
||||||
return; // solution dropped
|
return; // solution dropped
|
||||||
|
|
||||||
me()->forwardProperties(from, to);
|
me()->forwardProperties(from, to);
|
||||||
|
|
||||||
auto to_it = states_.insert(states_.end(), std::move(to));
|
auto to_it = states_.insert(states_.end(), std::move(to));
|
||||||
@ -180,16 +172,11 @@ void StagePrivate::sendBackward(InterfaceState&& from, const InterfaceState& to,
|
|||||||
|
|
||||||
solution->setCreator(me());
|
solution->setCreator(me());
|
||||||
|
|
||||||
if (!solution->isFailure()) {
|
computeCost(from, to, *solution);
|
||||||
InterfaceState tmp_from{ from }, tmp_to{ to };
|
|
||||||
solution->setStartState(tmp_from);
|
|
||||||
solution->setEndState(tmp_to);
|
|
||||||
|
|
||||||
computeCost(*solution);
|
|
||||||
}
|
|
||||||
|
|
||||||
if (!storeSolution(solution))
|
if (!storeSolution(solution))
|
||||||
return; // solution dropped
|
return; // solution dropped
|
||||||
|
|
||||||
me()->forwardProperties(to, from);
|
me()->forwardProperties(to, from);
|
||||||
|
|
||||||
auto from_it = states_.insert(states_.end(), std::move(from));
|
auto from_it = states_.insert(states_.end(), std::move(from));
|
||||||
@ -208,13 +195,7 @@ void StagePrivate::spawn(InterfaceState&& state, const SolutionBasePtr& solution
|
|||||||
|
|
||||||
solution->setCreator(me());
|
solution->setCreator(me());
|
||||||
|
|
||||||
if (!solution->isFailure()) {
|
computeCost(state, state, *solution);
|
||||||
InterfaceState tmp_from{ state }, tmp_to{ state };
|
|
||||||
solution->setStartState(tmp_from);
|
|
||||||
solution->setEndState(tmp_to);
|
|
||||||
|
|
||||||
computeCost(*solution);
|
|
||||||
}
|
|
||||||
|
|
||||||
if (!storeSolution(solution))
|
if (!storeSolution(solution))
|
||||||
return; // solution dropped
|
return; // solution dropped
|
||||||
@ -236,13 +217,7 @@ void StagePrivate::spawn(InterfaceState&& state, const SolutionBasePtr& solution
|
|||||||
void StagePrivate::connect(const InterfaceState& from, const InterfaceState& to, const SolutionBasePtr& solution) {
|
void StagePrivate::connect(const InterfaceState& from, const InterfaceState& to, const SolutionBasePtr& solution) {
|
||||||
solution->setCreator(me());
|
solution->setCreator(me());
|
||||||
|
|
||||||
if (!solution->isFailure()) {
|
computeCost(from, to, *solution);
|
||||||
InterfaceState tmp_from{ from }, tmp_to{ to };
|
|
||||||
solution->setStartState(tmp_from);
|
|
||||||
solution->setEndState(tmp_to);
|
|
||||||
|
|
||||||
computeCost(*solution);
|
|
||||||
}
|
|
||||||
|
|
||||||
if (!storeSolution(solution))
|
if (!storeSolution(solution))
|
||||||
return; // solution dropped
|
return; // solution dropped
|
||||||
@ -262,28 +237,30 @@ void StagePrivate::newSolution(const SolutionBasePtr& solution) {
|
|||||||
parent()->onNewSolution(*solution);
|
parent()->onNewSolution(*solution);
|
||||||
}
|
}
|
||||||
|
|
||||||
bool StagePrivate::computeCost(SolutionBase& solution) {
|
void StagePrivate::computeCost(const InterfaceState& from, const InterfaceState& to, SolutionBase& solution) {
|
||||||
double cost{ 0.0 };
|
// no reason to compute costs for a failed solution
|
||||||
|
if (solution.isFailure() || !cost_term_)
|
||||||
|
return;
|
||||||
|
|
||||||
|
// chicken-and-egg problem: we don't know whether/where we will store the solution yet,
|
||||||
|
// but need to store it properly to compute the cost with a clean interface:
|
||||||
|
// set state copies for solution before computing cost:
|
||||||
|
InterfaceState tmp_from{ from }, tmp_to{ to };
|
||||||
|
solution.setStartState(tmp_from);
|
||||||
|
solution.setEndState(tmp_to);
|
||||||
|
|
||||||
auto* trajectory{ dynamic_cast<const SubTrajectory*>(&solution) };
|
auto* trajectory{ dynamic_cast<const SubTrajectory*>(&solution) };
|
||||||
if (trajectory && cost_term_) {
|
if (trajectory) {
|
||||||
// compute specific cost
|
// compute specific cost
|
||||||
std::string comment;
|
std::string comment;
|
||||||
cost = cost_term_(*trajectory, comment);
|
solution.setCost(cost_term_(*trajectory, comment));
|
||||||
// If a comment was specified, add it to the solution
|
// If a comment was specified, add it to the solution
|
||||||
if (!comment.empty() && !solution.comment().empty()) {
|
if (!comment.empty() && !solution.comment().empty()) {
|
||||||
solution.setComment(solution.comment() + " (" + comment + ")");
|
solution.setComment(solution.comment() + " (" + comment + ")");
|
||||||
} else if (!comment.empty()) {
|
} else if (!comment.empty()) {
|
||||||
solution.setComment(comment);
|
solution.setComment(comment);
|
||||||
}
|
}
|
||||||
} else {
|
|
||||||
// otherwise forward current cost
|
|
||||||
cost = solution.cost();
|
|
||||||
}
|
}
|
||||||
|
|
||||||
solution.setCost(cost);
|
|
||||||
|
|
||||||
return !solution.isFailure();
|
|
||||||
}
|
}
|
||||||
|
|
||||||
Stage::Stage(StagePrivate* impl) : pimpl_(impl) {
|
Stage::Stage(StagePrivate* impl) : pimpl_(impl) {
|
||||||
|
|||||||
Loading…
Reference in New Issue
Block a user