diff --git a/include/moveit_task_constructor/container.h b/include/moveit_task_constructor/container.h index 3d7bf13f..42a5068c 100644 --- a/include/moveit_task_constructor/container.h +++ b/include/moveit_task_constructor/container.h @@ -37,6 +37,7 @@ public: PRIVATE_CLASS(SerialContainer) SerialContainer(const std::string& name); + void reset() override; void init(const planning_scene::PlanningSceneConstPtr &scene) override; bool canCompute() const override; bool compute() override; diff --git a/include/moveit_task_constructor/stage.h b/include/moveit_task_constructor/stage.h index 448a3a62..3ab87d8e 100644 --- a/include/moveit_task_constructor/stage.h +++ b/include/moveit_task_constructor/stage.h @@ -89,6 +89,8 @@ public: operator StagePrivate*(); operator const StagePrivate*() const; + /// reset stage, clearing all solutions, interfaces, etc. + virtual void reset(); /// initialize stage once before planning virtual void init(const planning_scene::PlanningSceneConstPtr& scene); @@ -109,7 +111,7 @@ class ComputeBasePrivate; class ComputeBase : public Stage { public: PRIVATE_CLASS(ComputeBase) - void init(const planning_scene::PlanningSceneConstPtr &scene) override; + void reset() override; virtual size_t numSolutions() const override; protected: diff --git a/src/container.cpp b/src/container.cpp index b6903b4d..f10f23e1 100644 --- a/src/container.cpp +++ b/src/container.cpp @@ -100,14 +100,6 @@ SerialContainerPrivate::SerialContainerPrivate(SerialContainer *me, const std::s pending_forward_.reset(new Interface(Interface::NotifyFunction())); } -InterfaceFlags SerialContainerPrivate::announcedFlags() const { - InterfaceFlags f; - if (children().empty()) return f; - f |= children().front()->pimpl()->announcedFlags() & INPUT_IF_MASK; - f |= children().back()->pimpl()->announcedFlags() & OUTPUT_IF_MASK; - return f; -} - inline ContainerBasePrivate::const_iterator SerialContainerPrivate::prev(const_iterator it) const { assert(it != children().cbegin()); @@ -226,8 +218,22 @@ SerialContainer::SerialContainer(SerialContainerPrivate *impl) SerialContainer::SerialContainer(const std::string &name) : SerialContainer(new SerialContainerPrivate(this, name)) {} + PIMPL_FUNCTIONS(SerialContainer) +void SerialContainer::reset() +{ + auto impl = pimpl(); + // clear queues + impl->internal_to_my_starts_.clear(); + impl->internal_to_my_ends_.clear(); + impl->solutions_.clear(); + + // recursively reset children + for (auto& child: impl->children()) + child->reset(); +} + void SerialContainerPrivate::connect(StagePrivate* prev, StagePrivate* next) { prev->setNextStarts(next->starts()); next->setPrevEnds(prev->ends()); @@ -236,23 +242,7 @@ void SerialContainerPrivate::connect(StagePrivate* prev, StagePrivate* next) { void SerialContainer::init(const planning_scene::PlanningSceneConstPtr &scene) { InitStageException errors; - 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()) { - try { - // should be alled by derived classes too, but you never know... - stage->Stage::init(scene); - stage->init(scene); - } catch (InitStageException &e) { - errors.append(e); - } - } // we need to have some children to do the actual work if (impl->children().empty()) { @@ -271,7 +261,8 @@ void SerialContainer::init(const planning_scene::PlanningSceneConstPtr &scene) })); auto last = --impl->children().end(); - if ((*cur)->pimpl()->ends()) + child_impl = **last; + if (child_impl->ends()) impl->ends_.reset(new Interface([impl, child_impl](const Interface::iterator& internal){ // new state in our ends_ interface is copied to last child, remembering the link auto it = child_impl->ends()->clone(*internal); @@ -296,15 +287,25 @@ void SerialContainer::init(const planning_scene::PlanningSceneConstPtr &scene) impl->connect(**prev, **cur); } - // validate connectivity of chain - for (const Stage::pointer& stage : impl->children()) { + // recursively init all children + for (auto& child : impl->children()) { try { - stage->pimpl()->validate(); + child->init(scene); } catch (InitStageException &e) { errors.append(e); } } + // validate connectivity of chain + for (auto& child : impl->children()) { + try { + child->pimpl()->validate(); + } catch (InitStageException &e) { + errors.append(e); + } + } + + std::cerr << errors; if (errors) throw errors; } diff --git a/src/container_p.h b/src/container_p.h index 5faa1af8..3257af8c 100644 --- a/src/container_p.h +++ b/src/container_p.h @@ -64,7 +64,6 @@ class SerialContainerPrivate : public ContainerBasePrivate { public: SerialContainerPrivate(SerialContainer* me, const std::string &name); - InterfaceFlags announcedFlags() const override; void onNewSolution(SolutionBase &s) override; void storeNewSolution(std::vector &&s, double cost); const std::list& solutions() const { return solutions_; } diff --git a/src/stage.cpp b/src/stage.cpp index 2ad37a55..1cb613e6 100644 --- a/src/stage.cpp +++ b/src/stage.cpp @@ -40,19 +40,10 @@ StagePrivate::StagePrivate(Stage *me, const std::string &name) {} InterfaceFlags StagePrivate::interfaceFlags() const -{ - InterfaceFlags result = announcedFlags(); - result &= ~InterfaceFlags(OWN_IF_MASK); - result |= deducedFlags(); - return result; -} - -// return the interface flags that can be deduced from the interface -inline InterfaceFlags StagePrivate::deducedFlags() const { InterfaceFlags f; - if (starts_) f |= READS_START; - if (ends_) f |= READS_END; + if (starts()) f |= READS_START; + if (ends()) f |= READS_END; if (prevEnds()) f |= WRITES_PREV_END; if (nextStarts()) f |= WRITES_NEXT_START; return f; @@ -62,7 +53,7 @@ inline bool implies(bool p, bool q) { return !p || q; } void StagePrivate::validate() const { InitStageException errors; - InterfaceFlags f = announcedFlags(); + InterfaceFlags f = interfaceFlags(); if (!implies(f & WRITES_NEXT_START, nextStarts())) errors.push_back(*me_, "sends forward, but next stage cannot receive"); @@ -91,7 +82,7 @@ Stage::operator const StagePrivate *() const { return pimpl(); } -void Stage::init(const planning_scene::PlanningSceneConstPtr &scene) +void Stage::reset() { auto impl = pimpl(); if (impl->starts_) impl->starts_->clear(); @@ -100,13 +91,17 @@ void Stage::init(const planning_scene::PlanningSceneConstPtr &scene) impl->next_starts_ = nullptr; } +void Stage::init(const planning_scene::PlanningSceneConstPtr &scene) +{ +} + const std::string& Stage::name() const { return pimpl_->name_; } template const char* direction(const StagePrivate& stage) { - InterfaceFlags f = stage.deducedFlags(); + InterfaceFlags f = stage.interfaceFlags(); bool own_if = f & own; bool other_if = f & other; bool reverse = own & INPUT_IF_MASK; @@ -160,9 +155,9 @@ const std::list &ComputeBase::trajectories() const { return pimpl()->trajectories_; } -void ComputeBase::init(const planning_scene::PlanningSceneConstPtr &scene) { +void ComputeBase::reset() { pimpl()->trajectories_.clear(); - Stage::init(scene); + Stage::reset(); } @@ -171,15 +166,6 @@ PropagatingEitherWayPrivate::PropagatingEitherWayPrivate(PropagatingEitherWay *m { } -InterfaceFlags PropagatingEitherWayPrivate::announcedFlags() const { - InterfaceFlags f; - if (dir & PropagatingEitherWay::FORWARD) - f |= InterfaceFlags({READS_START, WRITES_NEXT_START}); - if (dir & PropagatingEitherWay::BACKWARD) - f |= InterfaceFlags({READS_END, WRITES_PREV_END}); - return f; -} - inline bool PropagatingEitherWayPrivate::hasStartState() const{ return next_start_state_ != starts_->end(); } @@ -302,7 +288,11 @@ void PropagatingEitherWay::restrictDirection(PropagatingEitherWay::Direction dir void PropagatingEitherWay::init(const planning_scene::PlanningSceneConstPtr &scene) { + ComputeBase::init(scene); + auto impl = pimpl(); + + // after being connected, restrict actual interface directions if (!impl->nextStarts()) { impl->starts_.reset(); impl->next_start_state_ = Interface::iterator(); @@ -311,7 +301,8 @@ void PropagatingEitherWay::init(const planning_scene::PlanningSceneConstPtr &sce impl->ends_.reset(); impl->next_end_state_ = Interface::iterator(); } - ComputeBase::init(scene); + if (!impl->isConnected()) + throw InitStageException(*this, "can neither send forwards nor backwards"); } void PropagatingEitherWay::sendForward(const InterfaceState& from, @@ -383,10 +374,6 @@ GeneratorPrivate::GeneratorPrivate(Generator *me, const std::string &name) : ComputeBasePrivate(me, name) {} -InterfaceFlags GeneratorPrivate::announcedFlags() const { - return InterfaceFlags({WRITES_NEXT_START,WRITES_PREV_END}); -} - bool GeneratorPrivate::canCompute() const { return static_cast(me_)->canCompute(); } @@ -425,10 +412,6 @@ ConnectingPrivate::ConnectingPrivate(Connecting *me, const std::string &name) it_pairs_ = std::make_pair(starts_->begin(), ends_->begin()); } -InterfaceFlags ConnectingPrivate::announcedFlags() const { - return InterfaceFlags({READS_START, READS_END}); -} - void ConnectingPrivate::newStartState(const Interface::iterator& it) { // TODO: need to handle the pairs iterator diff --git a/src/stage_p.h b/src/stage_p.h index cb4d91ac..bcd91bfd 100644 --- a/src/stage_p.h +++ b/src/stage_p.h @@ -36,8 +36,6 @@ public: StagePrivate(Stage* me, const std::string& name); InterfaceFlags interfaceFlags() const; - InterfaceFlags deducedFlags() const; - virtual InterfaceFlags announcedFlags() const = 0; virtual bool canCompute() const = 0; virtual bool compute() = 0; @@ -108,8 +106,7 @@ public: PropagatingEitherWay::Direction dir; inline PropagatingEitherWayPrivate(PropagatingEitherWay *me, PropagatingEitherWay::Direction dir, - const std::string &name); - InterfaceFlags announcedFlags() const override; + const std::string &name); bool canCompute() const override; bool compute() override; @@ -145,7 +142,6 @@ public: class GeneratorPrivate : public ComputeBasePrivate { public: inline GeneratorPrivate(Generator *me, const std::string &name); - InterfaceFlags announcedFlags() const override; bool canCompute() const override; bool compute() override; @@ -157,7 +153,6 @@ class ConnectingPrivate : public ComputeBasePrivate { public: inline ConnectingPrivate(Connecting *me, const std::string &name); - InterfaceFlags announcedFlags() const override; bool canCompute() const override; bool compute() override; diff --git a/src/task.cpp b/src/task.cpp index ac5be761..e679b05c 100644 --- a/src/task.cpp +++ b/src/task.cpp @@ -118,6 +118,7 @@ bool Task::plan(){ auto impl = pimpl(); setSolutionCallback(NewSolutionPublisher()); + reset(); impl->initScene(); init(impl->scene_);