diff --git a/core/include/moveit/task_constructor/container.h b/core/include/moveit/task_constructor/container.h index 6dcf5bfa..cea4a418 100644 --- a/core/include/moveit/task_constructor/container.h +++ b/core/include/moveit/task_constructor/container.h @@ -167,12 +167,15 @@ public: void reset() override; void init(const moveit::core::RobotModelConstPtr& robot_model) override; - bool canCompute() const override; - void compute() override; protected: Fallbacks(FallbacksPrivate* impl); void onNewSolution(const SolutionBase& s) override; + +private: + // not needed, we directly use corresponding virtual methods of FallbacksPrivate + bool canCompute() const final { return false; } + void compute() final {} }; class MergerPrivate; diff --git a/core/include/moveit/task_constructor/container_p.h b/core/include/moveit/task_constructor/container_p.h index a98d1b53..a2265ec9 100644 --- a/core/include/moveit/task_constructor/container_p.h +++ b/core/include/moveit/task_constructor/container_p.h @@ -254,38 +254,52 @@ public: FallbacksPrivate(Fallbacks* me, const std::string& name); FallbacksPrivate(FallbacksPrivate&& other); - // methods common to all variants void initializeExternalInterfaces() final; void onNewFailure(const Stage& child, const InterfaceState* from, const InterfaceState* to) override; - void nextChild(); /// << Advance to next child // virtual methods specific to each variant - /// Advance to the next job, assuming that the current child is exhausted on the current job. - virtual bool nextJob() { return false; } - /// Reset data structures - virtual void reset(); - - container_type::const_iterator current_; // currently active child generator - bool job_has_solutions_; // flag indicating whether the current job generated solutions + virtual void onNewSolution(const SolutionBase& s); + virtual void reset() {} }; PIMPL_FUNCTIONS(Fallbacks) +/* Class shared between FallbacksPrivateGenerator and FallbacksPrivatePropagator, + which both have the notion of a currently active child stage */ +class FallbacksPrivateCommon : public FallbacksPrivate +{ +public: + FallbacksPrivateCommon(FallbacksPrivate&& other) : FallbacksPrivate(std::move(other)) {} + + /// Advance to next child + inline void nextChild(); + /// Advance to the next job, assuming that the current child is exhausted on the current job. + virtual bool nextJob() = 0; + + void reset() override; + bool canCompute() const override; + void compute() override; + + container_type::const_iterator current_; // currently active child +}; + /// Fallbacks implementation for GENERATOR interface -struct FallbacksPrivateGenerator : FallbacksPrivate +struct FallbacksPrivateGenerator : FallbacksPrivateCommon { FallbacksPrivateGenerator(FallbacksPrivate&& old); bool nextJob() override; }; /// Fallbacks implementation for FORWARD or BACKWARD interface -struct FallbacksPrivatePropagator : FallbacksPrivate +struct FallbacksPrivatePropagator : FallbacksPrivateCommon { FallbacksPrivatePropagator(FallbacksPrivate&& old); - bool nextJob() override; void reset() override; + void onNewSolution(const SolutionBase& s) override; + bool nextJob() override; Interface::Direction dir_; // propagation direction Interface::iterator job_; // pointer to currently processed external state + bool job_has_solutions_; // flag indicating whether the current job generated solutions }; class WrapperBasePrivate : public ParallelContainerBasePrivate diff --git a/core/src/container.cpp b/core/src/container.cpp index fdf9b320..4f4eb7e1 100644 --- a/core/src/container.cpp +++ b/core/src/container.cpp @@ -861,25 +861,8 @@ void Fallbacks::init(const moveit::core::RobotModelConstPtr& robot_model) { pimpl()->reset(); } -bool Fallbacks::canCompute() const { - auto impl = const_cast(pimpl()); - - while(impl->current_ != impl->children().end() && // not completely exhausted - !(*impl->current_)->pimpl()->canCompute()) // but current child cannot compute - return impl->nextJob(); // advance to next job - - // return value: current child is well defined and thus can compute? - return impl->current_ != impl->children().end(); -} - -void Fallbacks::compute() { - (*pimpl()->current_)->pimpl()->runCompute(); -} - void Fallbacks::onNewSolution(const SolutionBase& s) { - pimpl()->job_has_solutions_ = true; - // printChildrenInterfaces(*this->pimpl(), true, *s.creator()); - liftSolution(s); + pimpl()->onNewSolution(s); } inline void Fallbacks::replaceImpl() { @@ -909,17 +892,17 @@ FallbacksPrivate::FallbacksPrivate(FallbacksPrivate&& other) this->ParallelContainerBasePrivate::operator=(std::move(other)); } -void FallbacksPrivate::reset() { - current_ = children().begin(); - job_has_solutions_ = false; -} - void FallbacksPrivate::initializeExternalInterfaces() { // Here we know the final interface of the container (and all its children) // Thus replace, this pimpl() with a new interface-specific one: static_cast(me())->replaceImpl(); } +void FallbacksPrivate::onNewSolution(const SolutionBase& s) { + // printChildrenInterfaces(*this, true, *s.creator()); + static_cast(me())->liftSolution(s); +} + void FallbacksPrivate::onNewFailure(const Stage& child, const InterfaceState* /*from*/, const InterfaceState* /*to*/) { // This override is deliberately empty. // The method prunes solution paths when a child failed to find a valid solution for it, @@ -929,20 +912,37 @@ void FallbacksPrivate::onNewFailure(const Stage& child, const InterfaceState* /* (void)child; } -void FallbacksPrivate::nextChild() { +void FallbacksPrivateCommon::reset() { + current_ = children().begin(); +} + +bool FallbacksPrivateCommon::canCompute() const { + while(current_ != children().end() && // not completely exhausted + !(*current_)->pimpl()->canCompute()) // but current child cannot compute + return const_cast(this)->nextJob(); // advance to next job + + // return value: current child is well defined and thus can compute? + return current_ != children().end(); +} + +void FallbacksPrivateCommon::compute() { + (*current_)->pimpl()->runCompute(); +} + +inline void FallbacksPrivateCommon::nextChild() { if (std::next(current_) != children().end()) ROS_DEBUG_STREAM_NAMED("Fallbacks", "Child '" << (*current_)->name() << "' failed, trying next one."); ++current_; // advance to next child } FallbacksPrivateGenerator::FallbacksPrivateGenerator(FallbacksPrivate&& old) - : FallbacksPrivate(std::move(old)) { reset(); } + : FallbacksPrivateCommon(std::move(old)) { FallbacksPrivateCommon::reset(); } bool FallbacksPrivateGenerator::nextJob() { assert(current_ != children().end() && !(*current_)->pimpl()->canCompute()); // don't advance to next child when we already produced solutions - if (job_has_solutions_) { + if (!solutions_.empty()) { current_ = children().end(); // indicate that we are exhausted return false; } @@ -956,7 +956,7 @@ bool FallbacksPrivateGenerator::nextJob() { FallbacksPrivatePropagator::FallbacksPrivatePropagator(FallbacksPrivate&& old) - : FallbacksPrivate(std::move(old)) { + : FallbacksPrivateCommon(std::move(old)) { switch (requiredInterface()) { case PROPAGATE_FORWARDS: dir_ = Interface::FORWARD; @@ -973,8 +973,14 @@ FallbacksPrivatePropagator::FallbacksPrivatePropagator(FallbacksPrivate&& old) } void FallbacksPrivatePropagator::reset() { - FallbacksPrivate::reset(); + FallbacksPrivateCommon::reset(); job_ = pullInterface(dir_)->end(); // indicate fresh start + job_has_solutions_ = false; +} + +void FallbacksPrivatePropagator::onNewSolution(const SolutionBase& s) { + job_has_solutions_ = true; + FallbacksPrivateCommon::onNewSolution(s); } bool FallbacksPrivatePropagator::nextJob() {