mirror of
https://github.com/moveit/moveit_task_constructor.git
synced 2025-11-04 14:49:57 +08:00
FallbacksPrivate::nextChild()
... factoring out functionality shared between FallbacksPrivateGenerator and FallbacksPrivatePropagator to switch to next child in nextJob().
This commit is contained in:
parent
7af3d8ebd7
commit
b82b70ed64
@ -257,6 +257,7 @@ public:
|
|||||||
// methods common to all variants
|
// methods common to all variants
|
||||||
void initializeExternalInterfaces() final;
|
void initializeExternalInterfaces() final;
|
||||||
void onNewFailure(const Stage& child, const InterfaceState* from, const InterfaceState* to) override;
|
void onNewFailure(const Stage& child, const InterfaceState* from, const InterfaceState* to) override;
|
||||||
|
void nextChild(); /// << Advance to next child
|
||||||
|
|
||||||
// virtual methods specific to each variant
|
// virtual methods specific to each variant
|
||||||
/// Advance to the next job, assuming that the current child is exhausted on the current job.
|
/// Advance to the next job, assuming that the current child is exhausted on the current job.
|
||||||
|
|||||||
@ -219,6 +219,7 @@ void ContainerBasePrivate::setStatus(const InterfaceState* s, InterfaceState::St
|
|||||||
|
|
||||||
// if possible (i.e. if state s has an external counterpart), escalate setStatus to external interface
|
// if possible (i.e. if state s has an external counterpart), escalate setStatus to external interface
|
||||||
if (parent() && trajectories<dir>(*s).empty()) {
|
if (parent() && trajectories<dir>(*s).empty()) {
|
||||||
|
// TODO: This was coded with SerialContainer in mind. Not sure, it works for ParallelContainers
|
||||||
auto external{ internalToExternalMap().find(s) };
|
auto external{ internalToExternalMap().find(s) };
|
||||||
if (external != internalToExternalMap().end()) { // do we have an external state?
|
if (external != internalToExternalMap().end()) { // do we have an external state?
|
||||||
// only escalate if there is no other *enabled* internal state connected to the same external one
|
// only escalate if there is no other *enabled* internal state connected to the same external one
|
||||||
@ -928,6 +929,11 @@ void FallbacksPrivate::onNewFailure(const Stage& child, const InterfaceState* /*
|
|||||||
(void)child;
|
(void)child;
|
||||||
}
|
}
|
||||||
|
|
||||||
|
void FallbacksPrivate::nextChild() {
|
||||||
|
if (std::next(current_) != children().end())
|
||||||
|
ROS_DEBUG_STREAM_NAMED("Fallbacks", "Child '" << (*current_)->name() << "' failed, trying next one.");
|
||||||
|
++current_; // advance to next child
|
||||||
|
}
|
||||||
|
|
||||||
FallbacksPrivateGenerator::FallbacksPrivateGenerator(FallbacksPrivate&& old)
|
FallbacksPrivateGenerator::FallbacksPrivateGenerator(FallbacksPrivate&& old)
|
||||||
: FallbacksPrivate(std::move(old)) { reset(); }
|
: FallbacksPrivate(std::move(old)) { reset(); }
|
||||||
@ -941,11 +947,8 @@ bool FallbacksPrivateGenerator::nextJob() {
|
|||||||
return false;
|
return false;
|
||||||
}
|
}
|
||||||
|
|
||||||
do {
|
do { nextChild(); }
|
||||||
if (std::next(current_) != children().end())
|
while (current_ != children().end() && !(*current_)->pimpl()->canCompute());
|
||||||
ROS_DEBUG_STREAM_NAMED("Fallbacks", "Generator '" << (*current_)->name() << "' failed, trying next one.");
|
|
||||||
++current_; // advance to next child
|
|
||||||
} while (current_ != children().end() && !(*current_)->pimpl()->canCompute());
|
|
||||||
|
|
||||||
// return value shall indicate current_->canCompute()
|
// return value shall indicate current_->canCompute()
|
||||||
return current_ != children().end();
|
return current_ != children().end();
|
||||||
@ -979,11 +982,9 @@ bool FallbacksPrivatePropagator::nextJob() {
|
|||||||
const auto jobs = pullInterface(dir_);
|
const auto jobs = pullInterface(dir_);
|
||||||
|
|
||||||
if (job_ != jobs->end()) { // current job exists, but is exhausted on current child
|
if (job_ != jobs->end()) { // current job exists, but is exhausted on current child
|
||||||
if (!job_has_solutions_) { // job didn't produce solutions -> feed to next child
|
if (!job_has_solutions_) // job didn't produce solutions -> feed to next child
|
||||||
if (std::next(current_) != children().end())
|
nextChild();
|
||||||
ROS_DEBUG_STREAM_NAMED("Fallbacks", "Propagator '" << (*current_)->name() << "' failed, trying next one.");
|
else
|
||||||
++current_; // advance to next child
|
|
||||||
} else
|
|
||||||
current_ = children().end(); // indicate that this job is exhausted on all children
|
current_ = children().end(); // indicate that this job is exhausted on all children
|
||||||
}
|
}
|
||||||
job_has_solutions_ = false;
|
job_has_solutions_ = false;
|
||||||
|
|||||||
Loading…
Reference in New Issue
Block a user