implement bimap for internal_external state map

Get hashing for inverted lookups, but incur
structural overhead.

Whether this is worth it depends on the number of mapped interface states
and the number of pruning/reactivation requests.
This commit is contained in:
v4hn 2021-03-27 23:30:54 +01:00
parent 0e3ec0b6ec
commit 472a78800d
2 changed files with 23 additions and 16 deletions

View File

@ -42,6 +42,10 @@
#include <moveit/macros/class_forward.h>
#include "stage_p.h"
#include <boost/bimap.hpp>
#include <boost/bimap/unordered_set_of.hpp>
#include <boost/bimap/unordered_multiset_of.hpp>
#include <map>
#include <climits>
@ -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 <Interface::Direction>
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<const InterfaceState*, InterfaceState*> internal_to_external_;
boost::bimap<boost::bimaps::unordered_set_of<const InterfaceState*>,
boost::bimaps::unordered_multiset_of<const InterfaceState*>>
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. */

View File

@ -109,12 +109,10 @@ void ContainerBasePrivate::compute() {
template <Interface::Direction dir>
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<dir>(p.first, external->priority().status());
});
auto internals{ externalToInternalMap().equal_range(&*external) };
for (auto& i = internals.first; i != internals.second; ++i) {
setStatus<dir>(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<InterfaceState*>(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;