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 <moveit/macros/class_forward.h>
|
||||||
#include "stage_p.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 <map>
|
||||||
#include <climits>
|
#include <climits>
|
||||||
|
|
||||||
@ -118,7 +122,8 @@ public:
|
|||||||
InterfacePtr pendingForward() const { return pending_forward_; }
|
InterfacePtr pendingForward() const { return pending_forward_; }
|
||||||
|
|
||||||
// map InterfaceStates from children to external InterfaceStates of the container
|
// 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:
|
protected:
|
||||||
ContainerBasePrivate(ContainerBase* me, const std::string& name);
|
ContainerBasePrivate(ContainerBase* me, const std::string& name);
|
||||||
@ -135,14 +140,16 @@ protected:
|
|||||||
child->setNextStarts(allowed ? pending_forward_ : InterfacePtr());
|
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>
|
template <Interface::Direction>
|
||||||
void copyState(Interface::iterator external, const InterfacePtr& target, bool updated);
|
void copyState(Interface::iterator external, const InterfacePtr& target, bool updated);
|
||||||
/// lift solution from internal to external level
|
/// lift solution from internal to external level
|
||||||
void liftSolution(const SolutionBasePtr& solution, const InterfaceState* internal_from,
|
void liftSolution(const SolutionBasePtr& solution, const InterfaceState* internal_from,
|
||||||
const InterfaceState* internal_to);
|
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()
|
// set in resolveInterface()
|
||||||
InterfaceFlags required_interface_;
|
InterfaceFlags required_interface_;
|
||||||
@ -151,7 +158,9 @@ private:
|
|||||||
container_type children_;
|
container_type children_;
|
||||||
|
|
||||||
// map start/end states of children (internal) to corresponding states in our external interfaces
|
// 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.
|
/* TODO: these interfaces don't need to be priority-sorted.
|
||||||
* Introduce base class UnsortedInterface (which is a plain list) for this use case. */
|
* Introduce base class UnsortedInterface (which is a plain list) for this use case. */
|
||||||
|
|||||||
@ -109,12 +109,10 @@ void ContainerBasePrivate::compute() {
|
|||||||
template <Interface::Direction dir>
|
template <Interface::Direction dir>
|
||||||
void ContainerBasePrivate::copyState(Interface::iterator external, const InterfacePtr& target, bool updated) {
|
void ContainerBasePrivate::copyState(Interface::iterator external, const InterfacePtr& target, bool updated) {
|
||||||
if (updated) {
|
if (updated) {
|
||||||
// TODO(v4hn): This is inefficient, consider storing inverse mapping for each start/end pair in children
|
auto internals{ externalToInternalMap().equal_range(&*external) };
|
||||||
// for parallel containers there will be multiple internal states
|
for (auto& i = internals.first; i != internals.second; ++i) {
|
||||||
std::for_each(internal_to_external_.begin(), internal_to_external_.end(), [&external](auto& p) {
|
setStatus<dir>(i->second, external->priority().status());
|
||||||
if (p.second == &*external)
|
}
|
||||||
setStatus<dir>(p.first, external->priority().status());
|
|
||||||
});
|
|
||||||
return;
|
return;
|
||||||
}
|
}
|
||||||
|
|
||||||
@ -122,7 +120,7 @@ void ContainerBasePrivate::copyState(Interface::iterator external, const Interfa
|
|||||||
auto internal = states_.insert(states_.end(), InterfaceState(*external));
|
auto internal = states_.insert(states_.end(), InterfaceState(*external));
|
||||||
target->add(*internal);
|
target->add(*internal);
|
||||||
// and remember the mapping between them
|
// 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,
|
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
|
// map internal to external states
|
||||||
auto find_or_create_external = [this](const InterfaceState* internal, bool& created) -> InterfaceState* {
|
auto find_or_create_external = [this](const InterfaceState* internal, bool& created) -> InterfaceState* {
|
||||||
auto it = internal_to_external_.find(internal);
|
auto it = internalToExternalMap().find(internal);
|
||||||
if (it != internal_to_external_.end())
|
if (it != internalToExternalMap().end())
|
||||||
return it->second;
|
return const_cast<InterfaceState*>(it->second);
|
||||||
|
|
||||||
InterfaceState* external = &*states_.insert(states_.end(), InterfaceState(*internal));
|
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;
|
created = true;
|
||||||
return external;
|
return external;
|
||||||
};
|
};
|
||||||
@ -240,7 +238,7 @@ void ContainerBase::reset() {
|
|||||||
impl->pending_backward_->clear();
|
impl->pending_backward_->clear();
|
||||||
impl->pending_forward_->clear();
|
impl->pending_forward_->clear();
|
||||||
// ... and state mapping
|
// ... and state mapping
|
||||||
impl->internal_to_external_.clear();
|
impl->internalToExternalMap().clear();
|
||||||
|
|
||||||
// interfaces depend on children which might change
|
// interfaces depend on children which might change
|
||||||
impl->required_interface_ = UNKNOWN;
|
impl->required_interface_ = UNKNOWN;
|
||||||
|
|||||||
Loading…
Reference in New Issue
Block a user