mirror of
https://github.com/moveit/moveit_task_constructor.git
synced 2025-11-04 14:49:57 +08:00
Simplify: job_has_solutions_
Just set a flag when we received a full solution
This commit is contained in:
parent
7237e81547
commit
e296bd7aed
@ -261,8 +261,11 @@ public:
|
|||||||
// virtual method specific to each variant
|
// virtual method 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.
|
||||||
virtual bool nextJob() { return false; }
|
virtual bool nextJob() { return false; }
|
||||||
|
/// Reset data structures
|
||||||
|
virtual void reset();
|
||||||
|
|
||||||
container_type::const_iterator current_; // currently active child generator
|
container_type::const_iterator current_; // currently active child generator
|
||||||
|
bool job_has_solutions_; // flag indicating whether the current job generated solutions
|
||||||
};
|
};
|
||||||
PIMPL_FUNCTIONS(Fallbacks)
|
PIMPL_FUNCTIONS(Fallbacks)
|
||||||
|
|
||||||
@ -278,7 +281,7 @@ struct FallbacksPrivatePropagator : FallbacksPrivate
|
|||||||
{
|
{
|
||||||
FallbacksPrivatePropagator(FallbacksPrivate&& old);
|
FallbacksPrivatePropagator(FallbacksPrivate&& old);
|
||||||
bool nextJob() override;
|
bool nextJob() override;
|
||||||
bool jobHasSolutions() const;
|
void reset() override;
|
||||||
|
|
||||||
Interface::Direction dir_; // propagation direction
|
Interface::Direction dir_; // propagation direction
|
||||||
Interface::iterator job_; // pointer to currently processed external state
|
Interface::iterator job_; // pointer to currently processed external state
|
||||||
|
|||||||
@ -849,18 +849,18 @@ Fallbacks::Fallbacks(FallbacksPrivate* impl) : ParallelContainerBase(impl) {}
|
|||||||
|
|
||||||
void Fallbacks::reset() {
|
void Fallbacks::reset() {
|
||||||
ParallelContainerBase::reset();
|
ParallelContainerBase::reset();
|
||||||
|
pimpl()->reset();
|
||||||
}
|
}
|
||||||
|
|
||||||
void Fallbacks::init(const moveit::core::RobotModelConstPtr& robot_model) {
|
void Fallbacks::init(const moveit::core::RobotModelConstPtr& robot_model) {
|
||||||
ParallelContainerBase::init(robot_model);
|
ParallelContainerBase::init(robot_model);
|
||||||
auto impl = pimpl();
|
pimpl()->reset();
|
||||||
impl->current_ = impl->children().begin();
|
|
||||||
}
|
}
|
||||||
|
|
||||||
bool Fallbacks::canCompute() const {
|
bool Fallbacks::canCompute() const {
|
||||||
auto impl = const_cast<FallbacksPrivate*>(pimpl());
|
auto impl = const_cast<FallbacksPrivate*>(pimpl());
|
||||||
|
|
||||||
while(impl->current_ != impl->children().end() && // not completely exhaused
|
while(impl->current_ != impl->children().end() && // not completely exhausted
|
||||||
!(*impl->current_)->pimpl()->canCompute()) // but current child cannot compute
|
!(*impl->current_)->pimpl()->canCompute()) // but current child cannot compute
|
||||||
return impl->nextJob(); // advance to next job
|
return impl->nextJob(); // advance to next job
|
||||||
|
|
||||||
@ -873,6 +873,7 @@ void Fallbacks::compute() {
|
|||||||
}
|
}
|
||||||
|
|
||||||
void Fallbacks::onNewSolution(const SolutionBase& s) {
|
void Fallbacks::onNewSolution(const SolutionBase& s) {
|
||||||
|
pimpl()->job_has_solutions_ = true;
|
||||||
liftSolution(s);
|
liftSolution(s);
|
||||||
}
|
}
|
||||||
|
|
||||||
@ -901,8 +902,11 @@ FallbacksPrivate::FallbacksPrivate(FallbacksPrivate&& other)
|
|||||||
: ParallelContainerBasePrivate(static_cast<Fallbacks*>(other.me()), "") {
|
: ParallelContainerBasePrivate(static_cast<Fallbacks*>(other.me()), "") {
|
||||||
// move contents of other
|
// move contents of other
|
||||||
this->ParallelContainerBasePrivate::operator=(std::move(other));
|
this->ParallelContainerBasePrivate::operator=(std::move(other));
|
||||||
// (re)initialize
|
}
|
||||||
|
|
||||||
|
void FallbacksPrivate::reset() {
|
||||||
current_ = children().begin();
|
current_ = children().begin();
|
||||||
|
job_has_solutions_ = false;
|
||||||
}
|
}
|
||||||
|
|
||||||
void FallbacksPrivate::initializeExternalInterfaces() {
|
void FallbacksPrivate::initializeExternalInterfaces() {
|
||||||
@ -920,13 +924,13 @@ void FallbacksPrivate::onNewFailure(const Stage& /*child*/, const InterfaceState
|
|||||||
|
|
||||||
|
|
||||||
FallbacksPrivateGenerator::FallbacksPrivateGenerator(FallbacksPrivate&& old)
|
FallbacksPrivateGenerator::FallbacksPrivateGenerator(FallbacksPrivate&& old)
|
||||||
: FallbacksPrivate(std::move(old)) {}
|
: FallbacksPrivate(std::move(old)) { reset(); }
|
||||||
|
|
||||||
bool FallbacksPrivateGenerator::nextJob() {
|
bool FallbacksPrivateGenerator::nextJob() {
|
||||||
assert(current_ != children().end() && !(*current_)->pimpl()->canCompute());
|
assert(current_ != children().end() && !(*current_)->pimpl()->canCompute());
|
||||||
|
|
||||||
// don't advance to next child when we already produced solutions
|
// don't advance to next child when we already produced solutions
|
||||||
if (!solutions_.empty()) {
|
if (job_has_solutions_) {
|
||||||
current_ = children().end(); // indicate that we are exhausted
|
current_ = children().end(); // indicate that we are exhausted
|
||||||
return false;
|
return false;
|
||||||
}
|
}
|
||||||
@ -956,28 +960,27 @@ FallbacksPrivatePropagator::FallbacksPrivatePropagator(FallbacksPrivate&& old)
|
|||||||
default:
|
default:
|
||||||
assert(false);
|
assert(false);
|
||||||
}
|
}
|
||||||
job_ = pullInterface(dir_)->end(); // indicate fresh start
|
FallbacksPrivatePropagator::reset();
|
||||||
}
|
}
|
||||||
|
|
||||||
bool FallbacksPrivatePropagator::jobHasSolutions() const {
|
void FallbacksPrivatePropagator::reset() {
|
||||||
const auto& trajectories { dir_ == Interface::FORWARD ? job_->outgoingTrajectories()
|
FallbacksPrivate::reset();
|
||||||
: job_->incomingTrajectories() };
|
job_ = pullInterface(dir_)->end(); // indicate fresh start
|
||||||
return std::find_if(trajectories.cbegin(), trajectories.cend(),
|
}
|
||||||
[](const auto& t){ return !t->isFailure();}) != trajectories.cend();
|
|
||||||
};
|
|
||||||
|
|
||||||
bool FallbacksPrivatePropagator::nextJob() {
|
bool FallbacksPrivatePropagator::nextJob() {
|
||||||
assert(current_ != children().end() && !(*current_)->pimpl()->canCompute());
|
assert(current_ != children().end() && !(*current_)->pimpl()->canCompute());
|
||||||
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 (!jobHasSolutions()) { // 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())
|
if (std::next(current_) != children().end())
|
||||||
ROS_DEBUG_STREAM_NAMED("Fallbacks", "Propagator '" << (*current_)->name() << "' failed, trying next one.");
|
ROS_DEBUG_STREAM_NAMED("Fallbacks", "Propagator '" << (*current_)->name() << "' failed, trying next one.");
|
||||||
++current_; // advance to next child
|
++current_; // advance to next child
|
||||||
} else
|
} 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;
|
||||||
|
|
||||||
if (current_ == children().end()) { // all children processed the job_
|
if (current_ == children().end()) { // all children processed the job_
|
||||||
if (job_ != jobs->end()) {
|
if (job_ != jobs->end()) {
|
||||||
|
|||||||
Loading…
Reference in New Issue
Block a user