Simplify computePropagate()

- Drop variable current_external_state_
- Instead encode the info that the external state wasn't yet forwarded to any child via stage = children().cend()
- If all children have exhausted their solutions for this state, it is removed from the pending list
This commit is contained in:
Robert Haschke 2021-09-16 15:10:45 +02:00
parent c33b1967bc
commit dcb6857f36
2 changed files with 56 additions and 48 deletions

View File

@ -151,6 +151,8 @@ protected:
/// Set ENABLED / PRUNED status of the solution tree starting from s into given direction /// Set ENABLED / PRUNED status of the solution tree starting from s into given direction
template <Interface::Direction dir> template <Interface::Direction dir>
void setStatus(const InterfaceState* s, InterfaceState::Status status); void setStatus(const InterfaceState* s, InterfaceState::Status status);
/// non-template version
void setStatus(Interface::Direction dir, const InterfaceState* s, InterfaceState::Status status);
/// copy external_state to a child's interface and remember the link in internal_external map /// copy external_state to a child's interface and remember the link in internal_external map
template <Interface::Direction> template <Interface::Direction>
@ -260,8 +262,7 @@ protected:
inline bool operator<(const ExternalState& other) const { return *external_state < *other.external_state; } inline bool operator<(const ExternalState& other) const { return *external_state < *other.external_state; }
}; };
ordered<ExternalState> pending_states_; ordered<ExternalState> pending_states_; // pending external states for a PROPAGATE interface
ExternalState current_external_state_;
inline void computeGenerate(); inline void computeGenerate();
mutable container_type::const_iterator current_generator_; mutable container_type::const_iterator current_generator_;

View File

