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