mirror of
https://github.com/moveit/moveit_task_constructor.git
synced 2025-11-04 14:49:57 +08:00
Improve readability of internal-external bimap using tags (#293)
This commit is contained in:
parent
639481726e
commit
d0ab3c3703
@ -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.
|
||||
|
||||
@ -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
|
||||
|
||||
|
||||
Loading…
Reference in New Issue
Block a user