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;