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.