From f32e74764542aff2f181b7d9aecbb16ec8079816 Mon Sep 17 00:00:00 2001 From: v4hn Date: Wed, 18 Aug 2021 22:43:55 +0200 Subject: [PATCH 01/34] add hook to ParallelContainerBase to customize state propagation --- core/include/moveit/task_constructor/container_p.h | 7 +++++-- core/src/container.cpp | 14 +++++++++----- 2 files changed, 14 insertions(+), 7 deletions(-) diff --git a/core/include/moveit/task_constructor/container_p.h b/core/include/moveit/task_constructor/container_p.h index 3b58d0da..34f67eb4 100644 --- a/core/include/moveit/task_constructor/container_p.h +++ b/core/include/moveit/task_constructor/container_p.h @@ -227,10 +227,13 @@ public: protected: void validateInterfaces(const StagePrivate& child, InterfaceFlags& external, bool first = false) const; -private: /// callback for new externally received states template - void onNewExternalState(Interface::iterator external, bool updated); + void propagateStateToChildren(Interface::iterator external, bool updated); + +private: + // override for custom behavior on received interface states + virtual void initializeExternalInterfaces(InterfaceFlags expected); }; PIMPL_FUNCTIONS(ParallelContainerBase) diff --git a/core/src/container.cpp b/core/src/container.cpp index 3136dc01..34d04bd4 100644 --- a/core/src/container.cpp +++ b/core/src/container.cpp @@ -678,17 +678,21 @@ void ParallelContainerBasePrivate::resolveInterface(InterfaceFlags expected) { if (exceptions) throw exceptions; + initializeExternalInterfaces(expected); + + required_interface_ = expected; +} + +void ParallelContainerBasePrivate::initializeExternalInterfaces(InterfaceFlags expected) { // States received by the container need to be copied to all children's pull interfaces. if (expected & READS_START) starts().reset(new Interface([this](Interface::iterator external, bool updated) { - this->onNewExternalState(external, updated); + this->propagateStateToChildren(external, updated); })); if (expected & READS_END) ends().reset(new Interface([this](Interface::iterator external, bool updated) { - this->onNewExternalState(external, updated); + this->propagateStateToChildren(external, updated); })); - - required_interface_ = expected; } void ParallelContainerBasePrivate::validateInterfaces(const StagePrivate& child, InterfaceFlags& external, @@ -723,7 +727,7 @@ void ParallelContainerBasePrivate::validateConnectivity() const { } template -void ParallelContainerBasePrivate::onNewExternalState(Interface::iterator external, bool updated) { +void ParallelContainerBasePrivate::propagateStateToChildren(Interface::iterator external, bool updated) { for (const Stage::pointer& stage : children()) copyState(external, stage->pimpl()->pullInterface(dir), updated); } From 8719b1c3d6e67504b2cfd62995c5e7190b056c71 Mon Sep 17 00:00:00 2001 From: v4hn Date: Thu, 19 Aug 2021 20:50:24 +0200 Subject: [PATCH 02/34] 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"); From 3d5cecd75309b7a186da3ac8e09416553bfa104c Mon Sep 17 00:00:00 2001 From: v4hn Date: Thu, 2 Sep 2021 20:14:00 +0200 Subject: [PATCH 03/34] fallback generator can run a single job per compute call --- core/src/container.cpp | 6 +----- 1 file changed, 1 insertion(+), 5 deletions(-) diff --git a/core/src/container.cpp b/core/src/container.cpp index cc1d3be2..2df656a4 100644 --- a/core/src/container.cpp +++ b/core/src/container.cpp @@ -883,11 +883,7 @@ void FallbacksPrivate::computeGenerate() { 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(); - } + (*current_generator_)->pimpl()->runCompute(); } template From aa733fcf5fee86fe25f9a8e57ccdbba71159839f Mon Sep 17 00:00:00 2001 From: v4hn Date: Thu, 2 Sep 2021 22:00:41 +0200 Subject: [PATCH 04/34] simplify onNewFailure give an elaborate reason for an empty overload that doesn't call the parent. --- core/src/container.cpp | 10 +++++----- 1 file changed, 5 insertions(+), 5 deletions(-) diff --git a/core/src/container.cpp b/core/src/container.cpp index 2df656a4..a0c48c78 100644 --- a/core/src/container.cpp +++ b/core/src/container.cpp @@ -867,11 +867,11 @@ void FallbacksPrivate::initializeExternalInterfaces(InterfaceFlags expected) { 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::onNewFailure(const Stage& /*child*/, const InterfaceState* /*from*/, const InterfaceState* /*to*/) { + // This override is deliberately empty. + // The method prunes solution paths when a child failed to find a valid solution for it, + // but in Fallbacks the next child might still yield a successful solution + // Thus pruning must only occur once the last child is exhausted (inside computeFromExternal) } void FallbacksPrivate::computeGenerate() { From 5a9cfc50ea1c9c4c0799de0fd24fd162cc95aef9 Mon Sep 17 00:00:00 2001 From: v4hn Date: Thu, 2 Sep 2021 23:05:57 +0200 Subject: [PATCH 05/34] cleanup: get rid of superfluous parameter --- .../moveit/task_constructor/container_p.h | 4 ++-- core/src/container.cpp | 18 +++++++++--------- 2 files changed, 11 insertions(+), 11 deletions(-) diff --git a/core/include/moveit/task_constructor/container_p.h b/core/include/moveit/task_constructor/container_p.h index 533106fc..840490a2 100644 --- a/core/include/moveit/task_constructor/container_p.h +++ b/core/include/moveit/task_constructor/container_p.h @@ -235,7 +235,7 @@ protected: private: // override for custom behavior on received interface states - virtual void initializeExternalInterfaces(InterfaceFlags expected); + virtual void initializeExternalInterfaces(); }; PIMPL_FUNCTIONS(ParallelContainerBase) @@ -263,7 +263,7 @@ protected: container_type::const_iterator current_generator_; private: - void initializeExternalInterfaces(InterfaceFlags expected) override; + void initializeExternalInterfaces() override; template void onNewExternalState(Interface::iterator external, bool updated); void onNewFailure(const Stage& child, const InterfaceState* from, const InterfaceState* to) override; diff --git a/core/src/container.cpp b/core/src/container.cpp index a0c48c78..7e922894 100644 --- a/core/src/container.cpp +++ b/core/src/container.cpp @@ -686,18 +686,18 @@ void ParallelContainerBasePrivate::resolveInterface(InterfaceFlags expected) { if (exceptions) throw exceptions; - initializeExternalInterfaces(expected); - required_interface_ = expected; + + initializeExternalInterfaces(); } -void ParallelContainerBasePrivate::initializeExternalInterfaces(InterfaceFlags expected) { +void ParallelContainerBasePrivate::initializeExternalInterfaces() { // States received by the container need to be copied to all children's pull interfaces. - if (expected & READS_START) + if (requiredInterface() & READS_START) starts().reset(new Interface([this](Interface::iterator external, bool updated) { this->propagateStateToChildren(external, updated); })); - if (expected & READS_END) + if (requiredInterface() & READS_END) ends().reset(new Interface([this](Interface::iterator external, bool updated) { this->propagateStateToChildren(external, updated); })); @@ -851,19 +851,19 @@ void Fallbacks::onNewSolution(const SolutionBase& s) { FallbacksPrivate::FallbacksPrivate(Fallbacks* me, const std::string& name) : ParallelContainerBasePrivate(me, name) {} -void FallbacksPrivate::initializeExternalInterfaces(InterfaceFlags expected) { - if (expected & READS_START) +void FallbacksPrivate::initializeExternalInterfaces() { + if (requiredInterface() & READS_START) starts().reset(new Interface([this](Interface::iterator external, bool updated) { this->onNewExternalState(external, updated); })); - if (expected & READS_END) + if (requiredInterface() & 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) + if(requiredInterface() == GENERATE) current_generator_ = children().begin(); } From 9a583ab006d5515e89c3155a8d4374319776da38 Mon Sep 17 00:00:00 2001 From: v4hn Date: Fri, 3 Sep 2021 12:26:53 +0200 Subject: [PATCH 06/34] run only one compute step per call Note that while this ensures other stages outside the Fallbacks container can compute as well, it does not solve the problem internally. A new incoming state will only ever be considered once the current stage cannot compute any more. We have no way of telling a child to compute for *a specific state* for now. So once we copied a state to its interface we have to let it compute until all possibilities are exhausted to detect whether or not it could generate a solution for it. If we wouldn't do so, there were no way of knowing when to fall back to the next child as long as the stage can still compute on *any* copied solution. --- .../moveit/task_constructor/container_p.h | 5 +- core/src/container.cpp | 92 ++++++++++--------- 2 files changed, 50 insertions(+), 47 deletions(-) diff --git a/core/include/moveit/task_constructor/container_p.h b/core/include/moveit/task_constructor/container_p.h index 840490a2..3c646e55 100644 --- a/core/include/moveit/task_constructor/container_p.h +++ b/core/include/moveit/task_constructor/container_p.h @@ -248,8 +248,6 @@ public: protected: void computeFromExternal(); - void computeGenerate(); - struct ExternalState { ExternalState(Interface::iterator e, Interface::Direction d, container_type::const_iterator c) @@ -260,6 +258,9 @@ protected: container_type::const_iterator stage; }; std::deque pending_states_; + StagePrivate* current_stage_; + + void computeGenerate(); container_type::const_iterator current_generator_; private: diff --git a/core/src/container.cpp b/core/src/container.cpp index 7e922894..5f4f40ac 100644 --- a/core/src/container.cpp +++ b/core/src/container.cpp @@ -818,6 +818,7 @@ void Fallbacks::reset() { void Fallbacks::init(const moveit::core::RobotModelConstPtr& robot_model) { ParallelContainerBase::init(robot_model); + pimpl()->current_generator_ = pimpl()->children().begin(); } bool Fallbacks::canCompute() const { @@ -832,7 +833,7 @@ bool Fallbacks::canCompute() const { return impl->current_generator_ != impl->children().end(); } else - return !pimpl()->pending_states_.empty(); + return !impl->pending_states_.empty(); } void Fallbacks::compute() { @@ -849,22 +850,19 @@ void Fallbacks::onNewSolution(const SolutionBase& s) { } FallbacksPrivate::FallbacksPrivate(Fallbacks* me, const std::string& name) - : ParallelContainerBasePrivate(me, name) {} + : ParallelContainerBasePrivate(me, name) { + current_stage_ = nullptr; +} void FallbacksPrivate::initializeExternalInterfaces() { if (requiredInterface() & READS_START) starts().reset(new Interface([this](Interface::iterator external, bool updated) { - this->onNewExternalState(external, updated); - })); + this->onNewExternalState(external, updated); + })); if (requiredInterface() & 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(requiredInterface() == GENERATE) - current_generator_ = children().begin(); + this->onNewExternalState(external, updated); + })); } void FallbacksPrivate::onNewFailure(const Stage& /*child*/, const InterfaceState* /*from*/, const InterfaceState* /*to*/) { @@ -877,8 +875,10 @@ void FallbacksPrivate::onNewFailure(const Stage& /*child*/, const InterfaceState void FallbacksPrivate::computeGenerate() { if(solutions_.empty()) // move to first generator that can run - while(current_generator_ != children().end() && !(*current_generator_)->pimpl()->canCompute()) + while(current_generator_ != children().end() && !(*current_generator_)->pimpl()->canCompute()) { + ROS_DEBUG_STREAM_NAMED("Fallbacks", "Generator '" << (*current_generator_)->name() << "' can't compute, trying next one."); ++current_generator_; + } if(current_generator_ == children().end()) return; @@ -890,7 +890,7 @@ 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"); + ROS_DEBUG_NAMED("Fallbacks", "updating external states is not supported in Fallbacks"); return; } @@ -899,44 +899,46 @@ void FallbacksPrivate::onNewExternalState(Interface::iterator external, bool upd void FallbacksPrivate::computeFromExternal(){ assert(!pending_states_.empty()); - auto spec { pending_states_.front() }; - pending_states_.pop_front(); + if(!current_stage_) { + auto spec { pending_states_.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); + ROS_DEBUG_STREAM_NAMED("Fallbacks", "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); + current_stage_ = (*spec.stage)->pimpl(); } - auto has_solutions{ [](const InterfaceState& state, Interface::Direction dir){ - return dir == Interface::FORWARD - ? !state.outgoingTrajectories().empty() - : !state.incomingTrajectories().empty(); - } }; + if(current_stage_->canCompute()) + current_stage_->runCompute(); + else { + auto spec { pending_states_.front() }; + current_stage_ = nullptr; + pending_states_.pop_front(); - 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); + 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("Fallbacks", "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); + } + // if we did not compute a child this call, try again + if(!pending_states_.empty()) + computeFromExternal(); } } From 22809c04a58fdf9f129b092fbea9209321ca31f7 Mon Sep 17 00:00:00 2001 From: v4hn Date: Fri, 10 Sep 2021 23:19:21 +0200 Subject: [PATCH 07/34] order external states --- .../moveit/task_constructor/container_p.h | 7 ++- .../include/moveit/task_constructor/storage.h | 1 + core/src/container.cpp | 63 +++++++++---------- 3 files changed, 37 insertions(+), 34 deletions(-) diff --git a/core/include/moveit/task_constructor/container_p.h b/core/include/moveit/task_constructor/container_p.h index 3c646e55..a5985bca 100644 --- a/core/include/moveit/task_constructor/container_p.h +++ b/core/include/moveit/task_constructor/container_p.h @@ -250,15 +250,18 @@ protected: void computeFromExternal(); struct ExternalState { + ExternalState() = default; 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; + + inline bool operator<(const ExternalState& other) const { return *external_state < *other.external_state; } }; - std::deque pending_states_; - StagePrivate* current_stage_; + ordered pending_states_; + ExternalState current_external_state_; void computeGenerate(); container_type::const_iterator current_generator_; diff --git a/core/include/moveit/task_constructor/storage.h b/core/include/moveit/task_constructor/storage.h index 2d3397bb..b7a6d023 100644 --- a/core/include/moveit/task_constructor/storage.h +++ b/core/include/moveit/task_constructor/storage.h @@ -169,6 +169,7 @@ public: class iterator : public base_type::iterator { public: + iterator() = default; iterator(base_type::iterator other) : base_type::iterator(other) {} InterfaceState& operator*() const noexcept { return *base_type::iterator::operator*(); } diff --git a/core/src/container.cpp b/core/src/container.cpp index 5f4f40ac..7996b406 100644 --- a/core/src/container.cpp +++ b/core/src/container.cpp @@ -817,8 +817,10 @@ void Fallbacks::reset() { } void Fallbacks::init(const moveit::core::RobotModelConstPtr& robot_model) { + auto& impl{ *pimpl() }; 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 { @@ -850,9 +852,7 @@ void Fallbacks::onNewSolution(const SolutionBase& s) { } FallbacksPrivate::FallbacksPrivate(Fallbacks* me, const std::string& name) - : ParallelContainerBasePrivate(me, name) { - current_stage_ = nullptr; -} + : ParallelContainerBasePrivate(me, name) {} void FallbacksPrivate::initializeExternalInterfaces() { if (requiredInterface() & READS_START) @@ -894,48 +894,47 @@ void FallbacksPrivate::onNewExternalState(Interface::iterator external, bool upd return; } - pending_states_.push_back(ExternalState(external, dir, children().begin())); + pending_states_.push(ExternalState(external, dir, children().cbegin())); } void FallbacksPrivate::computeFromExternal(){ assert(!pending_states_.empty()); + if(current_external_state_.stage == children().cend()) { + current_external_state_ = pending_states_.pop(); - if(!current_stage_) { - auto spec { pending_states_.front() }; - - ROS_DEBUG_STREAM_NAMED("Fallbacks", "Push external state to '" << (*spec.stage)->name() << "'"); + ROS_DEBUG_STREAM_NAMED("Fallbacks", "Push external state to '" << (*current_external_state_.stage)->name() << "'"); // feed a new state - copyState(spec.dir, - spec.external_state, - (*spec.stage)->pimpl()->pullInterface(spec.dir), + copyState(current_external_state_.dir, + current_external_state_.external_state, + (*current_external_state_.stage)->pimpl()->pullInterface(current_external_state_.dir), false); - - current_stage_ = (*spec.stage)->pimpl(); } - if(current_stage_->canCompute()) - current_stage_->runCompute(); + auto& stage{ current_external_state_.stage }; + auto& state{ current_external_state_.external_state }; + auto dir { current_external_state_.dir }; + if((*stage)->pimpl()->canCompute()) + (*stage)->pimpl()->runCompute(); else { - auto spec { pending_states_.front() }; - current_stage_ = nullptr; - pending_states_.pop_front(); - - auto has_solutions{ [](const InterfaceState& state, Interface::Direction dir){ - return dir == Interface::FORWARD - ? !state.outgoingTrajectories().empty() - : !state.incomingTrajectories().empty(); + auto has_solutions{ [](const InterfaceState& s, Interface::Direction d){ + return d == Interface::FORWARD + ? !s.outgoingTrajectories().empty() + : !s.incomingTrajectories().empty(); } }; - if(!has_solutions(*spec.external_state, spec.dir)){ - ROS_DEBUG_STREAM_NAMED("Fallbacks", "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 + if(!has_solutions(*state, dir)){ + if(++stage != children().cend()){ + ROS_DEBUG_STREAM_NAMED("Fallbacks", "Child '" << (*stage)->name() << "' failed to generate a solution, schedule state with next child"); + pending_states_.push(current_external_state_); + } + else { + ROS_DEBUG_STREAM_NAMED("Fallbacks", "State failed to extend through any child, prune path"); ContainerBasePrivate::onNewFailure(*children().back(), - spec.dir == Interface::FORWARD ? &*spec.external_state : nullptr, - spec.dir == Interface::BACKWARD ? nullptr : &*spec.external_state); + dir == Interface::FORWARD ? &*state : nullptr, + dir == Interface::BACKWARD ? nullptr : &*state); + } } + current_external_state_.stage = children().cend(); // if we did not compute a child this call, try again if(!pending_states_.empty()) computeFromExternal(); From 887da5b094c2b4b687fba12ff6f2941ff547548f Mon Sep 17 00:00:00 2001 From: v4hn Date: Tue, 14 Sep 2021 23:54:38 +0200 Subject: [PATCH 08/34] fix fallbacks logic Setting up a demo for Fallbacks({CartesianPath,PTP,RRTConnect}) I found the logic did not work as expected yet. - process last job spec as well - ignore failures when looking for a solution - add more debug output --- core/src/container.cpp | 60 ++++++++++++++++++++++++------------------ 1 file changed, 34 insertions(+), 26 deletions(-) diff --git a/core/src/container.cpp b/core/src/container.cpp index 7996b406..132a25d0 100644 --- a/core/src/container.cpp +++ b/core/src/container.cpp @@ -835,7 +835,7 @@ bool Fallbacks::canCompute() const { return impl->current_generator_ != impl->children().end(); } else - return !impl->pending_states_.empty(); + return !impl->pending_states_.empty() || impl->current_external_state_.stage != impl->children().cend(); } void Fallbacks::compute() { @@ -898,7 +898,7 @@ void FallbacksPrivate::onNewExternalState(Interface::iterator external, bool upd } void FallbacksPrivate::computeFromExternal(){ - assert(!pending_states_.empty()); + assert(!pending_states_.empty() || current_external_state_.stage != children().cend()); if(current_external_state_.stage == children().cend()) { current_external_state_ = pending_states_.pop(); @@ -913,32 +913,40 @@ void FallbacksPrivate::computeFromExternal(){ auto& stage{ current_external_state_.stage }; auto& state{ current_external_state_.external_state }; auto dir { current_external_state_.dir }; - if((*stage)->pimpl()->canCompute()) + if((*stage)->pimpl()->canCompute()) { (*stage)->pimpl()->runCompute(); - else { - auto has_solutions{ [](const InterfaceState& s, Interface::Direction d){ - return d == Interface::FORWARD - ? !s.outgoingTrajectories().empty() - : !s.incomingTrajectories().empty(); - } }; - - if(!has_solutions(*state, dir)){ - if(++stage != children().cend()){ - ROS_DEBUG_STREAM_NAMED("Fallbacks", "Child '" << (*stage)->name() << "' failed to generate a solution, schedule state with next child"); - pending_states_.push(current_external_state_); - } - else { - ROS_DEBUG_STREAM_NAMED("Fallbacks", "State failed to extend through any child, prune path"); - ContainerBasePrivate::onNewFailure(*children().back(), - dir == Interface::FORWARD ? &*state : nullptr, - dir == Interface::BACKWARD ? nullptr : &*state); - } - } - current_external_state_.stage = children().cend(); - // if we did not compute a child this call, try again - if(!pending_states_.empty()) - computeFromExternal(); + return; } + + auto has_solutions{ [](const InterfaceState& s, Interface::Direction d){ + const auto& trajectories { d == Interface::FORWARD + ? s.outgoingTrajectories() + : s.incomingTrajectories() }; + return std::find_if(trajectories.cbegin(), trajectories.cend(), [](const auto& t){ return !t->isFailure();}) != trajectories.cend(); + }}; + + if(!has_solutions(*state, dir)){ + auto next_stage = std::next(stage); + if(next_stage != children().cend()){ + ROS_DEBUG_STREAM_NAMED("Fallbacks", "Child '" << (*stage)->name() << "' failed to generate a solution, schedule state with next child"); + ++stage; + pending_states_.push(current_external_state_); + } + else { + ROS_DEBUG_STREAM_NAMED("Fallbacks", "State failed to extend through any child, prune path"); + ContainerBasePrivate::onNewFailure(*children().back(), + dir == Interface::FORWARD ? &*state : nullptr, + dir == Interface::BACKWARD ? nullptr : &*state); + } + } + else { + ROS_DEBUG_STREAM_NAMED("Fallbacks", "Child '" << (*stage)->name() << "' produced a solution, not invoking further fallbacks"); + } + // invalidate current_external_state_ after we processed it + current_external_state_.stage = children().cend(); + // if we did not compute a child this call, try again + if(!pending_states_.empty()) + computeFromExternal(); } MergerPrivate::MergerPrivate(Merger* me, const std::string& name) : ParallelContainerBasePrivate(me, name) {} From b6ac5b09ba75e3803232a2674b8ac56eea9100ab Mon Sep 17 00:00:00 2001 From: v4hn Date: Wed, 15 Sep 2021 22:35:35 +0200 Subject: [PATCH 09/34] add demo illustrating useful fallbacks behavior --- demo/CMakeLists.txt | 1 + demo/src/fallbacks_move_to.cpp | 142 +++++++++++++++++++++++++++++++++ 2 files changed, 143 insertions(+) create mode 100644 demo/src/fallbacks_move_to.cpp diff --git a/demo/CMakeLists.txt b/demo/CMakeLists.txt index 4634461e..32b287f1 100644 --- a/demo/CMakeLists.txt +++ b/demo/CMakeLists.txt @@ -50,6 +50,7 @@ demo(cartesian) demo(modular) demo(alternative_path_costs) demo(ik_clearance_cost) +demo(fallbacks_move_to) demo(pick_place_demo) target_link_libraries(${PROJECT_NAME}_pick_place_demo ${PROJECT_NAME}_pick_place_task) diff --git a/demo/src/fallbacks_move_to.cpp b/demo/src/fallbacks_move_to.cpp new file mode 100644 index 00000000..0a586feb --- /dev/null +++ b/demo/src/fallbacks_move_to.cpp @@ -0,0 +1,142 @@ +#include + +#include +#include + +#include + +#include +#include +#include +#include +#include + +constexpr double TAU = 2 * M_PI; + +using namespace moveit::task_constructor; + +/** CurrentState -> Fallbacks( MoveTo, MoveTo, MoveTo )*/ +int main(int argc, char** argv) { + ros::init(argc, argv, "mtc_tutorial"); + + ros::AsyncSpinner spinner{ 1 }; + spinner.start(); + + // setup Task + Task t; + t.loadRobotModel(); + const moveit::core::RobotModelConstPtr robot{ t.getRobotModel() }; + + assert(robot->getName() == "panda"); + + // setup solvers + auto cartesian = std::make_shared(); + cartesian->setJumpThreshold(2.0); + + const auto ptp = []() { + auto pp{ std::make_shared("pilz_industrial_motion_planner") }; + pp->setPlannerId("PTP"); + return pp; + }(); + + const auto rrtconnect = []() { + auto pp{ std::make_shared("ompl") }; + pp->setPlannerId("RRTConnectkConfigDefault"); + return pp; + }(); + + // target state for Task + std::map target_state; + robot->getJointModelGroup("panda_arm")->getVariableDefaultPositions("ready", target_state); + target_state["panda_joint1"] = +TAU / 8; + + // define initial scenes + auto initial_scene{ std::make_shared(robot) }; + initial_scene->getCurrentStateNonConst().setToDefaultValues(robot->getJointModelGroup("panda_arm"), "ready"); + + auto initial_alternatives = std::make_unique("initial states"); + + { + // can reach target with Cartesian motion + auto fixed{ std::make_unique("current state") }; + auto scene{ initial_scene->diff() }; + scene->getCurrentStateNonConst().setVariablePositions({ { "panda_joint1", -TAU / 8 } }); + fixed->setState(scene); + initial_alternatives->add(std::move(fixed)); + } + { + // Cartesian motion to target is impossible, but PTP is collision-free + auto fixed{ std::make_unique("current state") }; + auto scene{ initial_scene->diff() }; + scene->getCurrentStateNonConst().setVariablePositions({ + { "panda_joint1", +TAU / 8 }, + { "panda_joint4", 0 }, + }); + fixed->setState(scene); + initial_alternatives->add(std::move(fixed)); + } + { + // Cartesian and PTP motion to target would be in collision + auto fixed = std::make_unique("current state"); + auto scene{ initial_scene->diff() }; + scene->getCurrentStateNonConst().setVariablePositions({ { "panda_joint1", -TAU / 8 } }); + scene->processCollisionObjectMsg([]() { + moveit_msgs::CollisionObject co; + co.id = "box"; + co.header.frame_id = "panda_link0"; + co.operation = co.ADD; +#if MOVEIT_HAS_OBJECT_POSE + auto& pose{ co.pose }; +#else + co.primitive_poses.emplace_back(); + auto& pose{ co.primitive_poses[0] }; +#endif + pose = []() { + geometry_msgs::Pose p; + p.position.x = 0.3; + p.position.y = 0.0; + p.position.z = 0.64 / 2; + p.orientation.w = 1.0; + return p; + }(); + co.primitives.push_back([]() { + shape_msgs::SolidPrimitive sp; + sp.type = sp.BOX; + sp.dimensions = { 0.2, 0.05, 0.64 }; + return sp; + }()); + return co; + }()); + fixed->setState(scene); + initial_alternatives->add(std::move(fixed)); + } + + t.add(std::move(initial_alternatives)); + + // fallbacks to reach target_state + auto fallbacks = std::make_unique("move to other side"); + + auto add_to_fallbacks{ [&](auto& solver, auto& name) { + auto move_to = std::make_unique(name, solver); + move_to->setGroup("panda_arm"); + move_to->setGoal(target_state); + fallbacks->add(std::move(move_to)); + } }; + add_to_fallbacks(cartesian, "Cartesian path"); + add_to_fallbacks(ptp, "PTP path"); + add_to_fallbacks(rrtconnect, "RRT path"); + + t.add(std::move(fallbacks)); + + try { + t.init(); + std::cout << t << std::endl; + t.plan(); + } catch (const InitStageException& e) { + std::cout << e << std::endl; + } + + ros::waitForShutdown(); + + return 0; +} From b783173b27e129598f49b943640925813697fcf7 Mon Sep 17 00:00:00 2001 From: Robert Haschke Date: Thu, 9 Sep 2021 11:52:44 +0200 Subject: [PATCH 10/34] GENERATE: return correct canCompute() result as early as possible Moving to next child generator only in compute() requires an extra call to canCompute() to notice the failure of the next generator(s). --- .../moveit/task_constructor/container_p.h | 4 ++-- core/src/container.cpp | 22 ++++++++----------- 2 files changed, 11 insertions(+), 15 deletions(-) diff --git a/core/include/moveit/task_constructor/container_p.h b/core/include/moveit/task_constructor/container_p.h index a5985bca..77881390 100644 --- a/core/include/moveit/task_constructor/container_p.h +++ b/core/include/moveit/task_constructor/container_p.h @@ -263,8 +263,8 @@ protected: ordered pending_states_; ExternalState current_external_state_; - void computeGenerate(); - container_type::const_iterator current_generator_; + inline void computeGenerate(); + mutable container_type::const_iterator current_generator_; private: void initializeExternalInterfaces() override; diff --git a/core/src/container.cpp b/core/src/container.cpp index 132a25d0..7d040d28 100644 --- a/core/src/container.cpp +++ b/core/src/container.cpp @@ -828,11 +828,16 @@ bool Fallbacks::canCompute() const { if (impl->requiredInterface() == GENERATE) { // current_generator_ is fixed if it produced solutions before - if( !solutions().empty() ) + if (!solutions().empty()) return (*impl->current_generator_)->pimpl()->canCompute(); - else - // we still have children to try + else { + // move to first generator that can run + while(impl->current_generator_ != impl->children().end() && !(*impl->current_generator_)->pimpl()->canCompute()) { + ROS_DEBUG_STREAM_NAMED("Fallbacks", "Generator '" << (*impl->current_generator_)->name() << "' can't compute, trying next one."); + ++impl->current_generator_; + } return impl->current_generator_ != impl->children().end(); + } } else return !impl->pending_states_.empty() || impl->current_external_state_.stage != impl->children().cend(); @@ -873,16 +878,7 @@ void FallbacksPrivate::onNewFailure(const Stage& /*child*/, const InterfaceState } void FallbacksPrivate::computeGenerate() { - if(solutions_.empty()) - // move to first generator that can run - while(current_generator_ != children().end() && !(*current_generator_)->pimpl()->canCompute()) { - ROS_DEBUG_STREAM_NAMED("Fallbacks", "Generator '" << (*current_generator_)->name() << "' can't compute, trying next one."); - ++current_generator_; - } - - if(current_generator_ == children().end()) - return; - + assert(current_generator_ != children().end()); (*current_generator_)->pimpl()->runCompute(); } From 6653c4853aeefed1cf00f31e82d228237a49a55c Mon Sep 17 00:00:00 2001 From: Robert Haschke Date: Thu, 16 Sep 2021 11:14:38 +0200 Subject: [PATCH 11/34] Rename: computeFromExternal -> computePropagate --- .../moveit/task_constructor/container_p.h | 2 +- core/src/container.cpp | 54 ++++++++++++------- 2 files changed, 35 insertions(+), 21 deletions(-) diff --git a/core/include/moveit/task_constructor/container_p.h b/core/include/moveit/task_constructor/container_p.h index 77881390..1b3bec04 100644 --- a/core/include/moveit/task_constructor/container_p.h +++ b/core/include/moveit/task_constructor/container_p.h @@ -247,7 +247,7 @@ public: FallbacksPrivate(Fallbacks* me, const std::string& name); protected: - void computeFromExternal(); + void computePropagate(); struct ExternalState { ExternalState() = default; diff --git a/core/src/container.cpp b/core/src/container.cpp index 7d040d28..09f4a4ec 100644 --- a/core/src/container.cpp +++ b/core/src/container.cpp @@ -826,30 +826,44 @@ void Fallbacks::init(const moveit::core::RobotModelConstPtr& robot_model) { bool Fallbacks::canCompute() const { auto impl { pimpl() }; - if (impl->requiredInterface() == GENERATE) { - // current_generator_ is fixed if it produced solutions before - if (!solutions().empty()) - return (*impl->current_generator_)->pimpl()->canCompute(); - else { - // move to first generator that can run - while(impl->current_generator_ != impl->children().end() && !(*impl->current_generator_)->pimpl()->canCompute()) { - ROS_DEBUG_STREAM_NAMED("Fallbacks", "Generator '" << (*impl->current_generator_)->name() << "' can't compute, trying next one."); - ++impl->current_generator_; + switch (impl->requiredInterface()) { + case GENERATE: + // current_generator_ is fixed if it produced solutions before + if (!solutions().empty()) + return (*impl->current_generator_)->pimpl()->canCompute(); + else { + // move to first generator that can run + while(impl->current_generator_ != impl->children().end() && !(*impl->current_generator_)->pimpl()->canCompute()) { + ROS_DEBUG_STREAM_NAMED("Fallbacks", "Generator '" << (*impl->current_generator_)->name() << "' can't compute, trying next one."); + ++impl->current_generator_; + } + return impl->current_generator_ != impl->children().end(); } - return impl->current_generator_ != impl->children().end(); - } + break; + case PROPAGATE_FORWARDS: + case PROPAGATE_BACKWARDS: + case CONNECT: + return !impl->pending_states_.empty() || impl->current_external_state_.stage != impl->children().cend(); + default: + assert(false); } - else - return !impl->pending_states_.empty() || impl->current_external_state_.stage != impl->children().cend(); } void Fallbacks::compute() { auto impl { pimpl() }; - if(impl->requiredInterface() == GENERATE) - impl->computeGenerate(); - else - impl->computeFromExternal(); + switch (impl->requiredInterface()) { + case GENERATE: + impl->computeGenerate(); + break; + case PROPAGATE_FORWARDS: + case PROPAGATE_BACKWARDS: + case CONNECT: + impl->computePropagate(); + break; + default: + assert(false); + } } void Fallbacks::onNewSolution(const SolutionBase& s) { @@ -874,7 +888,7 @@ void FallbacksPrivate::onNewFailure(const Stage& /*child*/, const InterfaceState // This override is deliberately empty. // The method prunes solution paths when a child failed to find a valid solution for it, // but in Fallbacks the next child might still yield a successful solution - // Thus pruning must only occur once the last child is exhausted (inside computeFromExternal) + // Thus pruning must only occur once the last child is exhausted (inside computePropagate) } void FallbacksPrivate::computeGenerate() { @@ -893,7 +907,7 @@ void FallbacksPrivate::onNewExternalState(Interface::iterator external, bool upd pending_states_.push(ExternalState(external, dir, children().cbegin())); } -void FallbacksPrivate::computeFromExternal(){ +void FallbacksPrivate::computePropagate(){ assert(!pending_states_.empty() || current_external_state_.stage != children().cend()); if(current_external_state_.stage == children().cend()) { current_external_state_ = pending_states_.pop(); @@ -942,7 +956,7 @@ void FallbacksPrivate::computeFromExternal(){ current_external_state_.stage = children().cend(); // if we did not compute a child this call, try again if(!pending_states_.empty()) - computeFromExternal(); + computePropagate(); } MergerPrivate::MergerPrivate(Merger* me, const std::string& name) : ParallelContainerBasePrivate(me, name) {} From e5b20ac11f007ee22f832bfcdee0fa7103fa02f3 Mon Sep 17 00:00:00 2001 From: Robert Haschke Date: Thu, 16 Sep 2021 23:37:15 +0200 Subject: [PATCH 12/34] Fix pruning Pruning - if acting on the external state - needs to pass the current stage (this). --- core/src/container.cpp | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/core/src/container.cpp b/core/src/container.cpp index 09f4a4ec..149e47cf 100644 --- a/core/src/container.cpp +++ b/core/src/container.cpp @@ -944,9 +944,9 @@ void FallbacksPrivate::computePropagate(){ } else { ROS_DEBUG_STREAM_NAMED("Fallbacks", "State failed to extend through any child, prune path"); - ContainerBasePrivate::onNewFailure(*children().back(), - dir == Interface::FORWARD ? &*state : nullptr, - dir == Interface::BACKWARD ? nullptr : &*state); + parent()->pimpl()->onNewFailure(*me(), + dir == Interface::FORWARD ? &*state : nullptr, + dir == Interface::BACKWARD ? nullptr : &*state); } } else { From c33b1967bc1fabdd0a70fa4a69e9c275d0493b26 Mon Sep 17 00:00:00 2001 From: Robert Haschke Date: Thu, 16 Sep 2021 21:46:11 +0200 Subject: [PATCH 13/34] Handle updates on external states --- core/src/container.cpp | 15 ++++++++++++--- 1 file changed, 12 insertions(+), 3 deletions(-) diff --git a/core/src/container.cpp b/core/src/container.cpp index 149e47cf..115bbcc9 100644 --- a/core/src/container.cpp +++ b/core/src/container.cpp @@ -195,8 +195,10 @@ template void ContainerBasePrivate::setStatus(const Interfa template void ContainerBasePrivate::copyState(Interface::iterator external, const InterfacePtr& target, bool updated) { if (updated) { + // update prio of all internal states linked to external auto internals{ externalToInternalMap().equal_range(&*external) }; for (auto& i = internals.first; i != internals.second; ++i) { + // TODO: Not only update status, but full priority! setStatus(i->second, external->priority().status()); } return; @@ -898,9 +900,16 @@ void FallbacksPrivate::computeGenerate() { template void FallbacksPrivate::onNewExternalState(Interface::iterator external, bool updated) { - // TODO(v4hn): updated is not implemented - if(updated){ - ROS_DEBUG_NAMED("Fallbacks", "updating external states is not supported in Fallbacks"); + if (updated) { + auto it = std::find_if(pending_states_.begin(), pending_states_.end(), + [external](const ExternalState& s) { return s.external_state == external; }); + if (it == pending_states_.cend()) + return; // already processed + + pending_states_.update(it); // update sorting pos of this single item + + // update prio of linked internal states as well + ContainerBasePrivate::copyState(external, InterfacePtr(), updated); return; } From dcb6857f36710da21ef456c65ee962cc1e9e1080 Mon Sep 17 00:00:00 2001 From: Robert Haschke Date: Thu, 16 Sep 2021 15:10:45 +0200 Subject: [PATCH 14/34] Simplify computePropagate() - Drop variable current_external_state_ - Instead encode the info that the external state wasn't yet forwarded to any child via stage = children().cend() - If all children have exhausted their solutions for this state, it is removed from the pending list --- .../moveit/task_constructor/container_p.h | 5 +- core/src/container.cpp | 99 ++++++++++--------- 2 files changed, 56 insertions(+), 48 deletions(-) diff --git a/core/include/moveit/task_constructor/container_p.h b/core/include/moveit/task_constructor/container_p.h index 1b3bec04..e67e1f95 100644 --- a/core/include/moveit/task_constructor/container_p.h +++ b/core/include/moveit/task_constructor/container_p.h @@ -151,6 +151,8 @@ protected: /// Set ENABLED / PRUNED status of the solution tree starting from s into given direction template void setStatus(const InterfaceState* s, InterfaceState::Status status); + /// non-template version + void setStatus(Interface::Direction dir, const InterfaceState* s, InterfaceState::Status status); /// copy external_state to a child's interface and remember the link in internal_external map template @@ -260,8 +262,7 @@ protected: inline bool operator<(const ExternalState& other) const { return *external_state < *other.external_state; } }; - ordered pending_states_; - ExternalState current_external_state_; + ordered pending_states_; // pending external states for a PROPAGATE interface inline void computeGenerate(); mutable container_type::const_iterator current_generator_; diff --git a/core/src/container.cpp b/core/src/container.cpp index 115bbcc9..c85304dc 100644 --- a/core/src/container.cpp +++ b/core/src/container.cpp @@ -189,8 +189,13 @@ void ContainerBasePrivate::setStatus(const InterfaceState* s, InterfaceState::St for (const SolutionBase* successor : trajectories(*s)) setStatus(state(*successor), status); } -template void ContainerBasePrivate::setStatus(const InterfaceState*, InterfaceState::Status); -template void ContainerBasePrivate::setStatus(const InterfaceState*, InterfaceState::Status); + +void ContainerBasePrivate::setStatus(Interface::Direction dir, const InterfaceState* s, InterfaceState::Status status) { + if (dir == Interface::FORWARD) + setStatus(s, InterfaceState::ENABLED); + else + setStatus(s, InterfaceState::ENABLED); +} template void ContainerBasePrivate::copyState(Interface::iterator external, const InterfacePtr& target, bool updated) { @@ -822,7 +827,6 @@ void Fallbacks::init(const moveit::core::RobotModelConstPtr& robot_model) { auto& impl{ *pimpl() }; ParallelContainerBase::init(robot_model); impl.current_generator_ = impl.children().begin(); - impl.current_external_state_.stage = impl.children().cend(); } bool Fallbacks::canCompute() const { @@ -845,7 +849,7 @@ bool Fallbacks::canCompute() const { case PROPAGATE_FORWARDS: case PROPAGATE_BACKWARDS: case CONNECT: - return !impl->pending_states_.empty() || impl->current_external_state_.stage != impl->children().cend(); + return !impl->pending_states_.empty() && !impl->pending_states_.front().external_state->priority().failed(); default: assert(false); } @@ -913,59 +917,62 @@ void FallbacksPrivate::onNewExternalState(Interface::iterator external, bool upd return; } - pending_states_.push(ExternalState(external, dir, children().cbegin())); + // remember external state for later processing by children. + // children().end() indicates that the states wasn't yet forwarded to any child + pending_states_.push(ExternalState(external, dir, children().cend())); } void FallbacksPrivate::computePropagate(){ - assert(!pending_states_.empty() || current_external_state_.stage != children().cend()); - if(current_external_state_.stage == children().cend()) { - current_external_state_ = pending_states_.pop(); + while (!pending_states_.empty()) { + auto current = pending_states_.begin(); + if (!current->external_state->priority().enabled()) + return; - ROS_DEBUG_STREAM_NAMED("Fallbacks", "Push external state to '" << (*current_external_state_.stage)->name() << "'"); - // feed a new state - copyState(current_external_state_.dir, - current_external_state_.external_state, - (*current_external_state_.stage)->pimpl()->pullInterface(current_external_state_.dir), - false); - } + auto pushState = [this](const ExternalState& ext) { + ROS_DEBUG_STREAM_NAMED("Fallbacks", "Push external state (" << ext.external_state->priority() + << ") to '" << (*ext.stage)->name() << "'"); + copyState(ext.dir, ext.external_state, (*ext.stage)->pimpl()->pullInterface(ext.dir), false); + }; - auto& stage{ current_external_state_.stage }; - auto& state{ current_external_state_.external_state }; - auto dir { current_external_state_.dir }; - if((*stage)->pimpl()->canCompute()) { - (*stage)->pimpl()->runCompute(); - return; - } + if (current->stage == children().cend()) { + current->stage = children().begin(); // activate first child + pushState(*current); + } - auto has_solutions{ [](const InterfaceState& s, Interface::Direction d){ - const auto& trajectories { d == Interface::FORWARD - ? s.outgoingTrajectories() - : s.incomingTrajectories() }; - return std::find_if(trajectories.cbegin(), trajectories.cend(), [](const auto& t){ return !t->isFailure();}) != trajectories.cend(); - }}; + StagePrivate* child = (*current->stage)->pimpl(); + if (child->canCompute()) { + child->runCompute(); + return; // return after first compute() + } - if(!has_solutions(*state, dir)){ - auto next_stage = std::next(stage); - if(next_stage != children().cend()){ - ROS_DEBUG_STREAM_NAMED("Fallbacks", "Child '" << (*stage)->name() << "' failed to generate a solution, schedule state with next child"); - ++stage; - pending_states_.push(current_external_state_); + auto has_solutions{ [](const InterfaceState& s, Interface::Direction d){ + const auto& trajectories { d == Interface::FORWARD ? s.outgoingTrajectories() + : s.incomingTrajectories() }; + return std::find_if(trajectories.cbegin(), trajectories.cend(), + [](const auto& t){ return !t->isFailure();}) != trajectories.cend(); + }}; + + if(!has_solutions(*current->external_state, current->dir)){ + ++current->stage; + if(current->stage != children().cend()){ + ROS_DEBUG_STREAM_NAMED("Fallbacks", "Child '" << child->name() << "' failed generating solutions, trying next child: '" + << (*current->stage)->name() << "'"); + pushState(*current); + } + else { + ROS_DEBUG_STREAM_NAMED("Fallbacks", "Failed to extend state with all children, pruning path"); + parent()->pimpl()->onNewFailure(*me(), + current->dir == Interface::FORWARD ? &*current->external_state : nullptr, + current->dir == Interface::BACKWARD ? nullptr : &*current->external_state); + pending_states_.erase(current); + } } else { - ROS_DEBUG_STREAM_NAMED("Fallbacks", "State failed to extend through any child, prune path"); - parent()->pimpl()->onNewFailure(*me(), - dir == Interface::FORWARD ? &*state : nullptr, - dir == Interface::BACKWARD ? nullptr : &*state); + ROS_DEBUG_STREAM_NAMED("Fallbacks", "Child '" << child->name() << "' exhausted, but produced solutions before, not invoking further fallbacks"); + pending_states_.erase(current); } + // continue processing with next pending state as we didn't runCompute() yet } - else { - ROS_DEBUG_STREAM_NAMED("Fallbacks", "Child '" << (*stage)->name() << "' produced a solution, not invoking further fallbacks"); - } - // invalidate current_external_state_ after we processed it - current_external_state_.stage = children().cend(); - // if we did not compute a child this call, try again - if(!pending_states_.empty()) - computePropagate(); } MergerPrivate::MergerPrivate(Merger* me, const std::string& name) : ParallelContainerBasePrivate(me, name) {} From 5c235ab580ce1d5e26da457aa08ea7a388463b3a Mon Sep 17 00:00:00 2001 From: Robert Haschke Date: Fri, 17 Sep 2021 11:51:04 +0200 Subject: [PATCH 15/34] debugging helper function --- core/include/moveit/task_constructor/container_p.h | 2 ++ core/src/container.cpp | 13 +++++++++++++ 2 files changed, 15 insertions(+) diff --git a/core/include/moveit/task_constructor/container_p.h b/core/include/moveit/task_constructor/container_p.h index e67e1f95..ab305a6f 100644 --- a/core/include/moveit/task_constructor/container_p.h +++ b/core/include/moveit/task_constructor/container_p.h @@ -272,6 +272,8 @@ private: template void onNewExternalState(Interface::iterator external, bool updated); void onNewFailure(const Stage& child, const InterfaceState* from, const InterfaceState* to) override; + // print pending states for debugging + void printPending(const char* comment = "pending: ") const; }; PIMPL_FUNCTIONS(Fallbacks) diff --git a/core/src/container.cpp b/core/src/container.cpp index c85304dc..904e0b5c 100644 --- a/core/src/container.cpp +++ b/core/src/container.cpp @@ -902,6 +902,15 @@ void FallbacksPrivate::computeGenerate() { (*current_generator_)->pimpl()->runCompute(); } +inline void FallbacksPrivate::printPending(const char* comment) const { + ROSCONSOLE_DEFINE_LOCATION(true, ::ros::console::levels::Debug, ROSCONSOLE_NAME_PREFIX ".Fallbacks"); + if (ROS_UNLIKELY(__rosconsole_define_location__enabled)) { + std::cout << name() << ": " << comment; + std::for_each(pending_states_.begin(), pending_states_.end(), [](const auto& e) { std::cout << e.external_state->priority() << " "; }); + std::cout << std::endl; + } +} + template void FallbacksPrivate::onNewExternalState(Interface::iterator external, bool updated) { if (updated) { @@ -911,6 +920,7 @@ void FallbacksPrivate::onNewExternalState(Interface::iterator external, bool upd return; // already processed pending_states_.update(it); // update sorting pos of this single item + printPending("after update: "); // update prio of linked internal states as well ContainerBasePrivate::copyState(external, InterfacePtr(), updated); @@ -920,10 +930,13 @@ void FallbacksPrivate::onNewExternalState(Interface::iterator external, bool upd // remember external state for later processing by children. // children().end() indicates that the states wasn't yet forwarded to any child pending_states_.push(ExternalState(external, dir, children().cend())); + printPending("after push: "); } void FallbacksPrivate::computePropagate(){ while (!pending_states_.empty()) { + printPending(); + auto current = pending_states_.begin(); if (!current->external_state->priority().enabled()) return; From 2e63c154aab41a9cde8684ac0880504cfc2a99d8 Mon Sep 17 00:00:00 2001 From: Robert Haschke Date: Fri, 17 Sep 2021 12:04:58 +0200 Subject: [PATCH 16/34] Reintroduce pending state --- .../include/moveit/task_constructor/container_p.h | 2 +- core/src/container.cpp | 15 +++++++++++---- 2 files changed, 12 insertions(+), 5 deletions(-) diff --git a/core/include/moveit/task_constructor/container_p.h b/core/include/moveit/task_constructor/container_p.h index ab305a6f..1e29526b 100644 --- a/core/include/moveit/task_constructor/container_p.h +++ b/core/include/moveit/task_constructor/container_p.h @@ -263,7 +263,7 @@ protected: inline bool operator<(const ExternalState& other) const { return *external_state < *other.external_state; } }; ordered pending_states_; // pending external states for a PROPAGATE interface - + ordered::iterator current_pending_; // currently active pending state inline void computeGenerate(); mutable container_type::const_iterator current_generator_; diff --git a/core/src/container.cpp b/core/src/container.cpp index 904e0b5c..ba9a5fb2 100644 --- a/core/src/container.cpp +++ b/core/src/container.cpp @@ -827,6 +827,7 @@ void Fallbacks::init(const moveit::core::RobotModelConstPtr& robot_model) { auto& impl{ *pimpl() }; ParallelContainerBase::init(robot_model); impl.current_generator_ = impl.children().begin(); + impl.current_pending_ = impl.pending_states_.end(); } bool Fallbacks::canCompute() const { @@ -849,7 +850,7 @@ bool Fallbacks::canCompute() const { case PROPAGATE_FORWARDS: case PROPAGATE_BACKWARDS: case CONNECT: - return !impl->pending_states_.empty() && !impl->pending_states_.front().external_state->priority().failed(); + return !impl->pending_states_.empty(); default: assert(false); } @@ -937,9 +938,13 @@ void FallbacksPrivate::computePropagate(){ while (!pending_states_.empty()) { printPending(); - auto current = pending_states_.begin(); - if (!current->external_state->priority().enabled()) - return; + // If we have a currently active pending state, proceed with this one + // even if pending_states_.front() might be different meanwhile. + // This is important as we need to feed states one by one to the children. + // Otherwise we cannot know if a child is exhausted on a specific input state. + if (current_pending_ == pending_states_.end()) + current_pending_ = pending_states_.begin(); + auto current = current_pending_; auto pushState = [this](const ExternalState& ext) { ROS_DEBUG_STREAM_NAMED("Fallbacks", "Push external state (" << ext.external_state->priority() @@ -978,11 +983,13 @@ void FallbacksPrivate::computePropagate(){ current->dir == Interface::FORWARD ? &*current->external_state : nullptr, current->dir == Interface::BACKWARD ? nullptr : &*current->external_state); pending_states_.erase(current); + current_pending_ = pending_states_.end(); } } else { ROS_DEBUG_STREAM_NAMED("Fallbacks", "Child '" << child->name() << "' exhausted, but produced solutions before, not invoking further fallbacks"); pending_states_.erase(current); + current_pending_ = pending_states_.end(); } // continue processing with next pending state as we didn't runCompute() yet } From a0bc0602e8f65a2dddd1d13c9ed6645254b7a2da Mon Sep 17 00:00:00 2001 From: Robert Haschke Date: Mon, 22 Nov 2021 01:47:56 +0100 Subject: [PATCH 17/34] Enable tests Adapt test results FallbacksFixturePropagate.computeFirstSuccessfulStagePerSolutionOnly due to 2e63c154aab41a9cde8684ac0880504cfc2a99d8: The order of computations has changed, because we lock the processed state as soon as it is forwarded to the first fallback child. In this case, after processing GEN1 und FWD1 once, we have the two states with costs 2, 4 in the queue. The first one, i.e. with cost 2 is forwarded to the child FWD2, which fails. In the next cycle, although we have new states in the queue (1, 2, 3, 4), we stick with state "2" and forward it two FWD3, which adds costs 210, resulting in 212. With previous code, the Fallback container switched to state "1", forwarded to FWD2. --- core/test/test_fallback.cpp | 12 +++++++----- 1 file changed, 7 insertions(+), 5 deletions(-) diff --git a/core/test/test_fallback.cpp b/core/test/test_fallback.cpp index 116fc001..d27b9fcf 100644 --- a/core/test/test_fallback.cpp +++ b/core/test/test_fallback.cpp @@ -67,7 +67,7 @@ TEST_F(FallbacksFixturePropagate, computeFirstSuccessfulStageOnly) { EXPECT_EQ(t.numSolutions(), 1u); } -TEST_F(FallbacksFixturePropagate, DISABLED_ComputeFirstSuccessfulStagePerSolutionOnly) { +TEST_F(FallbacksFixturePropagate, computeFirstSuccessfulStagePerSolutionOnly) { t.add(std::make_unique(PredefinedCosts({ 2.0, 1.0 }))); // duplicate generator solutions with resulting costs: 4, 2 | 3, 1 t.add(std::make_unique(PredefinedCosts({ 2.0, 0.0, 2.0, 0.0 }), 2)); @@ -78,10 +78,11 @@ TEST_F(FallbacksFixturePropagate, DISABLED_ComputeFirstSuccessfulStagePerSolutio t.add(std::move(fallbacks)); EXPECT_TRUE(t.plan()); - EXPECT_COSTS(t.solutions(), testing::ElementsAre(113, 124, 211, 222)); + EXPECT_COSTS(t.solutions(), testing::ElementsAre(113, 124, 212, 221)); } -TEST_F(FallbacksFixturePropagate, DISABLED_UpdateSolutionOrder) { +// requires individual job control in Fallbacks's children +TEST_F(FallbacksFixturePropagate, DISABLED_updateSolutionOrder) { t.add(std::make_unique(PredefinedCosts({ 10.0, 0.0 }))); t.add(std::make_unique(PredefinedCosts({ 1.0, 2.0 }))); // available solutions (sorted) in individual runs of fallbacks: 1 | 11, 2 | 2, 11 @@ -100,7 +101,8 @@ TEST_F(FallbacksFixturePropagate, DISABLED_UpdateSolutionOrder) { EXPECT_COSTS(t.solutions(), testing::ElementsAre(2)); // expecting less costly solution as result } -TEST_F(FallbacksFixturePropagate, DISABLED_MultipleActivePendingStates) { +// requires individual job control in Fallbacks's children +TEST_F(FallbacksFixturePropagate, DISABLED_multipleActivePendingStates) { t.add(std::make_unique(PredefinedCosts({ 2.0, 1.0, 3.0 }))); // use a fallback container to delay computation: the 1st child never succeeds, but only the 2nd auto inner = std::make_unique("Inner"); @@ -159,7 +161,7 @@ TEST_F(FallbacksFixturePropagate, activeChildReset) { using FallbacksFixtureConnect = TaskTestBase; -TEST_F(FallbacksFixtureConnect, DISABLED_ConnectStageInsideFallbacks) { +TEST_F(FallbacksFixtureConnect, connectStageInsideFallbacks) { t.add(std::make_unique(PredefinedCosts({ 1.0, 2.0 }))); auto fallbacks = std::make_unique("Fallbacks"); From c822fd38220201cd811b604bf2caad3539477d9d Mon Sep 17 00:00:00 2001 From: Robert Haschke Date: Mon, 22 Nov 2021 09:40:51 +0100 Subject: [PATCH 18/34] Remove logger configuration Logger config can be more easily handled via ROSCONSOLE_CONFIG_FILE. --- core/test/test_fallback.cpp | 13 ------------- 1 file changed, 13 deletions(-) diff --git a/core/test/test_fallback.cpp b/core/test/test_fallback.cpp index d27b9fcf..3858de2d 100644 --- a/core/test/test_fallback.cpp +++ b/core/test/test_fallback.cpp @@ -174,16 +174,3 @@ TEST_F(FallbacksFixtureConnect, connectStageInsideFallbacks) { EXPECT_TRUE(t.plan()); EXPECT_COSTS(t.solutions(), testing::ElementsAre(11, 12, 21, 22)); } - -int main(int argc, char** argv) { - for (int i = 1; i < argc; ++i) { - if (strcmp(argv[i], "--debug") == 0) { - if (ros::console::set_logger_level(ROSCONSOLE_DEFAULT_NAME, ros::console::levels::Debug)) - ros::console::notifyLoggerLevelsChanged(); - break; - } - } - - testing::InitGoogleTest(&argc, argv); - return RUN_ALL_TESTS(); -} From b84bb87102d2ff663a75a9ef8c97c3507ba21876 Mon Sep 17 00:00:00 2001 From: Robert Haschke Date: Wed, 24 Nov 2021 20:51:03 +0100 Subject: [PATCH 19/34] pre-commit autoupdate --- .pre-commit-config.yaml | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/.pre-commit-config.yaml b/.pre-commit-config.yaml index decc953a..f1063965 100644 --- a/.pre-commit-config.yaml +++ b/.pre-commit-config.yaml @@ -15,7 +15,7 @@ repos: # Standard hooks - repo: https://github.com/pre-commit/pre-commit-hooks - rev: v3.4.0 + rev: v4.0.1 hooks: - id: check-added-large-files - id: check-case-conflict @@ -29,7 +29,7 @@ repos: - id: trailing-whitespace - repo: https://github.com/psf/black - rev: 20.8b1 + rev: 21.11b1 hooks: - id: black From e67b3252fc8790a15c54e13b752e80c00b4736f7 Mon Sep 17 00:00:00 2001 From: Robert Haschke Date: Tue, 23 Nov 2021 22:38:50 +0100 Subject: [PATCH 20/34] static TaskPrivate::swap() -> ContainerBasePrivate::operator=() - Enable moving/swapping of other container impls (e.g. Fallbacks) - Clarify (via move semantics) that content of source impl will be lost - Get rid of friend declarations --- .../moveit/task_constructor/container_p.h | 2 +- .../include/moveit/task_constructor/stage_p.h | 3 +- core/include/moveit/task_constructor/task_p.h | 5 ++- core/src/container.cpp | 29 +++++++++++++++- core/src/stage.cpp | 27 +++++++++++++++ core/src/task.cpp | 34 +++++-------------- core/test/test_container.cpp | 4 --- 7 files changed, 69 insertions(+), 35 deletions(-) diff --git a/core/include/moveit/task_constructor/container_p.h b/core/include/moveit/task_constructor/container_p.h index 1e29526b..ae38b5f8 100644 --- a/core/include/moveit/task_constructor/container_p.h +++ b/core/include/moveit/task_constructor/container_p.h @@ -76,7 +76,6 @@ namespace task_constructor { class ContainerBasePrivate : public StagePrivate { friend class ContainerBase; - friend void swap(StagePrivate*& lhs, StagePrivate*& rhs); public: using container_type = StagePrivate::container_type; @@ -135,6 +134,7 @@ public: protected: ContainerBasePrivate(ContainerBase* me, const std::string& name); + ContainerBasePrivate& operator=(ContainerBasePrivate&& other); // Set child's push interfaces: allow pushing if child requires it. inline void setChildsPushBackwardInterface(StagePrivate* child) { diff --git a/core/include/moveit/task_constructor/stage_p.h b/core/include/moveit/task_constructor/stage_p.h index 87ed7b27..1ce91754 100644 --- a/core/include/moveit/task_constructor/stage_p.h +++ b/core/include/moveit/task_constructor/stage_p.h @@ -61,7 +61,6 @@ class StagePrivate { friend class Stage; friend std::ostream& operator<<(std::ostream& os, const StagePrivate& stage); - friend void swap(StagePrivate*& lhs, StagePrivate*& rhs); public: /// container type used to store children @@ -165,6 +164,8 @@ public: void computeCost(const InterfaceState& from, const InterfaceState& to, SolutionBase& solution); protected: + StagePrivate& operator=(StagePrivate&& other); + // associated/owning Stage instance Stage* me_; diff --git a/core/include/moveit/task_constructor/task_p.h b/core/include/moveit/task_constructor/task_p.h index 327f1631..f0e6dc0a 100644 --- a/core/include/moveit/task_constructor/task_p.h +++ b/core/include/moveit/task_constructor/task_p.h @@ -51,15 +51,14 @@ namespace task_constructor { class TaskPrivate : public WrapperBasePrivate { friend class Task; + TaskPrivate& operator=(TaskPrivate&& other); public: TaskPrivate(Task* me, const std::string& ns); + const std::string& ns() const { return ns_; } const ContainerBase* stages() const; -protected: - static void swap(StagePrivate*& lhs, StagePrivate*& rhs); - private: std::string ns_; robot_model_loader::RobotModelLoaderPtr robot_model_loader_; diff --git a/core/src/container.cpp b/core/src/container.cpp index ba9a5fb2..f2cdcee3 100644 --- a/core/src/container.cpp +++ b/core/src/container.cpp @@ -59,6 +59,33 @@ ContainerBasePrivate::ContainerBasePrivate(ContainerBase* me, const std::string& , pending_backward_(new Interface) , pending_forward_(new Interface) {} +ContainerBasePrivate& ContainerBasePrivate::operator=(ContainerBasePrivate&& other) { + assert(internal_external_.empty() && other.internal_external_.empty()); + + // move StagePrivate members + this->StagePrivate::operator=(std::move(other)); + + // swapping of container members needed to maintain valid pending_* interfaces + // and children (e.g. for TaskPrivate) + required_interface_ = other.required_interface_; + std::swap(pending_backward_, other.pending_backward_); + std::swap(pending_forward_, other.pending_forward_); + std::swap(children_, other.children_); + + // redirect all children's parent pointers to the new parent + auto reparent_children = [](ContainerBasePrivate& self) { + for (auto it = self.children_.begin(), end = self.children_.end(); it != end; ++it) { + auto cimpl = (*it)->pimpl(); + cimpl->unparent(); + cimpl->setParent(static_cast(self.me_)); + cimpl->setParentPosition(it); + } + }; + reparent_children(*this); + reparent_children(other); + return *this; +} + ContainerBasePrivate::const_iterator ContainerBasePrivate::childByIndex(int index, bool for_insert) const { if (!for_insert && index < 0) --index; @@ -681,8 +708,8 @@ void ParallelContainerBasePrivate::resolveInterface(InterfaceFlags expected) { child_impl->resolveInterface(expected); validateInterfaces(*child_impl, expected, first); // initialize push connections of children according to their demands - setChildsPushForwardInterface(child_impl); setChildsPushBackwardInterface(child_impl); + setChildsPushForwardInterface(child_impl); first = false; } catch (InitStageException& e) { exceptions.append(e); diff --git a/core/src/stage.cpp b/core/src/stage.cpp index 2eaf6a43..efa730dc 100644 --- a/core/src/stage.cpp +++ b/core/src/stage.cpp @@ -106,6 +106,33 @@ StagePrivate::StagePrivate(Stage* me, const std::string& name) , parent_{ nullptr } , introspection_{ nullptr } {} +StagePrivate& StagePrivate::operator=(StagePrivate&& other) { + assert(typeid(*this) == typeid(other)); + + assert(states_.empty() && other.states_.empty()); + assert((!starts_ || starts_->empty()) && (!other.starts_ || other.starts_->empty())); + assert((!ends_ || ends_->empty()) && (!other.ends_ || other.ends_->empty())); + assert(solutions_.empty() && other.solutions_.empty()); + assert(failures_.empty() && other.failures_.empty()); + + // me_ must not be changed! + name_ = std::move(other.name_); + properties_ = std::move(other.properties_); + cost_term_ = std::move(other.cost_term_); + solution_cbs_ = std::move(other.solution_cbs_); + + starts_ = std::move(other.starts_); + ends_ = std::move(other.ends_); + prev_ends_ = std::move(other.prev_ends_); + next_starts_ = std::move(other.next_starts_); + + parent_ = std::move(other.parent_); + it_ = std::move(other.it_); + other.unparent(); + + return *this; +} + InterfaceFlags StagePrivate::interfaceFlags() const { InterfaceFlags f; if (starts()) diff --git a/core/src/task.cpp b/core/src/task.cpp index 9f1db279..884215f1 100644 --- a/core/src/task.cpp +++ b/core/src/task.cpp @@ -74,30 +74,14 @@ namespace task_constructor { TaskPrivate::TaskPrivate(Task* me, const std::string& ns) : WrapperBasePrivate(me, std::string()), ns_(rosNormalizeName(ns)), preempt_requested_(false) {} -void swap(StagePrivate*& lhs, StagePrivate*& rhs) { - // It only makes sense to swap pimpl instances of a Task! - // However, due to member protection rules, we can only implement it here - assert(typeid(lhs) == typeid(rhs)); - - // swap instances - ::std::swap(lhs, rhs); - // as well as their me_ pointers - ::std::swap(lhs->me_, rhs->me_); - - // and redirect the parent pointers of children to new parents - auto& lhs_children = static_cast(lhs)->children_; - for (auto it = lhs_children.begin(), end = lhs_children.end(); it != end; ++it) { - (*it)->pimpl()->unparent(); - (*it)->pimpl()->setParent(static_cast(lhs->me_)); - (*it)->pimpl()->setParentPosition(it); - } - - auto& rhs_children = static_cast(rhs)->children_; - for (auto it = rhs_children.begin(), end = rhs_children.end(); it != end; ++it) { - (*it)->pimpl()->unparent(); - (*it)->pimpl()->setParent(static_cast(rhs->me_)); - (*it)->pimpl()->setParentPosition(it); - } +TaskPrivate& TaskPrivate::operator=(TaskPrivate&& other) { + this->WrapperBasePrivate::operator=(std::move(other)); + ns_ = std::move(other.ns_); + introspection_ = std::move(other.introspection_); + robot_model_ = std::move(other.robot_model_); + robot_model_loader_ = std::move(other.robot_model_loader_); + task_cbs_ = std::move(other.task_cbs_); + return *this; } const ContainerBase* TaskPrivate::stages() const { @@ -122,7 +106,7 @@ Task::Task(Task&& other) // NOLINT(performance-noexcept-move-constructor) Task& Task::operator=(Task&& other) { // NOLINT(performance-noexcept-move-constructor) clear(); // remove all stages of current task - swap(this->pimpl_, other.pimpl_); + *static_cast(pimpl_) = std::move(*static_cast(other.pimpl_)); return *this; } diff --git a/core/test/test_container.cpp b/core/test/test_container.cpp index 4eaa143b..73fae4ed 100644 --- a/core/test/test_container.cpp +++ b/core/test/test_container.cpp @@ -586,10 +586,6 @@ TEST(Task, move) { Task t2 = std::move(t1); EXPECT_EQ(t2.stages()->numChildren(), 2u); EXPECT_EQ(t1.stages()->numChildren(), 0u); // NOLINT(clang-analyzer-cplusplus.Move) - - t1 = std::move(t2); - EXPECT_EQ(t1.stages()->numChildren(), 2u); - EXPECT_EQ(t2.stages()->numChildren(), 0u); // NOLINT(clang-analyzer-cplusplus.Move) } TEST(Task, reuse) { From 8dd8022ef9a0d238af866d7ba7ccfc7319e6619b Mon Sep 17 00:00:00 2001 From: Robert Haschke Date: Tue, 23 Nov 2021 19:05:10 +0100 Subject: [PATCH 21/34] Factorize implementation of FallbacksPrivate into 3 classes --- .../moveit/task_constructor/container.h | 2 + .../moveit/task_constructor/container_p.h | 81 ++++--- core/src/container.cpp | 218 ++++++++++-------- 3 files changed, 179 insertions(+), 122 deletions(-) diff --git a/core/include/moveit/task_constructor/container.h b/core/include/moveit/task_constructor/container.h index 6fcd34dc..6dcf5bfa 100644 --- a/core/include/moveit/task_constructor/container.h +++ b/core/include/moveit/task_constructor/container.h @@ -159,6 +159,8 @@ class FallbacksPrivate; */ class Fallbacks : public ParallelContainerBase { + inline void replaceImpl(); + public: PRIVATE_CLASS(Fallbacks); Fallbacks(const std::string& name = "fallbacks"); diff --git a/core/include/moveit/task_constructor/container_p.h b/core/include/moveit/task_constructor/container_p.h index ae38b5f8..41fe2be9 100644 --- a/core/include/moveit/task_constructor/container_p.h +++ b/core/include/moveit/task_constructor/container_p.h @@ -241,42 +241,69 @@ private: }; PIMPL_FUNCTIONS(ParallelContainerBase) +/* The Fallbacks container needs to implement different behaviour based on its interface. + * Thus, we implement 3 different classes: for Generator, Propagator, and Connect-like interfaces. + * FallbacksPrivate is the common base class for all of them, defining the common API to be used + * by the Fallbacks container. + * The actual interface-specific class is instantiated in initializeExternalInterfaces() + * resp. Fallbacks::replaceImpl() when the actual interface is known. */ class FallbacksPrivate : public ParallelContainerBasePrivate { - friend class Fallbacks; - public: FallbacksPrivate(Fallbacks* me, const std::string& name); + FallbacksPrivate(FallbacksPrivate&& other); -protected: - void computePropagate(); - struct ExternalState - { - ExternalState() = default; - 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; - - inline bool operator<(const ExternalState& other) const { return *external_state < *other.external_state; } - }; - ordered pending_states_; // pending external states for a PROPAGATE interface - ordered::iterator current_pending_; // currently active pending state - inline void computeGenerate(); - mutable container_type::const_iterator current_generator_; - -private: - void initializeExternalInterfaces() override; - template - void onNewExternalState(Interface::iterator external, bool updated); + // shared method overrides + void initializeExternalInterfaces() final; void onNewFailure(const Stage& child, const InterfaceState* from, const InterfaceState* to) override; - // print pending states for debugging - void printPending(const char* comment = "pending: ") const; + + // interface-specific methods + virtual void _init(){}; + virtual bool _canCompute() const { return false; }; + virtual void _compute(){}; }; PIMPL_FUNCTIONS(Fallbacks) +/// Fallbacks implementation for GENERATOR interface +struct FallbacksPrivateGenerator : FallbacksPrivate +{ + FallbacksPrivateGenerator(FallbacksPrivate&& old); + void _init() override { current_ = children().begin(); } + bool _canCompute() const override; + void _compute() override; + + mutable container_type::const_iterator current_; // currently active child generator +}; + +/// Fallbacks implementation for FORWARD or BACKWARD interface +struct FallbacksPrivatePropagator : FallbacksPrivate +{ + FallbacksPrivatePropagator(FallbacksPrivate&& old); + void _init() override { current_ = pending_.end(); } + bool _canCompute() const override; + void _compute() override; + + // interface notify() callback + void onNewExternalState(Interface::iterator external, bool updated); + + // print pending states for debugging + void printPending(const char* comment = "pending: ") const; + + struct Job + { + Job() = default; + Job(Interface::iterator state, container_type::const_iterator child) : external_state(state), stage(child) {} + + Interface::iterator external_state; + container_type::const_iterator stage; + + inline bool operator<(const Job& other) const { return *external_state < *other.external_state; } + }; + Interface::Direction dir_; + ordered pending_; // pending external states to process + ordered::iterator current_; // currently active job +}; + class WrapperBasePrivate : public ParallelContainerBasePrivate { friend class WrapperBase; diff --git a/core/src/container.cpp b/core/src/container.cpp index f2cdcee3..a245fada 100644 --- a/core/src/container.cpp +++ b/core/src/container.cpp @@ -615,6 +615,7 @@ void SerialContainerPrivate::resolveInterface(InterfaceFlags expected) { StagePrivate* child_impl = (**it).pimpl(); StagePrivate* previous_impl = (**previous_it).pimpl(); child_impl->resolveInterface(invert(previous_impl->requiredInterface()) & START_IF_MASK); + child_impl = (**it).pimpl(); // re-assign as pimpl_ pointer of a Fallback container will change! connect(*previous_impl, *child_impl); } catch (InitStageException& e) { exceptions.append(e); @@ -851,71 +852,53 @@ void Fallbacks::reset() { } void Fallbacks::init(const moveit::core::RobotModelConstPtr& robot_model) { - auto& impl{ *pimpl() }; ParallelContainerBase::init(robot_model); - impl.current_generator_ = impl.children().begin(); - impl.current_pending_ = impl.pending_states_.end(); + pimpl()->_init(); } bool Fallbacks::canCompute() const { - auto impl { pimpl() }; - - switch (impl->requiredInterface()) { - case GENERATE: - // current_generator_ is fixed if it produced solutions before - if (!solutions().empty()) - return (*impl->current_generator_)->pimpl()->canCompute(); - else { - // move to first generator that can run - while(impl->current_generator_ != impl->children().end() && !(*impl->current_generator_)->pimpl()->canCompute()) { - ROS_DEBUG_STREAM_NAMED("Fallbacks", "Generator '" << (*impl->current_generator_)->name() << "' can't compute, trying next one."); - ++impl->current_generator_; - } - return impl->current_generator_ != impl->children().end(); - } - break; - case PROPAGATE_FORWARDS: - case PROPAGATE_BACKWARDS: - case CONNECT: - return !impl->pending_states_.empty(); - default: - assert(false); - } + return pimpl()->_canCompute(); } void Fallbacks::compute() { - auto impl { pimpl() }; - - switch (impl->requiredInterface()) { - case GENERATE: - impl->computeGenerate(); - break; - case PROPAGATE_FORWARDS: - case PROPAGATE_BACKWARDS: - case CONNECT: - impl->computePropagate(); - break; - default: - assert(false); - } + pimpl()->_compute(); } void Fallbacks::onNewSolution(const SolutionBase& s) { liftSolution(s); } +inline void Fallbacks::replaceImpl() { + FallbacksPrivate *impl = pimpl(); + switch (pimpl()->requiredInterface()) { + case GENERATE: + impl = new FallbacksPrivateGenerator(std::move(*impl)); + break; + case PROPAGATE_FORWARDS: + case PROPAGATE_BACKWARDS: + impl = new FallbacksPrivatePropagator(std::move(*impl)); + break; + case CONNECT: + throw std::runtime_error("Not yet implemented"); + break; + } + delete pimpl_; + pimpl_ = impl; +} + FallbacksPrivate::FallbacksPrivate(Fallbacks* me, const std::string& name) - : ParallelContainerBasePrivate(me, name) {} + : ParallelContainerBasePrivate(me, name) {} + +FallbacksPrivate::FallbacksPrivate(FallbacksPrivate&& other) +: ParallelContainerBasePrivate(static_cast(other.me()), "") { + // move contents of other + this->ParallelContainerBasePrivate::operator=(std::move(other)); +} void FallbacksPrivate::initializeExternalInterfaces() { - if (requiredInterface() & READS_START) - starts().reset(new Interface([this](Interface::iterator external, bool updated) { - this->onNewExternalState(external, updated); - })); - if (requiredInterface() & READS_END) - ends().reset(new Interface([this](Interface::iterator external, bool updated) { - this->onNewExternalState(external, updated); - })); + // Here we know the final interface of the container (and all its children) + // Thus replace, this pimpl() with a new interface-specific one: + static_cast(me())->replaceImpl(); } void FallbacksPrivate::onNewFailure(const Stage& /*child*/, const InterfaceState* /*from*/, const InterfaceState* /*to*/) { @@ -925,58 +908,72 @@ void FallbacksPrivate::onNewFailure(const Stage& /*child*/, const InterfaceState // Thus pruning must only occur once the last child is exhausted (inside computePropagate) } -void FallbacksPrivate::computeGenerate() { - assert(current_generator_ != children().end()); - (*current_generator_)->pimpl()->runCompute(); + +FallbacksPrivateGenerator::FallbacksPrivateGenerator(FallbacksPrivate&& old) + : FallbacksPrivate(std::move(old)) { + FallbacksPrivateGenerator::_init(); } -inline void FallbacksPrivate::printPending(const char* comment) const { - ROSCONSOLE_DEFINE_LOCATION(true, ::ros::console::levels::Debug, ROSCONSOLE_NAME_PREFIX ".Fallbacks"); - if (ROS_UNLIKELY(__rosconsole_define_location__enabled)) { - std::cout << name() << ": " << comment; - std::for_each(pending_states_.begin(), pending_states_.end(), [](const auto& e) { std::cout << e.external_state->priority() << " "; }); - std::cout << std::endl; +bool FallbacksPrivateGenerator::_canCompute() const { + // current_ is fixed if it produced solutions before + if (!solutions_.empty()) + return (*current_)->pimpl()->canCompute(); + else { + // move to first generator that can run + while(current_ != children().end() && !(*current_)->pimpl()->canCompute()) { + ROS_DEBUG_STREAM_NAMED("Fallbacks", "Generator '" << (*current_)->name() << "' can't compute, trying next one."); + ++current_; + } + return current_ != children().end(); } } -template -void FallbacksPrivate::onNewExternalState(Interface::iterator external, bool updated) { - if (updated) { - auto it = std::find_if(pending_states_.begin(), pending_states_.end(), - [external](const ExternalState& s) { return s.external_state == external; }); - if (it == pending_states_.cend()) - return; // already processed - - pending_states_.update(it); // update sorting pos of this single item - printPending("after update: "); - - // update prio of linked internal states as well - ContainerBasePrivate::copyState(external, InterfacePtr(), updated); - return; - } - - // remember external state for later processing by children. - // children().end() indicates that the states wasn't yet forwarded to any child - pending_states_.push(ExternalState(external, dir, children().cend())); - printPending("after push: "); +void FallbacksPrivateGenerator::_compute() { + assert(current_ != children().end()); + (*current_)->pimpl()->runCompute(); } -void FallbacksPrivate::computePropagate(){ - while (!pending_states_.empty()) { + +FallbacksPrivatePropagator::FallbacksPrivatePropagator(FallbacksPrivate&& old) + : FallbacksPrivate(std::move(old)) { + switch (requiredInterface()) { + case PROPAGATE_FORWARDS: + dir_ = Interface::FORWARD; + starts().reset(new Interface([this](Interface::iterator external, bool updated) { + this->onNewExternalState(external, updated); + })); + break; + case PROPAGATE_BACKWARDS: + dir_ = Interface::BACKWARD; + ends().reset(new Interface([this](Interface::iterator external, bool updated) { + this->onNewExternalState(external, updated); + })); + break; + default: + assert(false); + } + FallbacksPrivatePropagator::_init(); +} + +bool FallbacksPrivatePropagator::_canCompute() const { + return !pending_.empty(); +} + +void FallbacksPrivatePropagator::_compute() { + while (!pending_.empty()) { printPending(); - // If we have a currently active pending state, proceed with this one - // even if pending_states_.front() might be different meanwhile. - // This is important as we need to feed states one by one to the children. + // If we have a currently active job, proceed with this one even if pending_.front() + // might be different meanwhile. This is important as we need to feed jobs one by one to the children. // Otherwise we cannot know if a child is exhausted on a specific input state. - if (current_pending_ == pending_states_.end()) - current_pending_ = pending_states_.begin(); - auto current = current_pending_; + if (current_ == pending_.end()) + current_ = pending_.begin(); + auto current = current_; // Keep a copy here, as current_ might change due to resorting of pending_! - auto pushState = [this](const ExternalState& ext) { + auto pushState = [this](const Job& ext) { ROS_DEBUG_STREAM_NAMED("Fallbacks", "Push external state (" << ext.external_state->priority() << ") to '" << (*ext.stage)->name() << "'"); - copyState(ext.dir, ext.external_state, (*ext.stage)->pimpl()->pullInterface(ext.dir), false); + copyState(dir_, ext.external_state, (*ext.stage)->pimpl()->pullInterface(dir_), false); }; if (current->stage == children().cend()) { @@ -997,7 +994,7 @@ void FallbacksPrivate::computePropagate(){ [](const auto& t){ return !t->isFailure();}) != trajectories.cend(); }}; - if(!has_solutions(*current->external_state, current->dir)){ + if(!has_solutions(*current->external_state, dir_)){ ++current->stage; if(current->stage != children().cend()){ ROS_DEBUG_STREAM_NAMED("Fallbacks", "Child '" << child->name() << "' failed generating solutions, trying next child: '" @@ -1007,21 +1004,52 @@ void FallbacksPrivate::computePropagate(){ else { ROS_DEBUG_STREAM_NAMED("Fallbacks", "Failed to extend state with all children, pruning path"); parent()->pimpl()->onNewFailure(*me(), - current->dir == Interface::FORWARD ? &*current->external_state : nullptr, - current->dir == Interface::BACKWARD ? nullptr : &*current->external_state); - pending_states_.erase(current); - current_pending_ = pending_states_.end(); + dir_ == Interface::FORWARD ? &*current->external_state : nullptr, + dir_ == Interface::BACKWARD ? nullptr : &*current->external_state); + pending_.erase(current); + current_ = pending_.end(); } } else { ROS_DEBUG_STREAM_NAMED("Fallbacks", "Child '" << child->name() << "' exhausted, but produced solutions before, not invoking further fallbacks"); - pending_states_.erase(current); - current_pending_ = pending_states_.end(); + pending_.erase(current); + current_ = pending_.end(); } // continue processing with next pending state as we didn't runCompute() yet } } +void FallbacksPrivatePropagator::onNewExternalState(Interface::iterator external, bool updated) { + if (updated) { + auto it = std::find_if(pending_.begin(), pending_.end(), + [external](const Job& s) { return s.external_state == external; }); + if (it == pending_.cend()) + return; // already processed + + pending_.update(it); // update sorting pos of this single item + printPending("after update: "); + + // update prio of linked internal states as well + ContainerBasePrivate::copyState(dir_, external, InterfacePtr(), updated); + return; + } + + // remember external state for later processing by children. + // children().end() indicates that the states wasn't yet forwarded to any child + pending_.push(Job(external, children().cend())); + printPending("after push: "); +} + +inline void FallbacksPrivatePropagator::printPending(const char* comment) const { + ROSCONSOLE_DEFINE_LOCATION(true, ::ros::console::levels::Debug, ROSCONSOLE_NAME_PREFIX ".Fallbacks"); + if (ROS_UNLIKELY(__rosconsole_define_location__enabled)) { + std::cout << name() << ": " << comment; + std::for_each(pending_.begin(), pending_.end(), [](const auto& e) { std::cout << e.external_state->priority() << " "; }); + std::cout << std::endl; + } +} + + MergerPrivate::MergerPrivate(Merger* me, const std::string& name) : ParallelContainerBasePrivate(me, name) {} void MergerPrivate::resolveInterface(InterfaceFlags expected) { From 070c6e9ab650f09536019e257df2370c0d83776b Mon Sep 17 00:00:00 2001 From: Robert Haschke Date: Thu, 25 Nov 2021 07:35:28 +0100 Subject: [PATCH 22/34] Disable failing test FallbacksFixtureConnect.connectStageInsideFallbacks ... as we are now missing the implementation for CONNECT interfaces --- core/test/test_fallback.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/core/test/test_fallback.cpp b/core/test/test_fallback.cpp index 3858de2d..092d7890 100644 --- a/core/test/test_fallback.cpp +++ b/core/test/test_fallback.cpp @@ -161,7 +161,7 @@ TEST_F(FallbacksFixturePropagate, activeChildReset) { using FallbacksFixtureConnect = TaskTestBase; -TEST_F(FallbacksFixtureConnect, connectStageInsideFallbacks) { +TEST_F(FallbacksFixtureConnect, DISABLED_connectStageInsideFallbacks) { t.add(std::make_unique(PredefinedCosts({ 1.0, 2.0 }))); auto fallbacks = std::make_unique("Fallbacks"); From 7237e81547914ad0063193956817400ec9196c5c Mon Sep 17 00:00:00 2001 From: Robert Haschke Date: Wed, 24 Nov 2021 20:51:31 +0100 Subject: [PATCH 23/34] Rework FallbacksPrivate* Further factorize and simplify FallbacksPrivate classes employing ideas from @v4hn. The key difference between the variants his how they advance to the next job. Thus, the only virtual method required is nextJob(). --- .../moveit/task_constructor/container_p.h | 46 ++--- core/src/container.cpp | 195 +++++++----------- 2 files changed, 86 insertions(+), 155 deletions(-) diff --git a/core/include/moveit/task_constructor/container_p.h b/core/include/moveit/task_constructor/container_p.h index 41fe2be9..5858b4f9 100644 --- a/core/include/moveit/task_constructor/container_p.h +++ b/core/include/moveit/task_constructor/container_p.h @@ -246,21 +246,23 @@ PIMPL_FUNCTIONS(ParallelContainerBase) * FallbacksPrivate is the common base class for all of them, defining the common API to be used * by the Fallbacks container. * The actual interface-specific class is instantiated in initializeExternalInterfaces() - * resp. Fallbacks::replaceImpl() when the actual interface is known. */ + * resp. Fallbacks::replaceImpl() when the actual interface is known. + * The key difference between the 3 variants is how the advance to the next job. */ class FallbacksPrivate : public ParallelContainerBasePrivate { public: FallbacksPrivate(Fallbacks* me, const std::string& name); FallbacksPrivate(FallbacksPrivate&& other); - // shared method overrides + // method overrides common to 3 variants void initializeExternalInterfaces() final; void onNewFailure(const Stage& child, const InterfaceState* from, const InterfaceState* to) override; - // interface-specific methods - virtual void _init(){}; - virtual bool _canCompute() const { return false; }; - virtual void _compute(){}; + // virtual method specific to each variant + /// Advance to the next job, assuming that the current child is exhausted on the current job. + virtual bool nextJob() { return false; } + + container_type::const_iterator current_; // currently active child generator }; PIMPL_FUNCTIONS(Fallbacks) @@ -268,40 +270,18 @@ PIMPL_FUNCTIONS(Fallbacks) struct FallbacksPrivateGenerator : FallbacksPrivate { FallbacksPrivateGenerator(FallbacksPrivate&& old); - void _init() override { current_ = children().begin(); } - bool _canCompute() const override; - void _compute() override; - - mutable container_type::const_iterator current_; // currently active child generator + bool nextJob() override; }; /// Fallbacks implementation for FORWARD or BACKWARD interface struct FallbacksPrivatePropagator : FallbacksPrivate { FallbacksPrivatePropagator(FallbacksPrivate&& old); - void _init() override { current_ = pending_.end(); } - bool _canCompute() const override; - void _compute() override; + bool nextJob() override; + bool jobHasSolutions() const; - // interface notify() callback - void onNewExternalState(Interface::iterator external, bool updated); - - // print pending states for debugging - void printPending(const char* comment = "pending: ") const; - - struct Job - { - Job() = default; - Job(Interface::iterator state, container_type::const_iterator child) : external_state(state), stage(child) {} - - Interface::iterator external_state; - container_type::const_iterator stage; - - inline bool operator<(const Job& other) const { return *external_state < *other.external_state; } - }; - Interface::Direction dir_; - ordered pending_; // pending external states to process - ordered::iterator current_; // currently active job + Interface::Direction dir_; // propagation direction + Interface::iterator job_; // pointer to currently processed external state }; class WrapperBasePrivate : public ParallelContainerBasePrivate diff --git a/core/src/container.cpp b/core/src/container.cpp index a245fada..9ca40d8b 100644 --- a/core/src/container.cpp +++ b/core/src/container.cpp @@ -853,15 +853,23 @@ void Fallbacks::reset() { void Fallbacks::init(const moveit::core::RobotModelConstPtr& robot_model) { ParallelContainerBase::init(robot_model); - pimpl()->_init(); + auto impl = pimpl(); + impl->current_ = impl->children().begin(); } bool Fallbacks::canCompute() const { - return pimpl()->_canCompute(); + auto impl = const_cast(pimpl()); + + while(impl->current_ != impl->children().end() && // not completely exhaused + !(*impl->current_)->pimpl()->canCompute()) // but current child cannot compute + return impl->nextJob(); // advance to next job + + // return value: current child is well defined and thus can compute? + return impl->current_ != impl->children().end(); } void Fallbacks::compute() { - pimpl()->_compute(); + (*pimpl()->current_)->pimpl()->runCompute(); } void Fallbacks::onNewSolution(const SolutionBase& s) { @@ -893,6 +901,8 @@ FallbacksPrivate::FallbacksPrivate(FallbacksPrivate&& other) : ParallelContainerBasePrivate(static_cast(other.me()), "") { // move contents of other this->ParallelContainerBasePrivate::operator=(std::move(other)); + // (re)initialize + current_ = children().begin(); } void FallbacksPrivate::initializeExternalInterfaces() { @@ -910,143 +920,84 @@ void FallbacksPrivate::onNewFailure(const Stage& /*child*/, const InterfaceState FallbacksPrivateGenerator::FallbacksPrivateGenerator(FallbacksPrivate&& old) - : FallbacksPrivate(std::move(old)) { - FallbacksPrivateGenerator::_init(); -} + : FallbacksPrivate(std::move(old)) {} -bool FallbacksPrivateGenerator::_canCompute() const { - // current_ is fixed if it produced solutions before - if (!solutions_.empty()) - return (*current_)->pimpl()->canCompute(); - else { - // move to first generator that can run - while(current_ != children().end() && !(*current_)->pimpl()->canCompute()) { - ROS_DEBUG_STREAM_NAMED("Fallbacks", "Generator '" << (*current_)->name() << "' can't compute, trying next one."); - ++current_; - } - return current_ != children().end(); +bool FallbacksPrivateGenerator::nextJob() { + assert(current_ != children().end() && !(*current_)->pimpl()->canCompute()); + + // don't advance to next child when we already produced solutions + if (!solutions_.empty()) { + current_ = children().end(); // indicate that we are exhausted + return false; } -} -void FallbacksPrivateGenerator::_compute() { - assert(current_ != children().end()); - (*current_)->pimpl()->runCompute(); + do { + if (std::next(current_) != children().end()) + 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 current_ != children().end(); } FallbacksPrivatePropagator::FallbacksPrivatePropagator(FallbacksPrivate&& old) : FallbacksPrivate(std::move(old)) { switch (requiredInterface()) { - case PROPAGATE_FORWARDS: - dir_ = Interface::FORWARD; - starts().reset(new Interface([this](Interface::iterator external, bool updated) { - this->onNewExternalState(external, updated); - })); - break; - case PROPAGATE_BACKWARDS: - dir_ = Interface::BACKWARD; - ends().reset(new Interface([this](Interface::iterator external, bool updated) { - this->onNewExternalState(external, updated); - })); - break; - default: - assert(false); + case PROPAGATE_FORWARDS: + dir_ = Interface::FORWARD; + starts() = std::make_shared(); + break; + case PROPAGATE_BACKWARDS: + dir_ = Interface::BACKWARD; + ends() = std::make_shared(); + break; + default: + assert(false); } - FallbacksPrivatePropagator::_init(); + job_ = pullInterface(dir_)->end(); // indicate fresh start } -bool FallbacksPrivatePropagator::_canCompute() const { - return !pending_.empty(); -} - -void FallbacksPrivatePropagator::_compute() { - while (!pending_.empty()) { - printPending(); - - // If we have a currently active job, proceed with this one even if pending_.front() - // might be different meanwhile. This is important as we need to feed jobs one by one to the children. - // Otherwise we cannot know if a child is exhausted on a specific input state. - if (current_ == pending_.end()) - current_ = pending_.begin(); - auto current = current_; // Keep a copy here, as current_ might change due to resorting of pending_! - - auto pushState = [this](const Job& ext) { - ROS_DEBUG_STREAM_NAMED("Fallbacks", "Push external state (" << ext.external_state->priority() - << ") to '" << (*ext.stage)->name() << "'"); - copyState(dir_, ext.external_state, (*ext.stage)->pimpl()->pullInterface(dir_), false); - }; - - if (current->stage == children().cend()) { - current->stage = children().begin(); // activate first child - pushState(*current); - } - - StagePrivate* child = (*current->stage)->pimpl(); - if (child->canCompute()) { - child->runCompute(); - return; // return after first compute() - } - - auto has_solutions{ [](const InterfaceState& s, Interface::Direction d){ - const auto& trajectories { d == Interface::FORWARD ? s.outgoingTrajectories() - : s.incomingTrajectories() }; - return std::find_if(trajectories.cbegin(), trajectories.cend(), +bool FallbacksPrivatePropagator::jobHasSolutions() const { + const auto& trajectories { dir_ == Interface::FORWARD ? job_->outgoingTrajectories() + : job_->incomingTrajectories() }; + return std::find_if(trajectories.cbegin(), trajectories.cend(), [](const auto& t){ return !t->isFailure();}) != trajectories.cend(); - }}; +}; - if(!has_solutions(*current->external_state, dir_)){ - ++current->stage; - if(current->stage != children().cend()){ - ROS_DEBUG_STREAM_NAMED("Fallbacks", "Child '" << child->name() << "' failed generating solutions, trying next child: '" - << (*current->stage)->name() << "'"); - pushState(*current); - } - else { - ROS_DEBUG_STREAM_NAMED("Fallbacks", "Failed to extend state with all children, pruning path"); - parent()->pimpl()->onNewFailure(*me(), - dir_ == Interface::FORWARD ? &*current->external_state : nullptr, - dir_ == Interface::BACKWARD ? nullptr : &*current->external_state); - pending_.erase(current); - current_ = pending_.end(); - } +bool FallbacksPrivatePropagator::nextJob() { + assert(current_ != children().end() && !(*current_)->pimpl()->canCompute()); + const auto jobs = pullInterface(dir_); + + 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 (std::next(current_) != children().end()) + ROS_DEBUG_STREAM_NAMED("Fallbacks", "Propagator '" << (*current_)->name() << "' failed, trying next one."); + ++current_; // advance to next child + } else + current_ = children().end(); // indicate that this job is exhausted on all children + } + + if (current_ == children().end()) { // all children processed the job_ + if (job_ != jobs->end()) { + jobs->remove(job_); // we don't need the job in our interface list anymore + job_ = jobs->end(); // indicate that we need to fetch a new job } - else { - ROS_DEBUG_STREAM_NAMED("Fallbacks", "Child '" << child->name() << "' exhausted, but produced solutions before, not invoking further fallbacks"); - pending_.erase(current); - current_ = pending_.end(); - } - // continue processing with next pending state as we didn't runCompute() yet - } -} - -void FallbacksPrivatePropagator::onNewExternalState(Interface::iterator external, bool updated) { - if (updated) { - auto it = std::find_if(pending_.begin(), pending_.end(), - [external](const Job& s) { return s.external_state == external; }); - if (it == pending_.cend()) - return; // already processed - - pending_.update(it); // update sorting pos of this single item - printPending("after update: "); - - // update prio of linked internal states as well - ContainerBasePrivate::copyState(dir_, external, InterfacePtr(), updated); - return; + current_ = children().begin(); // start next job with first child again } - // remember external state for later processing by children. - // children().end() indicates that the states wasn't yet forwarded to any child - pending_.push(Job(external, children().cend())); - printPending("after push: "); -} - -inline void FallbacksPrivatePropagator::printPending(const char* comment) const { - ROSCONSOLE_DEFINE_LOCATION(true, ::ros::console::levels::Debug, ROSCONSOLE_NAME_PREFIX ".Fallbacks"); - if (ROS_UNLIKELY(__rosconsole_define_location__enabled)) { - std::cout << name() << ": " << comment; - std::for_each(pending_.begin(), pending_.end(), [](const auto& e) { std::cout << e.external_state->priority() << " "; }); - std::cout << std::endl; + // pick next job if needed and possible + if (job_ == jobs->end()) { // need to pick next job + if (!jobs->empty() && jobs->front()->priority().enabled()) + job_ = jobs->begin(); + else + return false; // no more jobs available } + + // When arriving here, we have a valid job_ and a current_ child to feed it. Let's do that. + copyState(dir_, job_, (*current_)->pimpl()->pullInterface(dir_), false); + return true; } From e296bd7aed81238aa3040248fa69271121f35aee Mon Sep 17 00:00:00 2001 From: Robert Haschke Date: Thu, 25 Nov 2021 00:35:56 +0100 Subject: [PATCH 24/34] Simplify: job_has_solutions_ Just set a flag when we received a full solution --- .../moveit/task_constructor/container_p.h | 5 ++- core/src/container.cpp | 31 ++++++++++--------- 2 files changed, 21 insertions(+), 15 deletions(-) diff --git a/core/include/moveit/task_constructor/container_p.h b/core/include/moveit/task_constructor/container_p.h index 5858b4f9..6f9dd589 100644 --- a/core/include/moveit/task_constructor/container_p.h +++ b/core/include/moveit/task_constructor/container_p.h @@ -261,8 +261,11 @@ public: // virtual method specific to each variant /// Advance to the next job, assuming that the current child is exhausted on the current job. virtual bool nextJob() { return false; } + /// Reset data structures + virtual void reset(); container_type::const_iterator current_; // currently active child generator + bool job_has_solutions_; // flag indicating whether the current job generated solutions }; PIMPL_FUNCTIONS(Fallbacks) @@ -278,7 +281,7 @@ struct FallbacksPrivatePropagator : FallbacksPrivate { FallbacksPrivatePropagator(FallbacksPrivate&& old); bool nextJob() override; - bool jobHasSolutions() const; + void reset() override; Interface::Direction dir_; // propagation direction Interface::iterator job_; // pointer to currently processed external state diff --git a/core/src/container.cpp b/core/src/container.cpp index 9ca40d8b..de14c469 100644 --- a/core/src/container.cpp +++ b/core/src/container.cpp @@ -849,18 +849,18 @@ Fallbacks::Fallbacks(FallbacksPrivate* impl) : ParallelContainerBase(impl) {} void Fallbacks::reset() { ParallelContainerBase::reset(); + pimpl()->reset(); } void Fallbacks::init(const moveit::core::RobotModelConstPtr& robot_model) { ParallelContainerBase::init(robot_model); - auto impl = pimpl(); - impl->current_ = impl->children().begin(); + pimpl()->reset(); } bool Fallbacks::canCompute() const { auto impl = const_cast(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 return impl->nextJob(); // advance to next job @@ -873,6 +873,7 @@ void Fallbacks::compute() { } void Fallbacks::onNewSolution(const SolutionBase& s) { + pimpl()->job_has_solutions_ = true; liftSolution(s); } @@ -901,8 +902,11 @@ FallbacksPrivate::FallbacksPrivate(FallbacksPrivate&& other) : ParallelContainerBasePrivate(static_cast(other.me()), "") { // move contents of other this->ParallelContainerBasePrivate::operator=(std::move(other)); - // (re)initialize +} + +void FallbacksPrivate::reset() { current_ = children().begin(); + job_has_solutions_ = false; } void FallbacksPrivate::initializeExternalInterfaces() { @@ -920,13 +924,13 @@ void FallbacksPrivate::onNewFailure(const Stage& /*child*/, const InterfaceState FallbacksPrivateGenerator::FallbacksPrivateGenerator(FallbacksPrivate&& old) - : FallbacksPrivate(std::move(old)) {} + : FallbacksPrivate(std::move(old)) { reset(); } bool FallbacksPrivateGenerator::nextJob() { assert(current_ != children().end() && !(*current_)->pimpl()->canCompute()); // 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 return false; } @@ -956,28 +960,27 @@ FallbacksPrivatePropagator::FallbacksPrivatePropagator(FallbacksPrivate&& old) default: assert(false); } - job_ = pullInterface(dir_)->end(); // indicate fresh start + FallbacksPrivatePropagator::reset(); } -bool FallbacksPrivatePropagator::jobHasSolutions() const { - const auto& trajectories { dir_ == Interface::FORWARD ? job_->outgoingTrajectories() - : job_->incomingTrajectories() }; - return std::find_if(trajectories.cbegin(), trajectories.cend(), - [](const auto& t){ return !t->isFailure();}) != trajectories.cend(); -}; +void FallbacksPrivatePropagator::reset() { + FallbacksPrivate::reset(); + job_ = pullInterface(dir_)->end(); // indicate fresh start +} bool FallbacksPrivatePropagator::nextJob() { assert(current_ != children().end() && !(*current_)->pimpl()->canCompute()); const auto jobs = pullInterface(dir_); 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()) ROS_DEBUG_STREAM_NAMED("Fallbacks", "Propagator '" << (*current_)->name() << "' failed, trying next one."); ++current_; // advance to next child } else 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 (job_ != jobs->end()) { From 058712991613ebcbffff488356e941d43392c85f Mon Sep 17 00:00:00 2001 From: Robert Haschke Date: Thu, 25 Nov 2021 21:25:11 +0100 Subject: [PATCH 25/34] CI: asan with debug symbols --- .github/workflows/ci.yaml | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/.github/workflows/ci.yaml b/.github/workflows/ci.yaml index c80d6f4e..2d46fd75 100644 --- a/.github/workflows/ci.yaml +++ b/.github/workflows/ci.yaml @@ -32,7 +32,7 @@ jobs: DOCKER_RUN_OPTS: >- -e PRELOAD=libasan.so.5 -e LSAN_OPTIONS="suppressions=$PWD/.github/workflows/lsan.suppressions" - TARGET_CMAKE_ARGS: -DCMAKE_CXX_FLAGS="-fsanitize=address -fno-omit-frame-pointer -O1" + TARGET_CMAKE_ARGS: -DCMAKE_CXX_FLAGS="-fsanitize=address -fno-omit-frame-pointer -O1 -g" env: CATKIN_LINT: true From 4be448641fce0a8ea27ec732a8f1bc73c21888f0 Mon Sep 17 00:00:00 2001 From: Robert Haschke Date: Mon, 15 Nov 2021 19:43:52 +0100 Subject: [PATCH 26/34] Improve debug output - printChildrenInterfaces(): fix/add usage - printPendingPairs(): full colorization according to status --- .../include/moveit/task_constructor/storage.h | 12 ++++ core/src/container.cpp | 61 +++++++++++-------- core/src/stage.cpp | 43 +++++++------ core/src/storage.cpp | 15 ++--- 4 files changed, 78 insertions(+), 53 deletions(-) diff --git a/core/include/moveit/task_constructor/storage.h b/core/include/moveit/task_constructor/storage.h index b7a6d023..53313971 100644 --- a/core/include/moveit/task_constructor/storage.h +++ b/core/include/moveit/task_constructor/storage.h @@ -85,6 +85,8 @@ public: PRUNED, // state is disabled because a required connected state failed FAILED, // state that failed, causing the whole partial solution to be disabled }; + static const char* STATUS_COLOR[]; + /** InterfaceStates are ordered according to two values: * Depth of interlinked trajectory parts and accumulated trajectory costs along that path. * Preference ordering considers high-depth first and within same depth, minimal cost paths. @@ -221,6 +223,16 @@ private: std::ostream& operator<<(std::ostream& os, const InterfaceState::Priority& prio); std::ostream& operator<<(std::ostream& os, const Interface& interface); +/// Find index of the iterator in the container. Counting starts at 1. Zero corresponds to not found. +template +size_t getIndex(const T& container, typename T::const_iterator search) { + size_t index = 1; + for (typename T::const_iterator it = container.begin(), end = container.end(); it != end; ++it, ++index) + if (it == search) + return index; + return 0; +} + class CostTerm; class StagePrivate; class ContainerBasePrivate; diff --git a/core/src/container.cpp b/core/src/container.cpp index de14c469..1cc4a617 100644 --- a/core/src/container.cpp +++ b/core/src/container.cpp @@ -53,6 +53,35 @@ using namespace std::placeholders; namespace moveit { namespace task_constructor { +// for debugging of how children interfaces evolve over time +static void printChildrenInterfaces(const ContainerBasePrivate& container, bool success, const Stage& creator, + std::ostream& os = std::cerr) { + auto printPendingPairs = [](const StagePrivate* impl, std::ostream& os) -> std::ostream& { + if (auto conn = dynamic_cast(impl)) + conn->printPendingPairs(os); + return os; + }; + static unsigned int id = 0; + const unsigned int width = 10; // indentation of name + os << std::endl << (success ? '+' : '-') << ' ' << creator.name() << ' '; + if (success) + os << ++id << ' '; + printPendingPairs(creator.pimpl(), os) << std::endl; + + for (const auto& child : container.children()) { + auto cimpl = child->pimpl(); + os << std::setw(width) << std::left << child->name(); + if (!cimpl->starts() && !cimpl->ends()) + os << "↕ " << std::endl; + if (cimpl->starts()) + os << "↓ " << *child->pimpl()->starts() << std::endl; + if (cimpl->starts() && cimpl->ends()) + os << std::setw(width) << " "; + if (cimpl->ends()) + os << "↑ " << *child->pimpl()->ends() << std::endl; + } +} + ContainerBasePrivate::ContainerBasePrivate(ContainerBase* me, const std::string& name) : StagePrivate(me, name) , required_interface_(UNKNOWN) @@ -414,31 +443,6 @@ std::ostream& operator<<(std::ostream& os, const ContainerBase& container) { return os; } -// for debugging of how children interfaces evolve over time -static void printChildrenInterfaces(const ContainerBase& container, bool success, const Stage& creator, - std::ostream& os = std::cerr) { - static unsigned int id = 0; - const unsigned int width = 10; // indentation of name - os << std::endl << (success ? '+' : '-') << ' ' << creator.name() << ' '; - if (success) - os << ++id << ' '; - if (const Connecting* conn = dynamic_cast(&creator)) - conn->pimpl()->printPendingPairs(os); - os << std::endl; - - for (const auto& child : container.pimpl()->children()) { - auto cimpl = child->pimpl(); - os << std::setw(width) << std::left << child->name(); - if (!cimpl->starts() && !cimpl->ends()) - os << "↕ " << std::endl; - if (cimpl->starts()) - os << "↓ " << *child->pimpl()->starts() << std::endl; - if (cimpl->starts() && cimpl->ends()) - os << std::setw(width) << " "; - if (cimpl->ends()) - os << "↑ " << *child->pimpl()->ends() << std::endl; - } -} /** Collect all partial solution sequences originating from start into given direction */ template struct SolutionCollector @@ -537,7 +541,7 @@ void SerialContainer::onNewSolution(const SolutionBase& current) { } } } - // printChildrenInterfaces(*this, true, *current.creator()); + // printChildrenInterfaces(*this->pimpl(), true, *current.creator()); // finally, store + announce new solutions to external interface for (const auto& solution : sorted) @@ -874,6 +878,7 @@ void Fallbacks::compute() { void Fallbacks::onNewSolution(const SolutionBase& s) { pimpl()->job_has_solutions_ = true; + // printChildrenInterfaces(*this->pimpl(), true, *s.creator()); liftSolution(s); } @@ -915,11 +920,13 @@ void FallbacksPrivate::initializeExternalInterfaces() { static_cast(me())->replaceImpl(); } -void FallbacksPrivate::onNewFailure(const Stage& /*child*/, const InterfaceState* /*from*/, const InterfaceState* /*to*/) { +void FallbacksPrivate::onNewFailure(const Stage& child, const InterfaceState* /*from*/, const InterfaceState* /*to*/) { // This override is deliberately empty. // The method prunes solution paths when a child failed to find a valid solution for it, // but in Fallbacks the next child might still yield a successful solution // Thus pruning must only occur once the last child is exhausted (inside computePropagate) + // printChildrenInterfaces(*this, false, child); + (void)child; } diff --git a/core/src/stage.cpp b/core/src/stage.cpp index efa730dc..bdde7f80 100644 --- a/core/src/stage.cpp +++ b/core/src/stage.cpp @@ -759,9 +759,23 @@ void ConnectingPrivate::newState(Interface::iterator it, bool updated) { } } } - // std::cerr << name_ << ": "; - // printPendingPairs(std::cerr); - // std::cerr << std::endl; +#if 0 + auto& os = std::cerr; + for (auto d : { Interface::FORWARD, Interface::BACKWARD }) { + bool fw = (d == Interface::FORWARD); + if (fw) + os << " " << std::setw(10) << std::left << this->name(); + else + os << std::setw(12) << std::right << ""; + if (dir != d) + os << (updated ? " !" : " +"); + else + os << " "; + os << (fw ? "↓ " : "↑ ") << this->pullInterface(d) << ": " << *this->pullInterface(d) << std::endl; + } + os << std::setw(15) << " "; + printPendingPairs(os) << std::endl; +#endif } // Check whether there are pending feasible states that could connect to source. @@ -802,24 +816,15 @@ void ConnectingPrivate::compute() { } std::ostream& ConnectingPrivate::printPendingPairs(std::ostream& os) const { - static const char* red = "\033[31m"; - static const char* reset = "\033[m"; + const char* reset = InterfaceState::STATUS_COLOR[3]; for (const auto& candidate : pending) { - if (!candidate.first->priority().enabled() || !candidate.second->priority().enabled()) - os << " " << red; - // find indeces of InterfaceState pointers in start/end Interfaces - unsigned int first = 0, second = 0; - std::find_if(starts()->begin(), starts()->end(), [&](const InterfaceState* s) { - ++first; - return &*candidate.first == s; - }); - std::find_if(ends()->begin(), ends()->end(), [&](const InterfaceState* s) { - ++second; - return &*candidate.second == s; - }); - os << first << ":" << second << " "; + size_t first = getIndex(*starts(), candidate.first); + size_t second = getIndex(*ends(), candidate.second); + os << InterfaceState::STATUS_COLOR[candidate.first->priority().status()] << first << reset << ":" + << InterfaceState::STATUS_COLOR[candidate.second->priority().status()] << second << reset << " "; } - os << reset; + if (pending.empty()) + os << "---"; return os; } diff --git a/core/src/storage.cpp b/core/src/storage.cpp index a98ffd3c..08185225 100644 --- a/core/src/storage.cpp +++ b/core/src/storage.cpp @@ -144,15 +144,16 @@ std::ostream& operator<<(std::ostream& os, const Interface& interface) { os << istate->priority() << " "; return os; } +const char* InterfaceState::STATUS_COLOR[] = { + "\033[32m", // ENABLED - green + "\033[33m", // PRUNED - yellow + "\033[31m", // FAILED - red + "\033[m" // reset +}; std::ostream& operator<<(std::ostream& os, const InterfaceState::Priority& prio) { // maps InterfaceState::Status values to output (color-changing) prefix - static const char* prefix[] = { - "\033[32me:", // ENABLED - green - "\033[33md:", // PRUNED - yellow - "\033[31mf:", // FAILED - red - }; - static const char* color_reset = "\033[m"; - os << prefix[prio.status()] << prio.depth() << ":" << prio.cost() << color_reset; + os << InterfaceState::STATUS_COLOR[prio.status()] << prio.depth() << ":" << prio.cost() + << InterfaceState::STATUS_COLOR[3]; return os; } From b2056745a81bd1f7da48a0a79d432742aa9d8776 Mon Sep 17 00:00:00 2001 From: Robert Haschke Date: Fri, 26 Nov 2021 09:12:39 +0100 Subject: [PATCH 27/34] Generalize connectStageInsideFallbacks Let's consider the following simple situation, where generators produce solutions in the given order. GEN 1 3 Fallbacks |X GEN 2 4 When passing state 4 to the Fallbacks' connector, it forms pending pairs with both 1 and 3. Thus, the container needs to check whether 1-4 or 3-4 was processed when receiving a success or failure, to correctly forward the failed one to the next child. --- core/test/test_fallback.cpp | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/core/test/test_fallback.cpp b/core/test/test_fallback.cpp index 092d7890..87f9368f 100644 --- a/core/test/test_fallback.cpp +++ b/core/test/test_fallback.cpp @@ -161,16 +161,16 @@ TEST_F(FallbacksFixturePropagate, activeChildReset) { using FallbacksFixtureConnect = TaskTestBase; -TEST_F(FallbacksFixtureConnect, DISABLED_connectStageInsideFallbacks) { +TEST_F(FallbacksFixtureConnect, connectStageInsideFallbacks) { t.add(std::make_unique(PredefinedCosts({ 1.0, 2.0 }))); auto fallbacks = std::make_unique("Fallbacks"); - fallbacks->add(std::make_unique(PredefinedCosts::constant(0.0))); + fallbacks->add(std::make_unique(PredefinedCosts({ 0.0, 0.0, INF, 0.0 }))); fallbacks->add(std::make_unique(PredefinedCosts::constant(100.0))); t.add(std::move(fallbacks)); t.add(std::make_unique(PredefinedCosts({ 10.0, 20.0 }))); EXPECT_TRUE(t.plan()); - EXPECT_COSTS(t.solutions(), testing::ElementsAre(11, 12, 21, 22)); + EXPECT_COSTS(t.solutions(), testing::ElementsAre(11, 12, 22, 121)); } From 442d39ad3ee7805168bbc29c2f790a6dc8a90800 Mon Sep 17 00:00:00 2001 From: Robert Haschke Date: Mon, 29 Nov 2021 15:04:40 +0100 Subject: [PATCH 28/34] Improve comments --- core/include/moveit/task_constructor/container_p.h | 10 +++++----- 1 file changed, 5 insertions(+), 5 deletions(-) diff --git a/core/include/moveit/task_constructor/container_p.h b/core/include/moveit/task_constructor/container_p.h index 6f9dd589..391cbb15 100644 --- a/core/include/moveit/task_constructor/container_p.h +++ b/core/include/moveit/task_constructor/container_p.h @@ -243,22 +243,22 @@ PIMPL_FUNCTIONS(ParallelContainerBase) /* The Fallbacks container needs to implement different behaviour based on its interface. * Thus, we implement 3 different classes: for Generator, Propagator, and Connect-like interfaces. - * FallbacksPrivate is the common base class for all of them, defining the common API to be used - * by the Fallbacks container. + * FallbacksPrivate is the common base class for all of them, defining the common API + * to be used by the Fallbacks container. * The actual interface-specific class is instantiated in initializeExternalInterfaces() * resp. Fallbacks::replaceImpl() when the actual interface is known. - * The key difference between the 3 variants is how the advance to the next job. */ + * The key difference between the 3 variants is how they advance to the next job. */ class FallbacksPrivate : public ParallelContainerBasePrivate { public: FallbacksPrivate(Fallbacks* me, const std::string& name); FallbacksPrivate(FallbacksPrivate&& other); - // method overrides common to 3 variants + // methods common to all variants void initializeExternalInterfaces() final; void onNewFailure(const Stage& child, const InterfaceState* from, const InterfaceState* to) override; - // virtual method 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. virtual bool nextJob() { return false; } /// Reset data structures From b2c116edabe6a850bb9629fa51466600f0d1f450 Mon Sep 17 00:00:00 2001 From: Robert Haschke Date: Mon, 29 Nov 2021 15:10:14 +0100 Subject: [PATCH 29/34] reset(new Interface()) -> std::make_shared() --- core/src/container.cpp | 16 ++++++++-------- core/src/stage.cpp | 12 ++++++------ 2 files changed, 14 insertions(+), 14 deletions(-) diff --git a/core/src/container.cpp b/core/src/container.cpp index 1cc4a617..2a360b2b 100644 --- a/core/src/container.cpp +++ b/core/src/container.cpp @@ -606,9 +606,9 @@ void SerialContainerPrivate::resolveInterface(InterfaceFlags expected) { validateInterface(*first.pimpl(), expected); // connect first child's (start) pull interface if (const InterfacePtr& target = first.pimpl()->starts()) - starts_.reset(new Interface([this, target](Interface::iterator it, bool updated) { + starts_ = std::make_shared([this, target](Interface::iterator it, bool updated) { this->copyState(it, target, updated); - })); + }); } catch (InitStageException& e) { exceptions.append(e); } @@ -632,9 +632,9 @@ void SerialContainerPrivate::resolveInterface(InterfaceFlags expected) { validateInterface(*last.pimpl(), expected); // connect last child's (end) pull interface if (const InterfacePtr& target = last.pimpl()->ends()) - ends_.reset(new Interface([this, target](Interface::iterator it, bool updated) { + ends_ = std::make_shared([this, target](Interface::iterator it, bool updated) { this->copyState(it, target, updated); - })); + }); } catch (InitStageException& e) { exceptions.append(e); } @@ -733,13 +733,13 @@ void ParallelContainerBasePrivate::resolveInterface(InterfaceFlags expected) { void ParallelContainerBasePrivate::initializeExternalInterfaces() { // States received by the container need to be copied to all children's pull interfaces. if (requiredInterface() & READS_START) - starts().reset(new Interface([this](Interface::iterator external, bool updated) { + starts() = std::make_shared([this](Interface::iterator external, bool updated) { this->propagateStateToChildren(external, updated); - })); + }); if (requiredInterface() & READS_END) - ends().reset(new Interface([this](Interface::iterator external, bool updated) { + ends() = std::make_shared([this](Interface::iterator external, bool updated) { this->propagateStateToChildren(external, updated); - })); + }); } void ParallelContainerBasePrivate::validateInterfaces(const StagePrivate& child, InterfaceFlags& external, diff --git a/core/src/stage.cpp b/core/src/stage.cpp index bdde7f80..7ea83c54 100644 --- a/core/src/stage.cpp +++ b/core/src/stage.cpp @@ -510,14 +510,14 @@ void PropagatingEitherWayPrivate::initInterface(PropagatingEitherWay::Direction case PropagatingEitherWay::FORWARD: required_interface_ = PROPAGATE_FORWARDS; if (!starts_) // keep existing interface if possible - starts_.reset(new Interface()); + starts_ = std::make_shared(); ends_.reset(); return; case PropagatingEitherWay::BACKWARD: required_interface_ = PROPAGATE_BACKWARDS; starts_.reset(); if (!ends_) // keep existing interface if possible - ends_.reset(new Interface()); + ends_ = std::make_shared(); return; case PropagatingEitherWay::AUTO: required_interface_ = UNKNOWN; @@ -715,10 +715,10 @@ void MonitoringGeneratorPrivate::solutionCB(const SolutionBase& s) { } ConnectingPrivate::ConnectingPrivate(Connecting* me, const std::string& name) : ComputeBasePrivate(me, name) { - starts_.reset(new Interface(std::bind(&ConnectingPrivate::newState, this, std::placeholders::_1, - std::placeholders::_2))); - ends_.reset(new Interface(std::bind(&ConnectingPrivate::newState, this, std::placeholders::_1, - std::placeholders::_2))); + starts_ = std::make_shared(std::bind(&ConnectingPrivate::newState, this, + std::placeholders::_1, std::placeholders::_2)); + ends_ = std::make_shared( + std::bind(&ConnectingPrivate::newState, this, std::placeholders::_1, std::placeholders::_2)); } InterfaceFlags ConnectingPrivate::requiredInterface() const { From 7af3d8ebd7c5f66f8ed77920af4ed2901d102433 Mon Sep 17 00:00:00 2001 From: Robert Haschke Date: Wed, 5 Jan 2022 11:40:26 +0100 Subject: [PATCH 30/34] Improve readability --- core/src/container.cpp | 7 +++---- 1 file changed, 3 insertions(+), 4 deletions(-) diff --git a/core/src/container.cpp b/core/src/container.cpp index 2a360b2b..d678ad7d 100644 --- a/core/src/container.cpp +++ b/core/src/container.cpp @@ -260,7 +260,7 @@ void ContainerBasePrivate::copyState(Interface::iterator external, const Interfa auto internals{ externalToInternalMap().equal_range(&*external) }; for (auto& i = internals.first; i != internals.second; ++i) { // TODO: Not only update status, but full priority! - setStatus(i->second, external->priority().status()); + setStatus(i->get(), external->priority().status()); } return; } @@ -690,9 +690,8 @@ bool SerialContainer::canCompute() const { void SerialContainer::compute() { for (const auto& stage : pimpl()->children()) { - if (!stage->pimpl()->canCompute()) - continue; - stage->pimpl()->runCompute(); + if (stage->pimpl()->canCompute()) + stage->pimpl()->runCompute(); } } From b82b70ed642afd932a40b49424eb979634babcb1 Mon Sep 17 00:00:00 2001 From: Robert Haschke Date: Wed, 5 Jan 2022 11:25:06 +0100 Subject: [PATCH 31/34] FallbacksPrivate::nextChild() ... factoring out functionality shared between FallbacksPrivateGenerator and FallbacksPrivatePropagator to switch to next child in nextJob(). --- .../moveit/task_constructor/container_p.h | 1 + core/src/container.cpp | 21 ++++++++++--------- 2 files changed, 12 insertions(+), 10 deletions(-) diff --git a/core/include/moveit/task_constructor/container_p.h b/core/include/moveit/task_constructor/container_p.h index 391cbb15..a98d1b53 100644 --- a/core/include/moveit/task_constructor/container_p.h +++ b/core/include/moveit/task_constructor/container_p.h @@ -257,6 +257,7 @@ public: // methods common to all variants void initializeExternalInterfaces() final; void onNewFailure(const Stage& child, const InterfaceState* from, const InterfaceState* to) override; + void nextChild(); /// << Advance to next child // virtual methods specific to each variant /// Advance to the next job, assuming that the current child is exhausted on the current job. diff --git a/core/src/container.cpp b/core/src/container.cpp index d678ad7d..fdf9b320 100644 --- a/core/src/container.cpp +++ b/core/src/container.cpp @@ -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 (parent() && trajectories(*s).empty()) { + // TODO: This was coded with SerialContainer in mind. Not sure, it works for ParallelContainers auto external{ internalToExternalMap().find(s) }; 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 @@ -928,6 +929,11 @@ void FallbacksPrivate::onNewFailure(const Stage& child, const InterfaceState* /* (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) : FallbacksPrivate(std::move(old)) { reset(); } @@ -941,11 +947,8 @@ bool FallbacksPrivateGenerator::nextJob() { return false; } - do { - if (std::next(current_) != children().end()) - ROS_DEBUG_STREAM_NAMED("Fallbacks", "Generator '" << (*current_)->name() << "' failed, trying next one."); - ++current_; // advance to next child - } while (current_ != children().end() && !(*current_)->pimpl()->canCompute()); + do { nextChild(); } + while (current_ != children().end() && !(*current_)->pimpl()->canCompute()); // return value shall indicate current_->canCompute() return current_ != children().end(); @@ -979,11 +982,9 @@ bool FallbacksPrivatePropagator::nextJob() { const auto jobs = pullInterface(dir_); 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 (std::next(current_) != children().end()) - ROS_DEBUG_STREAM_NAMED("Fallbacks", "Propagator '" << (*current_)->name() << "' failed, trying next one."); - ++current_; // advance to next child - } else + if (!job_has_solutions_) // job didn't produce solutions -> feed to next child + nextChild(); + else current_ = children().end(); // indicate that this job is exhausted on all children } job_has_solutions_ = false; From 986d3c876620bde714076c8659bb17483086061f Mon Sep 17 00:00:00 2001 From: Robert Haschke Date: Wed, 5 Jan 2022 11:57:31 +0100 Subject: [PATCH 32/34] FallbacksPrivateCommon: shared between Generator + Propagator --- .../moveit/task_constructor/container.h | 7 ++- .../moveit/task_constructor/container_p.h | 38 ++++++++---- core/src/container.cpp | 62 ++++++++++--------- 3 files changed, 65 insertions(+), 42 deletions(-) diff --git a/core/include/moveit/task_constructor/container.h b/core/include/moveit/task_constructor/container.h index 6dcf5bfa..cea4a418 100644 --- a/core/include/moveit/task_constructor/container.h +++ b/core/include/moveit/task_constructor/container.h @@ -167,12 +167,15 @@ public: 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; + +private: + // not needed, we directly use corresponding virtual methods of FallbacksPrivate + bool canCompute() const final { return false; } + void compute() final {} }; class MergerPrivate; diff --git a/core/include/moveit/task_constructor/container_p.h b/core/include/moveit/task_constructor/container_p.h index a98d1b53..a2265ec9 100644 --- a/core/include/moveit/task_constructor/container_p.h +++ b/core/include/moveit/task_constructor/container_p.h @@ -254,38 +254,52 @@ public: FallbacksPrivate(Fallbacks* me, const std::string& name); FallbacksPrivate(FallbacksPrivate&& other); - // methods common to all variants void initializeExternalInterfaces() final; void onNewFailure(const Stage& child, const InterfaceState* from, const InterfaceState* to) override; - void nextChild(); /// << Advance to next child // virtual methods specific to each variant - /// Advance to the next job, assuming that the current child is exhausted on the current job. - virtual bool nextJob() { return false; } - /// Reset data structures - virtual void reset(); - - container_type::const_iterator current_; // currently active child generator - bool job_has_solutions_; // flag indicating whether the current job generated solutions + virtual void onNewSolution(const SolutionBase& s); + virtual void reset() {} }; PIMPL_FUNCTIONS(Fallbacks) +/* Class shared between FallbacksPrivateGenerator and FallbacksPrivatePropagator, + which both have the notion of a currently active child stage */ +class FallbacksPrivateCommon : public FallbacksPrivate +{ +public: + FallbacksPrivateCommon(FallbacksPrivate&& other) : FallbacksPrivate(std::move(other)) {} + + /// Advance to next child + inline void nextChild(); + /// Advance to the next job, assuming that the current child is exhausted on the current job. + virtual bool nextJob() = 0; + + void reset() override; + bool canCompute() const override; + void compute() override; + + container_type::const_iterator current_; // currently active child +}; + /// Fallbacks implementation for GENERATOR interface -struct FallbacksPrivateGenerator : FallbacksPrivate +struct FallbacksPrivateGenerator : FallbacksPrivateCommon { FallbacksPrivateGenerator(FallbacksPrivate&& old); bool nextJob() override; }; /// Fallbacks implementation for FORWARD or BACKWARD interface -struct FallbacksPrivatePropagator : FallbacksPrivate +struct FallbacksPrivatePropagator : FallbacksPrivateCommon { FallbacksPrivatePropagator(FallbacksPrivate&& old); - bool nextJob() override; void reset() override; + void onNewSolution(const SolutionBase& s) override; + bool nextJob() override; Interface::Direction dir_; // propagation direction Interface::iterator job_; // pointer to currently processed external state + bool job_has_solutions_; // flag indicating whether the current job generated solutions }; class WrapperBasePrivate : public ParallelContainerBasePrivate diff --git a/core/src/container.cpp b/core/src/container.cpp index fdf9b320..4f4eb7e1 100644 --- a/core/src/container.cpp +++ b/core/src/container.cpp @@ -861,25 +861,8 @@ void Fallbacks::init(const moveit::core::RobotModelConstPtr& robot_model) { pimpl()->reset(); } -bool Fallbacks::canCompute() const { - auto impl = const_cast(pimpl()); - - while(impl->current_ != impl->children().end() && // not completely exhausted - !(*impl->current_)->pimpl()->canCompute()) // but current child cannot compute - return impl->nextJob(); // advance to next job - - // return value: current child is well defined and thus can compute? - return impl->current_ != impl->children().end(); -} - -void Fallbacks::compute() { - (*pimpl()->current_)->pimpl()->runCompute(); -} - void Fallbacks::onNewSolution(const SolutionBase& s) { - pimpl()->job_has_solutions_ = true; - // printChildrenInterfaces(*this->pimpl(), true, *s.creator()); - liftSolution(s); + pimpl()->onNewSolution(s); } inline void Fallbacks::replaceImpl() { @@ -909,17 +892,17 @@ FallbacksPrivate::FallbacksPrivate(FallbacksPrivate&& other) this->ParallelContainerBasePrivate::operator=(std::move(other)); } -void FallbacksPrivate::reset() { - current_ = children().begin(); - job_has_solutions_ = false; -} - void FallbacksPrivate::initializeExternalInterfaces() { // Here we know the final interface of the container (and all its children) // Thus replace, this pimpl() with a new interface-specific one: static_cast(me())->replaceImpl(); } +void FallbacksPrivate::onNewSolution(const SolutionBase& s) { + // printChildrenInterfaces(*this, true, *s.creator()); + static_cast(me())->liftSolution(s); +} + void FallbacksPrivate::onNewFailure(const Stage& child, const InterfaceState* /*from*/, const InterfaceState* /*to*/) { // This override is deliberately empty. // The method prunes solution paths when a child failed to find a valid solution for it, @@ -929,20 +912,37 @@ void FallbacksPrivate::onNewFailure(const Stage& child, const InterfaceState* /* (void)child; } -void FallbacksPrivate::nextChild() { +void FallbacksPrivateCommon::reset() { + current_ = children().begin(); +} + +bool FallbacksPrivateCommon::canCompute() const { + while(current_ != children().end() && // not completely exhausted + !(*current_)->pimpl()->canCompute()) // but current child cannot compute + return const_cast(this)->nextJob(); // advance to next job + + // return value: current child is well defined and thus can compute? + return current_ != children().end(); +} + +void FallbacksPrivateCommon::compute() { + (*current_)->pimpl()->runCompute(); +} + +inline void FallbacksPrivateCommon::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) - : FallbacksPrivate(std::move(old)) { reset(); } + : FallbacksPrivateCommon(std::move(old)) { FallbacksPrivateCommon::reset(); } bool FallbacksPrivateGenerator::nextJob() { assert(current_ != children().end() && !(*current_)->pimpl()->canCompute()); // don't advance to next child when we already produced solutions - if (job_has_solutions_) { + if (!solutions_.empty()) { current_ = children().end(); // indicate that we are exhausted return false; } @@ -956,7 +956,7 @@ bool FallbacksPrivateGenerator::nextJob() { FallbacksPrivatePropagator::FallbacksPrivatePropagator(FallbacksPrivate&& old) - : FallbacksPrivate(std::move(old)) { + : FallbacksPrivateCommon(std::move(old)) { switch (requiredInterface()) { case PROPAGATE_FORWARDS: dir_ = Interface::FORWARD; @@ -973,8 +973,14 @@ FallbacksPrivatePropagator::FallbacksPrivatePropagator(FallbacksPrivate&& old) } void FallbacksPrivatePropagator::reset() { - FallbacksPrivate::reset(); + FallbacksPrivateCommon::reset(); job_ = pullInterface(dir_)->end(); // indicate fresh start + job_has_solutions_ = false; +} + +void FallbacksPrivatePropagator::onNewSolution(const SolutionBase& s) { + job_has_solutions_ = true; + FallbacksPrivateCommon::onNewSolution(s); } bool FallbacksPrivatePropagator::nextJob() { From 7a04a9f6037eeac40170558ec68aa094fa8c69c3 Mon Sep 17 00:00:00 2001 From: Robert Haschke Date: Wed, 5 Jan 2022 14:06:55 +0100 Subject: [PATCH 33/34] ParallelContainerBasePrivate::propagateStateTo*All*Children rename method to emphasize that state updates are propagated to all children --- core/include/moveit/task_constructor/container_p.h | 2 +- core/src/container.cpp | 6 +++--- 2 files changed, 4 insertions(+), 4 deletions(-) diff --git a/core/include/moveit/task_constructor/container_p.h b/core/include/moveit/task_constructor/container_p.h index a2265ec9..c61195cf 100644 --- a/core/include/moveit/task_constructor/container_p.h +++ b/core/include/moveit/task_constructor/container_p.h @@ -233,7 +233,7 @@ protected: /// callback for new externally received states template - void propagateStateToChildren(Interface::iterator external, bool updated); + void propagateStateToAllChildren(Interface::iterator external, bool updated); private: // override for custom behavior on received interface states diff --git a/core/src/container.cpp b/core/src/container.cpp index 4f4eb7e1..54c10508 100644 --- a/core/src/container.cpp +++ b/core/src/container.cpp @@ -734,11 +734,11 @@ void ParallelContainerBasePrivate::initializeExternalInterfaces() { // States received by the container need to be copied to all children's pull interfaces. if (requiredInterface() & READS_START) starts() = std::make_shared([this](Interface::iterator external, bool updated) { - this->propagateStateToChildren(external, updated); + this->propagateStateToAllChildren(external, updated); }); if (requiredInterface() & READS_END) ends() = std::make_shared([this](Interface::iterator external, bool updated) { - this->propagateStateToChildren(external, updated); + this->propagateStateToAllChildren(external, updated); }); } @@ -774,7 +774,7 @@ void ParallelContainerBasePrivate::validateConnectivity() const { } template -void ParallelContainerBasePrivate::propagateStateToChildren(Interface::iterator external, bool updated) { +void ParallelContainerBasePrivate::propagateStateToAllChildren(Interface::iterator external, bool updated) { for (const Stage::pointer& stage : children()) copyState(external, stage->pimpl()->pullInterface(dir), updated); } From 4cc1f567d60f01483f49ff1a8cf6e90aadf32a1e Mon Sep 17 00:00:00 2001 From: Robert Haschke Date: Wed, 5 Jan 2022 15:38:05 +0100 Subject: [PATCH 34/34] FallbacksPrivateConnect Implement Fallbacks behavior for children of type Connecting. All other connect-like children are currently infeasible to handle, because we cannot forward a single job, i.e. a pair (from, to) to the next child, but only individual states. However, passing states, will cause creation of undesired state pairs as jobs in subsequent children. --- .../moveit/task_constructor/container_p.h | 15 +++++ .../include/moveit/task_constructor/stage_p.h | 1 + core/src/container.cpp | 65 ++++++++++++++++++- 3 files changed, 80 insertions(+), 1 deletion(-) diff --git a/core/include/moveit/task_constructor/container_p.h b/core/include/moveit/task_constructor/container_p.h index c61195cf..27594c50 100644 --- a/core/include/moveit/task_constructor/container_p.h +++ b/core/include/moveit/task_constructor/container_p.h @@ -302,6 +302,21 @@ struct FallbacksPrivatePropagator : FallbacksPrivateCommon bool job_has_solutions_; // flag indicating whether the current job generated solutions }; +/// Fallbacks implementation for CONNECT interface +struct FallbacksPrivateConnect : FallbacksPrivate +{ + FallbacksPrivateConnect(FallbacksPrivate&& old); + void reset() override; + bool canCompute() const override; + void compute() override; + void onNewFailure(const Stage& child, const InterfaceState* from, const InterfaceState* to) override; + + template + void propagateStateUpdate(Interface::iterator external, bool updated); + + mutable container_type::const_iterator active_; // child picked for compute() +}; + class WrapperBasePrivate : public ParallelContainerBasePrivate { friend class WrapperBase; diff --git a/core/include/moveit/task_constructor/stage_p.h b/core/include/moveit/task_constructor/stage_p.h index 1ce91754..63b4370f 100644 --- a/core/include/moveit/task_constructor/stage_p.h +++ b/core/include/moveit/task_constructor/stage_p.h @@ -301,6 +301,7 @@ PIMPL_FUNCTIONS(MonitoringGenerator) class ConnectingPrivate : public ComputeBasePrivate { friend class Connecting; + friend struct FallbacksPrivateConnect; public: struct StatePair : std::pair diff --git a/core/src/container.cpp b/core/src/container.cpp index 54c10508..6b7fe577 100644 --- a/core/src/container.cpp +++ b/core/src/container.cpp @@ -876,7 +876,11 @@ inline void Fallbacks::replaceImpl() { impl = new FallbacksPrivatePropagator(std::move(*impl)); break; case CONNECT: - throw std::runtime_error("Not yet implemented"); + // For now, we only support Connecting children + for (const auto& child : impl->children()) + if (!dynamic_cast(child.get())) + throw std::runtime_error("CONNECT-like interface is only supported for Connecting children"); + impl = new FallbacksPrivateConnect(std::move(*impl)); break; } delete pimpl_; @@ -1017,6 +1021,65 @@ bool FallbacksPrivatePropagator::nextJob() { } +FallbacksPrivateConnect::FallbacksPrivateConnect(FallbacksPrivate&& old) + : FallbacksPrivate(std::move(old)) { + starts_ = std::make_shared( + std::bind(&FallbacksPrivateConnect::propagateStateUpdate, this, std::placeholders::_1, std::placeholders::_2)); + ends_ = std::make_shared( + std::bind(&FallbacksPrivateConnect::propagateStateUpdate, this, std::placeholders::_1, std::placeholders::_2)); + + FallbacksPrivateConnect::reset(); +} + +void FallbacksPrivateConnect::reset() { + active_ = children().end(); +} + +template +void FallbacksPrivateConnect::propagateStateUpdate(Interface::iterator external, bool updated) { + copyState(external, children().front()->pimpl()->pullInterface(dir), updated); + // TODO: propagate updates to other children as well +} + +bool FallbacksPrivateConnect::canCompute() const { + for (auto it=children().begin(), end=children().end(); it!=end; ++it) + if ((*it)->pimpl()->canCompute()) { + active_ = it; + return true; + } + active_ = children().end(); + return false; +} + +void FallbacksPrivateConnect::compute() { + // Alternatively, we could also compute() all children that canCompute() + assert(active_ != children().end()); + (*active_)->pimpl()->runCompute(); +} + +void FallbacksPrivateConnect::onNewFailure(const Stage& child, const InterfaceState* from, const InterfaceState* to) { + // expect failure to be reported from active child + assert(active_ != children().end() && active_->get() == &child); + // ... thus we can use std::next(active_) to find the next child + auto next = std::next(active_); + + auto findIteratorFor = [](const InterfaceState* state, const Interface& interface) { + auto it = std::find(interface.begin(), interface.end(), state); + assert(it != interface.end()); + return it; + }; + + if (next != children().end()) { // pass job to next child + auto next_con = static_cast(const_cast((*next)->pimpl())); + auto first_con = static_cast(children().front()->pimpl()); + auto fromIt = findIteratorFor(from, *first_con->starts()); + auto toIt = findIteratorFor(to, *first_con->ends()); + next_con->pending.insert(std::make_pair(fromIt, toIt)); + } else // or report failure to parent + parent()->pimpl()->onNewFailure(*me(), from, to); +} + + MergerPrivate::MergerPrivate(Merger* me, const std::string& name) : ParallelContainerBasePrivate(me, name) {} void MergerPrivate::resolveInterface(InterfaceFlags expected) {