fix fallbacks logic

Setting up a demo for
Fallbacks({CartesianPath,PTP,RRTConnect})
I found the logic did not work as expected yet.

- process last job spec as well
- ignore failures when looking for a solution
- add more debug output
This commit is contained in:
v4hn 2021-09-14 23:54:38 +02:00
parent 22809c04a5
commit 887da5b094

View File

@ -835,7 +835,7 @@ bool Fallbacks::canCompute() const {
return impl->current_generator_ != impl->children().end(); return impl->current_generator_ != impl->children().end();
} }
else else
return !impl->pending_states_.empty(); return !impl->pending_states_.empty() || impl->current_external_state_.stage != impl->children().cend();
} }
void Fallbacks::compute() { void Fallbacks::compute() {
@ -898,7 +898,7 @@ void FallbacksPrivate::onNewExternalState(Interface::iterator external, bool upd
} }
void FallbacksPrivate::computeFromExternal(){ void FallbacksPrivate::computeFromExternal(){
assert(!pending_states_.empty()); assert(!pending_states_.empty() || current_external_state_.stage != children().cend());
if(current_external_state_.stage == children().cend()) { if(current_external_state_.stage == children().cend()) {
current_external_state_ = pending_states_.pop(); current_external_state_ = pending_states_.pop();
@ -913,18 +913,23 @@ void FallbacksPrivate::computeFromExternal(){
auto& stage{ current_external_state_.stage }; auto& stage{ current_external_state_.stage };
auto& state{ current_external_state_.external_state }; auto& state{ current_external_state_.external_state };
auto dir { current_external_state_.dir }; auto dir { current_external_state_.dir };
if((*stage)->pimpl()->canCompute()) if((*stage)->pimpl()->canCompute()) {
(*stage)->pimpl()->runCompute(); (*stage)->pimpl()->runCompute();
else { return;
}
auto has_solutions{ [](const InterfaceState& s, Interface::Direction d){ auto has_solutions{ [](const InterfaceState& s, Interface::Direction d){
return d == Interface::FORWARD const auto& trajectories { d == Interface::FORWARD
? !s.outgoingTrajectories().empty() ? s.outgoingTrajectories()
: !s.incomingTrajectories().empty(); : s.incomingTrajectories() };
} }; return std::find_if(trajectories.cbegin(), trajectories.cend(), [](const auto& t){ return !t->isFailure();}) != trajectories.cend();
}};
if(!has_solutions(*state, dir)){ if(!has_solutions(*state, dir)){
if(++stage != children().cend()){ auto next_stage = std::next(stage);
if(next_stage != children().cend()){
ROS_DEBUG_STREAM_NAMED("Fallbacks", "Child '" << (*stage)->name() << "' failed to generate a solution, schedule state with next child"); ROS_DEBUG_STREAM_NAMED("Fallbacks", "Child '" << (*stage)->name() << "' failed to generate a solution, schedule state with next child");
++stage;
pending_states_.push(current_external_state_); pending_states_.push(current_external_state_);
} }
else { else {
@ -934,11 +939,14 @@ void FallbacksPrivate::computeFromExternal(){
dir == Interface::BACKWARD ? nullptr : &*state); dir == Interface::BACKWARD ? nullptr : &*state);
} }
} }
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(); current_external_state_.stage = children().cend();
// if we did not compute a child this call, try again // if we did not compute a child this call, try again
if(!pending_states_.empty()) if(!pending_states_.empty())
computeFromExternal(); computeFromExternal();
}
} }
MergerPrivate::MergerPrivate(Merger* me, const std::string& name) : ParallelContainerBasePrivate(me, name) {} MergerPrivate::MergerPrivate(Merger* me, const std::string& name) : ParallelContainerBasePrivate(me, name) {}