mirror of
https://github.com/moveit/moveit_task_constructor.git
synced 2025-11-04 14:49:57 +08:00
FallbacksPrivateCommon: shared between Generator + Propagator
This commit is contained in:
parent
b82b70ed64
commit
986d3c8766
@ -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;
|
||||
|
||||
@ -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
|
||||
|
||||
@ -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() {
|
||||
|
||||
Loading…
Reference in New Issue
Block a user