diff --git a/core/include/moveit/task_constructor/container_p.h b/core/include/moveit/task_constructor/container_p.h index 04dd7858..36a7b2a2 100644 --- a/core/include/moveit/task_constructor/container_p.h +++ b/core/include/moveit/task_constructor/container_p.h @@ -42,6 +42,10 @@ #include #include "stage_p.h" +#include +#include +#include + #include #include @@ -118,7 +122,8 @@ public: InterfacePtr pendingForward() const { return pending_forward_; } // map InterfaceStates from children to external InterfaceStates of the container - const auto& internalToExternalMap() const { return internal_to_external_; } + const auto& internalToExternalMap() const { return internal_external_.left; } + const auto& externalToInternalMap() const { return internal_external_.right; } protected: ContainerBasePrivate(ContainerBase* me, const std::string& name); @@ -135,14 +140,16 @@ protected: child->setNextStarts(allowed ? pending_forward_ : InterfacePtr()); } - /// copy external_state to a child's interface and remember the link in internal_to map + /// copy external_state to a child's interface and remember the link in internal_external map template void copyState(Interface::iterator external, const InterfacePtr& target, bool updated); /// lift solution from internal to external level void liftSolution(const SolutionBasePtr& solution, const InterfaceState* internal_from, const InterfaceState* internal_to); - auto& internalToExternalMap() { return internal_to_external_; } + /// protected writable overloads + inline auto& internalToExternalMap() { return internal_external_.left; } + inline auto& ExternalToInternalMap() { return internal_external_.right; } // set in resolveInterface() InterfaceFlags required_interface_; @@ -151,7 +158,9 @@ private: container_type children_; // map start/end states of children (internal) to corresponding states in our external interfaces - std::map internal_to_external_; + boost::bimap, + boost::bimaps::unordered_multiset_of> + internal_external_; /* TODO: these interfaces don't need to be priority-sorted. * Introduce base class UnsortedInterface (which is a plain list) for this use case. */ diff --git a/core/src/container.cpp b/core/src/container.cpp index c0b9a990..495ad831 100644 --- a/core/src/container.cpp +++ b/core/src/container.cpp @@ -109,12 +109,10 @@ void ContainerBasePrivate::compute() { template void ContainerBasePrivate::copyState(Interface::iterator external, const InterfacePtr& target, bool updated) { if (updated) { - // TODO(v4hn): This is inefficient, consider storing inverse mapping for each start/end pair in children - // for parallel containers there will be multiple internal states - std::for_each(internal_to_external_.begin(), internal_to_external_.end(), [&external](auto& p) { - if (p.second == &*external) - setStatus(p.first, external->priority().status()); - }); + auto internals{ externalToInternalMap().equal_range(&*external) }; + for (auto& i = internals.first; i != internals.second; ++i) { + setStatus(i->second, external->priority().status()); + } return; } @@ -122,7 +120,7 @@ void ContainerBasePrivate::copyState(Interface::iterator external, const Interfa auto internal = states_.insert(states_.end(), InterfaceState(*external)); target->add(*internal); // and remember the mapping between them - internal_to_external_.insert(std::make_pair(&*internal, &*external)); + internalToExternalMap().insert(std::make_pair(&*internal, &*external)); } void ContainerBasePrivate::liftSolution(const SolutionBasePtr& solution, const InterfaceState* internal_from, @@ -131,12 +129,12 @@ void ContainerBasePrivate::liftSolution(const SolutionBasePtr& solution, const I // map internal to external states auto find_or_create_external = [this](const InterfaceState* internal, bool& created) -> InterfaceState* { - auto it = internal_to_external_.find(internal); - if (it != internal_to_external_.end()) - return it->second; + auto it = internalToExternalMap().find(internal); + if (it != internalToExternalMap().end()) + return const_cast(it->second); InterfaceState* external = &*states_.insert(states_.end(), InterfaceState(*internal)); - internal_to_external_.insert(std::make_pair(internal, external)); + internalToExternalMap().insert(std::make_pair(internal, external)); created = true; return external; }; @@ -240,7 +238,7 @@ void ContainerBase::reset() { impl->pending_backward_->clear(); impl->pending_forward_->clear(); // ... and state mapping - impl->internal_to_external_.clear(); + impl->internalToExternalMap().clear(); // interfaces depend on children which might change impl->required_interface_ = UNKNOWN;