From 1eacea6b35d4534811f3cccc22409c753bfbd819 Mon Sep 17 00:00:00 2001 From: Robert Haschke Date: Mon, 19 Feb 2018 08:46:52 +0100 Subject: [PATCH 1/3] reset InterfaceState::owner_ if state is removed from Interface --- core/include/moveit/task_constructor/storage.h | 11 +++++++++++ core/src/container.cpp | 6 +++--- core/src/stage.cpp | 12 +++++++----- core/src/storage.cpp | 8 ++++++++ 4 files changed, 29 insertions(+), 8 deletions(-) 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()) { From 373d7f204f87cd6a84882b9b1ed5783742f4f77f Mon Sep 17 00:00:00 2001 From: Robert Haschke Date: Mon, 19 Feb 2018 08:13:57 +0100 Subject: [PATCH 2/3] update priorities of all interface states along a (partial) solution path if a parallel container is involved somewhere in the middle, it will again access these states, e.g. planning alternative solutions --- core/src/container.cpp | 26 ++++++++++++++++++++------ 1 file changed, 20 insertions(+), 6 deletions(-) diff --git a/core/src/container.cpp b/core/src/container.cpp index d1a19448..eadfee49 100644 --- a/core/src/container.cpp +++ b/core/src/container.cpp @@ -235,10 +235,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(); @@ -283,11 +298,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 priority at both ends of the partial solution path - const InterfaceState* start = (in.first.empty() ? current : *in.first.back()).start(); - if (start->owner()) start->owner()->updatePriority(*const_cast(start), prio); - const InterfaceState* end = (out.first.empty() ? current : *out.first.back()).end(); - if (end->owner()) 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); } } } From e368fd9948c6f141cb473fb9736f7c41c6c4ef41 Mon Sep 17 00:00:00 2001 From: Robert Haschke Date: Mon, 19 Feb 2018 08:18:31 +0100 Subject: [PATCH 3/3] avoid accidental overwrite of InterfaceState --- core/include/moveit/task_constructor/storage.h | 2 +- core/src/container.cpp | 4 ++-- core/src/storage.cpp | 8 ++++---- 3 files changed, 7 insertions(+), 7 deletions(-) diff --git a/core/include/moveit/task_constructor/storage.h b/core/include/moveit/task_constructor/storage.h index 4089bcd0..c91e2441 100644 --- a/core/include/moveit/task_constructor/storage.h +++ b/core/include/moveit/task_constructor/storage.h @@ -156,7 +156,7 @@ public: 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_; diff --git a/core/src/container.cpp b/core/src/container.cpp index eadfee49..b0c13303 100644 --- a/core/src/container.cpp +++ b/core/src/container.cpp @@ -246,12 +246,12 @@ void updateStateCosts(const SerialContainer::solution_container &partial_solutio // 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); + 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); + if (state->owner()) state->owner()->updatePriority(state, prio); } void SerialContainer::onNewSolution(const SolutionBase ¤t) diff --git a/core/src/storage.cpp b/core/src/storage.cpp index 40b5f14f..aacd9959 100644 --- a/core/src/storage.cpp +++ b/core/src/storage.cpp @@ -104,13 +104,13 @@ Interface::container_type Interface::remove(iterator it) return result; } -void Interface::updatePriority(InterfaceState &state, const InterfaceState::Priority& priority) +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; }); + 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); } }