Simplify: job_has_solutions_

Just set a flag when we received a full solution
This commit is contained in:
Robert Haschke 2021-11-25 00:35:56 +01:00
parent 7237e81547
commit e296bd7aed
2 changed files with 21 additions and 15 deletions

View File

@ -261,8 +261,11 @@ public:
// virtual method specific to each variant // virtual method specific to each variant
/// Advance to the next job, assuming that the current child is exhausted on the current job. /// Advance to the next job, assuming that the current child is exhausted on the current job.
virtual bool nextJob() { return false; } virtual bool nextJob() { return false; }
/// Reset data structures
virtual void reset();
container_type::const_iterator current_; // currently active child generator 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)
@ -278,7 +281,7 @@ struct FallbacksPrivatePropagator : FallbacksPrivate
{ {
FallbacksPrivatePropagator(FallbacksPrivate&& old); FallbacksPrivatePropagator(FallbacksPrivate&& old);
bool nextJob() override; bool nextJob() override;
bool jobHasSolutions() const; void reset() 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

View File

@ -849,18 +849,18 @@ Fallbacks::Fallbacks(FallbacksPrivate* impl) : ParallelContainerBase(impl) {}
void Fallbacks::reset() { void Fallbacks::reset() {
ParallelContainerBase::reset(); ParallelContainerBase::reset();
pimpl()->reset();
} }
void Fallbacks::init(const moveit::core::RobotModelConstPtr& robot_model) { void Fallbacks::init(const moveit::core::RobotModelConstPtr& robot_model) {
ParallelContainerBase::init(robot_model); ParallelContainerBase::init(robot_model);
auto impl = pimpl(); pimpl()->reset();
impl->current_ = impl->children().begin();
} }
bool Fallbacks::canCompute() const { bool Fallbacks::canCompute() const {
auto impl = const_cast<FallbacksPrivate*>(pimpl()); auto impl = const_cast<FallbacksPrivate*>(pimpl());
while(impl->current_ != impl->children().end() && // not completely exhaused while(impl->current_ != impl->children().end() && // not completely exhausted
!(*impl->current_)->pimpl()->canCompute()) // but current child cannot compute !(*impl->current_)->pimpl()->canCompute()) // but current child cannot compute
return impl->nextJob(); // advance to next job return impl->nextJob(); // advance to next job
@ -873,6 +873,7 @@ void Fallbacks::compute() {
} }
void Fallbacks::onNewSolution(const SolutionBase& s) { void Fallbacks::onNewSolution(const SolutionBase& s) {
pimpl()->job_has_solutions_ = true;
liftSolution(s); liftSolution(s);
} }
@ -901,8 +902,11 @@ FallbacksPrivate::FallbacksPrivate(FallbacksPrivate&& other)
: ParallelContainerBasePrivate(static_cast<Fallbacks*>(other.me()), "") { : ParallelContainerBasePrivate(static_cast<Fallbacks*>(other.me()), "") {
// move contents of other // move contents of other
this->ParallelContainerBasePrivate::operator=(std::move(other)); this->ParallelContainerBasePrivate::operator=(std::move(other));
// (re)initialize }
void FallbacksPrivate::reset() {
current_ = children().begin(); current_ = children().begin();
job_has_solutions_ = false;
} }
void FallbacksPrivate::initializeExternalInterfaces() { void FallbacksPrivate::initializeExternalInterfaces() {
@ -920,13 +924,13 @@ void FallbacksPrivate::onNewFailure(const Stage& /*child*/, const InterfaceState
FallbacksPrivateGenerator::FallbacksPrivateGenerator(FallbacksPrivate&& old) FallbacksPrivateGenerator::FallbacksPrivateGenerator(FallbacksPrivate&& old)
: FallbacksPrivate(std::move(old)) {} : FallbacksPrivate(std::move(old)) { 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 (!solutions_.empty()) { if (job_has_solutions_) {
current_ = children().end(); // indicate that we are exhausted current_ = children().end(); // indicate that we are exhausted
return false; return false;
} }
@ -956,28 +960,27 @@ FallbacksPrivatePropagator::FallbacksPrivatePropagator(FallbacksPrivate&& old)
default: default:
assert(false); assert(false);
} }
job_ = pullInterface(dir_)->end(); // indicate fresh start FallbacksPrivatePropagator::reset();
} }
bool FallbacksPrivatePropagator::jobHasSolutions() const { void FallbacksPrivatePropagator::reset() {
const auto& trajectories { dir_ == Interface::FORWARD ? job_->outgoingTrajectories() FallbacksPrivate::reset();
: job_->incomingTrajectories() }; job_ = pullInterface(dir_)->end(); // indicate fresh start
return std::find_if(trajectories.cbegin(), trajectories.cend(), }
[](const auto& t){ return !t->isFailure();}) != trajectories.cend();
};
bool FallbacksPrivatePropagator::nextJob() { bool FallbacksPrivatePropagator::nextJob() {
assert(current_ != children().end() && !(*current_)->pimpl()->canCompute()); assert(current_ != children().end() && !(*current_)->pimpl()->canCompute());
const auto jobs = pullInterface(dir_); const auto jobs = pullInterface(dir_);
if (job_ != jobs->end()) { // current job exists, but is exhausted on current child if (job_ != jobs->end()) { // current job exists, but is exhausted on current child
if (!jobHasSolutions()) { // job didn't produce solutions -> feed to next child if (!job_has_solutions_) { // job didn't produce solutions -> feed to next child
if (std::next(current_) != children().end()) if (std::next(current_) != children().end())
ROS_DEBUG_STREAM_NAMED("Fallbacks", "Propagator '" << (*current_)->name() << "' failed, trying next one."); ROS_DEBUG_STREAM_NAMED("Fallbacks", "Propagator '" << (*current_)->name() << "' failed, trying next one.");
++current_; // advance to next child ++current_; // advance to next child
} else } else
current_ = children().end(); // indicate that this job is exhausted on all children current_ = children().end(); // indicate that this job is exhausted on all children
} }
job_has_solutions_ = false;
if (current_ == children().end()) { // all children processed the job_ if (current_ == children().end()) { // all children processed the job_
if (job_ != jobs->end()) { if (job_ != jobs->end()) {