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.
|
/** 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:
|
||||||
|
|||||||
@ -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)
|
||||||
|
|
||||||
|
|||||||
@ -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);
|
||||||
|
|||||||
Loading…
Reference in New Issue
Block a user