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
|
||||
void onNewSolution(const SolutionBase &s) override;
|
||||
|
||||
/// function type used for traversing solutions
|
||||
/// 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,
|
||||
typedef std::function<void(const solution_container& trace,
|
||||
double trace_accumulated_cost)> SolutionProcessor;
|
||||
|
||||
/// traverse all solutions, starting at start and call the callback for each subsolution
|
||||
/// The return value is always false, indicating that the traversal eventually stopped.
|
||||
/// Traverse all solution pathes starting at start and going in given direction dir
|
||||
/// 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>
|
||||
bool traverse(const SolutionBase &start, const SolutionProcessor &cb,
|
||||
void traverse(const SolutionBase &start, const SolutionProcessor &cb,
|
||||
solution_container &trace, double trace_cost = 0);
|
||||
|
||||
protected:
|
||||
|
||||
@ -203,18 +203,18 @@ SerialContainerPrivate::SerialContainerPrivate(SerialContainer *me, const std::s
|
||||
|
||||
|
||||
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) {
|
||||
if (current.creator() != stopping_stage)
|
||||
return true; // not yet traversed to stopping_stage
|
||||
void operator()(const SerialContainer::solution_container& trace, double cost) {
|
||||
// traced path should not extend past container boundaries
|
||||
assert(trace.size() <= max_depth);
|
||||
|
||||
if (trace.size() == max_depth) // reached max depth
|
||||
solutions.emplace_back(std::make_pair(trace, cost));
|
||||
return false; // we are done
|
||||
}
|
||||
|
||||
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)
|
||||
@ -223,22 +223,25 @@ void SerialContainer::onNewSolution(const SolutionBase ¤t)
|
||||
const StagePrivate *creator = current.creator();
|
||||
auto& children = impl->children();
|
||||
|
||||
// s.creator() should be one of our children
|
||||
assert(std::find_if(children.begin(), children.end(),
|
||||
[creator](const Stage::pointer& stage) { return stage->pimpl() == creator; } )
|
||||
!= children.end());
|
||||
// find number of stages before and after creator stage
|
||||
size_t num_before = 0, num_after = 0;
|
||||
for (auto it = children.begin(), end = children.end(); it != end; ++it, ++num_before)
|
||||
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());
|
||||
|
||||
// find all incoming trajectories connected to current solution
|
||||
SolutionCollector incoming(children.front());
|
||||
SolutionCollector incoming(num_before);
|
||||
traverse<BACKWARD>(current, std::ref(incoming), trace);
|
||||
if (incoming.solutions.empty())
|
||||
return; // no connection to front()
|
||||
|
||||
|
||||
// find all outgoing trajectories connected to current solution
|
||||
SolutionCollector outgoing(children.back());
|
||||
SolutionCollector outgoing(num_after);
|
||||
traverse<FORWARD>(current, std::ref(outgoing), trace);
|
||||
if (outgoing.solutions.empty())
|
||||
return; // no connection to back()
|
||||
@ -419,26 +422,21 @@ void SerialContainer::processSolutions(const ContainerBase::SolutionProcessor &p
|
||||
}
|
||||
|
||||
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)
|
||||
{
|
||||
if (!cb(start, trace, trace_cost))
|
||||
// stopping criterium met: stop traversal along dir
|
||||
return true; // but continue traversal of further trajectories
|
||||
|
||||
bool result = false; // if no trajectory traversed, return false
|
||||
for (SolutionBase* successor : trajectories<dir>(start)) {
|
||||
const InterfaceState::Solutions& solutions = trajectories<dir>(start);
|
||||
if (solutions.empty()) // if we reached the end, call the callback
|
||||
cb(trace, trace_cost);
|
||||
else for (SolutionBase* successor : solutions) {
|
||||
trace.push_back(successor);
|
||||
trace_cost += successor->cost();
|
||||
|
||||
result = traverse<dir>(*successor, cb, trace, trace_cost);
|
||||
traverse<dir>(*successor, cb, trace, trace_cost);
|
||||
|
||||
trace_cost -= successor->cost();
|
||||
trace.pop_back();
|
||||
|
||||
if (!result) break;
|
||||
}
|
||||
return result;
|
||||
}
|
||||
|
||||
void SerialSolution::fillMessage(moveit_task_constructor_msgs::Solution &msg,
|
||||
|
||||
Loading…
Reference in New Issue
Block a user