run only one compute step per call

Note that while this ensures other stages outside the Fallbacks container
can compute as well, it does not solve the problem internally.
A new incoming state will only ever be considered once
the current stage cannot compute any more.

We have no way of telling a child to compute for *a specific state* for now.
So once we copied a state to its interface we have to let it compute until
all possibilities are exhausted to detect whether or not it could generate a solution for it.
If we wouldn't do so, there were no way of knowing when to fall back
to the next child as long as the stage can still compute on *any* copied solution.
This commit is contained in:
v4hn 2021-09-03 12:26:53 +02:00
parent 5a9cfc50ea
commit 9a583ab006
2 changed files with 50 additions and 47 deletions

View File

@ -248,8 +248,6 @@ public:
protected: protected:
void computeFromExternal(); void computeFromExternal();
void computeGenerate();
struct ExternalState struct ExternalState
{ {
ExternalState(Interface::iterator e, Interface::Direction d, container_type::const_iterator c) ExternalState(Interface::iterator e, Interface::Direction d, container_type::const_iterator c)
@ -260,6 +258,9 @@ protected:
container_type::const_iterator stage; container_type::const_iterator stage;
}; };
std::deque<ExternalState> pending_states_; std::deque<ExternalState> pending_states_;
StagePrivate* current_stage_;
void computeGenerate();
container_type::const_iterator current_generator_; container_type::const_iterator current_generator_;
private: private:

View File

