diff --git a/core/include/moveit/task_constructor/storage.h b/core/include/moveit/task_constructor/storage.h index 0386ff3f..c91e2441 100644 --- a/core/include/moveit/task_constructor/storage.h +++ b/core/include/moveit/task_constructor/storage.h @@ -152,11 +152,22 @@ public: /// clone an existing InterfaceState, but without its incoming/outgoing connections iterator clone(const InterfaceState &state); + /// remove a state from the interface and return it as a one-element list + container_type remove(iterator it); + /// update state's priority if new priority is smaller and call notify_ - void updatePriority(InterfaceState &state, const InterfaceState::Priority &priority); + void updatePriority(InterfaceState *state, const InterfaceState::Priority &priority); private: const NotifyFunction notify_; + + // restrict access to some functions to ensure consistency + // (we need to set/unset InterfaceState::owner_) + using ordered::moveTo; + using ordered::moveFrom; + using ordered::insert; + using ordered::erase; + using ordered::remove_if; }; diff --git a/core/src/container.cpp b/core/src/container.cpp index f73dcf27..d1d5c1a1 100644 --- a/core/src/container.cpp +++ b/core/src/container.cpp @@ -256,10 +256,25 @@ struct SolutionCollector { solutions.emplace_back(std::make_pair(trace, cost)); } - std::list> solutions; + typedef std::list> SolutionCostPairs; + SolutionCostPairs solutions; const size_t max_depth; }; +void updateStateCosts(const SerialContainer::solution_container &partial_solution_path, + const InterfaceState::Priority &prio) { + for (const SolutionBase* solution : partial_solution_path) { + // here it suffices to update the start state, because the end state is the start state + // of the next solution (they are all connected) + InterfaceState* state = const_cast(solution->start()); + if (state->owner()) state->owner()->updatePriority(state, prio); + } + // finally update the end state of the last solution + if (partial_solution_path.empty()) return; + InterfaceState* state = const_cast(partial_solution_path.back()->end()); + if (state->owner()) state->owner()->updatePriority(state, prio); +} + void SerialContainer::onNewSolution(const SolutionBase ¤t) { auto impl = pimpl(); @@ -304,11 +319,10 @@ void SerialContainer::onNewSolution(const SolutionBase ¤t) // store solution in sorted list sorted.insert(SerialSolution(impl, std::move(solution), prio.cost())); } 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); - const InterfaceState* end = (out.first.empty() ? current : *out.first.back()).end(); - end->owner()->updatePriority(*const_cast(end), prio); + // update state priorities along the whole partial solution path + updateStateCosts(in.first, prio); + updateStateCosts({¤t}, prio); + updateStateCosts(out.first, prio); } } } diff --git a/core/src/stage.cpp b/core/src/stage.cpp index c4b02a11..ff74dfdd 100644 --- a/core/src/stage.cpp +++ b/core/src/stage.cpp @@ -306,12 +306,12 @@ InterfaceFlags PropagatingEitherWayPrivate::requiredInterface() const void PropagatingEitherWayPrivate::dropFailedStarts(Interface::iterator state) { // move infinite-cost states to processed list if (std::isinf(state->priority().cost())) - starts_->moveTo(state, processed, processed.end()); + processed.splice(processed.end(), starts_->remove(state)); } void PropagatingEitherWayPrivate::dropFailedEnds(Interface::iterator state) { // move infinite-cost states to processed list if (std::isinf(state->priority().cost())) - ends_->moveTo(state, processed, processed.end()); + processed.splice(processed.end(), ends_->remove(state)); } inline bool PropagatingEitherWayPrivate::hasStartState() const{ @@ -320,8 +320,9 @@ inline bool PropagatingEitherWayPrivate::hasStartState() const{ const InterfaceState& PropagatingEitherWayPrivate::fetchStartState(){ assert(hasStartState()); - // move state to processed list - return *starts_->moveTo(starts_->begin(), processed, processed.end()); + // move state to end of processed list + processed.splice(processed.end(), starts_->remove(starts_->begin())); + return processed.back(); } inline bool PropagatingEitherWayPrivate::hasEndState() const{ @@ -331,7 +332,8 @@ inline bool PropagatingEitherWayPrivate::hasEndState() const{ const InterfaceState& PropagatingEitherWayPrivate::fetchEndState(){ assert(hasEndState()); // move state to processed list - return *ends_->moveTo(ends_->begin(), processed, processed.end()); + processed.splice(processed.end(), ends_->remove(ends_->begin())); + return processed.back(); } bool PropagatingEitherWayPrivate::canCompute() const diff --git a/core/src/storage.cpp b/core/src/storage.cpp index 2638bf78..aacd9959 100644 --- a/core/src/storage.cpp +++ b/core/src/storage.cpp @@ -96,13 +96,21 @@ Interface::iterator Interface::clone(const InterfaceState &state) return it; } -void Interface::updatePriority(InterfaceState &state, const InterfaceState::Priority& priority) +Interface::container_type Interface::remove(iterator it) { - if (priority < state.priority()) { - auto it = std::find_if(begin(), end(), [&state](const InterfaceState& other) { return &state == &other; }); + container_type result; + moveTo(it, result, result.end()); + it->owner_ = nullptr; + return result; +} + +void Interface::updatePriority(InterfaceState *state, const InterfaceState::Priority& priority) +{ + if (priority < state->priority()) { + auto it = std::find_if(begin(), end(), [state](const InterfaceState& other) { return state == &other; }); // state should be part of the interface assert(it != end()); - state.priority_ = priority; + state->priority_ = priority; if (notify_) notify_(it, true); } }