diff --git a/include/moveit_task_constructor/stage.h b/include/moveit_task_constructor/stage.h index f1a95f2f..55d3bab5 100644 --- a/include/moveit_task_constructor/stage.h +++ b/include/moveit_task_constructor/stage.h @@ -59,6 +59,10 @@ public: typedef std::unique_ptr pointer; ~Stage(); + /// auto-convert Stage to StagePrivate* when needed + operator StagePrivate*(); + operator const StagePrivate*() const; + /// initialize stage once before planning virtual bool init(const planning_scene::PlanningSceneConstPtr& scene); @@ -79,6 +83,7 @@ class ComputeBasePrivate; class ComputeBase : public Stage { public: PRIVATE_CLASS(ComputeBase) + bool init(const planning_scene::PlanningSceneConstPtr &scene) override; virtual size_t numSolutions() const override; protected: diff --git a/src/container.cpp b/src/container.cpp index 515c40b6..c50136bf 100644 --- a/src/container.cpp +++ b/src/container.cpp @@ -95,8 +95,11 @@ void ContainerBase::clear() SerialContainerPrivate::SerialContainerPrivate(SerialContainer *me, const std::string &name) : ContainerBasePrivate(me, name) { + // TODO rhaschke: define notify functions starts_.reset(new Interface(Interface::NotifyFunction())); ends_.reset(new Interface(Interface::NotifyFunction())); + pending_backward_.reset(new Interface(Interface::NotifyFunction())); + pending_forward_.reset(new Interface(Interface::NotifyFunction())); } InterfaceFlags SerialContainerPrivate::announcedFlags() const { @@ -107,39 +110,6 @@ InterfaceFlags SerialContainerPrivate::announcedFlags() const { return f; } -#if 0 -bool SerialContainerPrivate::init() -{ - assert(canInsert(*stage, before)); - bool at_begin = (before == children().begin()); - bool at_end = (before == children().end()); - - StagePrivate *cur = stage->pimpl(); - /* set pointer cache (prev_ends_ and next_starts_) of prev, current, and next stage */ - if (children().empty()) { // first child inserted - cur->setPrevEnds(this->starts()); - cur->setNextStarts(this->ends()); - } else if (at_begin) { - StagePrivate *next = (*before)->pimpl(); - cur->setPrevEnds(this->starts()); - cur->setNextStarts(next->starts()); - next->setPrevEnds(cur->ends()); - } else if (at_end) { - StagePrivate *prev = (*this->prev(before))->pimpl(); - prev->setNextStarts(cur->starts()); - cur->setPrevEnds(prev->ends()); - cur->setNextStarts(this->ends()); - } else { - StagePrivate *prev = (*this->prev(before))->pimpl(); - StagePrivate *next = (*before)->pimpl(); - prev->setNextStarts(cur->starts()); - cur->setPrevEnds(prev->ends()); - cur->setNextStarts(next->starts()); - next->setPrevEnds(cur->ends()); - } -} -#endif - inline ContainerBasePrivate::const_iterator SerialContainerPrivate::prev(const_iterator it) const { assert(it != children().cbegin()); @@ -260,16 +230,55 @@ SerialContainer::SerialContainer(const std::string &name) {} PIMPL_FUNCTIONS(SerialContainer) +void SerialContainerPrivate::connect(StagePrivate* prev, StagePrivate* next) { + prev->setNextStarts(next->starts()); + next->setPrevEnds(prev->ends()); +} + bool SerialContainer::init(const planning_scene::PlanningSceneConstPtr &scene) { auto impl = pimpl(); + // clear queues + impl->internal_to_my_starts_.clear(); + impl->internal_to_my_ends_.clear(); + impl->solutions_.clear(); + // recursively init all children for (auto& stage : impl->children()) { - // derived classes should call Stage::init internally , but we cannot be sure... - if (!stage->init(scene) ||!stage->Stage::init(scene)) + if (!stage->Stage::init(scene) || !stage->init(scene)) return false; } - return !impl->children().empty(); + + // we need to have some children to do the actual work + if (impl->children().empty()) + return false; + + /*** connect children ***/ + // first stage sends backward to pending_backward_ + auto cur = impl->children().begin(); + (*cur)->pimpl()->setPrevEnds(impl->pending_backward_.get()); + + // last stage sends forward to pending_forward_ + auto last = --impl->children().end(); + (*last)->pimpl()->setNextStarts(impl->pending_forward_.get()); + + auto prev = cur; ++cur; // prev points to 1st, cur points to 2nd stage + if (prev != last) {// we have more than one children + auto next = cur; ++next; // next points to 3rd stage (or end) + for (; cur != last; ++prev, ++cur, ++next) { + impl->connect(**prev, **cur); + impl->connect(**cur, **next); + } + // finally connect last == cur and prev stage + impl->connect(**prev, **cur); + } + + // validate connectivity of chain + for (const Stage::pointer& stage : impl->children()) + if (!stage->pimpl()->validate()) + return false; + + return true; } bool SerialContainer::canCompute() const diff --git a/src/container_p.h b/src/container_p.h index 49306478..5faa1af8 100644 --- a/src/container_p.h +++ b/src/container_p.h @@ -77,6 +77,7 @@ public: private: inline const_iterator prev(const_iterator it) const; inline const_iterator next(const_iterator it) const; + void connect(StagePrivate *prev, StagePrivate *next); /* A container needs to decouple its interface from those of its children: * A solution of a container needs to connect start to end via a full path. diff --git a/src/stage.cpp b/src/stage.cpp index 4bd810ff..c4172e6d 100644 --- a/src/stage.cpp +++ b/src/stage.cpp @@ -34,6 +34,16 @@ inline InterfaceFlags StagePrivate::deducedFlags() const return f; } +inline bool implies(bool p, bool q) { return !p || q; } +bool StagePrivate::validate() const { + InterfaceFlags f = announcedFlags(); + if (!implies(f & WRITES_NEXT_START, nextStarts())) + return false; + + if (!implies(f & WRITES_PREV_END, prevEnds())) + return false; + return true; +} Stage::Stage(StagePrivate *impl) : pimpl_(impl) @@ -46,19 +56,21 @@ Stage::~Stage() delete pimpl_; } +Stage::operator StagePrivate *() { + return pimpl(); +} + +Stage::operator const StagePrivate *() const { + return pimpl(); +} -inline bool implies(bool p, bool q) { return !p || q; } bool Stage::init(const planning_scene::PlanningSceneConstPtr &scene) { -#if 0 auto impl = pimpl(); - InterfaceFlags f = impl->announcedFlags(); - if (!implies(f & WRITES_NEXT_START, impl->nextStarts())) - throw std::runtime_error("missing next unit for sendForward()"); - - if (!implies(f & WRITES_PREV_END, impl->prevEnds())) - throw std::runtime_error("missing previous unit for sendBackward()"); -#endif + if (impl->starts_) impl->starts_->clear(); + if (impl->ends_) impl->ends_->clear(); + impl->prev_ends_ = nullptr; + impl->next_starts_ = nullptr; return true; } @@ -118,11 +130,15 @@ size_t ComputeBase::numSolutions() const { return pimpl()->trajectories_.size(); } -const std::list &ComputeBase::trajectories() const -{ +const std::list &ComputeBase::trajectories() const { return pimpl()->trajectories_; } +bool ComputeBase::init(const planning_scene::PlanningSceneConstPtr &scene) { + pimpl()->trajectories_.clear(); + return Stage::init(scene); +} + PropagatingEitherWayPrivate::PropagatingEitherWayPrivate(PropagatingEitherWay *me, PropagatingEitherWay::Direction dir, const std::string &name) : ComputeBasePrivate(me, name), dir(dir) diff --git a/src/stage_p.h b/src/stage_p.h index f13c846d..956088cb 100644 --- a/src/stage_p.h +++ b/src/stage_p.h @@ -51,6 +51,8 @@ public: inline Interface* nextStarts() const { return next_starts_; } inline bool isConnected() const { return prev_ends_ || next_starts_; } + // validate that sendForward() and sendBackward() will succeed + bool validate() const; inline void setHierarchy(ContainerBasePrivate* parent, container_type::iterator it) { parent_ = parent;