mirror of
https://github.com/moveit/moveit_task_constructor.git
synced 2025-11-04 14:49:57 +08:00
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:
parent
5a9cfc50ea
commit
9a583ab006
@ -248,8 +248,6 @@ public:
|
||||
|
||||
protected:
|
||||
void computeFromExternal();
|
||||
void computeGenerate();
|
||||
|
||||
struct ExternalState
|
||||
{
|
||||
ExternalState(Interface::iterator e, Interface::Direction d, container_type::const_iterator c)
|
||||
@ -260,6 +258,9 @@ protected:
|
||||
container_type::const_iterator stage;
|
||||
};
|
||||
std::deque<ExternalState> pending_states_;
|
||||
StagePrivate* current_stage_;
|
||||
|
||||
void computeGenerate();
|
||||
container_type::const_iterator current_generator_;
|
||||
|
||||
private:
|
||||
|
||||
@ -818,6 +818,7 @@ void Fallbacks::reset() {
|
||||
|
||||
void Fallbacks::init(const moveit::core::RobotModelConstPtr& robot_model) {
|
||||
ParallelContainerBase::init(robot_model);
|
||||
pimpl()->current_generator_ = pimpl()->children().begin();
|
||||
}
|
||||
|
||||
bool Fallbacks::canCompute() const {
|
||||
@ -832,7 +833,7 @@ bool Fallbacks::canCompute() const {
|
||||
return impl->current_generator_ != impl->children().end();
|
||||
}
|
||||
else
|
||||
return !pimpl()->pending_states_.empty();
|
||||
return !impl->pending_states_.empty();
|
||||
}
|
||||
|
||||
void Fallbacks::compute() {
|
||||
@ -849,22 +850,19 @@ void Fallbacks::onNewSolution(const SolutionBase& s) {
|
||||
}
|
||||
|
||||
FallbacksPrivate::FallbacksPrivate(Fallbacks* me, const std::string& name)
|
||||
: ParallelContainerBasePrivate(me, name) {}
|
||||
: ParallelContainerBasePrivate(me, name) {
|
||||
current_stage_ = nullptr;
|
||||
}
|
||||
|
||||
void FallbacksPrivate::initializeExternalInterfaces() {
|
||||
if (requiredInterface() & READS_START)
|
||||
starts().reset(new Interface([this](Interface::iterator external, bool updated) {
|
||||
this->onNewExternalState<Interface::FORWARD>(external, updated);
|
||||
}));
|
||||
this->onNewExternalState<Interface::FORWARD>(external, updated);
|
||||
}));
|
||||
if (requiredInterface() & READS_END)
|
||||
ends().reset(new Interface([this](Interface::iterator external, bool 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();
|
||||
this->onNewExternalState<Interface::BACKWARD>(external, updated);
|
||||
}));
|
||||
}
|
||||
|
||||
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() {
|
||||
if(solutions_.empty())
|
||||
// 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_;
|
||||
}
|
||||
|
||||
if(current_generator_ == children().end())
|
||||
return;
|
||||
@ -890,7 +890,7 @@ template <typename Interface::Direction dir>
|
||||
void FallbacksPrivate::onNewExternalState(Interface::iterator external, bool updated) {
|
||||
// TODO(v4hn): updated is not implemented
|
||||
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;
|
||||
}
|
||||
|
||||
@ -899,44 +899,46 @@ void FallbacksPrivate::onNewExternalState(Interface::iterator external, bool upd
|
||||
|
||||
void FallbacksPrivate::computeFromExternal(){
|
||||
assert(!pending_states_.empty());
|
||||
auto spec { pending_states_.front() };
|
||||
|
||||
pending_states_.pop_front();
|
||||
if(!current_stage_) {
|
||||
auto spec { pending_states_.front() };
|
||||
|
||||
ROS_DEBUG_STREAM_NAMED("Fallback", "Push external state to '" << (*spec.stage)->name() << "'");
|
||||
// feed a new state
|
||||
copyState(spec.dir,
|
||||
spec.external_state,
|
||||
(*spec.stage)->pimpl()->pullInterface(spec.dir),
|
||||
false);
|
||||
ROS_DEBUG_STREAM_NAMED("Fallbacks", "Push external state to '" << (*spec.stage)->name() << "'");
|
||||
// feed a new state
|
||||
copyState(spec.dir,
|
||||
spec.external_state,
|
||||
(*spec.stage)->pimpl()->pullInterface(spec.dir),
|
||||
false);
|
||||
|
||||
const auto& stage { (*spec.stage)->pimpl() };
|
||||
|
||||
try {
|
||||
// run ALL possible computations (on new state)
|
||||
// this is needed to decide whether it should be passed to the next child too
|
||||
while (stage->canCompute()){
|
||||
stage->runCompute();
|
||||
}
|
||||
} catch (const Property::error& e) {
|
||||
stage->me()->reportPropertyError(e);
|
||||
current_stage_ = (*spec.stage)->pimpl();
|
||||
}
|
||||
|
||||
auto has_solutions{ [](const InterfaceState& state, Interface::Direction dir){
|
||||
return dir == Interface::FORWARD
|
||||
? !state.outgoingTrajectories().empty()
|
||||
: !state.incomingTrajectories().empty();
|
||||
} };
|
||||
if(current_stage_->canCompute())
|
||||
current_stage_->runCompute();
|
||||
else {
|
||||
auto spec { pending_states_.front() };
|
||||
current_stage_ = nullptr;
|
||||
pending_states_.pop_front();
|
||||
|
||||
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)");
|
||||
if(++spec.stage != children().cend())
|
||||
pending_states_.push_back(spec);
|
||||
else
|
||||
// prune solution path if there is no way to extend external_state through Fallbacks
|
||||
ContainerBasePrivate::onNewFailure(*children().back(),
|
||||
spec.dir == Interface::FORWARD ? &*spec.external_state : nullptr,
|
||||
spec.dir == Interface::BACKWARD ? nullptr : &*spec.external_state);
|
||||
auto has_solutions{ [](const InterfaceState& state, Interface::Direction dir){
|
||||
return dir == Interface::FORWARD
|
||||
? !state.outgoingTrajectories().empty()
|
||||
: !state.incomingTrajectories().empty();
|
||||
} };
|
||||
|
||||
if(!has_solutions(*spec.external_state, spec.dir)){
|
||||
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())
|
||||
pending_states_.push_back(spec);
|
||||
else
|
||||
// prune solution path if there is no way to extend external_state through Fallbacks
|
||||
ContainerBasePrivate::onNewFailure(*children().back(),
|
||||
spec.dir == Interface::FORWARD ? &*spec.external_state : nullptr,
|
||||
spec.dir == Interface::BACKWARD ? nullptr : &*spec.external_state);
|
||||
}
|
||||
// if we did not compute a child this call, try again
|
||||
if(!pending_states_.empty())
|
||||
computeFromExternal();
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
Loading…
Reference in New Issue
Block a user