unify use of buffer interfaces in containers

All containers need to buffer their children's sendBackward/sendForward states.
This commit is contained in:
Robert Haschke 2018-01-31 10:44:02 +01:00
parent 3a4dc6755b
commit cf71d2f0d2
3 changed files with 38 additions and 51 deletions

View File

@ -150,7 +150,7 @@ protected:
/** Wrap an existing solution - for use in containers. /** Wrap an existing solution - for use in containers.
* *
* This essentially wraps a solution of a child and thus allows * This essentially wraps a solution of a child and thus allows
* for new new clones of start / end states, which in turn will * for new clones of start / end states, which in turn will
* have separate incoming/outgoing trajectories */ * have separate incoming/outgoing trajectories */
class WrappedSolution : public SolutionBase { class WrappedSolution : public SolutionBase {
public: public:

View File

@ -47,13 +47,16 @@
namespace moveit { namespace task_constructor { namespace moveit { namespace task_constructor {
/* A container needs to decouple its interfaces from those of its children. /* A container needs to decouple its own (external) interfaces
* Both, the container and the children have their own starts_ and ends_. * from those (internal) of its children.
* The container needs to forward states received in its interfaces to * Both, the container and the children have their own pull interfaces: starts_ and ends_.
* the interfaces of the children. * The container forwards states received in its pull interfaces to the
* Solutions found by the children, then need to be connected to the * corresponding interfaces of the children.
* container's interface states. To this end, we remember the mapping * States pushed by children are temporarily stored in pending_backward_ and pending_forward_.
* from internal to external states. *
* Solutions found by the children need to be lifted from the internal level to the
* external level. To this end, we remember the mapping from internal to external states.
*
* Note, that there might be many solutions connecting a single start-end pair. * Note, that there might be many solutions connecting a single start-end pair.
* These solutions might origin from different children (ParallelContainer) * These solutions might origin from different children (ParallelContainer)
* or from different solution paths in a SerialContainer. * or from different solution paths in a SerialContainer.
@ -93,9 +96,7 @@ public:
bool compute() override; bool compute() override;
protected: protected:
ContainerBasePrivate(ContainerBase *me, const std::string &name) ContainerBasePrivate(ContainerBase *me, const std::string &name);
: StagePrivate(me, name)
{}
/// 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_to map
void copyState(Interface::iterator external, Stage &child, bool to_start, bool updated); void copyState(Interface::iterator external, Stage &child, bool to_start, bool updated);
@ -105,6 +106,11 @@ protected:
// 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*, Interface::iterator> internal_to_external_; std::map<const InterfaceState*, Interface::iterator> internal_to_external_;
// interface to receive children's sendBackward() states
InterfacePtr pending_backward_;
// interface to receive children's sendForward() states
InterfacePtr pending_forward_;
}; };
PIMPL_FUNCTIONS(ContainerBase) PIMPL_FUNCTIONS(ContainerBase)
@ -134,18 +140,14 @@ private:
* The solution of a single child stage is usually disconnected to the container's start or end. * The solution of a single child stage is usually disconnected to the container's start or end.
* Only if all the children in the chain have found a coherent solution from start to end, * Only if all the children in the chain have found a coherent solution from start to end,
* this solution can be announced as a solution of the SerialContainer. * this solution can be announced as a solution of the SerialContainer.
* */
* Particularly, the first/last stage's sendBackward()/sendForward() call
* cannot directly propagate their associated state to the previous/next stage of this container,
* because we cannot provide a full solution yet. Hence, the first/last stage
* propagate to the pending_backward_/pending_forward_ interface first.
* If eventually a full solution is found, it is propagated to prevEnds()/nextStarts() -
* together with the solution. */
class SerialContainerPrivate : public ContainerBasePrivate { class SerialContainerPrivate : public ContainerBasePrivate {
friend class SerialContainer; friend class SerialContainer;
public: public:
SerialContainerPrivate(SerialContainer* me, const std::string &name); SerialContainerPrivate(SerialContainer* me, const std::string &name)
: ContainerBasePrivate(me, name)
{}
void storeNewSolution(SerialSolution&& s); void storeNewSolution(SerialSolution&& s);
const ordered<SerialSolution>& solutions() const { return solutions_; } const ordered<SerialSolution>& solutions() const { return solutions_; }
@ -153,11 +155,6 @@ public:
private: private:
void connect(StagePrivate *prev, StagePrivate *next); void connect(StagePrivate *prev, StagePrivate *next);
// interface to buffer first child's sendBackward() states
InterfacePtr pending_backward_;
// interface to buffer last child's sendForward() states
InterfacePtr pending_forward_;
// set of all solutions // set of all solutions
ordered<SerialSolution> solutions_; ordered<SerialSolution> solutions_;
}; };
@ -177,11 +174,10 @@ class WrapperBasePrivate : public ContainerBasePrivate {
friend class WrapperBase; friend class WrapperBase;
public: public:
WrapperBasePrivate(WrapperBase* me, const std::string& name); WrapperBasePrivate(WrapperBase* me, const std::string& name)
: ContainerBasePrivate(me, name)
{}
private:
InterfacePtr dummy_starts_;
InterfacePtr dummy_ends_;
}; };
PIMPL_FUNCTIONS(WrapperBase) PIMPL_FUNCTIONS(WrapperBase)

View File

@ -49,6 +49,13 @@ using namespace std::placeholders;
namespace moveit { namespace task_constructor { namespace moveit { namespace task_constructor {
ContainerBasePrivate::ContainerBasePrivate(ContainerBase *me, const std::string &name)
: StagePrivate(me, name)
{
pending_backward_.reset(new Interface(Interface::NotifyFunction()));
pending_forward_.reset(new Interface(Interface::NotifyFunction()));
}
ContainerBasePrivate::const_iterator ContainerBasePrivate::position(int index) const { ContainerBasePrivate::const_iterator ContainerBasePrivate::position(int index) const {
const_iterator position = children_.begin(); const_iterator position = children_.begin();
if (index > 0) { if (index > 0) {
@ -156,7 +163,10 @@ void ContainerBase::reset()
for (auto& child: impl->children()) for (auto& child: impl->children())
child->reset(); child->reset();
// clear mapping // clear buffer interfaces
impl->pending_backward_->clear();
impl->pending_forward_->clear();
// ... and state mapping
impl->internal_to_external_.clear(); impl->internal_to_external_.clear();
Stage::reset(); Stage::reset();
@ -190,15 +200,6 @@ void ContainerBase::init(const planning_scene::PlanningSceneConstPtr &scene)
} }
SerialContainerPrivate::SerialContainerPrivate(SerialContainer *me, const std::string &name)
: ContainerBasePrivate(me, name)
{
// these lists don't need a notify function, connections are handled by onNewSolution()
pending_backward_.reset(new Interface(Interface::NotifyFunction()));
pending_forward_.reset(new Interface(Interface::NotifyFunction()));
}
struct SolutionCollector { struct SolutionCollector {
SolutionCollector(size_t max_depth) : max_depth(max_depth) {} SolutionCollector(size_t max_depth) : max_depth(max_depth) {}
@ -320,8 +321,6 @@ void SerialContainer::reset()
// clear queues // clear queues
impl->solutions_.clear(); impl->solutions_.clear();
impl->pending_backward_->clear();
impl->pending_forward_->clear();
// recursively reset children // recursively reset children
ContainerBase::reset(); ContainerBase::reset();
@ -521,13 +520,6 @@ void ParallelContainerBase::onNewSolution(const SolutionBase &s)
} }
WrapperBasePrivate::WrapperBasePrivate(WrapperBase *me, const std::string &name)
: ContainerBasePrivate(me, name)
{
dummy_starts_.reset(new Interface(Interface::NotifyFunction()));
dummy_ends_.reset(new Interface(Interface::NotifyFunction()));
}
WrapperBase::WrapperBase(const std::string &name, Stage::pointer &&child) WrapperBase::WrapperBase(const std::string &name, Stage::pointer &&child)
: WrapperBase(new WrapperBasePrivate(this, name), std::move(child)) : WrapperBase(new WrapperBasePrivate(this, name), std::move(child))
{} {}
@ -548,8 +540,7 @@ bool WrapperBase::insert(Stage::pointer &&stage, int before)
void WrapperBase::reset() void WrapperBase::reset()
{ {
pimpl()->dummy_starts_->clear(); ContainerBase::reset();
pimpl()->dummy_ends_->clear();
} }
void WrapperBase::init(const planning_scene::PlanningSceneConstPtr &scene) void WrapperBase::init(const planning_scene::PlanningSceneConstPtr &scene)
@ -564,8 +555,8 @@ void WrapperBase::init(const planning_scene::PlanningSceneConstPtr &scene)
assert(!impl->ends()); assert(!impl->ends());
// provide a dummy interface to receive interface states of wrapped child // provide a dummy interface to receive interface states of wrapped child
wrapped()->pimpl()->setPrevEnds(impl->dummy_ends_); wrapped()->pimpl()->setPrevEnds(impl->pending_backward_);
wrapped()->pimpl()->setNextStarts(impl->dummy_starts_); wrapped()->pimpl()->setNextStarts(impl->pending_forward_);
// init + validate children // init + validate children
ContainerBase::init(scene); ContainerBase::init(scene);