mirror of
https://github.com/moveit/moveit_task_constructor.git
synced 2025-11-04 14:49:57 +08:00
SerialContainer: traverse solutions w/o stopping stage
Always traverse from current solution to the start/end of a complete path and only call the callback once for the whole trace.
This commit is contained in:
parent
6a62f6be76
commit
e6af069139
@ -101,18 +101,15 @@ protected:
|
|||||||
/// called by a (direct) child when a new solution becomes available
|
/// called by a (direct) child when a new solution becomes available
|
||||||
void onNewSolution(const SolutionBase &s) override;
|
void onNewSolution(const SolutionBase &s) override;
|
||||||
|
|
||||||
/// function type used for traversing solutions
|
typedef std::function<void(const solution_container& trace,
|
||||||
/// For each sub solution (current), the trace from the start as well as the
|
|
||||||
/// accumulated cost of all solutions in the trace are provided.
|
|
||||||
/// Return true, if traversal should continue, otherwise false.
|
|
||||||
typedef std::function<bool(const SolutionBase& current,
|
|
||||||
const solution_container& trace,
|
|
||||||
double trace_accumulated_cost)> SolutionProcessor;
|
double trace_accumulated_cost)> SolutionProcessor;
|
||||||
|
|
||||||
/// traverse all solutions, starting at start and call the callback for each subsolution
|
/// Traverse all solution pathes starting at start and going in given direction dir
|
||||||
/// The return value is always false, indicating that the traversal eventually stopped.
|
/// until the end, i.e. until there are no more subsolutions in the given direction
|
||||||
|
/// For each solution path, callback the given processor passing
|
||||||
|
/// the full trace (from start to end, but not including start) and its accumulated costs
|
||||||
template<TraverseDirection dir>
|
template<TraverseDirection dir>
|
||||||
bool traverse(const SolutionBase &start, const SolutionProcessor &cb,
|
void traverse(const SolutionBase &start, const SolutionProcessor &cb,
|
||||||
solution_container &trace, double trace_cost = 0);
|
solution_container &trace, double trace_cost = 0);
|
||||||
|
|
||||||
protected:
|
protected:
|
||||||
|
|||||||
@ -203,18 +203,18 @@ SerialContainerPrivate::SerialContainerPrivate(SerialContainer *me, const std::s
|
|||||||
|
|
||||||
|
|
||||||
struct SolutionCollector {
|
struct SolutionCollector {
|
||||||
SolutionCollector(const Stage::pointer& stage) : stopping_stage(stage->pimpl()) {}
|
SolutionCollector(size_t max_depth) : max_depth(max_depth) {}
|
||||||
|
|
||||||
bool operator()(const SolutionBase& current, const SerialContainer::solution_container& trace, double cost) {
|
void operator()(const SerialContainer::solution_container& trace, double cost) {
|
||||||
if (current.creator() != stopping_stage)
|
// traced path should not extend past container boundaries
|
||||||
return true; // not yet traversed to stopping_stage
|
assert(trace.size() <= max_depth);
|
||||||
|
|
||||||
solutions.emplace_back(std::make_pair(trace, cost));
|
if (trace.size() == max_depth) // reached max depth
|
||||||
return false; // we are done
|
solutions.emplace_back(std::make_pair(trace, cost));
|
||||||
}
|
}
|
||||||
|
|
||||||
std::list<std::pair<SerialContainer::solution_container, double>> solutions;
|
std::list<std::pair<SerialContainer::solution_container, double>> solutions;
|
||||||
const StagePrivate* const stopping_stage;
|
const size_t max_depth;
|
||||||
};
|
};
|
||||||
|
|
||||||
void SerialContainer::onNewSolution(const SolutionBase ¤t)
|
void SerialContainer::onNewSolution(const SolutionBase ¤t)
|
||||||
@ -223,22 +223,25 @@ void SerialContainer::onNewSolution(const SolutionBase ¤t)
|
|||||||
const StagePrivate *creator = current.creator();
|
const StagePrivate *creator = current.creator();
|
||||||
auto& children = impl->children();
|
auto& children = impl->children();
|
||||||
|
|
||||||
// s.creator() should be one of our children
|
// find number of stages before and after creator stage
|
||||||
assert(std::find_if(children.begin(), children.end(),
|
size_t num_before = 0, num_after = 0;
|
||||||
[creator](const Stage::pointer& stage) { return stage->pimpl() == creator; } )
|
for (auto it = children.begin(), end = children.end(); it != end; ++it, ++num_before)
|
||||||
!= children.end());
|
if ((*it)->pimpl() == creator)
|
||||||
|
break;
|
||||||
|
assert(num_before < children.size()); // creator should be one of our children
|
||||||
|
num_after = children.size()-1 - num_before;
|
||||||
|
|
||||||
SerialContainer::solution_container trace; trace.reserve(children.size());
|
SerialContainer::solution_container trace; trace.reserve(children.size());
|
||||||
|
|
||||||
// find all incoming trajectories connected to current solution
|
// find all incoming trajectories connected to current solution
|
||||||
SolutionCollector incoming(children.front());
|
SolutionCollector incoming(num_before);
|
||||||
traverse<BACKWARD>(current, std::ref(incoming), trace);
|
traverse<BACKWARD>(current, std::ref(incoming), trace);
|
||||||
if (incoming.solutions.empty())
|
if (incoming.solutions.empty())
|
||||||
return; // no connection to front()
|
return; // no connection to front()
|
||||||
|
|
||||||
|
|
||||||
// find all outgoing trajectories connected to current solution
|
// find all outgoing trajectories connected to current solution
|
||||||
SolutionCollector outgoing(children.back());
|
SolutionCollector outgoing(num_after);
|
||||||
traverse<FORWARD>(current, std::ref(outgoing), trace);
|
traverse<FORWARD>(current, std::ref(outgoing), trace);
|
||||||
if (outgoing.solutions.empty())
|
if (outgoing.solutions.empty())
|
||||||
return; // no connection to back()
|
return; // no connection to back()
|
||||||
@ -419,26 +422,21 @@ void SerialContainer::processSolutions(const ContainerBase::SolutionProcessor &p
|
|||||||
}
|
}
|
||||||
|
|
||||||
template <TraverseDirection dir>
|
template <TraverseDirection dir>
|
||||||
bool SerialContainer::traverse(const SolutionBase &start, const SolutionProcessor &cb,
|
void SerialContainer::traverse(const SolutionBase &start, const SolutionProcessor &cb,
|
||||||
solution_container &trace, double trace_cost)
|
solution_container &trace, double trace_cost)
|
||||||
{
|
{
|
||||||
if (!cb(start, trace, trace_cost))
|
const InterfaceState::Solutions& solutions = trajectories<dir>(start);
|
||||||
// stopping criterium met: stop traversal along dir
|
if (solutions.empty()) // if we reached the end, call the callback
|
||||||
return true; // but continue traversal of further trajectories
|
cb(trace, trace_cost);
|
||||||
|
else for (SolutionBase* successor : solutions) {
|
||||||
bool result = false; // if no trajectory traversed, return false
|
|
||||||
for (SolutionBase* successor : trajectories<dir>(start)) {
|
|
||||||
trace.push_back(successor);
|
trace.push_back(successor);
|
||||||
trace_cost += successor->cost();
|
trace_cost += successor->cost();
|
||||||
|
|
||||||
result = traverse<dir>(*successor, cb, trace, trace_cost);
|
traverse<dir>(*successor, cb, trace, trace_cost);
|
||||||
|
|
||||||
trace_cost -= successor->cost();
|
trace_cost -= successor->cost();
|
||||||
trace.pop_back();
|
trace.pop_back();
|
||||||
|
|
||||||
if (!result) break;
|
|
||||||
}
|
}
|
||||||
return result;
|
|
||||||
}
|
}
|
||||||
|
|
||||||
void SerialSolution::fillMessage(moveit_task_constructor_msgs::Solution &msg,
|
void SerialSolution::fillMessage(moveit_task_constructor_msgs::Solution &msg,
|
||||||
|
|||||||
Loading…
Reference in New Issue
Block a user