From b783173b27e129598f49b943640925813697fcf7 Mon Sep 17 00:00:00 2001 From: Robert Haschke Date: Thu, 9 Sep 2021 11:52:44 +0200 Subject: [PATCH] GENERATE: return correct canCompute() result as early as possible Moving to next child generator only in compute() requires an extra call to canCompute() to notice the failure of the next generator(s). --- .../moveit/task_constructor/container_p.h | 4 ++-- core/src/container.cpp | 22 ++++++++----------- 2 files changed, 11 insertions(+), 15 deletions(-) diff --git a/core/include/moveit/task_constructor/container_p.h b/core/include/moveit/task_constructor/container_p.h index a5985bca..77881390 100644 --- a/core/include/moveit/task_constructor/container_p.h +++ b/core/include/moveit/task_constructor/container_p.h @@ -263,8 +263,8 @@ protected: ordered pending_states_; ExternalState current_external_state_; - void computeGenerate(); - container_type::const_iterator current_generator_; + inline void computeGenerate(); + mutable container_type::const_iterator current_generator_; private: void initializeExternalInterfaces() override; diff --git a/core/src/container.cpp b/core/src/container.cpp index 132a25d0..7d040d28 100644 --- a/core/src/container.cpp +++ b/core/src/container.cpp @@ -828,11 +828,16 @@ bool Fallbacks::canCompute() const { if (impl->requiredInterface() == GENERATE) { // current_generator_ is fixed if it produced solutions before - if( !solutions().empty() ) + if (!solutions().empty()) return (*impl->current_generator_)->pimpl()->canCompute(); - else - // we still have children to try + else { + // move to first generator that can run + while(impl->current_generator_ != impl->children().end() && !(*impl->current_generator_)->pimpl()->canCompute()) { + ROS_DEBUG_STREAM_NAMED("Fallbacks", "Generator '" << (*impl->current_generator_)->name() << "' can't compute, trying next one."); + ++impl->current_generator_; + } return impl->current_generator_ != impl->children().end(); + } } else return !impl->pending_states_.empty() || impl->current_external_state_.stage != impl->children().cend(); @@ -873,16 +878,7 @@ void FallbacksPrivate::onNewFailure(const Stage& /*child*/, const InterfaceState } void FallbacksPrivate::computeGenerate() { - if(solutions_.empty()) - // move to first generator that can run - while(current_generator_ != children().end() && !(*current_generator_)->pimpl()->canCompute()) { - ROS_DEBUG_STREAM_NAMED("Fallbacks", "Generator '" << (*current_generator_)->name() << "' can't compute, trying next one."); - ++current_generator_; - } - - if(current_generator_ == children().end()) - return; - + assert(current_generator_ != children().end()); (*current_generator_)->pimpl()->runCompute(); }