mirror of
https://github.com/moveit/moveit_task_constructor.git
synced 2025-11-04 14:49:57 +08:00
connect and validate chain
This commit is contained in:
parent
b967e8231b
commit
bc5c76578e
@ -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:
|
||||
|
||||
@ -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
|
||||
|
||||
@ -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.
|
||||
|
||||
@ -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)
|
||||
|
||||
@ -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;
|
||||
|
||||
Loading…
Reference in New Issue
Block a user