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:
Robert Haschke 2018-01-30 13:41:57 +01:00
parent 6a62f6be76
commit e6af069139
2 changed files with 28 additions and 33 deletions

View File

@ -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:

View File

@ -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 &current) void SerialContainer::onNewSolution(const SolutionBase &current)
@ -223,22 +223,25 @@ void SerialContainer::onNewSolution(const SolutionBase &current)
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,