From e6af069139ba9e993e23ed369af1ecebbdbed23f Mon Sep 17 00:00:00 2001 From: Robert Haschke Date: Tue, 30 Jan 2018 13:41:57 +0100 Subject: [PATCH] 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. --- .../moveit/task_constructor/container.h | 15 +++--- core/src/container.cpp | 46 +++++++++---------- 2 files changed, 28 insertions(+), 33 deletions(-) diff --git a/core/include/moveit/task_constructor/container.h b/core/include/moveit/task_constructor/container.h index 9a6d92c2..7191dd92 100644 --- a/core/include/moveit/task_constructor/container.h +++ b/core/include/moveit/task_constructor/container.h @@ -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 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 - bool traverse(const SolutionBase &start, const SolutionProcessor &cb, + void traverse(const SolutionBase &start, const SolutionProcessor &cb, solution_container &trace, double trace_cost = 0); protected: diff --git a/core/src/container.cpp b/core/src/container.cpp index 2180e678..1254b5ea 100644 --- a/core/src/container.cpp +++ b/core/src/container.cpp @@ -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); - solutions.emplace_back(std::make_pair(trace, cost)); - return false; // we are done + if (trace.size() == max_depth) // reached max depth + solutions.emplace_back(std::make_pair(trace, cost)); } std::list> 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(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(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 -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(start)) { + const InterfaceState::Solutions& solutions = trajectories(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(*successor, cb, trace, trace_cost); + traverse(*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,