mirror of
https://github.com/moveit/moveit_task_constructor.git
synced 2025-11-04 14:49:57 +08:00
unify use of buffer interfaces in containers
All containers need to buffer their children's sendBackward/sendForward states.
This commit is contained in:
parent
3a4dc6755b
commit
cf71d2f0d2
@ -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:
|
||||
|
||||
@ -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)
|
||||
|
||||
|
||||
@ -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);
|
||||
|
||||
Loading…
Reference in New Issue
Block a user