From 2e63c154aab41a9cde8684ac0880504cfc2a99d8 Mon Sep 17 00:00:00 2001 From: Robert Haschke Date: Fri, 17 Sep 2021 12:04:58 +0200 Subject: [PATCH] Reintroduce pending state --- .../include/moveit/task_constructor/container_p.h | 2 +- core/src/container.cpp | 15 +++++++++++---- 2 files changed, 12 insertions(+), 5 deletions(-) diff --git a/core/include/moveit/task_constructor/container_p.h b/core/include/moveit/task_constructor/container_p.h index ab305a6f..1e29526b 100644 --- a/core/include/moveit/task_constructor/container_p.h +++ b/core/include/moveit/task_constructor/container_p.h @@ -263,7 +263,7 @@ protected: inline bool operator<(const ExternalState& other) const { return *external_state < *other.external_state; } }; ordered pending_states_; // pending external states for a PROPAGATE interface - + ordered::iterator current_pending_; // currently active pending state inline void computeGenerate(); mutable container_type::const_iterator current_generator_; diff --git a/core/src/container.cpp b/core/src/container.cpp index 904e0b5c..ba9a5fb2 100644 --- a/core/src/container.cpp +++ b/core/src/container.cpp @@ -827,6 +827,7 @@ void Fallbacks::init(const moveit::core::RobotModelConstPtr& robot_model) { auto& impl{ *pimpl() }; ParallelContainerBase::init(robot_model); impl.current_generator_ = impl.children().begin(); + impl.current_pending_ = impl.pending_states_.end(); } bool Fallbacks::canCompute() const { @@ -849,7 +850,7 @@ bool Fallbacks::canCompute() const { case PROPAGATE_FORWARDS: case PROPAGATE_BACKWARDS: case CONNECT: - return !impl->pending_states_.empty() && !impl->pending_states_.front().external_state->priority().failed(); + return !impl->pending_states_.empty(); default: assert(false); } @@ -937,9 +938,13 @@ void FallbacksPrivate::computePropagate(){ while (!pending_states_.empty()) { printPending(); - auto current = pending_states_.begin(); - if (!current->external_state->priority().enabled()) - return; + // If we have a currently active pending state, proceed with this one + // even if pending_states_.front() might be different meanwhile. + // This is important as we need to feed states one by one to the children. + // Otherwise we cannot know if a child is exhausted on a specific input state. + if (current_pending_ == pending_states_.end()) + current_pending_ = pending_states_.begin(); + auto current = current_pending_; auto pushState = [this](const ExternalState& ext) { ROS_DEBUG_STREAM_NAMED("Fallbacks", "Push external state (" << ext.external_state->priority() @@ -978,11 +983,13 @@ void FallbacksPrivate::computePropagate(){ current->dir == Interface::FORWARD ? &*current->external_state : nullptr, current->dir == Interface::BACKWARD ? nullptr : &*current->external_state); pending_states_.erase(current); + current_pending_ = pending_states_.end(); } } else { ROS_DEBUG_STREAM_NAMED("Fallbacks", "Child '" << child->name() << "' exhausted, but produced solutions before, not invoking further fallbacks"); pending_states_.erase(current); + current_pending_ = pending_states_.end(); } // continue processing with next pending state as we didn't runCompute() yet }