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;
~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:

View File

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

View File

@ -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.

View File

@ -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<SubTrajectory> &ComputeBase::trajectories() const
{
const std::list<SubTrajectory> &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)

View File

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