diff --git a/core/include/moveit/task_constructor/storage.h b/core/include/moveit/task_constructor/storage.h index 0386ff3f..4089bcd0 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); 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 a8b9f4a6..d1a19448 100644 --- a/core/src/container.cpp +++ b/core/src/container.cpp @@ -283,11 +283,11 @@ 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 + // update state priority at both ends of the partial solution path const InterfaceState* start = (in.first.empty() ? current : *in.first.back()).start(); - start->owner()->updatePriority(*const_cast(start), prio); + if (start->owner()) 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); + if (end->owner()) end->owner()->updatePriority(*const_cast(end), prio); } } } diff --git a/core/src/stage.cpp b/core/src/stage.cpp index ca8c338b..4c003a1e 100644 --- a/core/src/stage.cpp +++ b/core/src/stage.cpp @@ -262,12 +262,12 @@ PropagatingEitherWayPrivate::PropagatingEitherWayPrivate(PropagatingEitherWay *m 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{ @@ -276,8 +276,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{ @@ -287,7 +288,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..40b5f14f 100644 --- a/core/src/storage.cpp +++ b/core/src/storage.cpp @@ -96,6 +96,14 @@ Interface::iterator Interface::clone(const InterfaceState &state) return it; } +Interface::container_type Interface::remove(iterator it) +{ + 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()) {