FallbacksPrivateCommon: shared between Generator + Propagator

Author: rhaschke

core/include/moveit/task_constructor/container.h

@@ -167,12 +167,15 @@ class Fallbacks : public ParallelContainerBase
 
 	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;

core/include/moveit/task_constructor/container_p.h

@@ -254,38 +254,52 @@ class FallbacksPrivate : public ParallelContainerBasePrivate
 	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
+	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() { return false; }
-	/// Reset data structures
-	virtual void reset();
+	virtual bool nextJob() = 0;
 
-	container_type::const_iterator current_;  // currently active child generator
-	bool job_has_solutions_;  // flag indicating whether the current job generated solutions
+	void reset() override;
+	bool canCompute() const override;
+	void compute() override;
+
+	container_type::const_iterator current_;  // currently active child
 };
-PIMPL_FUNCTIONS(Fallbacks)
 
 /// 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

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<FallbacksPrivate*>(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<Fallbacks*>(me())->replaceImpl();
 }
 
+void FallbacksPrivate::onNewSolution(const SolutionBase& s) {
+	// printChildrenInterfaces(*this, true, *s.creator());
+	static_cast<Fallbacks*>(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<FallbacksPrivateCommon*>(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() {