order external states

This commit is contained in:
v4hn 2021-09-10 23:19:21 +02:00
parent 9a583ab006
commit 22809c04a5
3 changed files with 37 additions and 34 deletions

View File

@ -250,15 +250,18 @@ protected:
void computeFromExternal(); void computeFromExternal();
struct ExternalState struct ExternalState
{ {
ExternalState() = default;
ExternalState(Interface::iterator e, Interface::Direction d, container_type::const_iterator c) ExternalState(Interface::iterator e, Interface::Direction d, container_type::const_iterator c)
: external_state(e), dir(d), stage(c) {} : external_state(e), dir(d), stage(c) {}
Interface::iterator external_state; Interface::iterator external_state;
Interface::Direction dir; Interface::Direction dir;
container_type::const_iterator stage; container_type::const_iterator stage;
inline bool operator<(const ExternalState& other) const { return *external_state < *other.external_state; }
}; };
std::deque<ExternalState> pending_states_; ordered<ExternalState> pending_states_;
StagePrivate* current_stage_; ExternalState current_external_state_;
void computeGenerate(); void computeGenerate();
container_type::const_iterator current_generator_; container_type::const_iterator current_generator_;

View File

@ -169,6 +169,7 @@ public:
class iterator : public base_type::iterator class iterator : public base_type::iterator
{ {
public: public:
iterator() = default;
iterator(base_type::iterator other) : base_type::iterator(other) {} iterator(base_type::iterator other) : base_type::iterator(other) {}
InterfaceState& operator*() const noexcept { return *base_type::iterator::operator*(); } InterfaceState& operator*() const noexcept { return *base_type::iterator::operator*(); }

View File

@ -817,8 +817,10 @@ void Fallbacks::reset() {
} }
void Fallbacks::init(const moveit::core::RobotModelConstPtr& robot_model) { void Fallbacks::init(const moveit::core::RobotModelConstPtr& robot_model) {
auto& impl{ *pimpl() };
ParallelContainerBase::init(robot_model); ParallelContainerBase::init(robot_model);
pimpl()->current_generator_ = pimpl()->children().begin(); impl.current_generator_ = impl.children().begin();
impl.current_external_state_.stage = impl.children().cend();
} }
bool Fallbacks::canCompute() const { bool Fallbacks::canCompute() const {
@ -850,9 +852,7 @@ 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)
@ -894,48 +894,47 @@ void FallbacksPrivate::onNewExternalState(Interface::iterator external, bool upd
return; return;
} }
pending_states_.push_back(ExternalState(external, dir, children().begin())); pending_states_.push(ExternalState(external, dir, children().cbegin()));
} }
void FallbacksPrivate::computeFromExternal(){ void FallbacksPrivate::computeFromExternal(){
assert(!pending_states_.empty()); assert(!pending_states_.empty());
if(current_external_state_.stage == children().cend()) {
current_external_state_ = pending_states_.pop();
if(!current_stage_) { ROS_DEBUG_STREAM_NAMED("Fallbacks", "Push external state to '" << (*current_external_state_.stage)->name() << "'");
auto spec { pending_states_.front() };
ROS_DEBUG_STREAM_NAMED("Fallbacks", "Push external state to '" << (*spec.stage)->name() << "'");
// feed a new state // feed a new state
copyState(spec.dir, copyState(current_external_state_.dir,
spec.external_state, current_external_state_.external_state,
(*spec.stage)->pimpl()->pullInterface(spec.dir), (*current_external_state_.stage)->pimpl()->pullInterface(current_external_state_.dir),
false); false);
current_stage_ = (*spec.stage)->pimpl();
} }
if(current_stage_->canCompute()) auto& stage{ current_external_state_.stage };
current_stage_->runCompute(); auto& state{ current_external_state_.external_state };
auto dir { current_external_state_.dir };
if((*stage)->pimpl()->canCompute())
(*stage)->pimpl()->runCompute();
else { else {
auto spec { pending_states_.front() }; auto has_solutions{ [](const InterfaceState& s, Interface::Direction d){
current_stage_ = nullptr; return d == Interface::FORWARD
pending_states_.pop_front(); ? !s.outgoingTrajectories().empty()
: !s.incomingTrajectories().empty();
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)){ if(!has_solutions(*state, dir)){
ROS_DEBUG_STREAM_NAMED("Fallbacks", "Child '" << (*spec.stage)->name() << "' failed to generate a solution, schedule state with next child (if any)"); if(++stage != children().cend()){
if(++spec.stage != children().cend()) ROS_DEBUG_STREAM_NAMED("Fallbacks", "Child '" << (*stage)->name() << "' failed to generate a solution, schedule state with next child");
pending_states_.push_back(spec); pending_states_.push(current_external_state_);
else }
// prune solution path if there is no way to extend external_state through Fallbacks else {
ROS_DEBUG_STREAM_NAMED("Fallbacks", "State failed to extend through any child, prune path");
ContainerBasePrivate::onNewFailure(*children().back(), ContainerBasePrivate::onNewFailure(*children().back(),
spec.dir == Interface::FORWARD ? &*spec.external_state : nullptr, dir == Interface::FORWARD ? &*state : nullptr,
spec.dir == Interface::BACKWARD ? nullptr : &*spec.external_state); dir == Interface::BACKWARD ? nullptr : &*state);
}
} }
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();