diff --git a/core/include/moveit/task_constructor/container_p.h b/core/include/moveit/task_constructor/container_p.h index d62b9fbc..88ea239a 100644 --- a/core/include/moveit/task_constructor/container_p.h +++ b/core/include/moveit/task_constructor/container_p.h @@ -121,9 +121,14 @@ public: InterfacePtr pendingBackward() const { return pending_backward_; } InterfacePtr pendingForward() const { return pending_forward_; } + // tags for internal_external_ bimap + struct INTERNAL + {}; + struct EXTERNAL + {}; // map InterfaceStates from children to external InterfaceStates of the container - const auto& internalToExternalMap() const { return internal_external_.left; } - const auto& externalToInternalMap() const { return internal_external_.right; } + inline const auto& internalToExternalMap() const { return internal_external_.by(); } + inline const auto& externalToInternalMap() const { return internal_external_.by(); } /// called by a (direct) child when a solution failed void onNewFailure(const Stage& child, const InterfaceState* from, const InterfaceState* to); @@ -155,8 +160,8 @@ protected: const InterfaceState* internal_to); /// protected writable overloads - inline auto& internalToExternalMap() { return internal_external_.left; } - inline auto& ExternalToInternalMap() { return internal_external_.right; } + inline auto& internalToExternalMap() { return internal_external_.by(); } + inline auto& externalToInternalMap() { return internal_external_.by(); } // set in resolveInterface() InterfaceFlags required_interface_; @@ -165,8 +170,8 @@ private: container_type children_; // map start/end states of children (internal) to corresponding states in our external interfaces - boost::bimap, - boost::bimaps::unordered_multiset_of> + boost::bimap>, + boost::bimaps::unordered_multiset_of>> internal_external_; /* TODO: these interfaces don't need to be priority-sorted. diff --git a/core/src/container.cpp b/core/src/container.cpp index c144f114..34eb445d 100644 --- a/core/src/container.cpp +++ b/core/src/container.cpp @@ -159,13 +159,14 @@ void ContainerBasePrivate::setStatus(const InterfaceState* s, InterfaceState::St // if possible (i.e. if state s has an external counterpart), escalate setStatus to external interface if (parent()) { auto external{ internalToExternalMap().find(s) }; - if (external != internalToExternalMap().end()) { - // only escalate if there is no enabled alternative child - auto internals{ externalToInternalMap().equal_range(external->second) }; - auto is_enabled = [](auto&& state_pair) { return state_pair.second->priority().enabled(); }; + if (external != internalToExternalMap().end()) { // do we have an external state? + // only escalate if there is no other *enabled* internal state connected to the same external one + // all internal states linked to external + auto internals{ externalToInternalMap().equal_range(external->get()) }; + auto is_enabled = [](const auto& ext_int_pair) { return ext_int_pair.second->priority().enabled(); }; auto other_path{ std::find_if(internals.first, internals.second, is_enabled) }; if (other_path == internals.second) - parent()->pimpl()->setStatus(external->second, status); + parent()->pimpl()->setStatus(external->get(), status); return; } } @@ -175,6 +176,7 @@ void ContainerBasePrivate::setStatus(const InterfaceState* s, InterfaceState::St // This allows us to re-enable the FAILED side, while not (yet) consider the DISABLED states again, // when new states arrive in a Connecting stage. // All DISABLED states are only re-enabled if the FAILED state actually gets connected. + // For details, see: https://github.com/ros-planning/moveit_task_constructor/pull/221 if (status == InterfaceState::DISABLED_FAILED) status = InterfaceState::DISABLED; // only the first state is marked as FAILED