Implement state-wise Fallbacks

Keep the previous logic around for Generator stages.
Note that this only makes sense for *pure* Generators and not for MonitoringGenerator,
because for the latter we would expect monitored solutions to be passed individually
(similar to pruning).
This commit is contained in:
v4hn 2021-08-19 20:50:24 +02:00
parent f32e747645
commit 8719b1c3d6
4 changed files with 166 additions and 25 deletions

View File

@ -150,6 +150,7 @@ public:
void onNewSolution(const SolutionBase& s) override; void onNewSolution(const SolutionBase& s) override;
}; };
class FallbacksPrivate;
/** Plan for different alternatives in sequence. /** Plan for different alternatives in sequence.
* *
* Try to find feasible solutions using first child. Only if this fails, * Try to find feasible solutions using first child. Only if this fails,
@ -158,16 +159,17 @@ public:
*/ */
class Fallbacks : public ParallelContainerBase class Fallbacks : public ParallelContainerBase
{ {
mutable Stage* active_child_ = nullptr;
public: public:
Fallbacks(const std::string& name = "fallbacks") : ParallelContainerBase(name) {} PRIVATE_CLASS(Fallbacks);
Fallbacks(const std::string& name = "fallbacks");
void reset() override; void reset() override;
void init(const moveit::core::RobotModelConstPtr& robot_model) override; void init(const moveit::core::RobotModelConstPtr& robot_model) override;
bool canCompute() const override; bool canCompute() const override;
void compute() override; void compute() override;
protected:
Fallbacks(FallbacksPrivate* impl);
void onNewSolution(const SolutionBase& s) override; void onNewSolution(const SolutionBase& s) override;
}; };

View File

