From 8719b1c3d6e67504b2cfd62995c5e7190b056c71 Mon Sep 17 00:00:00 2001 From: v4hn Date: Thu, 19 Aug 2021 20:50:24 +0200 Subject: [PATCH] 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). --- .../moveit/task_constructor/container.h | 8 +- .../moveit/task_constructor/container_p.h | 35 ++++- core/src/container.cpp | 138 ++++++++++++++++-- core/test/test_fallback.cpp | 10 +- 4 files changed, 166 insertions(+), 25 deletions(-) diff --git a/core/include/moveit/task_constructor/container.h b/core/include/moveit/task_constructor/container.h index fab732a7..6fcd34dc 100644 --- a/core/include/moveit/task_constructor/container.h +++ b/core/include/moveit/task_constructor/container.h @@ -150,6 +150,7 @@ public: void onNewSolution(const SolutionBase& s) override; }; +class FallbacksPrivate; /** Plan for different alternatives in sequence. * * Try to find feasible solutions using first child. Only if this fails, @@ -158,16 +159,17 @@ public: */ class Fallbacks : public ParallelContainerBase { - mutable Stage* active_child_ = nullptr; - public: - Fallbacks(const std::string& name = "fallbacks") : ParallelContainerBase(name) {} + PRIVATE_CLASS(Fallbacks); + Fallbacks(const std::string& name = "fallbacks"); void reset() override; void init(const moveit::core::RobotModelConstPtr& robot_model) override; bool canCompute() const override; void compute() override; +protected: + Fallbacks(FallbacksPrivate* impl); void onNewSolution(const SolutionBase& s) override; }; diff --git a/core/include/moveit/task_constructor/container_p.h b/core/include/moveit/task_constructor/container_p.h index 34f67eb4..533106fc 100644 --- a/core/include/moveit/task_constructor/container_p.h +++ b/core/include/moveit/task_constructor/container_p.h @@ -131,7 +131,7 @@ public: inline const auto& externalToInternalMap() const { return internal_external_.by(); } /// 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: 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 template 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 void liftSolution(const SolutionBasePtr& solution, const InterfaceState* internal_from, const InterfaceState* internal_to); @@ -237,6 +239,37 @@ private: }; 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 pending_states_; + container_type::const_iterator current_generator_; + +private: + void initializeExternalInterfaces(InterfaceFlags expected) override; + template + 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 { friend class WrapperBase; diff --git a/core/src/container.cpp b/core/src/container.cpp index 34d04bd4..cc1d3be2 100644 --- a/core/src/container.cpp +++ b/core/src/container.cpp @@ -209,6 +209,14 @@ void ContainerBasePrivate::copyState(Interface::iterator external, const Interfa 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(external, target, updated); + else + copyState(external, target, updated); +} + void ContainerBasePrivate::liftSolution(const SolutionBasePtr& solution, const InterfaceState* internal_from, const InterfaceState* internal_to) { computeCost(*internal_from, *internal_to, *solution); @@ -800,44 +808,142 @@ void Alternatives::onNewSolution(const SolutionBase& s) { liftSolution(s); } +Fallbacks::Fallbacks(const std::string& name) : Fallbacks(new FallbacksPrivate(this, name)) {} + +Fallbacks::Fallbacks(FallbacksPrivate* impl) : ParallelContainerBase(impl) {} + void Fallbacks::reset() { - active_child_ = nullptr; ParallelContainerBase::reset(); } void Fallbacks::init(const moveit::core::RobotModelConstPtr& robot_model) { ParallelContainerBase::init(robot_model); - active_child_ = pimpl()->children().front().get(); } bool Fallbacks::canCompute() const { - while (active_child_) { - StagePrivate* child = active_child_->pimpl(); - if (child->canCompute()) - return true; + auto impl { pimpl() }; - // active child failed, continue with next - auto next = child->it(); - ++next; - if (next == pimpl()->children().end()) - active_child_ = nullptr; + if (impl->requiredInterface() == GENERATE) { + // current_generator_ is fixed if it produced solutions before + if( !solutions().empty() ) + return (*impl->current_generator_)->pimpl()->canCompute(); 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() { - if (!active_child_) - return; + auto impl { pimpl() }; - active_child_->pimpl()->runCompute(); + if(impl->requiredInterface() == GENERATE) + impl->computeGenerate(); + else + impl->computeFromExternal(); } void Fallbacks::onNewSolution(const SolutionBase& 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(external, updated); + })); + if (expected & READS_END) + ends().reset(new Interface([this](Interface::iterator external, bool updated) { + this->onNewExternalState(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 +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) {} void MergerPrivate::resolveInterface(InterfaceFlags expected) { diff --git a/core/test/test_fallback.cpp b/core/test/test_fallback.cpp index d37d1e13..116fc001 100644 --- a/core/test/test_fallback.cpp +++ b/core/test/test_fallback.cpp @@ -17,7 +17,7 @@ using namespace moveit::task_constructor; using FallbacksFixtureGenerator = TaskTestBase; -TEST_F(FallbacksFixtureGenerator, DISABLED_stayWithFirstSuccessful) { +TEST_F(FallbacksFixtureGenerator, stayWithFirstSuccessful) { auto fallback = std::make_unique("Fallbacks"); fallback->add(std::make_unique(PredefinedCosts::single(INF))); fallback->add(std::make_unique(PredefinedCosts::single(1.0))); @@ -55,7 +55,7 @@ TEST_F(FallbacksFixturePropagate, failingWithFailedSolutions) { EXPECT_EQ(t.solutions().size(), 0u); } -TEST_F(FallbacksFixturePropagate, DISABLED_ComputeFirstSuccessfulStageOnly) { +TEST_F(FallbacksFixturePropagate, computeFirstSuccessfulStageOnly) { t.add(std::make_unique()); auto fallbacks = std::make_unique("Fallbacks"); @@ -117,7 +117,7 @@ TEST_F(FallbacksFixturePropagate, DISABLED_MultipleActivePendingStates) { // check that first solution is not marked as pruned } -TEST_F(FallbacksFixturePropagate, DISABLED_successfulWithMixedSolutions) { +TEST_F(FallbacksFixturePropagate, successfulWithMixedSolutions) { t.add(std::make_unique()); auto fallback = std::make_unique("Fallbacks"); @@ -129,7 +129,7 @@ TEST_F(FallbacksFixturePropagate, DISABLED_successfulWithMixedSolutions) { EXPECT_COSTS(t.solutions(), testing::ElementsAre(1.0)); } -TEST_F(FallbacksFixturePropagate, DISABLED_successfulWithMixedSolutions2) { +TEST_F(FallbacksFixturePropagate, successfulWithMixedSolutions2) { t.add(std::make_unique()); auto fallback = std::make_unique("Fallbacks"); @@ -141,7 +141,7 @@ TEST_F(FallbacksFixturePropagate, DISABLED_successfulWithMixedSolutions2) { EXPECT_COSTS(t.solutions(), testing::ElementsAre(1.0)); } -TEST_F(FallbacksFixturePropagate, DISABLED_ActiveChildReset) { +TEST_F(FallbacksFixturePropagate, activeChildReset) { t.add(std::make_unique(PredefinedCosts({ 1.0, INF, 3.0 }))); auto fallbacks = std::make_unique("Fallbacks");