From cf71d2f0d25e531faf05d409cefbd14b5f42a6b8 Mon Sep 17 00:00:00 2001 From: Robert Haschke Date: Wed, 31 Jan 2018 10:44:02 +0100 Subject: [PATCH] unify use of buffer interfaces in containers All containers need to buffer their children's sendBackward/sendForward states. --- .../moveit/task_constructor/container.h | 2 +- .../moveit/task_constructor/container_p.h | 50 +++++++++---------- core/src/container.cpp | 37 ++++++-------- 3 files changed, 38 insertions(+), 51 deletions(-) diff --git a/core/include/moveit/task_constructor/container.h b/core/include/moveit/task_constructor/container.h index 22178bc3..5cfcc150 100644 --- a/core/include/moveit/task_constructor/container.h +++ b/core/include/moveit/task_constructor/container.h @@ -150,7 +150,7 @@ protected: /** Wrap an existing solution - for use in containers. * * 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 */ class WrappedSolution : public SolutionBase { public: diff --git a/core/include/moveit/task_constructor/container_p.h b/core/include/moveit/task_constructor/container_p.h index 66fadb12..306cefcb 100644 --- a/core/include/moveit/task_constructor/container_p.h +++ b/core/include/moveit/task_constructor/container_p.h @@ -47,13 +47,16 @@ namespace moveit { namespace task_constructor { -/* A container needs to decouple its interfaces from those of its children. - * Both, the container and the children have their own starts_ and ends_. - * The container needs to forward states received in its interfaces to - * the interfaces of the children. - * Solutions found by the children, then need to be connected to the - * container's interface states. To this end, we remember the mapping - * from internal to external states. +/* A container needs to decouple its own (external) interfaces + * from those (internal) of its children. + * Both, the container and the children have their own pull interfaces: starts_ and ends_. + * The container forwards states received in its pull interfaces to the + * corresponding interfaces of the children. + * States pushed by children are temporarily stored in pending_backward_ and pending_forward_. + * + * 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. * These solutions might origin from different children (ParallelContainer) * or from different solution paths in a SerialContainer. @@ -93,9 +96,7 @@ public: bool compute() override; protected: - ContainerBasePrivate(ContainerBase *me, const std::string &name) - : StagePrivate(me, name) - {} + ContainerBasePrivate(ContainerBase *me, const std::string &name); /// 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); @@ -105,6 +106,11 @@ protected: // map start/end states of children (internal) to corresponding states in our external interfaces std::map 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) @@ -134,18 +140,14 @@ private: * 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, * 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 { friend class SerialContainer; public: - SerialContainerPrivate(SerialContainer* me, const std::string &name); + SerialContainerPrivate(SerialContainer* me, const std::string &name) + : ContainerBasePrivate(me, name) + {} void storeNewSolution(SerialSolution&& s); const ordered& solutions() const { return solutions_; } @@ -153,11 +155,6 @@ public: private: 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 ordered solutions_; }; @@ -177,11 +174,10 @@ class WrapperBasePrivate : public ContainerBasePrivate { friend class WrapperBase; 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) diff --git a/core/src/container.cpp b/core/src/container.cpp index 0fd19e25..246c3553 100644 --- a/core/src/container.cpp +++ b/core/src/container.cpp @@ -49,6 +49,13 @@ using namespace std::placeholders; 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 { const_iterator position = children_.begin(); if (index > 0) { @@ -156,7 +163,10 @@ void ContainerBase::reset() for (auto& child: impl->children()) child->reset(); - // clear mapping + // clear buffer interfaces + impl->pending_backward_->clear(); + impl->pending_forward_->clear(); + // ... and state mapping impl->internal_to_external_.clear(); 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 { SolutionCollector(size_t max_depth) : max_depth(max_depth) {} @@ -320,8 +321,6 @@ void SerialContainer::reset() // clear queues impl->solutions_.clear(); - impl->pending_backward_->clear(); - impl->pending_forward_->clear(); // recursively reset children 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(new WrapperBasePrivate(this, name), std::move(child)) {} @@ -548,8 +540,7 @@ bool WrapperBase::insert(Stage::pointer &&stage, int before) void WrapperBase::reset() { - pimpl()->dummy_starts_->clear(); - pimpl()->dummy_ends_->clear(); + ContainerBase::reset(); } void WrapperBase::init(const planning_scene::PlanningSceneConstPtr &scene) @@ -564,8 +555,8 @@ void WrapperBase::init(const planning_scene::PlanningSceneConstPtr &scene) assert(!impl->ends()); // provide a dummy interface to receive interface states of wrapped child - wrapped()->pimpl()->setPrevEnds(impl->dummy_ends_); - wrapped()->pimpl()->setNextStarts(impl->dummy_starts_); + wrapped()->pimpl()->setPrevEnds(impl->pending_backward_); + wrapped()->pimpl()->setNextStarts(impl->pending_forward_); // init + validate children ContainerBase::init(scene);