From acffd390910321f0ec47317e99b3c8d1ec61a6a5 Mon Sep 17 00:00:00 2001 From: Robert Haschke Date: Wed, 14 Feb 2018 09:04:26 +0100 Subject: [PATCH] SerialContainer: fix priority propagation - traverse all (also partial) solution paths - and update priority at both ends - remove pending state pairs if cost increased to infinity --- .../moveit/task_constructor/cost_queue.h | 3 +++ core/src/container.cpp | 14 +++++++------- core/src/stage.cpp | 17 +++++++++++------ core/src/storage.cpp | 3 ++- 4 files changed, 23 insertions(+), 14 deletions(-) diff --git a/core/include/moveit/task_constructor/cost_queue.h b/core/include/moveit/task_constructor/cost_queue.h index 98156294..14fff8b5 100644 --- a/core/include/moveit/task_constructor/cost_queue.h +++ b/core/include/moveit/task_constructor/cost_queue.h @@ -104,6 +104,9 @@ public: c.splice(at, other, pos); return pos; } + + template + void remove_if(Predicate p) { c.remove_if(p); } }; namespace detail { diff --git a/core/src/container.cpp b/core/src/container.cpp index 19373791..59997787 100644 --- a/core/src/container.cpp +++ b/core/src/container.cpp @@ -232,9 +232,7 @@ struct SolutionCollector { 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)); + solutions.emplace_back(std::make_pair(trace, cost)); } std::list> solutions; @@ -257,11 +255,11 @@ void SerialContainer::onNewSolution(const SolutionBase ¤t) SerialContainer::solution_container trace; trace.reserve(children.size()); - // find all incoming solution pathes ending at current solution + // find all incoming solution paths ending at current solution SolutionCollector incoming(num_before); traverse(current, std::ref(incoming), trace); - // find all outgoing solution pathes starting at current solution + // find all outgoing solution paths starting at current solution SolutionCollector outgoing(num_after); traverse(current, std::ref(outgoing), trace); @@ -284,7 +282,7 @@ void SerialContainer::onNewSolution(const SolutionBase ¤t) solution.insert(solution.end(), out.first.begin(), out.first.end()); // store solution in sorted list sorted.insert(SerialSolution(impl, std::move(solution), prio.cost())); - } else { + } else if (prio.depth() > 1) { // update state costs const InterfaceState* start = (in.first.empty() ? current : *in.first.back()).start(); start->owner()->updatePriority(*const_cast(start), prio); @@ -592,8 +590,10 @@ void ParallelContainerBase::spawn(InterfaceState &&state, SubTrajectory&& t) auto it = impl->created_solutions_.insert(impl->created_solutions_.end(), std::move(t)); if (it->isFailure()) { - auto state_it = impl->states_.insert(impl->states_.end(), std::move(state)); + // attach state (different for start / end) to trajectory + auto state_it = impl->states_.insert(impl->states_.end(), InterfaceState(state)); it->setStartState(*state_it); + state_it = impl->states_.insert(impl->states_.end(), std::move(state)); it->setEndState(*state_it); impl->failures_.push_back(&*it); } else { diff --git a/core/src/stage.cpp b/core/src/stage.cpp index 5a7f57ae..ff2a38dd 100644 --- a/core/src/stage.cpp +++ b/core/src/stage.cpp @@ -88,10 +88,6 @@ void StagePrivate::newSolution(const SolutionBase &solution) if (introspection_) introspection_->registerSolution(solution); - // ignore invalid / failure solutions - if (solution.isFailure()) - return; - // call solution callbacks for both, valid solutions and failures for (const auto& cb : solution_cbs_) cb(solution); @@ -486,8 +482,13 @@ ConnectingPrivate::ConnectingPrivate(Connecting *me, const std::string &name) void ConnectingPrivate::newStartState(Interface::iterator it, bool updated) { - if (!std::isfinite(it->priority().cost())) + // TODO: only consider interface states with priority depth > threshold + if (!std::isfinite(it->priority().cost())) { + // remove pending pairs, if cost updated to infinity + if (updated) + pending.remove_if([it](const StatePair& p) { return p.first == it; }); return; + } if (updated) { // many pairs might be affected: sort pending.sort(); @@ -502,8 +503,12 @@ void ConnectingPrivate::newStartState(Interface::iterator it, bool updated) void ConnectingPrivate::newEndState(Interface::iterator it, bool updated) { - if (!std::isfinite(it->priority().cost())) + if (!std::isfinite(it->priority().cost())) { + // remove pending pairs, if cost updated to infinity + if (updated) + pending.remove_if([it](const StatePair& p) { return p.second == it; }); return; + } if (updated) { // many pairs might be affected: sort pending.sort(); diff --git a/core/src/storage.cpp b/core/src/storage.cpp index 4c0b0775..2638bf78 100644 --- a/core/src/storage.cpp +++ b/core/src/storage.cpp @@ -100,7 +100,8 @@ void Interface::updatePriority(InterfaceState &state, const InterfaceState::Prio { if (priority < state.priority()) { auto it = std::find_if(begin(), end(), [&state](const InterfaceState& other) { return &state == &other; }); - assert(it != end()); // state should be part of this interface + // state should be part of the interface + assert(it != end()); state.priority_ = priority; if (notify_) notify_(it, true); }