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.
*
* 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:

View File

@ -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<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)
@ -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<SerialSolution>& 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<SerialSolution> 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)

View File

@ -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);