@ -189,8 +189,13 @@ void ContainerBasePrivate::setStatus(const InterfaceState* s, InterfaceState::St
for (const SolutionBase* successor : trajectories<dir>(*s)) for (const SolutionBase* successor : trajectories<dir>(*s))
setStatus<dir>(state<dir>(*successor), status); setStatus<dir>(state<dir>(*successor), status);
} }
template void ContainerBasePrivate::setStatus<Interface::FORWARD>(const InterfaceState*, InterfaceState::Status);
template void ContainerBasePrivate::setStatus<Interface::BACKWARD>(const InterfaceState*, InterfaceState::Status); void ContainerBasePrivate::setStatus(Interface::Direction dir, const InterfaceState* s, InterfaceState::Status status) {
if (dir == Interface::FORWARD)
setStatus<Interface::FORWARD>(s, InterfaceState::ENABLED);
else
setStatus<Interface::BACKWARD>(s, InterfaceState::ENABLED);
}
template <Interface::Direction dir> template <Interface::Direction dir>
void ContainerBasePrivate::copyState(Interface::iterator external, const InterfacePtr& target, bool updated) { void ContainerBasePrivate::copyState(Interface::iterator external, const InterfacePtr& target, bool updated) {
@ -822,7 +827,6 @@ void Fallbacks::init(const moveit::core::RobotModelConstPtr& robot_model) {
auto& impl{ *pimpl() }; auto& impl{ *pimpl() };
ParallelContainerBase::init(robot_model); ParallelContainerBase::init(robot_model);
impl.current_generator_ = impl.children().begin(); impl.current_generator_ = impl.children().begin();
impl.current_external_state_.stage = impl.children().cend();
} }
bool Fallbacks::canCompute() const { bool Fallbacks::canCompute() const {
@ -845,7 +849,7 @@ bool Fallbacks::canCompute() const {
case PROPAGATE_FORWARDS: case PROPAGATE_FORWARDS:
case PROPAGATE_BACKWARDS: case PROPAGATE_BACKWARDS:
case CONNECT: case CONNECT:
return !impl->pending_states_.empty() || impl->current_external_state_.stage != impl->children().cend(); return !impl->pending_states_.empty() && !impl->pending_states_.front().external_state->priority().failed();
default: default:
assert(false); assert(false);
} }
@ -913,59 +917,62 @@ void FallbacksPrivate::onNewExternalState(Interface::iterator external, bool upd
return; return;
} }
pending_states_.push(ExternalState(external, dir, children().cbegin())); // remember external state for later processing by children.
// children().end() indicates that the states wasn't yet forwarded to any child
pending_states_.push(ExternalState(external, dir, children().cend()));
} }
void FallbacksPrivate::computePropagate(){ void FallbacksPrivate::computePropagate(){
assert(!pending_states_.empty() || current_external_state_.stage != children().cend()); while (!pending_states_.empty()) {
if(current_external_state_.stage == children().cend()) { auto current = pending_states_.begin();
current_external_state_ = pending_states_.pop(); if (!current->external_state->priority().enabled())
return;
ROS_DEBUG_STREAM_NAMED("Fallbacks", "Push external state to '" << (*current_external_state_.stage)->name() << "'"); auto pushState = [this](const ExternalState& ext) {
// feed a new state ROS_DEBUG_STREAM_NAMED("Fallbacks", "Push external state (" << ext.external_state->priority()
copyState(current_external_state_.dir, << ") to '" << (*ext.stage)->name() << "'");
current_external_state_.external_state, copyState(ext.dir, ext.external_state, (*ext.stage)->pimpl()->pullInterface(ext.dir), false);
(*current_external_state_.stage)->pimpl()->pullInterface(current_external_state_.dir), };
false);
}
auto& stage{ current_external_state_.stage }; if (current->stage == children().cend()) {
auto& state{ current_external_state_.external_state }; current->stage = children().begin(); // activate first child
auto dir { current_external_state_.dir }; pushState(*current);
if((*stage)->pimpl()->canCompute()) { }
(*stage)->pimpl()->runCompute();
return;
}
auto has_solutions{ [](const InterfaceState& s, Interface::Direction d){ StagePrivate* child = (*current->stage)->pimpl();
const auto& trajectories { d == Interface::FORWARD if (child->canCompute()) {
? s.outgoingTrajectories() child->runCompute();
: s.incomingTrajectories() }; return; // return after first compute()
return std::find_if(trajectories.cbegin(), trajectories.cend(), [](const auto& t){ return !t->isFailure();}) != trajectories.cend(); }
}};
if(!has_solutions(*state, dir)){ auto has_solutions{ [](const InterfaceState& s, Interface::Direction d){
auto next_stage = std::next(stage); const auto& trajectories { d == Interface::FORWARD ? s.outgoingTrajectories()
if(next_stage != children().cend()){ : s.incomingTrajectories() };
ROS_DEBUG_STREAM_NAMED("Fallbacks", "Child '" << (*stage)->name() << "' failed to generate a solution, schedule state with next child"); return std::find_if(trajectories.cbegin(), trajectories.cend(),
++stage; [](const auto& t){ return !t->isFailure();}) != trajectories.cend();
pending_states_.push(current_external_state_); }};
if(!has_solutions(*current->external_state, current->dir)){
++current->stage;
if(current->stage != children().cend()){
ROS_DEBUG_STREAM_NAMED("Fallbacks", "Child '" << child->name() << "' failed generating solutions, trying next child: '"
<< (*current->stage)->name() << "'");
pushState(*current);
}
else {
ROS_DEBUG_STREAM_NAMED("Fallbacks", "Failed to extend state with all children, pruning path");
parent()->pimpl()->onNewFailure(*me(),
current->dir == Interface::FORWARD ? &*current->external_state : nullptr,
current->dir == Interface::BACKWARD ? nullptr : &*current->external_state);
pending_states_.erase(current);
}
} }
else { else {
ROS_DEBUG_STREAM_NAMED("Fallbacks", "State failed to extend through any child, prune path"); ROS_DEBUG_STREAM_NAMED("Fallbacks", "Child '" << child->name() << "' exhausted, but produced solutions before, not invoking further fallbacks");
parent()->pimpl()->onNewFailure(*me(), pending_states_.erase(current);
dir == Interface::FORWARD ? &*state : nullptr,
dir == Interface::BACKWARD ? nullptr : &*state);
} }
// continue processing with next pending state as we didn't runCompute() yet
} }
else {
ROS_DEBUG_STREAM_NAMED("Fallbacks", "Child '" << (*stage)->name() << "' produced a solution, not invoking further fallbacks");
}
// invalidate current_external_state_ after we processed it
current_external_state_.stage = children().cend();
// if we did not compute a child this call, try again
if(!pending_states_.empty())
computePropagate();
} }
MergerPrivate::MergerPrivate(Merger* me, const std::string& name) : ParallelContainerBasePrivate(me, name) {} MergerPrivate::MergerPrivate(Merger* me, const std::string& name) : ParallelContainerBasePrivate(me, name) {}