Improve readability of internal-external bimap using tags (#293)

This commit is contained in:
Robert Haschke 2021-09-16 13:52:34 +02:00 committed by GitHub
parent 639481726e
commit d0ab3c3703
No known key found for this signature in database
GPG Key ID: 4AEE18F83AFDEB23
2 changed files with 18 additions and 11 deletions

View File

@ -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<INTERNAL>(); }
inline const auto& externalToInternalMap() const { return internal_external_.by<EXTERNAL>(); }
/// 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<INTERNAL>(); }
inline auto& externalToInternalMap() { return internal_external_.by<EXTERNAL>(); }
// 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_set_of<const InterfaceState*>,
boost::bimaps::unordered_multiset_of<const InterfaceState*>>
boost::bimap<boost::bimaps::unordered_set_of<boost::bimaps::tagged<const InterfaceState*, INTERNAL>>,
boost::bimaps::unordered_multiset_of<boost::bimaps::tagged<const InterfaceState*, EXTERNAL>>>
internal_external_;
/* TODO: these interfaces don't need to be priority-sorted.

View File

@ -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<EXTERNAL>()) };
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<dir>(external->second, status);
parent()->pimpl()->setStatus<dir>(external->get<EXTERNAL>(), 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