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 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;
|
||||||
|
|||||||
@ -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
|
||||||
|
|||||||
@ -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() {
|
||||||
|
|||||||
Loading…
Reference in New Issue
Block a user