connect and validate chain

This commit is contained in:
Robert Haschke 2017-10-20 13:16:25 +02:00
parent b967e8231b
commit bc5c76578e
5 changed files with 80 additions and 47 deletions

View File

@ -59,6 +59,10 @@ public:
typedef std::unique_ptr<Stage> pointer; typedef std::unique_ptr<Stage> pointer;
~Stage(); ~Stage();
/// auto-convert Stage to StagePrivate* when needed
operator StagePrivate*();
operator const StagePrivate*() const;
/// initialize stage once before planning /// initialize stage once before planning
virtual bool init(const planning_scene::PlanningSceneConstPtr& scene); virtual bool init(const planning_scene::PlanningSceneConstPtr& scene);
@ -79,6 +83,7 @@ class ComputeBasePrivate;
class ComputeBase : public Stage { class ComputeBase : public Stage {
public: public:
PRIVATE_CLASS(ComputeBase) PRIVATE_CLASS(ComputeBase)
bool init(const planning_scene::PlanningSceneConstPtr &scene) override;
virtual size_t numSolutions() const override; virtual size_t numSolutions() const override;
protected: protected:

View File

@ -95,8 +95,11 @@ void ContainerBase::clear()
SerialContainerPrivate::SerialContainerPrivate(SerialContainer *me, const std::string &name) SerialContainerPrivate::SerialContainerPrivate(SerialContainer *me, const std::string &name)
: ContainerBasePrivate(me, name) : ContainerBasePrivate(me, name)
{ {
// TODO rhaschke: define notify functions
starts_.reset(new Interface(Interface::NotifyFunction())); starts_.reset(new Interface(Interface::NotifyFunction()));
ends_.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 { InterfaceFlags SerialContainerPrivate::announcedFlags() const {
@ -107,39 +110,6 @@ InterfaceFlags SerialContainerPrivate::announcedFlags() const {
return f; 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 inline ContainerBasePrivate::const_iterator SerialContainerPrivate::prev(const_iterator it) const
{ {
assert(it != children().cbegin()); assert(it != children().cbegin());
@ -260,16 +230,55 @@ SerialContainer::SerialContainer(const std::string &name)
{} {}
PIMPL_FUNCTIONS(SerialContainer) 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) bool SerialContainer::init(const planning_scene::PlanningSceneConstPtr &scene)
{ {
auto impl = pimpl(); auto impl = pimpl();
// clear queues
impl->internal_to_my_starts_.clear();
impl->internal_to_my_ends_.clear();
impl->solutions_.clear();
// recursively init all children // recursively init all children
for (auto& stage : impl->children()) { for (auto& stage : impl->children()) {
// derived classes should call Stage::init internally , but we cannot be sure... if (!stage->Stage::init(scene) || !stage->init(scene))
if (!stage->init(scene) ||!stage->Stage::init(scene))
return false; 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 bool SerialContainer::canCompute() const

View File

@ -77,6 +77,7 @@ public:
private: private:
inline const_iterator prev(const_iterator it) const; inline const_iterator prev(const_iterator it) const;
inline const_iterator next(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 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. * A solution of a container needs to connect start to end via a full path.

View File

@ -34,6 +34,16 @@ inline InterfaceFlags StagePrivate::deducedFlags() const
return f; 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) Stage::Stage(StagePrivate *impl)
: pimpl_(impl) : pimpl_(impl)
@ -46,19 +56,21 @@ Stage::~Stage()
delete pimpl_; 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) bool Stage::init(const planning_scene::PlanningSceneConstPtr &scene)
{ {
#if 0
auto impl = pimpl(); auto impl = pimpl();
InterfaceFlags f = impl->announcedFlags(); if (impl->starts_) impl->starts_->clear();
if (!implies(f & WRITES_NEXT_START, impl->nextStarts())) if (impl->ends_) impl->ends_->clear();
throw std::runtime_error("missing next unit for sendForward()"); impl->prev_ends_ = nullptr;
impl->next_starts_ = nullptr;
if (!implies(f & WRITES_PREV_END, impl->prevEnds()))
throw std::runtime_error("missing previous unit for sendBackward()");
#endif
return true; return true;
} }
@ -118,11 +130,15 @@ size_t ComputeBase::numSolutions() const {
return pimpl()->trajectories_.size(); return pimpl()->trajectories_.size();
} }
const std::list<SubTrajectory> &ComputeBase::trajectories() const const std::list<SubTrajectory> &ComputeBase::trajectories() const {
{
return pimpl()->trajectories_; 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) PropagatingEitherWayPrivate::PropagatingEitherWayPrivate(PropagatingEitherWay *me, PropagatingEitherWay::Direction dir, const std::string &name)
: ComputeBasePrivate(me, name), dir(dir) : ComputeBasePrivate(me, name), dir(dir)

View File

@ -51,6 +51,8 @@ public:
inline Interface* nextStarts() const { return next_starts_; } inline Interface* nextStarts() const { return next_starts_; }
inline bool isConnected() const { return prev_ends_ || 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) { inline void setHierarchy(ContainerBasePrivate* parent, container_type::iterator it) {
parent_ = parent; parent_ = parent;