@ -818,6 +818,7 @@ void Fallbacks::reset() {
void Fallbacks::init(const moveit::core::RobotModelConstPtr& robot_model) { void Fallbacks::init(const moveit::core::RobotModelConstPtr& robot_model) {
ParallelContainerBase::init(robot_model); ParallelContainerBase::init(robot_model);
pimpl()->current_generator_ = pimpl()->children().begin();
} }
bool Fallbacks::canCompute() const { bool Fallbacks::canCompute() const {
@ -832,7 +833,7 @@ bool Fallbacks::canCompute() const {
return impl->current_generator_ != impl->children().end(); return impl->current_generator_ != impl->children().end();
} }
else else
return !pimpl()->pending_states_.empty(); return !impl->pending_states_.empty();
} }
void Fallbacks::compute() { void Fallbacks::compute() {
@ -849,7 +850,9 @@ void Fallbacks::onNewSolution(const SolutionBase& s) {
} }
FallbacksPrivate::FallbacksPrivate(Fallbacks* me, const std::string& name) FallbacksPrivate::FallbacksPrivate(Fallbacks* me, const std::string& name)
: ParallelContainerBasePrivate(me, name) {} : ParallelContainerBasePrivate(me, name) {
current_stage_ = nullptr;
}
void FallbacksPrivate::initializeExternalInterfaces() { void FallbacksPrivate::initializeExternalInterfaces() {
if (requiredInterface() & READS_START) if (requiredInterface() & READS_START)
@ -860,11 +863,6 @@ void FallbacksPrivate::initializeExternalInterfaces() {
ends().reset(new Interface([this](Interface::iterator external, bool updated) { ends().reset(new Interface([this](Interface::iterator external, bool updated) {
this->onNewExternalState<Interface::BACKWARD>(external, updated); this->onNewExternalState<Interface::BACKWARD>(external, updated);
})); }));
// we've got to set this somewhere once the interface flags are known.
// so we might as well do it here
if(requiredInterface() == GENERATE)
current_generator_ = children().begin();
} }
void FallbacksPrivate::onNewFailure(const Stage& /*child*/, const InterfaceState* /*from*/, const InterfaceState* /*to*/) { void FallbacksPrivate::onNewFailure(const Stage& /*child*/, const InterfaceState* /*from*/, const InterfaceState* /*to*/) {
@ -877,8 +875,10 @@ void FallbacksPrivate::onNewFailure(const Stage& /*child*/, const InterfaceState
void FallbacksPrivate::computeGenerate() { void FallbacksPrivate::computeGenerate() {
if(solutions_.empty()) if(solutions_.empty())
// move to first generator that can run // move to first generator that can run
while(current_generator_ != children().end() && !(*current_generator_)->pimpl()->canCompute()) 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_; ++current_generator_;
}
if(current_generator_ == children().end()) if(current_generator_ == children().end())
return; return;
@ -890,7 +890,7 @@ template <typename Interface::Direction dir>
void FallbacksPrivate::onNewExternalState(Interface::iterator external, bool updated) { void FallbacksPrivate::onNewExternalState(Interface::iterator external, bool updated) {
// TODO(v4hn): updated is not implemented // TODO(v4hn): updated is not implemented
if(updated){ if(updated){
ROS_DEBUG_NAMED("Fallback", "updating external states is not supported in Fallbacks"); ROS_DEBUG_NAMED("Fallbacks", "updating external states is not supported in Fallbacks");
return; return;
} }
@ -899,28 +899,26 @@ void FallbacksPrivate::onNewExternalState(Interface::iterator external, bool upd
void FallbacksPrivate::computeFromExternal(){ void FallbacksPrivate::computeFromExternal(){
assert(!pending_states_.empty()); assert(!pending_states_.empty());
if(!current_stage_) {
auto spec { pending_states_.front() }; auto spec { pending_states_.front() };
pending_states_.pop_front(); ROS_DEBUG_STREAM_NAMED("Fallbacks", "Push external state to '" << (*spec.stage)->name() << "'");
ROS_DEBUG_STREAM_NAMED("Fallback", "Push external state to '" << (*spec.stage)->name() << "'");
// feed a new state // feed a new state
copyState(spec.dir, copyState(spec.dir,
spec.external_state, spec.external_state,
(*spec.stage)->pimpl()->pullInterface(spec.dir), (*spec.stage)->pimpl()->pullInterface(spec.dir),
false); false);
const auto& stage { (*spec.stage)->pimpl() }; current_stage_ = (*spec.stage)->pimpl();
}
try { if(current_stage_->canCompute())
// run ALL possible computations (on new state) current_stage_->runCompute();
// this is needed to decide whether it should be passed to the next child too else {
while (stage->canCompute()){ auto spec { pending_states_.front() };
stage->runCompute(); current_stage_ = nullptr;
} pending_states_.pop_front();
} catch (const Property::error& e) {
stage->me()->reportPropertyError(e);
}
auto has_solutions{ [](const InterfaceState& state, Interface::Direction dir){ auto has_solutions{ [](const InterfaceState& state, Interface::Direction dir){
return dir == Interface::FORWARD return dir == Interface::FORWARD
@ -929,7 +927,7 @@ void FallbacksPrivate::computeFromExternal(){
} }; } };
if(!has_solutions(*spec.external_state, spec.dir)){ if(!has_solutions(*spec.external_state, spec.dir)){
ROS_DEBUG_STREAM_NAMED("Fallback", "Child '" << (*spec.stage)->name() << "' failed to generate a solution, schedule state with next child (if any)"); ROS_DEBUG_STREAM_NAMED("Fallbacks", "Child '" << (*spec.stage)->name() << "' failed to generate a solution, schedule state with next child (if any)");
if(++spec.stage != children().cend()) if(++spec.stage != children().cend())
pending_states_.push_back(spec); pending_states_.push_back(spec);
else else
@ -938,6 +936,10 @@ void FallbacksPrivate::computeFromExternal(){
spec.dir == Interface::FORWARD ? &*spec.external_state : nullptr, spec.dir == Interface::FORWARD ? &*spec.external_state : nullptr,
spec.dir == Interface::BACKWARD ? nullptr : &*spec.external_state); spec.dir == Interface::BACKWARD ? nullptr : &*spec.external_state);
} }
// if we did not compute a child this call, try again
if(!pending_states_.empty())
computeFromExternal();
}
} }
MergerPrivate::MergerPrivate(Merger* me, const std::string& name) : ParallelContainerBasePrivate(me, name) {} MergerPrivate::MergerPrivate(Merger* me, const std::string& name) : ParallelContainerBasePrivate(me, name) {}