mirror of
https://github.com/moveit/moveit_task_constructor.git
synced 2025-11-04 14:49:57 +08:00
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:
parent
0e3ec0b6ec
commit
472a78800d
@ -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. */
|
||||
|
||||
@ -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;
|
||||
|
||||
Loading…
Reference in New Issue
Block a user