@ -131,7 +131,7 @@ public:
inline const auto& externalToInternalMap() const { return internal_external_.by<EXTERNAL>(); } inline const auto& externalToInternalMap() const { return internal_external_.by<EXTERNAL>(); }
/// called by a (direct) child when a solution failed /// called by a (direct) child when a solution failed
void onNewFailure(const Stage& child, const InterfaceState* from, const InterfaceState* to); virtual void onNewFailure(const Stage& child, const InterfaceState* from, const InterfaceState* to);
protected: protected:
ContainerBasePrivate(ContainerBase* me, const std::string& name); ContainerBasePrivate(ContainerBase* me, const std::string& name);
@ -155,6 +155,8 @@ protected:
/// 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>
void copyState(Interface::iterator external, const InterfacePtr& target, bool updated); void copyState(Interface::iterator external, const InterfacePtr& target, bool updated);
/// non-template version
void copyState(Interface::Direction dir, Interface::iterator external, const InterfacePtr& target, bool updated);
/// lift solution from internal to external level /// lift solution from internal to external level
void liftSolution(const SolutionBasePtr& solution, const InterfaceState* internal_from, void liftSolution(const SolutionBasePtr& solution, const InterfaceState* internal_from,
const InterfaceState* internal_to); const InterfaceState* internal_to);
@ -237,6 +239,37 @@ private:
}; };
PIMPL_FUNCTIONS(ParallelContainerBase) PIMPL_FUNCTIONS(ParallelContainerBase)
class FallbacksPrivate : public ParallelContainerBasePrivate
{
friend class Fallbacks;
public:
FallbacksPrivate(Fallbacks* me, const std::string& name);
protected:
void computeFromExternal();
void computeGenerate();
struct ExternalState
{
ExternalState(Interface::iterator e, Interface::Direction d, container_type::const_iterator c)
: external_state(e), dir(d), stage(c) {}
Interface::iterator external_state;
Interface::Direction dir;
container_type::const_iterator stage;
};
std::deque<ExternalState> pending_states_;
container_type::const_iterator current_generator_;
private:
void initializeExternalInterfaces(InterfaceFlags expected) override;
template <typename Interface::Direction>
void onNewExternalState(Interface::iterator external, bool updated);
void onNewFailure(const Stage& child, const InterfaceState* from, const InterfaceState* to) override;
};
PIMPL_FUNCTIONS(Fallbacks)
class WrapperBasePrivate : public ParallelContainerBasePrivate class WrapperBasePrivate : public ParallelContainerBasePrivate
{ {
friend class WrapperBase; friend class WrapperBase;

View File

@ -209,6 +209,14 @@ void ContainerBasePrivate::copyState(Interface::iterator external, const Interfa
internalToExternalMap().insert(std::make_pair(&*internal, &*external)); internalToExternalMap().insert(std::make_pair(&*internal, &*external));
} }
void ContainerBasePrivate::copyState(Interface::Direction dir, Interface::iterator external, const InterfacePtr& target,
bool updated) {
if (dir == Interface::FORWARD)
copyState<Interface::FORWARD>(external, target, updated);
else
copyState<Interface::BACKWARD>(external, target, updated);
}
void ContainerBasePrivate::liftSolution(const SolutionBasePtr& solution, const InterfaceState* internal_from, void ContainerBasePrivate::liftSolution(const SolutionBasePtr& solution, const InterfaceState* internal_from,
const InterfaceState* internal_to) { const InterfaceState* internal_to) {
computeCost(*internal_from, *internal_to, *solution); computeCost(*internal_from, *internal_to, *solution);
@ -800,44 +808,142 @@ void Alternatives::onNewSolution(const SolutionBase& s) {
liftSolution(s); liftSolution(s);
} }
Fallbacks::Fallbacks(const std::string& name) : Fallbacks(new FallbacksPrivate(this, name)) {}
Fallbacks::Fallbacks(FallbacksPrivate* impl) : ParallelContainerBase(impl) {}
void Fallbacks::reset() { void Fallbacks::reset() {
active_child_ = nullptr;
ParallelContainerBase::reset(); ParallelContainerBase::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);
active_child_ = pimpl()->children().front().get();
} }
bool Fallbacks::canCompute() const { bool Fallbacks::canCompute() const {
while (active_child_) { auto impl { pimpl() };
StagePrivate* child = active_child_->pimpl();
if (child->canCompute())
return true;
// active child failed, continue with next if (impl->requiredInterface() == GENERATE) {
auto next = child->it(); // current_generator_ is fixed if it produced solutions before
++next; if( !solutions().empty() )
if (next == pimpl()->children().end()) return (*impl->current_generator_)->pimpl()->canCompute();
active_child_ = nullptr;
else else
active_child_ = next->get(); // we still have children to try
return impl->current_generator_ != impl->children().end();
} }
return false; else
return !pimpl()->pending_states_.empty();
} }
void Fallbacks::compute() { void Fallbacks::compute() {
if (!active_child_) auto impl { pimpl() };
return;
active_child_->pimpl()->runCompute(); if(impl->requiredInterface() == GENERATE)
impl->computeGenerate();
else
impl->computeFromExternal();
} }
void Fallbacks::onNewSolution(const SolutionBase& s) { void Fallbacks::onNewSolution(const SolutionBase& s) {
liftSolution(s); liftSolution(s);
} }
FallbacksPrivate::FallbacksPrivate(Fallbacks* me, const std::string& name)
: ParallelContainerBasePrivate(me, name) {}
void FallbacksPrivate::initializeExternalInterfaces(InterfaceFlags expected) {
if (expected & READS_START)
starts().reset(new Interface([this](Interface::iterator external, bool updated) {
this->onNewExternalState<Interface::FORWARD>(external, updated);
}));
if (expected & READS_END)
ends().reset(new Interface([this](Interface::iterator external, bool updated) {
this->onNewExternalState<Interface::BACKWARD>(external, updated);
}));
// we've got to set this somewhere once the interface flags are known.
// so we might as well do it here
if(expected == GENERATE)
current_generator_ = children().begin();
}
void FallbacksPrivate::onNewFailure(const Stage& child, const InterfaceState* from, const InterfaceState* to) {
// only react to failure if it's the last possible candidate failing
// otherwise there might still be a feasible solution
if(&child == &*children().back())
ContainerBasePrivate::onNewFailure(child, from, to);
}
void FallbacksPrivate::computeGenerate() {
if(solutions_.empty())
// move to first generator that can run
while(current_generator_ != children().end() && !(*current_generator_)->pimpl()->canCompute())
++current_generator_;
if(current_generator_ == children().end())
return;
// run ALL possible computations (on new state)
// this is needed to decide whether it should be passed to the next child too
while ((*current_generator_)->pimpl()->canCompute()){
(*current_generator_)->pimpl()->runCompute();
}
}
template <typename Interface::Direction dir>
void FallbacksPrivate::onNewExternalState(Interface::iterator external, bool updated) {
// TODO(v4hn): updated is not implemented
if(updated){
ROS_DEBUG_NAMED("Fallback", "updating external states is not supported in Fallbacks");
return;
}
pending_states_.push_back(ExternalState(external, dir, children().begin()));
}
void FallbacksPrivate::computeFromExternal(){
assert(!pending_states_.empty());
auto spec { pending_states_.front() };
pending_states_.pop_front();
ROS_DEBUG_STREAM_NAMED("Fallback", "Push external state to '" << (*spec.stage)->name() << "'");
// feed a new state
copyState(spec.dir,
spec.external_state,
(*spec.stage)->pimpl()->pullInterface(spec.dir),
false);
const auto& stage { (*spec.stage)->pimpl() };
try {
// run ALL possible computations (on new state)
// this is needed to decide whether it should be passed to the next child too
while (stage->canCompute()){
stage->runCompute();
}
} catch (const Property::error& e) {
stage->me()->reportPropertyError(e);
}
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)){
ROS_DEBUG_STREAM_NAMED("Fallback", "Child '" << (*spec.stage)->name() << "' failed to generate a solution, schedule state with next child (if any)");
if(++spec.stage != children().cend())
pending_states_.push_back(spec);
else
// prune solution path if there is no way to extend external_state through Fallbacks
ContainerBasePrivate::onNewFailure(*children().back(),
spec.dir == Interface::FORWARD ? &*spec.external_state : nullptr,
spec.dir == Interface::BACKWARD ? nullptr : &*spec.external_state);
}
}
MergerPrivate::MergerPrivate(Merger* me, const std::string& name) : ParallelContainerBasePrivate(me, name) {} MergerPrivate::MergerPrivate(Merger* me, const std::string& name) : ParallelContainerBasePrivate(me, name) {}
void MergerPrivate::resolveInterface(InterfaceFlags expected) { void MergerPrivate::resolveInterface(InterfaceFlags expected) {

View File

@ -17,7 +17,7 @@ using namespace moveit::task_constructor;
using FallbacksFixtureGenerator = TaskTestBase; using FallbacksFixtureGenerator = TaskTestBase;
TEST_F(FallbacksFixtureGenerator, DISABLED_stayWithFirstSuccessful) { TEST_F(FallbacksFixtureGenerator, stayWithFirstSuccessful) {
auto fallback = std::make_unique<Fallbacks>("Fallbacks"); auto fallback = std::make_unique<Fallbacks>("Fallbacks");
fallback->add(std::make_unique<GeneratorMockup>(PredefinedCosts::single(INF))); fallback->add(std::make_unique<GeneratorMockup>(PredefinedCosts::single(INF)));
fallback->add(std::make_unique<GeneratorMockup>(PredefinedCosts::single(1.0))); fallback->add(std::make_unique<GeneratorMockup>(PredefinedCosts::single(1.0)));
@ -55,7 +55,7 @@ TEST_F(FallbacksFixturePropagate, failingWithFailedSolutions) {
EXPECT_EQ(t.solutions().size(), 0u); EXPECT_EQ(t.solutions().size(), 0u);
} }
TEST_F(FallbacksFixturePropagate, DISABLED_ComputeFirstSuccessfulStageOnly) { TEST_F(FallbacksFixturePropagate, computeFirstSuccessfulStageOnly) {
t.add(std::make_unique<GeneratorMockup>()); t.add(std::make_unique<GeneratorMockup>());
auto fallbacks = std::make_unique<Fallbacks>("Fallbacks"); auto fallbacks = std::make_unique<Fallbacks>("Fallbacks");
@ -117,7 +117,7 @@ TEST_F(FallbacksFixturePropagate, DISABLED_MultipleActivePendingStates) {
// check that first solution is not marked as pruned // check that first solution is not marked as pruned
} }
TEST_F(FallbacksFixturePropagate, DISABLED_successfulWithMixedSolutions) { TEST_F(FallbacksFixturePropagate, successfulWithMixedSolutions) {
t.add(std::make_unique<GeneratorMockup>()); t.add(std::make_unique<GeneratorMockup>());
auto fallback = std::make_unique<Fallbacks>("Fallbacks"); auto fallback = std::make_unique<Fallbacks>("Fallbacks");
@ -129,7 +129,7 @@ TEST_F(FallbacksFixturePropagate, DISABLED_successfulWithMixedSolutions) {
EXPECT_COSTS(t.solutions(), testing::ElementsAre(1.0)); EXPECT_COSTS(t.solutions(), testing::ElementsAre(1.0));
} }
TEST_F(FallbacksFixturePropagate, DISABLED_successfulWithMixedSolutions2) { TEST_F(FallbacksFixturePropagate, successfulWithMixedSolutions2) {
t.add(std::make_unique<GeneratorMockup>()); t.add(std::make_unique<GeneratorMockup>());
auto fallback = std::make_unique<Fallbacks>("Fallbacks"); auto fallback = std::make_unique<Fallbacks>("Fallbacks");
@ -141,7 +141,7 @@ TEST_F(FallbacksFixturePropagate, DISABLED_successfulWithMixedSolutions2) {
EXPECT_COSTS(t.solutions(), testing::ElementsAre(1.0)); EXPECT_COSTS(t.solutions(), testing::ElementsAre(1.0));
} }
TEST_F(FallbacksFixturePropagate, DISABLED_ActiveChildReset) { TEST_F(FallbacksFixturePropagate, activeChildReset) {
t.add(std::make_unique<GeneratorMockup>(PredefinedCosts({ 1.0, INF, 3.0 }))); t.add(std::make_unique<GeneratorMockup>(PredefinedCosts({ 1.0, INF, 3.0 })));
auto fallbacks = std::make_unique<Fallbacks>("Fallbacks"); auto fallbacks = std::make_unique<Fallbacks>("Fallbacks");