From b82b70ed642afd932a40b49424eb979634babcb1 Mon Sep 17 00:00:00 2001 From: Robert Haschke Date: Wed, 5 Jan 2022 11:25:06 +0100 Subject: [PATCH] FallbacksPrivate::nextChild() ... factoring out functionality shared between FallbacksPrivateGenerator and FallbacksPrivatePropagator to switch to next child in nextJob(). --- .../moveit/task_constructor/container_p.h | 1 + core/src/container.cpp | 21 ++++++++++--------- 2 files changed, 12 insertions(+), 10 deletions(-) diff --git a/core/include/moveit/task_constructor/container_p.h b/core/include/moveit/task_constructor/container_p.h index 391cbb15..a98d1b53 100644 --- a/core/include/moveit/task_constructor/container_p.h +++ b/core/include/moveit/task_constructor/container_p.h @@ -257,6 +257,7 @@ public: // 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. diff --git a/core/src/container.cpp b/core/src/container.cpp index d678ad7d..fdf9b320 100644 --- a/core/src/container.cpp +++ b/core/src/container.cpp @@ -219,6 +219,7 @@ void ContainerBasePrivate::setStatus(const InterfaceState* s, InterfaceState::St // if possible (i.e. if state s has an external counterpart), escalate setStatus to external interface if (parent() && trajectories(*s).empty()) { + // TODO: This was coded with SerialContainer in mind. Not sure, it works for ParallelContainers auto external{ internalToExternalMap().find(s) }; if (external != internalToExternalMap().end()) { // do we have an external state? // only escalate if there is no other *enabled* internal state connected to the same external one @@ -928,6 +929,11 @@ void FallbacksPrivate::onNewFailure(const Stage& child, const InterfaceState* /* (void)child; } +void FallbacksPrivate::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(); } @@ -941,11 +947,8 @@ bool FallbacksPrivateGenerator::nextJob() { return false; } - do { - if (std::next(current_) != children().end()) - ROS_DEBUG_STREAM_NAMED("Fallbacks", "Generator '" << (*current_)->name() << "' failed, trying next one."); - ++current_; // advance to next child - } while (current_ != children().end() && !(*current_)->pimpl()->canCompute()); + do { nextChild(); } + while (current_ != children().end() && !(*current_)->pimpl()->canCompute()); // return value shall indicate current_->canCompute() return current_ != children().end(); @@ -979,11 +982,9 @@ bool FallbacksPrivatePropagator::nextJob() { const auto jobs = pullInterface(dir_); if (job_ != jobs->end()) { // current job exists, but is exhausted on current child - if (!job_has_solutions_) { // job didn't produce solutions -> feed to next child - if (std::next(current_) != children().end()) - ROS_DEBUG_STREAM_NAMED("Fallbacks", "Propagator '" << (*current_)->name() << "' failed, trying next one."); - ++current_; // advance to next child - } else + if (!job_has_solutions_) // job didn't produce solutions -> feed to next child + nextChild(); + else current_ = children().end(); // indicate that this job is exhausted on all children } job_has_solutions_ = false;