mirror of
https://github.com/moveit/moveit_task_constructor.git
synced 2025-11-04 14:49:57 +08:00
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:
parent
22809c04a5
commit
887da5b094
@ -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) {}
|
||||||
|
|||||||
Loading…
Reference in New Issue
Block a user