From e33962364d248431679cd608ecfbd5ce6b282a8f Mon Sep 17 00:00:00 2001 From: v4hn Date: Wed, 17 Mar 2021 00:15:12 +0100 Subject: [PATCH] resolve PruningMultiForward test Every InterfaceState along the partial solution has to be disabled for pruning, not just the ends that are currently relevant. --- core/include/moveit/task_constructor/storage.h | 2 ++ core/src/stage.cpp | 18 ++++++++++++++++-- 2 files changed, 18 insertions(+), 2 deletions(-) diff --git a/core/include/moveit/task_constructor/storage.h b/core/include/moveit/task_constructor/storage.h index fb561649..24e0cb45 100644 --- a/core/include/moveit/task_constructor/storage.h +++ b/core/include/moveit/task_constructor/storage.h @@ -76,6 +76,8 @@ class InterfaceState { friend class SolutionBase; // addIncoming() / addOutgoing() should be called only by SolutionBase friend class Interface; // allow Interface to set owner_ and priority_ + friend class StagePrivate; + public: enum Status { diff --git a/core/src/stage.cpp b/core/src/stage.cpp index c0d3e15a..860f89b0 100644 --- a/core/src/stage.cpp +++ b/core/src/stage.cpp @@ -153,9 +153,23 @@ void StagePrivate::setStatus(const InterfaceState* s, InterfaceState::Status sta if (s->priority().status() == status) return; // nothing changing - // actually enable/disable the state - if (s->owner()) + // if we should disable the state, only do so when there is no enabled alternative path + if (status == InterfaceState::DISABLED) { + auto solution_is_enabled = [](auto&& solution) { + return state()>(*solution)->priority().enabled(); + }; + const auto& alternatives = trajectories()>(*s); + auto alternative_path = std::find_if(alternatives.cbegin(), alternatives.cend(), solution_is_enabled); + if (alternative_path != alternatives.cend()) + return; + } + + if (s->owner()) { + // actually enable/disable the state s->owner()->updatePriority(const_cast(s), InterfaceState::Priority(s->priority(), status)); + } else { + const_cast(s)->priority_ = InterfaceState::Priority(s->priority(), status); + } // To break symmetry between both ends of a partial solution sequence that gets disabled, // we mark the first state with DISABLED_FAILED and all other states down the tree only with DISABLED.