FallbacksPrivateCommon: shared between Generator + Propagator

This commit is contained in:
Robert Haschke 2022-01-05 11:57:31 +01:00
parent b82b70ed64
commit 986d3c8766
3 changed files with 65 additions and 42 deletions

View File

@ -167,12 +167,15 @@ public:
void reset() override; void reset() override;
void init(const moveit::core::RobotModelConstPtr& robot_model) override; void init(const moveit::core::RobotModelConstPtr& robot_model) override;
bool canCompute() const override;
void compute() override;
protected: protected:
Fallbacks(FallbacksPrivate* impl); Fallbacks(FallbacksPrivate* impl);
void onNewSolution(const SolutionBase& s) override; 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; class MergerPrivate;

View File

@ -254,38 +254,52 @@ public:
FallbacksPrivate(Fallbacks* me, const std::string& name); FallbacksPrivate(Fallbacks* me, const std::string& name);
FallbacksPrivate(FallbacksPrivate&& other); FallbacksPrivate(FallbacksPrivate&& other);
// methods common to all variants
void initializeExternalInterfaces() final; void initializeExternalInterfaces() final;
void onNewFailure(const Stage& child, const InterfaceState* from, const InterfaceState* to) override; 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 methods specific to each variant
/// Advance to the next job, assuming that the current child is exhausted on the current job. virtual void onNewSolution(const SolutionBase& s);
virtual bool nextJob() { return false; } virtual void reset() {}
/// 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
}; };
PIMPL_FUNCTIONS(Fallbacks) 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 /// Fallbacks implementation for GENERATOR interface
struct FallbacksPrivateGenerator : FallbacksPrivate struct FallbacksPrivateGenerator : FallbacksPrivateCommon
{ {
FallbacksPrivateGenerator(FallbacksPrivate&& old); FallbacksPrivateGenerator(FallbacksPrivate&& old);
bool nextJob() override; bool nextJob() override;
}; };
/// Fallbacks implementation for FORWARD or BACKWARD interface /// Fallbacks implementation for FORWARD or BACKWARD interface
struct FallbacksPrivatePropagator : FallbacksPrivate struct FallbacksPrivatePropagator : FallbacksPrivateCommon
{ {
FallbacksPrivatePropagator(FallbacksPrivate&& old); FallbacksPrivatePropagator(FallbacksPrivate&& old);
bool nextJob() override;
void reset() override; void reset() override;
void onNewSolution(const SolutionBase& s) override;
bool nextJob() override;
Interface::Direction dir_; // propagation direction Interface::Direction dir_; // propagation direction
Interface::iterator job_; // pointer to currently processed external state 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 class WrapperBasePrivate : public ParallelContainerBasePrivate

View File

@ -861,25 +861,8 @@ void Fallbacks::init(const moveit::core::RobotModelConstPtr& robot_model) {
pimpl()->reset(); 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) { void Fallbacks::onNewSolution(const SolutionBase& s) {
pimpl()->job_has_solutions_ = true; pimpl()->onNewSolution(s);
// printChildrenInterfaces(*this->pimpl(), true, *s.creator());
liftSolution(s);
} }
inline void Fallbacks::replaceImpl() { inline void Fallbacks::replaceImpl() {
@ -909,17 +892,17 @@ FallbacksPrivate::FallbacksPrivate(FallbacksPrivate&& other)
this->ParallelContainerBasePrivate::operator=(std::move(other)); this->ParallelContainerBasePrivate::operator=(std::move(other));
} }
void FallbacksPrivate::reset() {
current_ = children().begin();
job_has_solutions_ = false;
}
void FallbacksPrivate::initializeExternalInterfaces() { void FallbacksPrivate::initializeExternalInterfaces() {
// Here we know the final interface of the container (and all its children) // Here we know the final interface of the container (and all its children)
// Thus replace, this pimpl() with a new interface-specific one: // Thus replace, this pimpl() with a new interface-specific one:
static_cast<Fallbacks*>(me())->replaceImpl(); 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*/) { void FallbacksPrivate::onNewFailure(const Stage& child, const InterfaceState* /*from*/, const InterfaceState* /*to*/) {
// This override is deliberately empty. // This override is deliberately empty.
// The method prunes solution paths when a child failed to find a valid solution for it, // 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)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()) if (std::next(current_) != children().end())
ROS_DEBUG_STREAM_NAMED("Fallbacks", "Child '" << (*current_)->name() << "' failed, trying next one."); ROS_DEBUG_STREAM_NAMED("Fallbacks", "Child '" << (*current_)->name() << "' failed, trying next one.");
++current_; // advance to next child ++current_; // advance to next child
} }
FallbacksPrivateGenerator::FallbacksPrivateGenerator(FallbacksPrivate&& old) FallbacksPrivateGenerator::FallbacksPrivateGenerator(FallbacksPrivate&& old)
: FallbacksPrivate(std::move(old)) { reset(); } : FallbacksPrivateCommon(std::move(old)) { FallbacksPrivateCommon::reset(); }
bool FallbacksPrivateGenerator::nextJob() { bool FallbacksPrivateGenerator::nextJob() {
assert(current_ != children().end() && !(*current_)->pimpl()->canCompute()); assert(current_ != children().end() && !(*current_)->pimpl()->canCompute());
// don't advance to next child when we already produced solutions // 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 current_ = children().end(); // indicate that we are exhausted
return false; return false;
} }
@ -956,7 +956,7 @@ bool FallbacksPrivateGenerator::nextJob() {
FallbacksPrivatePropagator::FallbacksPrivatePropagator(FallbacksPrivate&& old) FallbacksPrivatePropagator::FallbacksPrivatePropagator(FallbacksPrivate&& old)
: FallbacksPrivate(std::move(old)) { : FallbacksPrivateCommon(std::move(old)) {
switch (requiredInterface()) { switch (requiredInterface()) {
case PROPAGATE_FORWARDS: case PROPAGATE_FORWARDS:
dir_ = Interface::FORWARD; dir_ = Interface::FORWARD;
@ -973,8 +973,14 @@ FallbacksPrivatePropagator::FallbacksPrivatePropagator(FallbacksPrivate&& old)
} }
void FallbacksPrivatePropagator::reset() { void FallbacksPrivatePropagator::reset() {
FallbacksPrivate::reset(); FallbacksPrivateCommon::reset();
job_ = pullInterface(dir_)->end(); // indicate fresh start 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() { bool FallbacksPrivatePropagator::nextJob() {