From f32e74764542aff2f181b7d9aecbb16ec8079816 Mon Sep 17 00:00:00 2001 From: v4hn Date: Wed, 18 Aug 2021 22:43:55 +0200 Subject: [PATCH 01/70] 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/70] 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/70] 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/70] 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/70] 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/70] 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/70] 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/70] 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/70] 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 66e141db7b5118d638c39bdbbd55a9611b5f2db2 Mon Sep 17 00:00:00 2001 From: Robert Haschke Date: Mon, 15 Nov 2021 19:43:52 +0100 Subject: [PATCH 10/70] Fix printChildrenInterfaces() --- core/include/moveit/task_constructor/storage.h | 2 ++ core/src/container.cpp | 9 ++++++--- core/src/stage.cpp | 9 +++------ core/src/storage.cpp | 15 ++++++++------- 4 files changed, 19 insertions(+), 16 deletions(-) diff --git a/core/include/moveit/task_constructor/storage.h b/core/include/moveit/task_constructor/storage.h index 2d3397bb..e71b3760 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. diff --git a/core/src/container.cpp b/core/src/container.cpp index 422dc92c..2ccc47ea 100644 --- a/core/src/container.cpp +++ b/core/src/container.cpp @@ -53,6 +53,9 @@ using namespace std::placeholders; namespace moveit { namespace task_constructor { +static void printChildrenInterfaces(const ContainerBasePrivate& container, bool success, const Stage& creator, + std::ostream& os = std::cerr); + ContainerBasePrivate::ContainerBasePrivate(ContainerBase* me, const std::string& name) : StagePrivate(me, name) , required_interface_(UNKNOWN) @@ -375,8 +378,8 @@ std::ostream& operator<<(std::ostream& os, const ContainerBase& container) { } // 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 void printChildrenInterfaces(const ContainerBasePrivate& container, bool success, const Stage& creator, + std::ostream& os) { static unsigned int id = 0; const unsigned int width = 10; // indentation of name os << std::endl << (success ? '+' : '-') << ' ' << creator.name() << ' '; @@ -386,7 +389,7 @@ static void printChildrenInterfaces(const ContainerBase& container, bool success conn->pimpl()->printPendingPairs(os); os << std::endl; - for (const auto& child : container.pimpl()->children()) { + for (const auto& child : container.children()) { auto cimpl = child->pimpl(); os << std::setw(width) << std::left << child->name(); if (!cimpl->starts() && !cimpl->ends()) diff --git a/core/src/stage.cpp b/core/src/stage.cpp index 2eaf6a43..09eaf517 100644 --- a/core/src/stage.cpp +++ b/core/src/stage.cpp @@ -775,11 +775,8 @@ 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) { @@ -790,9 +787,9 @@ std::ostream& ConnectingPrivate::printPendingPairs(std::ostream& os) const { ++second; return &*candidate.second == s; }); - os << first << ":" << second << " "; + os << InterfaceState::STATUS_COLOR[candidate.first->priority().status()] << first << reset << ":" + << InterfaceState::STATUS_COLOR[candidate.second->priority().status()] << second << reset << " "; } - os << reset; 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 011e4be059622da7b5ea5b7cc588e394f64fa0f8 Mon Sep 17 00:00:00 2001 From: Robert Haschke Date: Fri, 19 Nov 2021 00:17:28 +0100 Subject: [PATCH 11/70] Add more pruning tests --- core/test/test_cost_queue.cpp | 12 +++------ core/test/test_pruning.cpp | 50 +++++++++++++++++++++++++++++++++++ 2 files changed, 54 insertions(+), 8 deletions(-) diff --git a/core/test/test_cost_queue.cpp b/core/test/test_cost_queue.cpp index 0f4a0433..1d665704 100644 --- a/core/test/test_cost_queue.cpp +++ b/core/test/test_cost_queue.cpp @@ -5,6 +5,10 @@ #include #include +#ifndef TYPED_TEST_SUITE +#define TYPED_TEST_SUITE(SUITE, TYPES) TYPED_TEST_CASE(SUITE, TYPES) +#endif + namespace mtc = moveit::task_constructor; // type-trait functions for OrderedTest @@ -62,11 +66,7 @@ protected: }; // set of template types to test for using TypeInstances = ::testing::Types; -#ifdef TYPED_TEST_SUITE TYPED_TEST_SUITE(ValueOrPointeeLessTest, TypeInstances); -#else -TYPED_TEST_CASE(ValueOrPointeeLessTest, TypeInstances); -#endif TYPED_TEST(ValueOrPointeeLessTest, less) { EXPECT_TRUE(this->less(2, 3)); EXPECT_FALSE(this->less(1, 1)); @@ -105,11 +105,7 @@ protected: SCOPED_TRACE("pushAndValidate(" #cost ", " #__VA_ARGS__ ")"); \ this->pushAndValidate(cost, __VA_ARGS__); \ } -#ifdef TYPED_TEST_SUITE TYPED_TEST_SUITE(OrderedTest, TypeInstances); -#else -TYPED_TEST_CASE(OrderedTest, TypeInstances); -#endif TYPED_TEST(OrderedTest, sorting) { pushAndValidate(2, { 2 }); pushAndValidate(1, { 1, 2 }); diff --git a/core/test/test_pruning.cpp b/core/test/test_pruning.cpp index 5da29cdd..38a57094 100644 --- a/core/test/test_pruning.cpp +++ b/core/test/test_pruning.cpp @@ -8,6 +8,10 @@ #include +#ifndef TYPED_TEST_SUITE +#define TYPED_TEST_SUITE(SUITE, TYPES) TYPED_TEST_CASE(SUITE, TYPES) +#endif + using namespace moveit::task_constructor; using Pruning = TaskTestBase; @@ -40,6 +44,52 @@ TEST_F(Pruning, PruningMultiForward) { EXPECT_EQ((*t.solutions().begin())->cost(), 0u); } +// The 2nd failing FW attempt would prune the path through CON, +// but shouldn't because there exist two more GEN2 solutions +TEST_F(Pruning, NoPruningIfAlternativesExist) { + add(t, new GeneratorMockup(PredefinedCosts({ 0.0 }))); + add(t, new ConnectMockup()); + add(t, new GeneratorMockup(std::list{ 0, 10, 20, 30 }, 2)); + add(t, new ForwardMockup({ INF, INF, 0.0, INF })); + + t.plan(); + + EXPECT_EQ(t.solutions().size(), 1u); +} + +TEST_F(Pruning, ConnectReactivatesPrunedPaths) { + add(t, new BackwardMockup); + add(t, new GeneratorMockup({ 0 })); + add(t, new ConnectMockup()); + // the solution here should re-activate the initially pruned backward path + add(t, new GeneratorMockup({ 0 })); + + EXPECT_TRUE(t.plan()); + EXPECT_EQ(t.solutions().size(), 1u); +} + +// same as before, but wrapping Connect into a container +template +struct PruningContainerTests : public Pruning +{ + void test() { + add(t, new BackwardMockup); + add(t, new GeneratorMockup({ 0 })); + auto c = new T(); + add(*c, new ConnectMockup()); + add(t, c); + add(t, new GeneratorMockup({ 0 })); + + EXPECT_TRUE(t.plan()); + EXPECT_EQ(t.solutions().size(), 1u); + } +}; +using ContainerTypes = ::testing::Types; +TYPED_TEST_SUITE(PruningContainerTests, ContainerTypes); +TYPED_TEST(PruningContainerTests, ConnectReactivatesPrunedPaths) { + this->test(); +} + TEST_F(Pruning, ConnectConnectForward) { add(t, new GeneratorMockup()); auto c1 = add(t, new ConnectMockup({ INF, 0, 0 })); // 1st attempt is a failue From c617e3353d5a62d255502c46a71844e146a13156 Mon Sep 17 00:00:00 2001 From: Robert Haschke Date: Fri, 19 Nov 2021 21:46:09 +0100 Subject: [PATCH 12/70] Disable failing tests --- core/test/test_pruning.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/core/test/test_pruning.cpp b/core/test/test_pruning.cpp index 38a57094..43f0b161 100644 --- a/core/test/test_pruning.cpp +++ b/core/test/test_pruning.cpp @@ -46,7 +46,7 @@ TEST_F(Pruning, PruningMultiForward) { // The 2nd failing FW attempt would prune the path through CON, // but shouldn't because there exist two more GEN2 solutions -TEST_F(Pruning, NoPruningIfAlternativesExist) { +TEST_F(Pruning, DISABLED_NoPruningIfAlternativesExist) { add(t, new GeneratorMockup(PredefinedCosts({ 0.0 }))); add(t, new ConnectMockup()); add(t, new GeneratorMockup(std::list{ 0, 10, 20, 30 }, 2)); @@ -84,7 +84,7 @@ struct PruningContainerTests : public Pruning EXPECT_EQ(t.solutions().size(), 1u); } }; -using ContainerTypes = ::testing::Types; +using ContainerTypes = ::testing::Types; // TODO: fails for Fallbacks! TYPED_TEST_SUITE(PruningContainerTests, ContainerTypes); TYPED_TEST(PruningContainerTests, ConnectReactivatesPrunedPaths) { this->test(); From 718170ab1ee7add53d381b53fee1d98da8ddbff9 Mon Sep 17 00:00:00 2001 From: Robert Haschke Date: Fri, 19 Nov 2021 06:49:30 +0100 Subject: [PATCH 13/70] Always skip pruning if there exist alternative enabled solutions --- core/src/container.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/core/src/container.cpp b/core/src/container.cpp index 2ccc47ea..2449e21e 100644 --- a/core/src/container.cpp +++ b/core/src/container.cpp @@ -148,8 +148,8 @@ void ContainerBasePrivate::setStatus(const InterfaceState* s, InterfaceState::St if (s->priority().status() == status) return; // nothing changing - // if we should disable the state, only do so when there is no enabled alternative path - if (status == InterfaceState::PRUNED) { + // Skip disabling the state, if there are alternative enabled solutions + if (status != InterfaceState::ENABLED) { auto solution_is_enabled = [](auto&& solution) { return state()>(*solution)->priority().enabled(); }; From 97c21304049348dead1ef8304e83f9dbe19fbd7a Mon Sep 17 00:00:00 2001 From: Robert Haschke Date: Fri, 19 Nov 2021 06:52:31 +0100 Subject: [PATCH 14/70] Improve readability --- core/src/stage.cpp | 9 +++++---- 1 file changed, 5 insertions(+), 4 deletions(-) diff --git a/core/src/stage.cpp b/core/src/stage.cpp index 09eaf517..0de28926 100644 --- a/core/src/stage.cpp +++ b/core/src/stage.cpp @@ -722,9 +722,10 @@ void ConnectingPrivate::newState(Interface::iterator it, bool updated) { assert(it->priority().enabled()); // new solutions are feasible, aren't they? InterfacePtr other_interface = pullInterface(other); for (Interface::iterator oit = other_interface->begin(), oend = other_interface->end(); oit != oend; ++oit) { - // Don't re-enable states that are marked DISABLED if (static_cast(me_)->compatible(*it, *oit)) { - // re-enable the opposing state oit if its status is FAILED + // re-enable the opposing state oit if its status is FAILED, + // but don't re-enable states that are marked DISABLED + // https://github.com/ros-planning/moveit_task_constructor/pull/221 if (oit->priority().status() == InterfaceState::Status::FAILED) oit->owner()->updatePriority(&*oit, InterfaceState::Priority(oit->priority(), InterfaceState::Status::ENABLED)); @@ -742,9 +743,9 @@ void ConnectingPrivate::newState(Interface::iterator it, bool updated) { template inline bool ConnectingPrivate::hasPendingOpposites(const InterfaceState* source) const { for (const auto& candidate : this->pending) { - static_assert(Interface::FORWARD == 0, "This code assumes FORWARD=0, BACKWARD=1. Don't change their order!"); + static_assert(Interface::FORWARD == 0 && Interface::BACKWARD == 1, + "This code assumes FORWARD=0, BACKWARD=1. Don't change their order!"); const auto src = std::get(candidate); - static_assert(Interface::BACKWARD == 1, "This code assumes FORWARD=0, BACKWARD=1. Don't change their order!"); const auto tgt = std::get()>(candidate); if (&*src == source && tgt->priority().enabled()) From 1ddf7dd3f051a33733b1efe321e8ff5e368a96a3 Mon Sep 17 00:00:00 2001 From: Robert Haschke Date: Fri, 19 Nov 2021 06:59:09 +0100 Subject: [PATCH 15/70] Never remove pending CONNECT pairs Both, failed and pruned states might get re-enabled later! This also required rework (simplification) of the sorting function for pending pairs. --- core/include/moveit/task_constructor/stage_p.h | 17 +++++++---------- core/src/stage.cpp | 9 ++------- core/test/test_interface_state.cpp | 11 +++++++---- 3 files changed, 16 insertions(+), 21 deletions(-) diff --git a/core/include/moveit/task_constructor/stage_p.h b/core/include/moveit/task_constructor/stage_p.h index 87ed7b27..883a5f29 100644 --- a/core/include/moveit/task_constructor/stage_p.h +++ b/core/include/moveit/task_constructor/stage_p.h @@ -310,18 +310,15 @@ public: } static inline bool less(const InterfaceState::Priority& lhsA, const InterfaceState::Priority& lhsB, const InterfaceState::Priority& rhsA, const InterfaceState::Priority& rhsB) { - unsigned char lhs = (lhsA.enabled() << 1) | lhsB.enabled(); // combine bits into two-digit binary number - unsigned char rhs = (rhsA.enabled() << 1) | rhsB.enabled(); + bool lhs = lhsA.enabled() && lhsB.enabled(); + bool rhs = rhsA.enabled() && rhsB.enabled(); + if (lhs == rhs) // if enabled status is identical return lhsA + lhsB < rhsA + rhsB; // compare the sums of both contributions - // one of the states in each pair should be enabled - assert(lhs != 0b00 && rhs != 0b00); - // both states valid (b11) - if (lhs == 0b11) - return true; - if (rhs == 0b11) - return false; - return lhs < rhs; // disabled states in 1st component go before disabled states in 2nd component + + // sort both-enabled pairs first + static_assert(true > false, "Comparing enabled states requires true > false"); + return lhs > rhs; } }; diff --git a/core/src/stage.cpp b/core/src/stage.cpp index 0de28926..4140922a 100644 --- a/core/src/stage.cpp +++ b/core/src/stage.cpp @@ -712,12 +712,7 @@ ConnectingPrivate::StatePair ConnectingPrivate::make_pair(In template void ConnectingPrivate::newState(Interface::iterator it, bool updated) { if (updated) { // many pairs might be affected: resort - if (it->priority().pruned()) - // remove all pending pairs involving this state - pending.remove_if([it](const StatePair& p) { return std::get()>(p) == it; }); - else - // TODO(v4hn): If a state becomes reenabled, this skips all previously removed pairs, right? - pending.sort(); + pending.sort(); } else { // new state: insert all pairs with other interface assert(it->priority().enabled()); // new solutions are feasible, aren't they? InterfacePtr other_interface = pullInterface(other); @@ -752,7 +747,7 @@ inline bool ConnectingPrivate::hasPendingOpposites(const InterfaceState* source) return true; // early stopping when only infeasible pairs are to come - if (!std::get<0>(candidate)->priority().enabled()) + if (!std::get<0>(candidate)->priority().enabled() || !std::get<1>(candidate)->priority().enabled()) break; } return false; diff --git a/core/test/test_interface_state.cpp b/core/test/test_interface_state.cpp index 9ca6af53..ec4261a2 100644 --- a/core/test/test_interface_state.cpp +++ b/core/test/test_interface_state.cpp @@ -87,8 +87,11 @@ TEST(StatePairs, compare) { EXPECT_TRUE(pair(Prio(1, 1), Prio(1, 1)) < pair(Prio(1, 0), Prio(0, 0))); auto good = InterfaceState::Status::ENABLED; - auto bad = InterfaceState::Status::FAILED; - EXPECT_TRUE(pair(good, good) < pair(good, bad)); - EXPECT_TRUE(pair(good, good) < pair(bad, good)); - EXPECT_TRUE(pair(bad, good) < pair(good, bad)); + auto good_good = pair(Prio(0, 10, good), Prio(0, 0, good)); + ASSERT_TRUE(good_good > pair(good, good)); // a bad status should reverse this relation + for (auto bad : { InterfaceState::Status::FAILED, InterfaceState::Status::PRUNED }) { + EXPECT_TRUE(good_good < pair(bad, good)); + EXPECT_TRUE(good_good < pair(good, bad)); + EXPECT_TRUE(good_good < pair(bad, bad)); + } } From 4170a1c93aa1d5483fd04412a8ad2bc2a267310d Mon Sep 17 00:00:00 2001 From: Robert Haschke Date: Fri, 19 Nov 2021 18:16:58 +0100 Subject: [PATCH 16/70] Drop unused and misleading Direction enums --- core/include/moveit/task_constructor/storage.h | 2 -- 1 file changed, 2 deletions(-) diff --git a/core/include/moveit/task_constructor/storage.h b/core/include/moveit/task_constructor/storage.h index e71b3760..26ac05f7 100644 --- a/core/include/moveit/task_constructor/storage.h +++ b/core/include/moveit/task_constructor/storage.h @@ -192,8 +192,6 @@ public: { FORWARD, BACKWARD, - START = FORWARD, - END = BACKWARD }; using NotifyFunction = std::function; Interface(const NotifyFunction& notify = NotifyFunction()); From 29d1e44c5da4649ee5a31a2903dfc3a57c27e341 Mon Sep 17 00:00:00 2001 From: Robert Haschke Date: Fri, 19 Nov 2021 14:37:22 +0100 Subject: [PATCH 17/70] Rework updatePriority() functions - Centrally distinguish between have owner() or not in InterfaceState::updatePriority() - Have a separate updateStatus() method to just update the pruning status - Split Interface::updatePriority() into a method taking the InterfaceState* and one taking an Interface::iterator (for efficiency) - Early return in container.cpp's updateStatePrios() --- .../include/moveit/task_constructor/storage.h | 10 ++++++ core/src/container.cpp | 31 +++++++------------ core/src/stage.cpp | 2 +- core/src/storage.cpp | 17 +++++++++- 4 files changed, 38 insertions(+), 22 deletions(-) diff --git a/core/include/moveit/task_constructor/storage.h b/core/include/moveit/task_constructor/storage.h index 26ac05f7..22322a97 100644 --- a/core/include/moveit/task_constructor/storage.h +++ b/core/include/moveit/task_constructor/storage.h @@ -140,13 +140,21 @@ public: /// states are ordered by priority inline bool operator<(const InterfaceState& other) const { return this->priority_ < other.priority_; } + inline const Priority& priority() const { return priority_; } + /// Update priority and call owner's notify() if possible + void updatePriority(const InterfaceState::Priority& priority); + /// Update status, but keep current priority + void updateStatus(Status status); + Interface* owner() const { return owner_; } private: // these methods should be only called by SolutionBase::set[Start|End]State() inline void addIncoming(SolutionBase* t) { incoming_trajectories_.push_back(t); } inline void addOutgoing(SolutionBase* t) { outgoing_trajectories_.push_back(t); } + // Set new priority without updating the owning interface (USE WITH CARE) + inline void setPriority(const Priority& prio) { priority_ = prio; } private: planning_scene::PlanningSceneConstPtr scene_; @@ -204,6 +212,8 @@ public: /// update state's priority (and call notify_ if it really has changed) void updatePriority(InterfaceState* state, const InterfaceState::Priority& priority); + /// more efficient variant of the above, because we can skip searching the state + void updatePriority(Interface::iterator it, const InterfaceState::Priority& priority); private: const NotifyFunction notify_; diff --git a/core/src/container.cpp b/core/src/container.cpp index 2449e21e..dea2c4b0 100644 --- a/core/src/container.cpp +++ b/core/src/container.cpp @@ -160,11 +160,7 @@ void ContainerBasePrivate::setStatus(const InterfaceState* s, InterfaceState::St } // actually enable/disable the state - if (s->owner()) { - s->owner()->updatePriority(const_cast(s), InterfaceState::Priority(s->priority(), status)); - } else { - const_cast(s)->priority_ = InterfaceState::Priority(s->priority(), status); - } + const_cast(s)->updateStatus(status); // if possible (i.e. if state s has an external counterpart), escalate setStatus to external interface if (parent() && trajectories(*s).empty()) { @@ -434,18 +430,15 @@ struct SolutionCollector SolutionSequence::container_type trace; }; -inline void updateStatePrio(const InterfaceState* state, const InterfaceState::Priority& prio) { - if (state->owner()) // owner becomes NULL if state is removed from (pending) Interface list - state->owner()->updatePriority(const_cast(state), - // update depth + cost, but keep current status - InterfaceState::Priority(prio, state->priority().status())); -} - +// recursively update state priorities along solution path template -inline void updateStatePrios(const SolutionSequence::container_type& partial_solution_path, - const InterfaceState::Priority& prio) { - for (const SolutionBase* solution : partial_solution_path) - updateStatePrio(state(*solution), prio); +inline void updateStatePrios(const InterfaceState& s, const InterfaceState::Priority& prio) { + InterfaceState::Priority priority(prio, s.priority().status()); + if (s.priority() == priority) + return; + const_cast(s).updatePriority(priority); + for (const SolutionBase* successor : trajectories(s)) + updateStatePrios(*state(*successor), prio); } void SerialContainer::onNewSolution(const SolutionBase& current) { @@ -496,10 +489,8 @@ void SerialContainer::onNewSolution(const SolutionBase& current) { } if (prio.depth() > 1) { // update state priorities along the whole partial solution path - updateStatePrio(current.start(), prio); - updateStatePrio(current.end(), prio); - updateStatePrios(in.first, prio); - updateStatePrios(out.first, prio); + updateStatePrios(*current.start(), prio); + updateStatePrios(*current.end(), prio); } } } diff --git a/core/src/stage.cpp b/core/src/stage.cpp index 4140922a..841c4532 100644 --- a/core/src/stage.cpp +++ b/core/src/stage.cpp @@ -722,7 +722,7 @@ void ConnectingPrivate::newState(Interface::iterator it, bool updated) { // but don't re-enable states that are marked DISABLED // https://github.com/ros-planning/moveit_task_constructor/pull/221 if (oit->priority().status() == InterfaceState::Status::FAILED) - oit->owner()->updatePriority(&*oit, + oit->owner()->updatePriority(oit, InterfaceState::Priority(oit->priority(), InterfaceState::Status::ENABLED)); pending.insert(make_pair(it, oit)); } diff --git a/core/src/storage.cpp b/core/src/storage.cpp index 08185225..f88ac960 100644 --- a/core/src/storage.cpp +++ b/core/src/storage.cpp @@ -82,6 +82,17 @@ bool InterfaceState::Priority::operator<(const InterfaceState::Priority& other) return cost() < other.cost(); } +void InterfaceState::updatePriority(const InterfaceState::Priority& priority) { + if (owner()) { + owner()->updatePriority(this, priority); + } else { + setPriority(priority); + } +} +void InterfaceState::updateStatus(Status status) { + updatePriority(InterfaceState::Priority(priority_, status)); +} + Interface::Interface(const Interface::NotifyFunction& notify) : notify_(notify) {} // Announce a new InterfaceState @@ -131,7 +142,11 @@ void Interface::updatePriority(InterfaceState* state, const InterfaceState::Prio auto it = std::find(begin(), end(), state); // find iterator to state assert(it != end()); // state should be part of this interface - state->priority_ = priority; // update priority + updatePriority(it, priority); +} + +void Interface::updatePriority(Interface::iterator it, const InterfaceState::Priority& priority) { + it->priority_ = priority; // update priority update(it); // update position in ordered list if (notify_) notify_(it, true); // notify callback From 06ae5ddf9ca940d08d262f942b67bb7892c3fd9b Mon Sep 17 00:00:00 2001 From: Robert Haschke Date: Fri, 19 Nov 2021 23:02:59 +0100 Subject: [PATCH 18/70] Fix hasPendingOpposites() - Switch directions: FORWARD <-> BACKWARD to make the function reusable for status propagation. - We need to ignore the source state when looking for opposite states of the target state. Thus add both, source and target state arguments. --- core/include/moveit/task_constructor/stage_p.h | 2 +- core/src/container.cpp | 14 ++++++-------- core/src/stage.cpp | 18 ++++++++++-------- 3 files changed, 17 insertions(+), 17 deletions(-) diff --git a/core/include/moveit/task_constructor/stage_p.h b/core/include/moveit/task_constructor/stage_p.h index 883a5f29..1bdeb5ad 100644 --- a/core/include/moveit/task_constructor/stage_p.h +++ b/core/include/moveit/task_constructor/stage_p.h @@ -330,7 +330,7 @@ public: // Check whether there are pending feasible states that could connect to source template - bool hasPendingOpposites(const InterfaceState* source) const; + bool hasPendingOpposites(const InterfaceState* source, const InterfaceState* target) const; std::ostream& printPendingPairs(std::ostream& os = std::cerr) const; diff --git a/core/src/container.cpp b/core/src/container.cpp index dea2c4b0..391a4f17 100644 --- a/core/src/container.cpp +++ b/core/src/container.cpp @@ -118,25 +118,23 @@ void ContainerBasePrivate::onNewFailure(const Stage& child, const InterfaceState break; case PROPAGATE_FORWARDS: // mark from as failed (backwards) - ROS_DEBUG_STREAM_NAMED("Pruning", "prune backward branch"); setStatus(from, InterfaceState::Status::FAILED); break; case PROPAGATE_BACKWARDS: // mark to as failed (forwards) - ROS_DEBUG_STREAM_NAMED("Pruning", "prune backward branch"); setStatus(to, InterfaceState::Status::FAILED); break; case CONNECT: if (const Connecting* conn = dynamic_cast(&child)) { + // only prune if there are no opposite pending states auto cimpl = conn->pimpl(); - if (!cimpl->hasPendingOpposites(from)) { - ROS_DEBUG_STREAM_NAMED("Pruning", "prune backward branch"); + if (!cimpl->hasPendingOpposites(to, from)) setStatus(from, InterfaceState::Status::FAILED); - } - if (!cimpl->hasPendingOpposites(to)) { - ROS_DEBUG_STREAM_NAMED("Pruning", "prune forward branch"); + if (!cimpl->hasPendingOpposites(from, to)) setStatus(to, InterfaceState::Status::FAILED); - } + } else { // other CONNECT-like stages, e.g. containers are always pruned + setStatus(from, InterfaceState::Status::FAILED); + setStatus(to, InterfaceState::Status::FAILED); } break; } diff --git a/core/src/stage.cpp b/core/src/stage.cpp index 841c4532..ed1e97c4 100644 --- a/core/src/stage.cpp +++ b/core/src/stage.cpp @@ -733,17 +733,17 @@ void ConnectingPrivate::newState(Interface::iterator it, bool updated) { // std::cerr << std::endl; } -// Check whether there are pending feasible states that could connect to source. -// If not, we exhausted all solution candidates for source and thus should mark it as failure. +// Check whether there are pending feasible states (other than source) that could connect to target. +// If not, we exhausted all solution candidates for target and thus should mark it as failure. template -inline bool ConnectingPrivate::hasPendingOpposites(const InterfaceState* source) const { +inline bool ConnectingPrivate::hasPendingOpposites(const InterfaceState* source, const InterfaceState* target) const { for (const auto& candidate : this->pending) { static_assert(Interface::FORWARD == 0 && Interface::BACKWARD == 1, "This code assumes FORWARD=0, BACKWARD=1. Don't change their order!"); - const auto src = std::get(candidate); - const auto tgt = std::get()>(candidate); + const InterfaceState* src = &*std::get(candidate); + const InterfaceState* tgt = &*std::get()>(candidate); - if (&*src == source && tgt->priority().enabled()) + if (tgt == target && src != source && src->priority().enabled()) return true; // early stopping when only infeasible pairs are to come @@ -753,8 +753,10 @@ inline bool ConnectingPrivate::hasPendingOpposites(const InterfaceState* source) return false; } // explicitly instantiate templates for both directions -template bool ConnectingPrivate::hasPendingOpposites(const InterfaceState* source) const; -template bool ConnectingPrivate::hasPendingOpposites(const InterfaceState* source) const; +template bool ConnectingPrivate::hasPendingOpposites(const InterfaceState* start, + const InterfaceState* end) const; +template bool ConnectingPrivate::hasPendingOpposites(const InterfaceState* end, + const InterfaceState* start) const; bool ConnectingPrivate::canCompute() const { // Do we still have feasible pending state pairs? From 52dc49452598c0dbac375ca9877c69b7c479ab3e Mon Sep 17 00:00:00 2001 From: Robert Haschke Date: Sat, 20 Nov 2021 00:54:09 +0100 Subject: [PATCH 19/70] Fix test Pruning.NoPruningIfAlternativesExist --- .../moveit/task_constructor/container_p.h | 5 +- core/src/container.cpp | 52 +++++++++---------- core/test/test_pruning.cpp | 2 +- 3 files changed, 29 insertions(+), 30 deletions(-) diff --git a/core/include/moveit/task_constructor/container_p.h b/core/include/moveit/task_constructor/container_p.h index 3b58d0da..3f73ee02 100644 --- a/core/include/moveit/task_constructor/container_p.h +++ b/core/include/moveit/task_constructor/container_p.h @@ -148,9 +148,10 @@ protected: child->setNextStarts(allowed ? pending_forward_ : InterfacePtr()); } - /// Set ENABLED / PRUNED status of the solution tree starting from s into given direction + /// Set ENABLED/PRUNED/FAILED status of a solution branch starting from target into the given direction template - void setStatus(const InterfaceState* s, InterfaceState::Status status); + void setStatus(const Stage* creator, const InterfaceState* source, const InterfaceState* target, + InterfaceState::Status status); /// copy external_state to a child's interface and remember the link in internal_external map template diff --git a/core/src/container.cpp b/core/src/container.cpp index 391a4f17..57623b5d 100644 --- a/core/src/container.cpp +++ b/core/src/container.cpp @@ -118,32 +118,32 @@ void ContainerBasePrivate::onNewFailure(const Stage& child, const InterfaceState break; case PROPAGATE_FORWARDS: // mark from as failed (backwards) - setStatus(from, InterfaceState::Status::FAILED); + setStatus(nullptr, nullptr, from, InterfaceState::Status::FAILED); break; case PROPAGATE_BACKWARDS: // mark to as failed (forwards) - setStatus(to, InterfaceState::Status::FAILED); + setStatus(nullptr, nullptr, to, InterfaceState::Status::FAILED); break; case CONNECT: - if (const Connecting* conn = dynamic_cast(&child)) { - // only prune if there are no opposite pending states - auto cimpl = conn->pimpl(); - if (!cimpl->hasPendingOpposites(to, from)) - setStatus(from, InterfaceState::Status::FAILED); - if (!cimpl->hasPendingOpposites(from, to)) - setStatus(to, InterfaceState::Status::FAILED); - } else { // other CONNECT-like stages, e.g. containers are always pruned - setStatus(from, InterfaceState::Status::FAILED); - setStatus(to, InterfaceState::Status::FAILED); - } + setStatus(&child, to, from, InterfaceState::Status::FAILED); + setStatus(&child, from, to, InterfaceState::Status::FAILED); break; } // printChildrenInterfaces(*this, false, child); } template -void ContainerBasePrivate::setStatus(const InterfaceState* s, InterfaceState::Status status) { - if (s->priority().status() == status) +void ContainerBasePrivate::setStatus(const Stage* creator, const InterfaceState* source, const InterfaceState* target, + InterfaceState::Status status) { + if (status != InterfaceState::Status::ENABLED && creator) { + if (const auto* conn = dynamic_cast(creator)) { + auto cimpl = conn->pimpl(); + // if creator is a Connecting stage and target has enabled opposite states (other than source) + if (cimpl->hasPendingOpposites(source, target)) + return; // don't prune + } + } + if (target->priority().status() == status) return; // nothing changing // Skip disabling the state, if there are alternative enabled solutions @@ -151,18 +151,18 @@ void ContainerBasePrivate::setStatus(const InterfaceState* s, InterfaceState::St auto solution_is_enabled = [](auto&& solution) { return state()>(*solution)->priority().enabled(); }; - const auto& alternatives = trajectories()>(*s); + const auto& alternatives = trajectories()>(*target); auto alternative_path = std::find_if(alternatives.cbegin(), alternatives.cend(), solution_is_enabled); if (alternative_path != alternatives.cend()) return; } // actually enable/disable the state - const_cast(s)->updateStatus(status); + const_cast(target)->updateStatus(status); - // if possible (i.e. if state s has an external counterpart), escalate setStatus to external interface - if (parent() && trajectories(*s).empty()) { - auto external{ internalToExternalMap().find(s) }; + // if possible (i.e. if target has an external counterpart), escalate setStatus to external interface + if (parent() && trajectories(*target).empty()) { + auto external{ internalToExternalMap().find(target) }; 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 // all internal states linked to external @@ -170,7 +170,7 @@ void ContainerBasePrivate::setStatus(const InterfaceState* s, InterfaceState::St auto is_enabled = [](const auto& ext_int_pair) { return ext_int_pair.second->priority().enabled(); }; auto other_path{ std::find_if(internals.first, internals.second, is_enabled) }; if (other_path == internals.second) - parent()->pimpl()->setStatus(external->get(), status); + parent()->pimpl()->setStatus(nullptr, nullptr, external->get(), status); return; } } @@ -185,18 +185,16 @@ void ContainerBasePrivate::setStatus(const InterfaceState* s, InterfaceState::St status = InterfaceState::Status::PRUNED; // only the first state is marked as FAILED // traverse solution tree - for (const SolutionBase* successor : trajectories(*s)) - setStatus(state(*successor), status); + for (const SolutionBase* successor : trajectories(*target)) + setStatus(successor->creator(), target, state(*successor), status); } -template void ContainerBasePrivate::setStatus(const InterfaceState*, InterfaceState::Status); -template void ContainerBasePrivate::setStatus(const InterfaceState*, InterfaceState::Status); template void ContainerBasePrivate::copyState(Interface::iterator external, const InterfacePtr& target, bool updated) { - if (updated) { + if (updated) { // propagate external state update to internal copies auto internals{ externalToInternalMap().equal_range(&*external) }; for (auto& i = internals.first; i != internals.second; ++i) { - setStatus(i->second, external->priority().status()); + setStatus(nullptr, nullptr, i->second, external->priority().status()); } return; } diff --git a/core/test/test_pruning.cpp b/core/test/test_pruning.cpp index 43f0b161..c82f9f35 100644 --- a/core/test/test_pruning.cpp +++ b/core/test/test_pruning.cpp @@ -46,7 +46,7 @@ TEST_F(Pruning, PruningMultiForward) { // The 2nd failing FW attempt would prune the path through CON, // but shouldn't because there exist two more GEN2 solutions -TEST_F(Pruning, DISABLED_NoPruningIfAlternativesExist) { +TEST_F(Pruning, NoPruningIfAlternativesExist) { add(t, new GeneratorMockup(PredefinedCosts({ 0.0 }))); add(t, new ConnectMockup()); add(t, new GeneratorMockup(std::list{ 0, 10, 20, 30 }, 2)); From 1dda3d14d89d9692cc3daaf312039d5171590346 Mon Sep 17 00:00:00 2001 From: Robert Haschke Date: Sat, 20 Nov 2021 00:55:06 +0100 Subject: [PATCH 20/70] Switch order of function declarations ... to avoid explicit template initialization --- core/src/container.cpp | 46 +++++++++++++++++++++--------------------- 1 file changed, 23 insertions(+), 23 deletions(-) diff --git a/core/src/container.cpp b/core/src/container.cpp index 57623b5d..4c556806 100644 --- a/core/src/container.cpp +++ b/core/src/container.cpp @@ -109,29 +109,6 @@ void ContainerBasePrivate::compute() { static_cast(me_)->compute(); } -void ContainerBasePrivate::onNewFailure(const Stage& child, const InterfaceState* from, const InterfaceState* to) { - ROS_DEBUG_STREAM_NAMED("Pruning", "'" << child.name() << "' generated a failure"); - switch (child.pimpl()->interfaceFlags()) { - case GENERATE: - // just ignore: the pair of (new) states isn't known to us anyway - // TODO: If child is a container, from and to might have associated solutions already! - break; - - case PROPAGATE_FORWARDS: // mark from as failed (backwards) - setStatus(nullptr, nullptr, from, InterfaceState::Status::FAILED); - break; - case PROPAGATE_BACKWARDS: // mark to as failed (forwards) - setStatus(nullptr, nullptr, to, InterfaceState::Status::FAILED); - break; - - case CONNECT: - setStatus(&child, to, from, InterfaceState::Status::FAILED); - setStatus(&child, from, to, InterfaceState::Status::FAILED); - break; - } - // printChildrenInterfaces(*this, false, child); -} - template void ContainerBasePrivate::setStatus(const Stage* creator, const InterfaceState* source, const InterfaceState* target, InterfaceState::Status status) { @@ -189,6 +166,29 @@ void ContainerBasePrivate::setStatus(const Stage* creator, const InterfaceState* setStatus(successor->creator(), target, state(*successor), status); } +void ContainerBasePrivate::onNewFailure(const Stage& child, const InterfaceState* from, const InterfaceState* to) { + ROS_DEBUG_STREAM_NAMED("Pruning", "'" << child.name() << "' generated a failure"); + switch (child.pimpl()->interfaceFlags()) { + case GENERATE: + // just ignore: the pair of (new) states isn't known to us anyway + // TODO: If child is a container, from and to might have associated solutions already! + break; + + case PROPAGATE_FORWARDS: // mark from as failed (backwards) + setStatus(nullptr, nullptr, from, InterfaceState::Status::FAILED); + break; + case PROPAGATE_BACKWARDS: // mark to as failed (forwards) + setStatus(nullptr, nullptr, to, InterfaceState::Status::FAILED); + break; + + case CONNECT: + setStatus(&child, to, from, InterfaceState::Status::FAILED); + setStatus(&child, from, to, InterfaceState::Status::FAILED); + break; + } + // printChildrenInterfaces(*this, false, child); +} + template void ContainerBasePrivate::copyState(Interface::iterator external, const InterfacePtr& target, bool updated) { if (updated) { // propagate external state update to internal copies From 3c4ef68dbefd2b32949b99e34257441b85070265 Mon Sep 17 00:00:00 2001 From: Robert Haschke Date: Sat, 20 Nov 2021 14:29:39 +0100 Subject: [PATCH 21/70] Rename Interface::Status FAILED -> ARMED ... to better indicate that such a state can be immediately re-enabled. --- .../moveit/task_constructor/container_p.h | 2 +- .../include/moveit/task_constructor/storage.h | 6 ++---- core/src/container.cpp | 19 +++++++++---------- core/src/stage.cpp | 4 ++-- core/src/storage.cpp | 4 ++-- core/test/test_interface_state.cpp | 6 +++--- 6 files changed, 19 insertions(+), 22 deletions(-) diff --git a/core/include/moveit/task_constructor/container_p.h b/core/include/moveit/task_constructor/container_p.h index 3f73ee02..6202c141 100644 --- a/core/include/moveit/task_constructor/container_p.h +++ b/core/include/moveit/task_constructor/container_p.h @@ -148,7 +148,7 @@ protected: child->setNextStarts(allowed ? pending_forward_ : InterfacePtr()); } - /// Set ENABLED/PRUNED/FAILED status of a solution branch starting from target into the given direction + /// Set ENABLED/PRUNED status of a solution branch starting from target into the given direction template void setStatus(const Stage* creator, const InterfaceState* source, const InterfaceState* target, InterfaceState::Status status); diff --git a/core/include/moveit/task_constructor/storage.h b/core/include/moveit/task_constructor/storage.h index 22322a97..893dc09d 100644 --- a/core/include/moveit/task_constructor/storage.h +++ b/core/include/moveit/task_constructor/storage.h @@ -82,8 +82,8 @@ public: enum Status { ENABLED, // state is actively considered during planning - PRUNED, // state is disabled because a required connected state failed - FAILED, // state that failed, causing the whole partial solution to be disabled + ARMED, // disabled state in a Connecting interface that will become re-enabled with a new opposite state + PRUNED, // disabled state on a pruned solution branch }; static const char* STATUS_COLOR[]; @@ -102,8 +102,6 @@ public: inline Status status() const { return std::get<0>(*this); } inline bool enabled() const { return std::get<0>(*this) == ENABLED; } - inline bool failed() const { return std::get<0>(*this) == FAILED; } - inline bool pruned() const { return std::get<0>(*this) == PRUNED; } inline unsigned int depth() const { return std::get<1>(*this); } inline double cost() const { return std::get<2>(*this); } diff --git a/core/src/container.cpp b/core/src/container.cpp index 4c556806..adb9f841 100644 --- a/core/src/container.cpp +++ b/core/src/container.cpp @@ -153,13 +153,12 @@ void ContainerBasePrivate::setStatus(const Stage* creator, const InterfaceState* } // To break symmetry between both ends of a partial solution sequence that gets disabled, - // we mark the first state with FAILED and all other states down the tree only with PRUNED. - // This allows us to re-enable the FAILED side, while not (yet) consider the PRUNED states again, + // we mark the first state with ARMED and all other states down the tree with PRUNED. + // This allows us to re-enable the ARMED state, but not the PRUNED states, // when new states arrive in a Connecting stage. - // All PRUNED states are only re-enabled if the FAILED state actually gets connected. - // For details, see: https://github.com/ros-planning/moveit_task_constructor/pull/221 - if (status == InterfaceState::Status::FAILED) - status = InterfaceState::Status::PRUNED; // only the first state is marked as FAILED + // For details, https://github.com/ros-planning/moveit_task_constructor/pull/309#issuecomment-974636202 + if (status == InterfaceState::Status::ARMED) + status = InterfaceState::Status::PRUNED; // only the first state is marked as ARMED // traverse solution tree for (const SolutionBase* successor : trajectories(*target)) @@ -175,15 +174,15 @@ void ContainerBasePrivate::onNewFailure(const Stage& child, const InterfaceState break; case PROPAGATE_FORWARDS: // mark from as failed (backwards) - setStatus(nullptr, nullptr, from, InterfaceState::Status::FAILED); + setStatus(nullptr, nullptr, from, InterfaceState::Status::PRUNED); break; case PROPAGATE_BACKWARDS: // mark to as failed (forwards) - setStatus(nullptr, nullptr, to, InterfaceState::Status::FAILED); + setStatus(nullptr, nullptr, to, InterfaceState::Status::PRUNED); break; case CONNECT: - setStatus(&child, to, from, InterfaceState::Status::FAILED); - setStatus(&child, from, to, InterfaceState::Status::FAILED); + setStatus(&child, to, from, InterfaceState::Status::ARMED); + setStatus(&child, from, to, InterfaceState::Status::ARMED); break; } // printChildrenInterfaces(*this, false, child); diff --git a/core/src/stage.cpp b/core/src/stage.cpp index ed1e97c4..667e9352 100644 --- a/core/src/stage.cpp +++ b/core/src/stage.cpp @@ -718,10 +718,10 @@ void ConnectingPrivate::newState(Interface::iterator it, bool updated) { InterfacePtr other_interface = pullInterface(other); for (Interface::iterator oit = other_interface->begin(), oend = other_interface->end(); oit != oend; ++oit) { if (static_cast(me_)->compatible(*it, *oit)) { - // re-enable the opposing state oit if its status is FAILED, + // re-enable the opposing state oit if its status is ARMED, // but don't re-enable states that are marked DISABLED // https://github.com/ros-planning/moveit_task_constructor/pull/221 - if (oit->priority().status() == InterfaceState::Status::FAILED) + if (oit->priority().status() == InterfaceState::Status::ARMED) oit->owner()->updatePriority(oit, InterfaceState::Priority(oit->priority(), InterfaceState::Status::ENABLED)); pending.insert(make_pair(it, oit)); diff --git a/core/src/storage.cpp b/core/src/storage.cpp index f88ac960..54f3a846 100644 --- a/core/src/storage.cpp +++ b/core/src/storage.cpp @@ -161,8 +161,8 @@ std::ostream& operator<<(std::ostream& os, const Interface& interface) { } const char* InterfaceState::STATUS_COLOR[] = { "\033[32m", // ENABLED - green - "\033[33m", // PRUNED - yellow - "\033[31m", // FAILED - red + "\033[33m", // ARMED - yellow + "\033[31m", // PRUNED - red "\033[m" // reset }; std::ostream& operator<<(std::ostream& os, const InterfaceState::Priority& prio) { diff --git a/core/test/test_interface_state.cpp b/core/test/test_interface_state.cpp index ec4261a2..65aeee36 100644 --- a/core/test/test_interface_state.cpp +++ b/core/test/test_interface_state.cpp @@ -19,7 +19,7 @@ TEST(InterfaceStatePriority, compare) { EXPECT_TRUE(Prio(1, 42) < Prio(0, 0)); EXPECT_TRUE(Prio(0, 0) < Prio(0, 42)); // at same depth, higher cost is larger - auto dstart = InterfaceState::Status::FAILED; + auto dstart = InterfaceState::Status::ARMED; EXPECT_TRUE(Prio(0, 0, dstart) == Prio(0, 0, dstart)); EXPECT_TRUE(Prio(1, 0, dstart) < Prio(0, 0, dstart)); EXPECT_TRUE(Prio(1, 42, dstart) < Prio(0, 0, dstart)); @@ -68,7 +68,7 @@ TEST(Interface, update) { i.updatePriority(*i.rbegin(), Prio(5, 0.0)); EXPECT_THAT(i.depths(), ::testing::ElementsAreArray({ 5, 3 })); - i.updatePriority(*i.begin(), Prio(6, 0, InterfaceState::Status::FAILED)); + i.updatePriority(*i.begin(), Prio(6, 0, InterfaceState::Status::ARMED)); EXPECT_THAT(i.depths(), ::testing::ElementsAreArray({ 3, 6 })); } @@ -89,7 +89,7 @@ TEST(StatePairs, compare) { auto good = InterfaceState::Status::ENABLED; auto good_good = pair(Prio(0, 10, good), Prio(0, 0, good)); ASSERT_TRUE(good_good > pair(good, good)); // a bad status should reverse this relation - for (auto bad : { InterfaceState::Status::FAILED, InterfaceState::Status::PRUNED }) { + for (auto bad : { InterfaceState::Status::ARMED, InterfaceState::Status::PRUNED }) { EXPECT_TRUE(good_good < pair(bad, good)); EXPECT_TRUE(good_good < pair(good, bad)); EXPECT_TRUE(good_good < pair(bad, bad)); From ab9af6a0fae4a7790d1a330df9ea4c9cc7b24a7b Mon Sep 17 00:00:00 2001 From: Robert Haschke Date: Sat, 20 Nov 2021 14:43:37 +0100 Subject: [PATCH 22/70] Recursively re-enable states when matching an ARMED state --- .../moveit/task_constructor/container_p.h | 1 + core/src/stage.cpp | 24 ++++++++++--------- 2 files changed, 14 insertions(+), 11 deletions(-) diff --git a/core/include/moveit/task_constructor/container_p.h b/core/include/moveit/task_constructor/container_p.h index 6202c141..44dd7fa0 100644 --- a/core/include/moveit/task_constructor/container_p.h +++ b/core/include/moveit/task_constructor/container_p.h @@ -76,6 +76,7 @@ namespace task_constructor { class ContainerBasePrivate : public StagePrivate { friend class ContainerBase; + friend class ConnectingPrivate; // needs to call protected setStatus() friend void swap(StagePrivate*& lhs, StagePrivate*& rhs); public: diff --git a/core/src/stage.cpp b/core/src/stage.cpp index 667e9352..ef711e73 100644 --- a/core/src/stage.cpp +++ b/core/src/stage.cpp @@ -709,23 +709,25 @@ ConnectingPrivate::StatePair ConnectingPrivate::make_pair(In return StatePair(second, first); } -template +template void ConnectingPrivate::newState(Interface::iterator it, bool updated) { if (updated) { // many pairs might be affected: resort pending.sort(); } else { // new state: insert all pairs with other interface assert(it->priority().enabled()); // new solutions are feasible, aren't they? - InterfacePtr other_interface = pullInterface(other); + auto parent_pimpl = parent()->pimpl(); + InterfacePtr other_interface = pullInterface(dir); for (Interface::iterator oit = other_interface->begin(), oend = other_interface->end(); oit != oend; ++oit) { - if (static_cast(me_)->compatible(*it, *oit)) { - // re-enable the opposing state oit if its status is ARMED, - // but don't re-enable states that are marked DISABLED - // https://github.com/ros-planning/moveit_task_constructor/pull/221 - if (oit->priority().status() == InterfaceState::Status::ARMED) - oit->owner()->updatePriority(oit, - InterfaceState::Priority(oit->priority(), InterfaceState::Status::ENABLED)); - pending.insert(make_pair(it, oit)); - } + if (!static_cast(me_)->compatible(*it, *oit)) + continue; + + // re-enable the opposing state oit (and its associated solution branch) if its status is ARMED + // https://github.com/ros-planning/moveit_task_constructor/pull/309#issuecomment-974636202 + if (oit->priority().status() == InterfaceState::Status::ARMED) + parent_pimpl->setStatus()>(me(), &*it, &*oit, InterfaceState::Status::ENABLED); + + // Remember all pending states, regardless of their status! + pending.insert(make_pair(it, oit)); } } // std::cerr << name_ << ": "; From ffb85983634ee80fda5473fd96b8ba2f5811eb23 Mon Sep 17 00:00:00 2001 From: Robert Haschke Date: Sat, 20 Nov 2021 15:02:35 +0100 Subject: [PATCH 23/70] Recursively prune new CONNECT state if there is no enabled opposite This also requires to drop the assertion in SerialContainer::onNewSolution() that new solutions will have enabled start+end states (a CONNECT stage's solution might not). --- core/src/container.cpp | 4 ---- core/src/stage.cpp | 6 ++++++ 2 files changed, 6 insertions(+), 4 deletions(-) diff --git a/core/src/container.cpp b/core/src/container.cpp index adb9f841..601244ba 100644 --- a/core/src/container.cpp +++ b/core/src/container.cpp @@ -443,10 +443,6 @@ void SerialContainer::onNewSolution(const SolutionBase& current) { // failures should never trigger this callback assert(!current.isFailure()); - // states of solution must be active, otherwise this would not have been computed - assert(current.start()->priority().enabled()); - assert(current.end()->priority().enabled()); - auto impl = pimpl(); const Stage* creator = current.creator(); auto& children = impl->children(); diff --git a/core/src/stage.cpp b/core/src/stage.cpp index ef711e73..010b712a 100644 --- a/core/src/stage.cpp +++ b/core/src/stage.cpp @@ -717,6 +717,7 @@ void ConnectingPrivate::newState(Interface::iterator it, bool updated) { assert(it->priority().enabled()); // new solutions are feasible, aren't they? auto parent_pimpl = parent()->pimpl(); InterfacePtr other_interface = pullInterface(dir); + bool have_enabled_opposites = false; for (Interface::iterator oit = other_interface->begin(), oend = other_interface->end(); oit != oend; ++oit) { if (!static_cast(me_)->compatible(*it, *oit)) continue; @@ -725,10 +726,15 @@ void ConnectingPrivate::newState(Interface::iterator it, bool updated) { // https://github.com/ros-planning/moveit_task_constructor/pull/309#issuecomment-974636202 if (oit->priority().status() == InterfaceState::Status::ARMED) parent_pimpl->setStatus()>(me(), &*it, &*oit, InterfaceState::Status::ENABLED); + if (oit->priority().enabled()) + have_enabled_opposites = true; // Remember all pending states, regardless of their status! pending.insert(make_pair(it, oit)); } + if (!have_enabled_opposites) // prune new state and associated branch if necessary + // pass creator=nullptr to skip hasPendingOpposites() check as we did this here already + parent_pimpl->setStatus(nullptr, nullptr, &*it, InterfaceState::Status::ARMED); } // std::cerr << name_ << ": "; // printPendingPairs(std::cerr); From 9d37495c0b32bec09b425ab6dd9f69f5cff84bbc Mon Sep 17 00:00:00 2001 From: Robert Haschke Date: Sun, 21 Nov 2021 04:40:33 +0100 Subject: [PATCH 24/70] Recombine both variants of Interface::updatePriority() As only the InterfaceState* variant is actually called, we can drop the splitting introduced for performance reasons in 29d1e44c5da4649ee5a31a2903dfc3a57c27e341 --- core/include/moveit/task_constructor/storage.h | 2 -- core/src/storage.cpp | 5 +---- 2 files changed, 1 insertion(+), 6 deletions(-) diff --git a/core/include/moveit/task_constructor/storage.h b/core/include/moveit/task_constructor/storage.h index 893dc09d..72d8181f 100644 --- a/core/include/moveit/task_constructor/storage.h +++ b/core/include/moveit/task_constructor/storage.h @@ -210,8 +210,6 @@ public: /// update state's priority (and call notify_ if it really has changed) void updatePriority(InterfaceState* state, const InterfaceState::Priority& priority); - /// more efficient variant of the above, because we can skip searching the state - void updatePriority(Interface::iterator it, const InterfaceState::Priority& priority); private: const NotifyFunction notify_; diff --git a/core/src/storage.cpp b/core/src/storage.cpp index 54f3a846..44082a65 100644 --- a/core/src/storage.cpp +++ b/core/src/storage.cpp @@ -142,11 +142,8 @@ void Interface::updatePriority(InterfaceState* state, const InterfaceState::Prio auto it = std::find(begin(), end(), state); // find iterator to state assert(it != end()); // state should be part of this interface - updatePriority(it, priority); -} -void Interface::updatePriority(Interface::iterator it, const InterfaceState::Priority& priority) { - it->priority_ = priority; // update priority + state->priority_ = priority; // update priority update(it); // update position in ordered list if (notify_) notify_(it, true); // notify callback From 3648db43ed2354f16787843fd261ef4d61f8e606 Mon Sep 17 00:00:00 2001 From: Robert Haschke Date: Sun, 21 Nov 2021 05:53:29 +0100 Subject: [PATCH 25/70] Never overwrite ARMED with PRUNED --- core/src/storage.cpp | 3 +++ 1 file changed, 3 insertions(+) diff --git a/core/src/storage.cpp b/core/src/storage.cpp index 44082a65..f8f7e3f5 100644 --- a/core/src/storage.cpp +++ b/core/src/storage.cpp @@ -83,6 +83,9 @@ bool InterfaceState::Priority::operator<(const InterfaceState::Priority& other) } void InterfaceState::updatePriority(const InterfaceState::Priority& priority) { + // Never overwrite ARMED with PRUNED: PRUNED => !ARMED + assert(priority.status() != InterfaceState::Status::PRUNED || priority_.status() != InterfaceState::Status::ARMED); + if (owner()) { owner()->updatePriority(this, priority); } else { From a31e52dd53078741f2d35d690ad3845c3164541b Mon Sep 17 00:00:00 2001 From: Robert Haschke Date: Sun, 21 Nov 2021 06:19:13 +0100 Subject: [PATCH 26/70] Propagate status across Connecting gap Not only propagate updates along solution paths, but also bridge the gap of a `Connecting` stage. - If a state becomes enabled, re-enable opposite `ARMED` states as well. - If a state becomes pruned, also prune opposite states if they don't have alternatives. - Make sure that we don't run into a recursive update loop by disabling notify() callbacks. --- .../include/moveit/task_constructor/storage.h | 15 +++++++- core/src/stage.cpp | 35 +++++++++++++++++-- core/test/test_pruning.cpp | 2 ++ 3 files changed, 49 insertions(+), 3 deletions(-) diff --git a/core/include/moveit/task_constructor/storage.h b/core/include/moveit/task_constructor/storage.h index 72d8181f..28824d7b 100644 --- a/core/include/moveit/task_constructor/storage.h +++ b/core/include/moveit/task_constructor/storage.h @@ -200,6 +200,18 @@ public: BACKWARD, }; using NotifyFunction = std::function; + + class DisableNotify + { + Interface& if_; + Interface::NotifyFunction old_; + + public: + DisableNotify(Interface& i) : if_(i) { old_.swap(if_.notify_); } + ~DisableNotify() { old_.swap(if_.notify_); } + }; + friend class DisableNotify; + Interface(const NotifyFunction& notify = NotifyFunction()); /// add a new InterfaceState @@ -210,9 +222,10 @@ public: /// update state's priority (and call notify_ if it really has changed) void updatePriority(InterfaceState* state, const InterfaceState::Priority& priority); + inline bool notifyEnabled() const { return static_cast(notify_); } private: - const NotifyFunction notify_; + NotifyFunction notify_; // restrict access to some functions to ensure consistency // (we need to set/unset InterfaceState::owner_) diff --git a/core/src/stage.cpp b/core/src/stage.cpp index 010b712a..b63daf57 100644 --- a/core/src/stage.cpp +++ b/core/src/stage.cpp @@ -709,13 +709,44 @@ ConnectingPrivate::StatePair ConnectingPrivate::make_pair(In return StatePair(second, first); } +// TODO: bool updated -> uint_8 updated (bitfield of PRIORITY | STATUS) template void ConnectingPrivate::newState(Interface::iterator it, bool updated) { - if (updated) { // many pairs might be affected: resort + auto parent_pimpl = parent()->pimpl(); + Interface::DisableNotify disable_source_interface(*pullInterface(dir)); + if (updated) { + if (pullInterface(opposite())->notifyEnabled()) // suppress recursive loop + { + // If status has changed, propagate the update to the opposite side + auto status = it->priority().status(); + if (status == InterfaceState::Status::PRUNED) // PRUNED becomes ARMED on opposite side + status = InterfaceState::Status::ARMED; // (only for pending state pairs) + + for (const auto& candidate : this->pending) { + if (std::get()>(candidate) != it) // only consider pairs with source state == state + continue; + auto oit = std::get(candidate); // opposite target state + auto ostatus = oit->priority().status(); + if (ostatus != status) { + if (status != InterfaceState::Status::ENABLED) { + // quicker check for hasPendingOpposites(): search in it->owner() for an enabled alternative + bool cancel = false; // if found, cancel propagation of new status + for (const auto alternative : *it->owner()) + if ((cancel = alternative->priority().enabled())) + break; + if (cancel) + continue; + } + // pass creator=nullptr to skip hasPendingOpposites() check + parent_pimpl->setStatus()>(nullptr, nullptr, &*oit, status); + } + } + } + + // many pairs will have changed priorities: resort pending list pending.sort(); } else { // new state: insert all pairs with other interface assert(it->priority().enabled()); // new solutions are feasible, aren't they? - auto parent_pimpl = parent()->pimpl(); InterfacePtr other_interface = pullInterface(dir); bool have_enabled_opposites = false; for (Interface::iterator oit = other_interface->begin(), oend = other_interface->end(); oit != oend; ++oit) { diff --git a/core/test/test_pruning.cpp b/core/test/test_pruning.cpp index c82f9f35..d386f1a3 100644 --- a/core/test/test_pruning.cpp +++ b/core/test/test_pruning.cpp @@ -91,6 +91,7 @@ TYPED_TEST(PruningContainerTests, ConnectReactivatesPrunedPaths) { } TEST_F(Pruning, ConnectConnectForward) { + add(t, new BackwardMockup()); add(t, new GeneratorMockup()); auto c1 = add(t, new ConnectMockup({ INF, 0, 0 })); // 1st attempt is a failue add(t, new GeneratorMockup({ 0, 10, 20 })); @@ -112,6 +113,7 @@ TEST_F(Pruning, ConnectConnectForward) { } TEST_F(Pruning, ConnectConnectBackward) { + add(t, new BackwardMockup()); add(t, new GeneratorMockup({ 1, 2, 3 })); auto c1 = add(t, new ConnectMockup()); add(t, new BackwardMockup()); From fdd5ff880b643a091f51ebafa09118382d91ebfd Mon Sep 17 00:00:00 2001 From: Robert Haschke Date: Sun, 21 Nov 2021 10:27:57 +0100 Subject: [PATCH 27/70] templatize: pullInterface(dir) -> pullInterface() Also remove unused pushInterface(dir) --- .../include/moveit/task_constructor/stage_p.h | 23 ++++++++++--------- core/src/container.cpp | 2 +- core/src/stage.cpp | 6 ++--- 3 files changed, 16 insertions(+), 15 deletions(-) diff --git a/core/include/moveit/task_constructor/stage_p.h b/core/include/moveit/task_constructor/stage_p.h index 1bdeb5ad..7e82f4c2 100644 --- a/core/include/moveit/task_constructor/stage_p.h +++ b/core/include/moveit/task_constructor/stage_p.h @@ -100,17 +100,9 @@ public: inline InterfaceConstPtr prevEnds() const { return prev_ends_.lock(); } inline InterfaceConstPtr nextStarts() const { return next_starts_.lock(); } - /// direction-based access to pull/push interfaces - inline InterfacePtr& pullInterface(Interface::Direction dir) { return dir == Interface::FORWARD ? starts_ : ends_; } - inline InterfacePtr pushInterface(Interface::Direction dir) { - return dir == Interface::FORWARD ? next_starts_.lock() : prev_ends_.lock(); - } - inline InterfaceConstPtr pullInterface(Interface::Direction dir) const { - return dir == Interface::FORWARD ? starts_ : ends_; - } - inline InterfaceConstPtr pushInterface(Interface::Direction dir) const { - return dir == Interface::FORWARD ? next_starts_.lock() : prev_ends_.lock(); - } + /// direction-based access to pull interface + template + inline InterfacePtr pullInterface(); /// set parent of stage /// enforce only one parent exists @@ -204,6 +196,15 @@ private: PIMPL_FUNCTIONS(Stage) std::ostream& operator<<(std::ostream& os, const StagePrivate& stage); +template <> +inline InterfacePtr StagePrivate::pullInterface() { + return starts_; +} +template <> +inline InterfacePtr StagePrivate::pullInterface() { + return ends_; +} + template <> inline void StagePrivate::send(const InterfaceState& start, InterfaceState&& end, const SolutionBasePtr& solution) { diff --git a/core/src/container.cpp b/core/src/container.cpp index 601244ba..429c59e9 100644 --- a/core/src/container.cpp +++ b/core/src/container.cpp @@ -715,7 +715,7 @@ void ParallelContainerBasePrivate::validateConnectivity() const { template void ParallelContainerBasePrivate::onNewExternalState(Interface::iterator external, bool updated) { for (const Stage::pointer& stage : children()) - copyState(external, stage->pimpl()->pullInterface(dir), updated); + copyState(external, stage->pimpl()->pullInterface(), updated); } ParallelContainerBase::ParallelContainerBase(ParallelContainerBasePrivate* impl) : ContainerBase(impl) {} diff --git a/core/src/stage.cpp b/core/src/stage.cpp index b63daf57..6a856cad 100644 --- a/core/src/stage.cpp +++ b/core/src/stage.cpp @@ -713,9 +713,9 @@ ConnectingPrivate::StatePair ConnectingPrivate::make_pair(In template void ConnectingPrivate::newState(Interface::iterator it, bool updated) { auto parent_pimpl = parent()->pimpl(); - Interface::DisableNotify disable_source_interface(*pullInterface(dir)); + Interface::DisableNotify disable_source_interface(*pullInterface()); if (updated) { - if (pullInterface(opposite())->notifyEnabled()) // suppress recursive loop + if (pullInterface()>()->notifyEnabled()) // suppress recursive loop { // If status has changed, propagate the update to the opposite side auto status = it->priority().status(); @@ -747,7 +747,7 @@ void ConnectingPrivate::newState(Interface::iterator it, bool updated) { pending.sort(); } else { // new state: insert all pairs with other interface assert(it->priority().enabled()); // new solutions are feasible, aren't they? - InterfacePtr other_interface = pullInterface(dir); + InterfacePtr other_interface = pullInterface(); bool have_enabled_opposites = false; for (Interface::iterator oit = other_interface->begin(), oend = other_interface->end(); oit != oend; ++oit) { if (!static_cast(me_)->compatible(*it, *oit)) From a48b932dce91feaf7e3440e00952b36a1f07be4d Mon Sep 17 00:00:00 2001 From: Robert Haschke Date: Sun, 21 Nov 2021 11:06:15 +0100 Subject: [PATCH 28/70] Distinguish STATUS and PRIORITY updates in notify() callbacks to allow propagating status updates only if the STATUS actually changed. --- .../include/moveit/task_constructor/container_p.h | 6 +++--- core/include/moveit/task_constructor/stage_p.h | 4 ++-- core/include/moveit/task_constructor/storage.h | 10 +++++++++- core/include/moveit/task_constructor/utils.h | 2 -- core/src/container.cpp | 13 +++++++------ core/src/stage.cpp | 5 +++-- core/src/storage.cpp | 15 +++++++++++---- 7 files changed, 35 insertions(+), 20 deletions(-) diff --git a/core/include/moveit/task_constructor/container_p.h b/core/include/moveit/task_constructor/container_p.h index 44dd7fa0..427bfefc 100644 --- a/core/include/moveit/task_constructor/container_p.h +++ b/core/include/moveit/task_constructor/container_p.h @@ -156,7 +156,7 @@ 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); + void copyState(Interface::iterator external, const InterfacePtr& target, Interface::UpdateFlags updated); /// lift solution from internal to external level void liftSolution(const SolutionBasePtr& solution, const InterfaceState* internal_from, const InterfaceState* internal_to); @@ -230,9 +230,9 @@ protected: void validateInterfaces(const StagePrivate& child, InterfaceFlags& external, bool first = false) const; private: - /// callback for new externally received states + /// notify callback for new externally received states template - void onNewExternalState(Interface::iterator external, bool updated); + void onNewExternalState(Interface::iterator external, Interface::UpdateFlags updated); }; PIMPL_FUNCTIONS(ParallelContainerBase) diff --git a/core/include/moveit/task_constructor/stage_p.h b/core/include/moveit/task_constructor/stage_p.h index 7e82f4c2..8e3dda52 100644 --- a/core/include/moveit/task_constructor/stage_p.h +++ b/core/include/moveit/task_constructor/stage_p.h @@ -340,9 +340,9 @@ private: template inline StatePair make_pair(Interface::const_iterator first, Interface::const_iterator second); - // get informed when new start or end state becomes available + // notify callback to get informed about newly inserted (or updated) start or end states template - void newState(Interface::iterator it, bool updated); + void newState(Interface::iterator it, Interface::UpdateFlags updated); // ordered list of pending state pairs ordered pending; diff --git a/core/include/moveit/task_constructor/storage.h b/core/include/moveit/task_constructor/storage.h index 28824d7b..258d0b7a 100644 --- a/core/include/moveit/task_constructor/storage.h +++ b/core/include/moveit/task_constructor/storage.h @@ -42,6 +42,7 @@ #include #include #include +#include #include #include @@ -199,7 +200,14 @@ public: FORWARD, BACKWARD, }; - using NotifyFunction = std::function; + enum Update + { + STATUS = 1 << 0, + PRIORITY = 1 << 1, + ALL = STATUS | PRIORITY, + }; + using UpdateFlags = utils::Flags; + using NotifyFunction = std::function; class DisableNotify { diff --git a/core/include/moveit/task_constructor/utils.h b/core/include/moveit/task_constructor/utils.h index 302d4780..70b89482 100644 --- a/core/include/moveit/task_constructor/utils.h +++ b/core/include/moveit/task_constructor/utils.h @@ -140,8 +140,6 @@ private: Int i; }; -#define DECLARE_FLAGS(Flags, Enum) using Flags = QFlags; - const moveit::core::LinkModel* getRigidlyConnectedParentLinkModel(const moveit::core::RobotState& state, std::string frame); diff --git a/core/src/container.cpp b/core/src/container.cpp index 429c59e9..16567c13 100644 --- a/core/src/container.cpp +++ b/core/src/container.cpp @@ -189,7 +189,8 @@ void ContainerBasePrivate::onNewFailure(const Stage& child, const InterfaceState } template -void ContainerBasePrivate::copyState(Interface::iterator external, const InterfacePtr& target, bool updated) { +void ContainerBasePrivate::copyState(Interface::iterator external, const InterfacePtr& target, + Interface::UpdateFlags updated) { if (updated) { // propagate external state update to internal copies auto internals{ externalToInternalMap().equal_range(&*external) }; for (auto& i = internals.first; i != internals.second; ++i) { @@ -550,7 +551,7 @@ 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_.reset(new Interface([this, target](Interface::iterator it, Interface::UpdateFlags updated) { this->copyState(it, target, updated); })); } catch (InitStageException& e) { @@ -575,7 +576,7 @@ 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_.reset(new Interface([this, target](Interface::iterator it, Interface::UpdateFlags updated) { this->copyState(it, target, updated); })); } catch (InitStageException& e) { @@ -670,11 +671,11 @@ void ParallelContainerBasePrivate::resolveInterface(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) { + starts().reset(new Interface([this](Interface::iterator external, Interface::UpdateFlags updated) { this->onNewExternalState(external, updated); })); if (expected & READS_END) - ends().reset(new Interface([this](Interface::iterator external, bool updated) { + ends().reset(new Interface([this](Interface::iterator external, Interface::UpdateFlags updated) { this->onNewExternalState(external, updated); })); @@ -713,7 +714,7 @@ void ParallelContainerBasePrivate::validateConnectivity() const { } template -void ParallelContainerBasePrivate::onNewExternalState(Interface::iterator external, bool updated) { +void ParallelContainerBasePrivate::onNewExternalState(Interface::iterator external, Interface::UpdateFlags updated) { for (const Stage::pointer& stage : children()) copyState(external, stage->pimpl()->pullInterface(), updated); } diff --git a/core/src/stage.cpp b/core/src/stage.cpp index 6a856cad..94419636 100644 --- a/core/src/stage.cpp +++ b/core/src/stage.cpp @@ -711,11 +711,12 @@ ConnectingPrivate::StatePair ConnectingPrivate::make_pair(In // TODO: bool updated -> uint_8 updated (bitfield of PRIORITY | STATUS) template -void ConnectingPrivate::newState(Interface::iterator it, bool updated) { +void ConnectingPrivate::newState(Interface::iterator it, Interface::UpdateFlags updated) { auto parent_pimpl = parent()->pimpl(); Interface::DisableNotify disable_source_interface(*pullInterface()); if (updated) { - if (pullInterface()>()->notifyEnabled()) // suppress recursive loop + if (updated.testFlag(Interface::STATUS) && // only perform these costly operations if needed + pullInterface()>()->notifyEnabled()) // suppress recursive loop { // If status has changed, propagate the update to the opposite side auto status = it->priority().status(); diff --git a/core/src/storage.cpp b/core/src/storage.cpp index f8f7e3f5..8b825a05 100644 --- a/core/src/storage.cpp +++ b/core/src/storage.cpp @@ -129,7 +129,7 @@ void Interface::add(InterfaceState& state) { moveFrom(it, container); // and finally call notify callback if (notify_) - notify_(it, false); + notify_(it, UpdateFlags()); } Interface::container_type Interface::remove(iterator it) { @@ -140,7 +140,8 @@ Interface::container_type Interface::remove(iterator it) { } void Interface::updatePriority(InterfaceState* state, const InterfaceState::Priority& priority) { - if (priority == state->priority()) + const auto old_prio = state->priority(); + if (priority == old_prio) return; // nothing to do auto it = std::find(begin(), end(), state); // find iterator to state @@ -148,8 +149,14 @@ void Interface::updatePriority(InterfaceState* state, const InterfaceState::Prio state->priority_ = priority; // update priority update(it); // update position in ordered list - if (notify_) - notify_(it, true); // notify callback + + if (notify_) { + UpdateFlags updated(Update::ALL); + if (old_prio.status() == priority.status()) + updated &= ~STATUS; + + notify_(it, updated); // notify callback + } } std::ostream& operator<<(std::ostream& os, const Interface& interface) { From aef991602f837ac804087774ccf3f45b67bf8aff Mon Sep 17 00:00:00 2001 From: Robert Haschke Date: Mon, 22 Nov 2021 00:06:09 +0100 Subject: [PATCH 29/70] Propagate either STATUS or PRIORITY updates into a container --- core/src/container.cpp | 40 +++++++++++++++++++++++----------------- 1 file changed, 23 insertions(+), 17 deletions(-) diff --git a/core/src/container.cpp b/core/src/container.cpp index 16567c13..951a1adc 100644 --- a/core/src/container.cpp +++ b/core/src/container.cpp @@ -165,6 +165,17 @@ void ContainerBasePrivate::setStatus(const Stage* creator, const InterfaceState* setStatus(successor->creator(), target, state(*successor), status); } +// recursively update state priorities along solution path +template +inline void updateStatePrios(const InterfaceState& s, const InterfaceState::Priority& prio) { + InterfaceState::Priority priority(prio, s.priority().status()); + if (s.priority() == priority) + return; + const_cast(s).updatePriority(priority); + for (const SolutionBase* successor : trajectories(s)) + updateStatePrios(*state(*successor), prio); +} + void ContainerBasePrivate::onNewFailure(const Stage& child, const InterfaceState* from, const InterfaceState* to) { ROS_DEBUG_STREAM_NAMED("Pruning", "'" << child.name() << "' generated a failure"); switch (child.pimpl()->interfaceFlags()) { @@ -191,14 +202,20 @@ void ContainerBasePrivate::onNewFailure(const Stage& child, const InterfaceState template void ContainerBasePrivate::copyState(Interface::iterator external, const InterfacePtr& target, Interface::UpdateFlags updated) { - if (updated) { // propagate external state update to internal copies - auto internals{ externalToInternalMap().equal_range(&*external) }; - for (auto& i = internals.first; i != internals.second; ++i) { - setStatus(nullptr, nullptr, i->second, external->priority().status()); - } + if (updated) { + auto prio = external->priority(); + auto internals = externalToInternalMap().equal_range(&*external); + + if (updated.testFlag(Interface::Update::STATUS)) { // propagate external status updates to internal copies + for (auto& i = internals.first; i != internals.second; ++i) + setStatus(nullptr, nullptr, i->second, prio.status()); + } else if (updated.testFlag(Interface::Update::PRIORITY)) { + for (auto& i = internals.first; i != internals.second; ++i) + updateStatePrios()>(*i->second, prio); + } else + assert(false); // Expecting either STATUS or PRIORITY updates, not both! return; } - // create a clone of external state within target interface (child's starts() or ends()) auto internal = states_.insert(states_.end(), InterfaceState(*external)); target->add(*internal); @@ -426,17 +443,6 @@ struct SolutionCollector SolutionSequence::container_type trace; }; -// recursively update state priorities along solution path -template -inline void updateStatePrios(const InterfaceState& s, const InterfaceState::Priority& prio) { - InterfaceState::Priority priority(prio, s.priority().status()); - if (s.priority() == priority) - return; - const_cast(s).updatePriority(priority); - for (const SolutionBase* successor : trajectories(s)) - updateStatePrios(*state(*successor), prio); -} - void SerialContainer::onNewSolution(const SolutionBase& current) { ROS_DEBUG_STREAM_NAMED("SerialContainer", "'" << this->name() << "' received solution of child stage '" << current.creator()->name() << "'"); From b783173b27e129598f49b943640925813697fcf7 Mon Sep 17 00:00:00 2001 From: Robert Haschke Date: Thu, 9 Sep 2021 11:52:44 +0200 Subject: [PATCH 30/70] 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 31/70] 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 32/70] 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 33/70] 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 34/70] 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 35/70] 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 36/70] 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 37/70] 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 38/70] 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 39/70] 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 40/70] 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 41/70] 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 42/70] 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 43/70] 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 44/70] 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 b4a9e2033d340b95d535984737f4f0d4cd592b43 Mon Sep 17 00:00:00 2001 From: Robert Haschke Date: Thu, 25 Nov 2021 17:47:00 +0100 Subject: [PATCH 45/70] Stage::reset() should reset total_compute_time_ (#310) --- core/src/stage.cpp | 1 + 1 file changed, 1 insertion(+) diff --git a/core/src/stage.cpp b/core/src/stage.cpp index 2eaf6a43..e62936cb 100644 --- a/core/src/stage.cpp +++ b/core/src/stage.cpp @@ -316,6 +316,7 @@ void Stage::reset() { impl->next_starts_.reset(); // reset inherited properties impl->properties_.reset(); + impl->total_compute_time_ = std::chrono::duration::zero(); } void Stage::init(const moveit::core::RobotModelConstPtr& /* robot_model */) { From 058712991613ebcbffff488356e941d43392c85f Mon Sep 17 00:00:00 2001 From: Robert Haschke Date: Thu, 25 Nov 2021 21:25:11 +0100 Subject: [PATCH 46/70] 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 47/70] 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 191ff253fd789dc6106d02d42edfd1799b019598 Mon Sep 17 00:00:00 2001 From: v4hn Date: Tue, 14 Dec 2021 14:26:58 +0100 Subject: [PATCH 48/70] add tests for MoveRelative --- core/test/CMakeLists.txt | 1 + core/test/move_relative.test | 7 ++ core/test/test_move_relative.cpp | 129 +++++++++++++++++++++++++++++++ 3 files changed, 137 insertions(+) create mode 100644 core/test/move_relative.test create mode 100644 core/test/test_move_relative.cpp diff --git a/core/test/CMakeLists.txt b/core/test/CMakeLists.txt index 808e7b0d..3918b584 100644 --- a/core/test/CMakeLists.txt +++ b/core/test/CMakeLists.txt @@ -46,6 +46,7 @@ if (CATKIN_ENABLE_TESTING) mtc_add_gmock(test_interface_state.cpp) mtc_add_gtest(test_move_to.cpp move_to.test) + mtc_add_gtest(test_move_relative.cpp move_relative.test) # building these integration tests works without moveit config packages add_executable(pick_ur5 pick_ur5.cpp) diff --git a/core/test/move_relative.test b/core/test/move_relative.test new file mode 100644 index 00000000..6dbb0446 --- /dev/null +++ b/core/test/move_relative.test @@ -0,0 +1,7 @@ + + + + + + + diff --git a/core/test/test_move_relative.cpp b/core/test/test_move_relative.cpp new file mode 100644 index 00000000..8b59074a --- /dev/null +++ b/core/test/test_move_relative.cpp @@ -0,0 +1,129 @@ +#include "models.h" + +#include +#include +#include +#include +#include + +#include + +#include +#include + +#include + +using namespace moveit::task_constructor; +using namespace planning_scene; +using namespace moveit::core; + +constexpr double TAU{ 2 * M_PI }; +constexpr double EPS{ 1e-6 }; + +// provide a basic test fixture that prepares a Task +struct PandaMoveRelative : public testing::Test +{ + Task t; + stages::MoveRelative* move; + PlanningScenePtr scene; + + const JointModelGroup* group; + + PandaMoveRelative() { + t.setRobotModel(loadModel()); + + group = t.getRobotModel()->getJointModelGroup("panda_arm"); + + scene = std::make_shared(t.getRobotModel()); + scene->getCurrentStateNonConst().setToDefaultValues(); + scene->getCurrentStateNonConst().setToDefaultValues(t.getRobotModel()->getJointModelGroup("panda_arm"), "ready"); + t.add(std::make_unique("start", scene)); + + auto move_relative = std::make_unique("move", std::make_shared()); + move_relative->setGroup(group->getName()); + move = move_relative.get(); + t.add(std::move(move_relative)); + } +}; + +moveit_msgs::AttachedCollisionObject createAttachedObject(const std::string& id) { + moveit_msgs::AttachedCollisionObject aco; + aco.link_name = "panda_hand"; + aco.object.header.frame_id = aco.link_name; + aco.object.operation = aco.object.ADD; + aco.object.id = id; + aco.object.primitives.resize(1, [] { + shape_msgs::SolidPrimitive p; + p.type = p.SPHERE; + p.dimensions.resize(1); + p.dimensions[p.SPHERE_RADIUS] = 0.01; + return p; + }()); + + geometry_msgs::Pose p; + p.position.x = 0.1; + p.orientation.w = 1.0; +#if MOVEIT_HAS_OBJECT_POSE + aco.object.pose = p; +#else + aco.object.primitive_poses.resize(1, p); + aco.object.primitive_poses[0] = p; +#endif + return aco; +} + +inline auto position(const PlanningSceneConstPtr& scene, const std::string& frame) { + return scene->getFrameTransform(frame).translation(); +} + +TEST_F(PandaMoveRelative, cartesianRotateEEF) { + move->setDirection([] { + geometry_msgs::TwistStamped twist; + twist.header.frame_id = "world"; + twist.twist.angular.z = TAU / 8.0; + return twist; + }()); + + ASSERT_TRUE(t.plan()) << "Failed to plan"; + + const auto& tip_name{ group->getOnlyOneEndEffectorTip()->getName() }; + const auto start_eef_position{ position(scene, tip_name) }; + const auto end_eef_position{ position(move->solutions().front()->end()->scene(), tip_name) }; + + EXPECT_TRUE(start_eef_position.isApprox(end_eef_position, EPS)) + << "Cartesian rotation unexpectedly changed position of '" << tip_name << "' (must only change orientation)\n" + << start_eef_position << "\nvs\n" + << end_eef_position; +} + +TEST_F(PandaMoveRelative, cartesianRotateAttachedIKFrame) { + const std::string ATTACHED_OBJECT{ "attached_object" }; + scene->processAttachedCollisionObjectMsg(createAttachedObject(ATTACHED_OBJECT)); + move->setIKFrame(ATTACHED_OBJECT); + + move->setDirection([] { + geometry_msgs::TwistStamped twist; + twist.header.frame_id = "world"; + twist.twist.angular.z = TAU / 8.0; + return twist; + }()); + + ASSERT_TRUE(t.plan()); + + const auto start_eef_position{ position(scene, ATTACHED_OBJECT) }; + const auto end_eef_position{ position(move->solutions().front()->end()->scene(), ATTACHED_OBJECT) }; + + EXPECT_TRUE(start_eef_position.isApprox(end_eef_position, EPS)) + << "Cartesian rotation unexpectedly changed position of ik frame (must only change orientation)\n" + << start_eef_position << "\nvs\n" + << end_eef_position; +} + +int main(int argc, char** argv) { + testing::InitGoogleTest(&argc, argv); + ros::init(argc, argv, "move_relative_test"); + ros::AsyncSpinner spinner(1); + spinner.start(); + + return RUN_ALL_TESTS(); +} From 84f96ec74cd4132871d7bf6f02b98fec0e67b6f9 Mon Sep 17 00:00:00 2001 From: v4hn Date: Tue, 14 Dec 2021 14:29:29 +0100 Subject: [PATCH 49/70] MoveRelative: Interpret direction relative to IKFrame bugfix --- core/src/stages/move_relative.cpp | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) diff --git a/core/src/stages/move_relative.cpp b/core/src/stages/move_relative.cpp index dbf5ab7f..f9b2825f 100644 --- a/core/src/stages/move_relative.cpp +++ b/core/src/stages/move_relative.cpp @@ -243,9 +243,9 @@ bool MoveRelative::compute(const InterfaceState& state, planning_scene::Planning // compute absolute transform for link linear = frame_pose.linear() * linear; angular = frame_pose.linear() * angular; - target_eigen = link_pose; + target_eigen = ik_pose_world; target_eigen.linear() = - target_eigen.linear() * Eigen::AngleAxisd(angular_norm, link_pose.linear().transpose() * angular); + target_eigen.linear() * Eigen::AngleAxisd(angular_norm, ik_pose_world.linear().transpose() * angular); target_eigen.translation() += linear; goto COMPUTE; } catch (const boost::bad_any_cast&) { /* continue with Vector */ @@ -269,7 +269,7 @@ bool MoveRelative::compute(const InterfaceState& state, planning_scene::Planning // compute absolute transform for link linear = frame_pose.linear() * linear; - target_eigen = link_pose; + target_eigen = ik_pose_world; target_eigen.translation() += linear; } catch (const boost::bad_any_cast&) { solution.markAsFailure(std::string("invalid direction type: ") + direction.type().name()); @@ -278,7 +278,7 @@ bool MoveRelative::compute(const InterfaceState& state, planning_scene::Planning COMPUTE: // transform target pose such that ik frame will reach there if link does - target_eigen = target_eigen * scene->getCurrentState().getGlobalLinkTransform(link).inverse() * ik_pose_world; + target_eigen = target_eigen * ik_pose_world.inverse() * scene->getCurrentState().getGlobalLinkTransform(link); success = planner_->plan(state.scene(), *link, target_eigen, jmg, timeout, robot_trajectory, path_constraints); From 7dbe0b87e11ab8c769621eab5da403803759993f Mon Sep 17 00:00:00 2001 From: Jafar Abdi Date: Sun, 2 Jan 2022 17:32:37 +0300 Subject: [PATCH 50/70] Return MoveItErrorCode from task::plan (#319) ... to know whether the plan failed due to timeout, preemption, or actual planning failure --- core/include/moveit/task_constructor/task.h | 5 +++-- core/src/task.cpp | 21 ++++++++++++++------- core/test/test_container.cpp | 2 +- demo/src/pick_place_task.cpp | 2 +- 4 files changed, 19 insertions(+), 11 deletions(-) diff --git a/core/include/moveit/task_constructor/task.h b/core/include/moveit/task_constructor/task.h index 6bac28d7..f8e2c5dc 100644 --- a/core/include/moveit/task_constructor/task.h +++ b/core/include/moveit/task_constructor/task.h @@ -47,6 +47,7 @@ #include #include +#include namespace moveit { namespace core { @@ -117,11 +118,11 @@ public: void init(); /// reset, init scene (if not yet done), and init all stages, then start planning - bool plan(size_t max_solutions = 0); + moveit::core::MoveItErrorCode plan(size_t max_solutions = 0); /// interrupt current planning (or execution) void preempt(); /// execute solution, return the result - moveit_msgs::MoveItErrorCodes execute(const SolutionBase& s); + moveit::core::MoveItErrorCode execute(const SolutionBase& s); /// print current task state (number of found solutions and propagated states) to std::cout void printState(std::ostream& os = std::cout) const; diff --git a/core/src/task.cpp b/core/src/task.cpp index 9f1db279..738e43f5 100644 --- a/core/src/task.cpp +++ b/core/src/task.cpp @@ -246,30 +246,37 @@ void Task::compute() { stages()->pimpl()->runCompute(); } -bool Task::plan(size_t max_solutions) { +moveit::core::MoveItErrorCode Task::plan(size_t max_solutions) { auto impl = pimpl(); init(); + // Print state and return success if there are solutions otherwise the input error_code + const auto success_or = [this](const int32_t error_code) { + printState(); + return numSolutions() > 0 ? moveit::core::MoveItErrorCode::SUCCESS : error_code; + }; impl->preempt_requested_ = false; const double available_time = timeout(); const auto start_time = std::chrono::steady_clock::now(); - while (!impl->preempt_requested_ && canCompute() && (max_solutions == 0 || numSolutions() < max_solutions) && - std::chrono::duration(std::chrono::steady_clock::now() - start_time).count() < available_time) { + while (canCompute() && (max_solutions == 0 || numSolutions() < max_solutions)) { + if (impl->preempt_requested_) + return success_or(moveit::core::MoveItErrorCode::PREEMPTED); + if (std::chrono::duration(std::chrono::steady_clock::now() - start_time).count() > available_time) + return success_or(moveit::core::MoveItErrorCode::TIMED_OUT); compute(); for (const auto& cb : impl->task_cbs_) cb(*this); if (impl->introspection_) impl->introspection_->publishTaskState(); - } - printState(); - return numSolutions() > 0; + }; + return success_or(moveit::core::MoveItErrorCode::PLANNING_FAILED); } void Task::preempt() { pimpl()->preempt_requested_ = true; } -moveit_msgs::MoveItErrorCodes Task::execute(const SolutionBase& s) { +moveit::core::MoveItErrorCode Task::execute(const SolutionBase& s) { actionlib::SimpleActionClient ac("execute_task_solution"); ac.waitForServer(); diff --git a/core/test/test_container.cpp b/core/test/test_container.cpp index 4eaa143b..ec810d9e 100644 --- a/core/test/test_container.cpp +++ b/core/test/test_container.cpp @@ -655,7 +655,7 @@ TEST(Task, timeout) { // zero timeout fails t.reset(); t.setTimeout(0.0); - EXPECT_FALSE(t.plan()); + EXPECT_EQ(t.plan(), moveit::core::MoveItErrorCode::TIMED_OUT); // time for 1 solution t.reset(); diff --git a/demo/src/pick_place_task.cpp b/demo/src/pick_place_task.cpp index 72b3cf69..207ac338 100644 --- a/demo/src/pick_place_task.cpp +++ b/demo/src/pick_place_task.cpp @@ -490,7 +490,7 @@ bool PickPlaceTask::plan() { ROS_INFO_NAMED(LOGNAME, "Start searching for task solutions"); int max_solutions = pnh_.param("max_solutions", 10); - return task_->plan(max_solutions); + return static_cast(task_->plan(max_solutions)); } bool PickPlaceTask::execute() { From 9630f4d7892744d6150edbee0bae005d2662db3e Mon Sep 17 00:00:00 2001 From: Robert Haschke Date: Sun, 12 Sep 2021 15:28:30 +0200 Subject: [PATCH 51/70] ComputeIK: Improve markers - always provide eef markers (also in case of success) - tint failures in red - use different names for "ik frame" and "target frame" markers - reduce code duplication --- core/src/stages/compute_ik.cpp | 35 ++++++++++++++++++++++------------ 1 file changed, 23 insertions(+), 12 deletions(-) diff --git a/core/src/stages/compute_ik.cpp b/core/src/stages/compute_ik.cpp index 0e5a6b55..77aa8bd3 100644 --- a/core/src/stages/compute_ik.cpp +++ b/core/src/stages/compute_ik.cpp @@ -306,24 +306,26 @@ void ComputeIK::compute() { bool colliding = !ignore_collisions && isTargetPoseCollidingInEEF(scene, sandbox_state, target_pose, link, &collisions); - // markers used for failures - std::deque failure_markers; // frames at target pose and ik frame - rviz_marker_tools::appendFrame(failure_markers, target_pose_msg, 0.1, "target frame"); - rviz_marker_tools::appendFrame(failure_markers, ik_pose_msg, 0.1, "ik frame"); + std::deque frame_markers; + rviz_marker_tools::appendFrame(frame_markers, target_pose_msg, 0.1, "target frame"); + rviz_marker_tools::appendFrame(frame_markers, ik_pose_msg, 0.1, "ik frame"); + // end-effector markers + std::deque eef_markers; // visualize placed end-effector - auto appender = [&failure_markers](visualization_msgs::Marker& marker, const std::string& /*name*/) { + auto appender = [&eef_markers](visualization_msgs::Marker& marker, const std::string& /*name*/) { marker.ns = "ik target"; marker.color.a *= 0.5; - failure_markers.push_back(marker); + eef_markers.push_back(marker); }; const auto& links_to_visualize = moveit::core::RobotModel::getRigidlyConnectedParentLinkModel(link) ->getParentJointModel() ->getDescendantLinkModels(); if (colliding) { SubTrajectory solution; + std::copy(frame_markers.begin(), frame_markers.end(), std::back_inserter(solution.markers())); generateCollisionMarkers(sandbox_state, appender, links_to_visualize); - std::copy(failure_markers.begin(), failure_markers.end(), std::back_inserter(solution.markers())); + std::copy(eef_markers.begin(), eef_markers.end(), std::back_inserter(solution.markers())); solution.markAsFailure(); // TODO: visualize collisions solution.setComment(s.comment() + " eef in collision: " + listCollisionPairs(collisions.contacts, ", ")); @@ -384,10 +386,7 @@ void ComputeIK::compute() { planning_scene::PlanningScenePtr solution_scene = scene->diff(); SubTrajectory solution; solution.setComment(s.comment()); - - // frames at target pose and ik frame - rviz_marker_tools::appendFrame(solution.markers(), target_pose_msg, 0.1, "ik frame"); - rviz_marker_tools::appendFrame(solution.markers(), ik_pose_msg, 0.1, "ik frame"); + std::copy(frame_markers.begin(), frame_markers.end(), std::back_inserter(solution.markers())); if (succeeded && i + 1 == ik_solutions.size()) // compute cost as distance to compare_pose @@ -402,6 +401,10 @@ void ComputeIK::compute() { InterfaceState state(solution_scene); forwardProperties(*s.start(), state); + + // ik target link placement + std::copy(eef_markers.begin(), eef_markers.end(), std::back_inserter(solution.markers())); + spawn(std::move(state), std::move(solution)); } @@ -418,9 +421,17 @@ void ComputeIK::compute() { solution.markAsFailure(); solution.setComment(s.comment() + " no IK found"); + std::copy(frame_markers.begin(), frame_markers.end(), std::back_inserter(solution.markers())); // ik target link placement - std::copy(failure_markers.begin(), failure_markers.end(), std::back_inserter(solution.markers())); + std_msgs::ColorRGBA tint_color; + tint_color.r = 1.0; + tint_color.g = 0.0; + tint_color.b = 0.0; + tint_color.a = 0.5; + for (auto& marker : eef_markers) + marker.color = tint_color; + std::copy(eef_markers.begin(), eef_markers.end(), std::back_inserter(solution.markers())); spawn(InterfaceState(scene), std::move(solution)); } From 184fab8e0a0b9dd5b2bf1be3d3e7d7d2dfe8454e Mon Sep 17 00:00:00 2001 From: Robert Haschke Date: Wed, 15 Sep 2021 07:53:38 +0200 Subject: [PATCH 52/70] GeneratePlacePose: add property 'allow_z_flip' --- core/src/stages/generate_place_pose.cpp | 10 ++++++---- 1 file changed, 6 insertions(+), 4 deletions(-) diff --git a/core/src/stages/generate_place_pose.cpp b/core/src/stages/generate_place_pose.cpp index 3aae1132..28a8dc43 100644 --- a/core/src/stages/generate_place_pose.cpp +++ b/core/src/stages/generate_place_pose.cpp @@ -55,6 +55,7 @@ GeneratePlacePose::GeneratePlacePose(const std::string& name) : GeneratePose(nam auto& p = properties(); p.declare("object"); p.declare("ik_frame"); + p.declare("allow_z_flip", false, "allow placing objects upside down"); } void GeneratePlacePose::onNewSolution(const SolutionBase& s) { @@ -110,7 +111,7 @@ void GeneratePlacePose::compute() { // spawn the nominal target object pose, considering flip about z and rotations about z-axis auto spawner = [&s, &scene, &object_to_ik, this](const Eigen::Isometry3d& nominal, uint z_flips, uint z_rotations = 10) { - for (uint flip = 0; flip < z_flips; ++flip) { + for (uint flip = 0; flip <= z_flips; ++flip) { // flip about object's x-axis Eigen::Isometry3d object = nominal * Eigen::AngleAxisd(flip * M_PI, Eigen::Vector3d::UnitX()); for (uint i = 0; i < z_rotations; ++i) { @@ -138,20 +139,21 @@ void GeneratePlacePose::compute() { } }; + uint z_flips = props.get("allow_z_flip") ? 1 : 0; if (object->getShapes().size() == 1) { switch (object->getShapes()[0]->type) { case shapes::CYLINDER: - spawner(target_pose, 2); + spawner(target_pose, z_flips); return; case shapes::BOX: { // consider 180/90 degree rotations about z axis const double* dims = static_cast(*object->getShapes()[0]).size; - spawner(target_pose, 2, (std::abs(dims[0] - dims[1]) < 1e-5) ? 4 : 2); + spawner(target_pose, z_flips, (std::abs(dims[0] - dims[1]) < 1e-5) ? 4 : 2); return; } case shapes::SPHERE: // keep original orientation and rotate about world's z target_pose.linear() = orig_object_pose.linear(); - spawner(target_pose, 1); + spawner(target_pose, z_flips); return; default: break; From b2056745a81bd1f7da48a0a79d432742aa9d8776 Mon Sep 17 00:00:00 2001 From: Robert Haschke Date: Fri, 26 Nov 2021 09:12:39 +0100 Subject: [PATCH 53/70] 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 54/70] 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 55/70] 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 56/70] 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 57/70] 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 58/70] 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 59/70] 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 60/70] 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) { From b2c990b675e7f511edaf372af10eea8e0d015f2e Mon Sep 17 00:00:00 2001 From: Robert Haschke Date: Fri, 7 Jan 2022 14:35:15 +0100 Subject: [PATCH 61/70] core: export rviz_marker_tools dependency --- core/CMakeLists.txt | 1 + 1 file changed, 1 insertion(+) diff --git a/core/CMakeLists.txt b/core/CMakeLists.txt index be97790b..5a0c704f 100644 --- a/core/CMakeLists.txt +++ b/core/CMakeLists.txt @@ -25,6 +25,7 @@ catkin_package( geometry_msgs moveit_core moveit_task_constructor_msgs + rviz_marker_tools visualization_msgs ) From ca38d11303ba8a0ea7da6c917fda011eb9cefd45 Mon Sep 17 00:00:00 2001 From: Robert Haschke Date: Wed, 2 Feb 2022 20:51:58 +0100 Subject: [PATCH 62/70] Enable InterfaceState's copy operator --- core/include/moveit/task_constructor/storage.h | 1 + 1 file changed, 1 insertion(+) diff --git a/core/include/moveit/task_constructor/storage.h b/core/include/moveit/task_constructor/storage.h index 2e4d7c70..54058eeb 100644 --- a/core/include/moveit/task_constructor/storage.h +++ b/core/include/moveit/task_constructor/storage.h @@ -129,6 +129,7 @@ public: /// copy an existing InterfaceState, but not including incoming/outgoing trajectories InterfaceState(const InterfaceState& other); InterfaceState(InterfaceState&& other) = default; + InterfaceState& operator=(const InterfaceState& other) = default; inline const planning_scene::PlanningSceneConstPtr& scene() const { return scene_; } inline const Solutions& incomingTrajectories() const { return incoming_trajectories_; } From ee7cec2aab896f692864e5de320a217005b22dd1 Mon Sep 17 00:00:00 2001 From: Robert Haschke Date: Thu, 3 Mar 2022 17:01:10 +0100 Subject: [PATCH 63/70] FixedState: ignore_collisions=false Check collisions for FixedState's scene and report failure if needed. Optionally, disable the check via the property ignore_collisions=true. --- core/src/stages/fixed_state.cpp | 6 +++++- 1 file changed, 5 insertions(+), 1 deletion(-) diff --git a/core/src/stages/fixed_state.cpp b/core/src/stages/fixed_state.cpp index 71ad60ad..f439af4d 100644 --- a/core/src/stages/fixed_state.cpp +++ b/core/src/stages/fixed_state.cpp @@ -45,6 +45,7 @@ namespace stages { FixedState::FixedState(const std::string& name, planning_scene::PlanningScenePtr scene) : Generator(name), scene_(scene) { + properties().declare("ignore_collisions", false); setCostTerm(std::make_unique(0.0)); } @@ -62,7 +63,10 @@ bool FixedState::canCompute() const { } void FixedState::compute() { - spawn(InterfaceState(scene_), 0.0); + auto cost = !properties().get("ignore_collisions") && scene_->isStateColliding() ? + std::numeric_limits::infinity() : + 0.0; + spawn(InterfaceState(scene_), cost); ran_ = true; } } // namespace stages From 5310f9063a610cd47ee15d2740df8c76026dedfc Mon Sep 17 00:00:00 2001 From: Robert Haschke Date: Fri, 4 Mar 2022 08:54:26 +0100 Subject: [PATCH 64/70] operator<< for Interface::Direction --- core/include/moveit/task_constructor/storage.h | 1 + core/src/stage.cpp | 6 ++---- core/src/storage.cpp | 4 ++++ 3 files changed, 7 insertions(+), 4 deletions(-) diff --git a/core/include/moveit/task_constructor/storage.h b/core/include/moveit/task_constructor/storage.h index 54058eeb..ed8ef9d2 100644 --- a/core/include/moveit/task_constructor/storage.h +++ b/core/include/moveit/task_constructor/storage.h @@ -248,6 +248,7 @@ private: std::ostream& operator<<(std::ostream& os, const InterfaceState::Priority& prio); std::ostream& operator<<(std::ostream& os, const Interface& interface); +std::ostream& operator<<(std::ostream& os, Interface::Direction); /// Find index of the iterator in the container. Counting starts at 1. Zero corresponds to not found. template diff --git a/core/src/stage.cpp b/core/src/stage.cpp index 9c6410cb..f0efb54f 100644 --- a/core/src/stage.cpp +++ b/core/src/stage.cpp @@ -737,7 +737,6 @@ ConnectingPrivate::StatePair ConnectingPrivate::make_pair(In return StatePair(second, first); } -// TODO: bool updated -> uint_8 updated (bitfield of PRIORITY | STATUS) template void ConnectingPrivate::newState(Interface::iterator it, Interface::UpdateFlags updated) { auto parent_pimpl = parent()->pimpl(); @@ -799,8 +798,7 @@ void ConnectingPrivate::newState(Interface::iterator it, Interface::UpdateFlags #if 0 auto& os = std::cerr; for (auto d : { Interface::FORWARD, Interface::BACKWARD }) { - bool fw = (d == Interface::FORWARD); - if (fw) + if (d == Interface::FORWARD) os << " " << std::setw(10) << std::left << this->name(); else os << std::setw(12) << std::right << ""; @@ -808,7 +806,7 @@ void ConnectingPrivate::newState(Interface::iterator it, Interface::UpdateFlags os << (updated ? " !" : " +"); else os << " "; - os << (fw ? "↓ " : "↑ ") << this->pullInterface(d) << ": " << *this->pullInterface(d) << std::endl; + os << d << " " << this->pullInterface(d) << ": " << *this->pullInterface(d) << std::endl; } os << std::setw(15) << " "; printPendingPairs(os) << std::endl; diff --git a/core/src/storage.cpp b/core/src/storage.cpp index 8b825a05..bd0211d9 100644 --- a/core/src/storage.cpp +++ b/core/src/storage.cpp @@ -178,6 +178,10 @@ std::ostream& operator<<(std::ostream& os, const InterfaceState::Priority& prio) << InterfaceState::STATUS_COLOR[3]; return os; } +std::ostream& operator<<(std::ostream& os, Interface::Direction dir) { + os << (dir == Interface::FORWARD ? "↓" : "↑"); + return os; +} void SolutionBase::setCreator(Stage* creator) { assert(creator_ == nullptr || creator_ == creator); // creator must only set once From 0a3dd3a314fae5da09a070c5857c614cb73c5682 Mon Sep 17 00:00:00 2001 From: v4hn Date: Fri, 4 Mar 2022 14:39:53 +0100 Subject: [PATCH 65/70] properly set comment markAsFailure without prior comment --- core/src/storage.cpp | 9 +++++++-- 1 file changed, 7 insertions(+), 2 deletions(-) diff --git a/core/src/storage.cpp b/core/src/storage.cpp index 8b825a05..32b26ec4 100644 --- a/core/src/storage.cpp +++ b/core/src/storage.cpp @@ -190,8 +190,13 @@ void SolutionBase::setCost(double cost) { void SolutionBase::markAsFailure(const std::string& msg) { setCost(std::numeric_limits::infinity()); - if (!msg.empty()) - setComment(msg + "\n" + comment()); + if (!msg.empty()) { + std::stringstream ss; + ss << msg; + if (!comment().empty()) + ss << "\n" << comment(); + setComment(ss.str()); + } } void SolutionBase::fillInfo(moveit_task_constructor_msgs::SolutionInfo& info, Introspection* introspection) const { From 6d104e837e6c79b2578d1f13f89b519942c34659 Mon Sep 17 00:00:00 2001 From: v4hn Date: Fri, 4 Mar 2022 14:41:00 +0100 Subject: [PATCH 66/70] polish: FixedState supports collision checking --- core/src/stages/fixed_state.cpp | 10 ++++++---- 1 file changed, 6 insertions(+), 4 deletions(-) diff --git a/core/src/stages/fixed_state.cpp b/core/src/stages/fixed_state.cpp index f439af4d..9c4d99b0 100644 --- a/core/src/stages/fixed_state.cpp +++ b/core/src/stages/fixed_state.cpp @@ -63,10 +63,12 @@ bool FixedState::canCompute() const { } void FixedState::compute() { - auto cost = !properties().get("ignore_collisions") && scene_->isStateColliding() ? - std::numeric_limits::infinity() : - 0.0; - spawn(InterfaceState(scene_), cost); + SubTrajectory trajectory; + if (!properties().get("ignore_collisions") && scene_->isStateColliding()) { + trajectory.markAsFailure("in collision"); + } + + spawn(InterfaceState(scene_), std::move(trajectory)); ran_ = true; } } // namespace stages From 8d7225d3b6b50b596a0ce0df943f499a34536ba2 Mon Sep 17 00:00:00 2001 From: Robert Haschke Date: Fri, 4 Mar 2022 12:08:09 +0100 Subject: [PATCH 67/70] Connect: better document suppressing recursive loop --- core/src/stage.cpp | 4 +++- 1 file changed, 3 insertions(+), 1 deletion(-) diff --git a/core/src/stage.cpp b/core/src/stage.cpp index f0efb54f..5ad85e49 100644 --- a/core/src/stage.cpp +++ b/core/src/stage.cpp @@ -740,10 +740,12 @@ ConnectingPrivate::StatePair ConnectingPrivate::make_pair(In template void ConnectingPrivate::newState(Interface::iterator it, Interface::UpdateFlags updated) { auto parent_pimpl = parent()->pimpl(); + // disable current interface to break loop (jumping back and forth between both interfaces) + // this will be checked by notifyEnabled() below Interface::DisableNotify disable_source_interface(*pullInterface()); if (updated) { if (updated.testFlag(Interface::STATUS) && // only perform these costly operations if needed - pullInterface()>()->notifyEnabled()) // suppress recursive loop + pullInterface()>()->notifyEnabled()) // suppressing recursive loop? { // If status has changed, propagate the update to the opposite side auto status = it->priority().status(); From 096c671887c0c526b1138deba5344004ab2587e8 Mon Sep 17 00:00:00 2001 From: Robert Haschke Date: Fri, 4 Mar 2022 09:07:40 +0100 Subject: [PATCH 68/70] Pruning: Relax too strong assertion: PRUNED => !ARMED If two Connect stages are sequenced, both sides can become ARMED. However, that means that the wave of PRUNED status updates, shouldn't overwrite a present ARMED state. Added unit test. --- core/src/storage.cpp | 5 +++-- core/test/test_pruning.cpp | 14 ++++++++++++++ 2 files changed, 17 insertions(+), 2 deletions(-) diff --git a/core/src/storage.cpp b/core/src/storage.cpp index bd0211d9..42c7d8ff 100644 --- a/core/src/storage.cpp +++ b/core/src/storage.cpp @@ -83,8 +83,9 @@ bool InterfaceState::Priority::operator<(const InterfaceState::Priority& other) } void InterfaceState::updatePriority(const InterfaceState::Priority& priority) { - // Never overwrite ARMED with PRUNED: PRUNED => !ARMED - assert(priority.status() != InterfaceState::Status::PRUNED || priority_.status() != InterfaceState::Status::ARMED); + // Never overwrite ARMED with PRUNED + if (priority.status() == InterfaceState::Status::PRUNED && priority_.status() == InterfaceState::Status::ARMED) + return; if (owner()) { owner()->updatePriority(this, priority); diff --git a/core/test/test_pruning.cpp b/core/test/test_pruning.cpp index d386f1a3..b26ac527 100644 --- a/core/test/test_pruning.cpp +++ b/core/test/test_pruning.cpp @@ -193,3 +193,17 @@ TEST_F(Pruning, PropagateFromParallelContainerMultiplePaths) { // the failure in one branch of Alternatives must not prune computing back EXPECT_EQ(back->runs_, 1u); } + +TEST_F(Pruning, TwoConnects) { + add(t, new GeneratorMockup({ 0 })); + add(t, new ForwardMockup({ INF })); + add(t, new ConnectMockup()); + + add(t, new GeneratorMockup()); + add(t, new ConnectMockup()); + + add(t, new GeneratorMockup()); + add(t, new ForwardMockup()); + + EXPECT_FALSE(t.plan()); +} From 8beb0f4243b4a3be1cc3ce8b7a0935eed0e9cb43 Mon Sep 17 00:00:00 2001 From: Stephanie Eng Date: Tue, 29 Mar 2022 11:59:20 -0400 Subject: [PATCH 69/70] Update black version (#348) --- .pre-commit-config.yaml | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/.pre-commit-config.yaml b/.pre-commit-config.yaml index f1063965..89f7060f 100644 --- a/.pre-commit-config.yaml +++ b/.pre-commit-config.yaml @@ -29,7 +29,7 @@ repos: - id: trailing-whitespace - repo: https://github.com/psf/black - rev: 21.11b1 + rev: 22.3.0 hooks: - id: black From 9026ac87465afabb4ed97f56f0565f69b2218b24 Mon Sep 17 00:00:00 2001 From: Robert Haschke Date: Sun, 8 May 2022 11:54:05 +0200 Subject: [PATCH 70/70] Make TimeParamerization configurable (#339) --- core/include/moveit/task_constructor/merge.h | 4 +++- .../moveit/task_constructor/moveit_compat.h | 8 ++++++++ core/src/container.cpp | 17 ++++++++++++----- core/src/merge.cpp | 7 +++---- core/src/solvers/cartesian_path.cpp | 10 ++++++---- core/src/solvers/joint_interpolation.cpp | 12 +++++++----- core/src/solvers/planner_interface.cpp | 5 +++++ core/src/stages/connect.cpp | 13 ++++++++++--- 8 files changed, 54 insertions(+), 22 deletions(-) diff --git a/core/include/moveit/task_constructor/merge.h b/core/include/moveit/task_constructor/merge.h index 7ac7eef5..734418f1 100644 --- a/core/include/moveit/task_constructor/merge.h +++ b/core/include/moveit/task_constructor/merge.h @@ -39,6 +39,7 @@ #include #include #include +#include namespace moveit { namespace task_constructor { @@ -57,6 +58,7 @@ moveit::core::JointModelGroup* merge(const std::vector& sub_trajectories, - const moveit::core::RobotState& base_state, moveit::core::JointModelGroup*& merged_group); + const moveit::core::RobotState& base_state, moveit::core::JointModelGroup*& merged_group, + const trajectory_processing::TimeParameterization& time_parameterization); } // namespace task_constructor } // namespace moveit diff --git a/core/include/moveit/task_constructor/moveit_compat.h b/core/include/moveit/task_constructor/moveit_compat.h index 91e7d12b..202a47a9 100644 --- a/core/include/moveit/task_constructor/moveit_compat.h +++ b/core/include/moveit/task_constructor/moveit_compat.h @@ -39,6 +39,7 @@ #pragma once #include +#include #define MOVEIT_VERSION_GE(major, minor, patch) \ (MOVEIT_VERSION_MAJOR * 1'000'000 + MOVEIT_VERSION_MINOR * 1'000 + MOVEIT_VERSION_PATCH >= \ @@ -57,3 +58,10 @@ #define MOVEIT_HAS_OBJECT_POSE MOVEIT_VERSION_GE(1, 1, 6) #define MOVEIT_HAS_STATE_RIGID_PARENT_LINK MOVEIT_VERSION_GE(1, 1, 6) + +#if !MOVEIT_VERSION_GE(1, 1, 9) +// the pointers are not yet available +namespace trajectory_processing { +MOVEIT_CLASS_FORWARD(TimeParameterization); +} +#endif diff --git a/core/src/container.cpp b/core/src/container.cpp index ae9f6698..f9b79cfe 100644 --- a/core/src/container.cpp +++ b/core/src/container.cpp @@ -37,7 +37,9 @@ #include #include #include +#include #include +#include #include @@ -49,6 +51,7 @@ #include using namespace std::placeholders; +using namespace trajectory_processing; namespace moveit { namespace task_constructor { @@ -635,7 +638,7 @@ void SerialContainerPrivate::validateConnectivity() const { ContainerBasePrivate::validateConnectivity(); InterfaceFlags mine = interfaceFlags(); - // check that input / output interface of first / last child matches this' resp. interface + // check that input/output interface of first/last child matches this' resp. interface validateInterface(*children().front()->pimpl(), mine); validateInterface(*children().back()->pimpl(), mine); @@ -647,7 +650,7 @@ void SerialContainerPrivate::validateConnectivity() const { const StagePrivate* const cur_impl = **cur; InterfaceFlags required = cur_impl->interfaceFlags(); - // get iterators to prev / next stage in sequence + // get iterators to prev/next stage in sequence auto prev = cur; --prev; auto next = cur; @@ -750,7 +753,7 @@ void ParallelContainerBasePrivate::validateInterfaces(const StagePrivate& child, void ParallelContainerBasePrivate::validateConnectivity() const { InterfaceFlags my_interface = interfaceFlags(); - // check that input / output interfaces of all children are handled by my interface + // check that input/output interfaces of all children are handled by my interface for (const auto& child : children()) validateInterfaces(*child->pimpl(), my_interface); @@ -1084,7 +1087,10 @@ void MergerPrivate::resolveInterface(InterfaceFlags expected) { } } -Merger::Merger(const std::string& name) : Merger(new MergerPrivate(this, name)) {} +Merger::Merger(const std::string& name) : Merger(new MergerPrivate(this, name)) { + properties().declare("time_parameterization", + std::make_shared()); +} void Merger::reset() { ParallelContainerBase::reset(); @@ -1237,7 +1243,8 @@ void MergerPrivate::merge(const ChildSolutionList& sub_solutions, moveit::core::JointModelGroup* jmg = jmg_merged_.get(); robot_trajectory::RobotTrajectoryPtr merged; try { - merged = task_constructor::merge(sub_trajectories, start_scene->getCurrentState(), jmg); + auto timing = me_->properties().get("time_parameterization"); + merged = task_constructor::merge(sub_trajectories, start_scene->getCurrentState(), jmg, *timing); } catch (const std::runtime_error& e) { SubTrajectory t; t.markAsFailure(); diff --git a/core/src/merge.cpp b/core/src/merge.cpp index a0711509..3bae7a32 100644 --- a/core/src/merge.cpp +++ b/core/src/merge.cpp @@ -36,7 +36,6 @@ /* Authors: Luca Lach, Robert Haschke */ #include -#include #include #include @@ -106,7 +105,8 @@ moveit::core::JointModelGroup* merge(const std::vector& sub_trajectories, - const robot_state::RobotState& base_state, moveit::core::JointModelGroup*& merged_group) { + const robot_state::RobotState& base_state, moveit::core::JointModelGroup*& merged_group, + const trajectory_processing::TimeParameterization& time_parameterization) { if (sub_trajectories.size() <= 1) throw std::runtime_error("Expected multiple sub solutions"); @@ -166,8 +166,7 @@ merge(const std::vector& sub_trajecto } // add timing - trajectory_processing::IterativeParabolicTimeParameterization timing; - timing.computeTimeStamps(*merged_traj, 1.0, 1.0); + time_parameterization.computeTimeStamps(*merged_traj, 1.0, 1.0); return merged_traj; } } // namespace task_constructor diff --git a/core/src/solvers/cartesian_path.cpp b/core/src/solvers/cartesian_path.cpp index e15306ab..6044c830 100644 --- a/core/src/solvers/cartesian_path.cpp +++ b/core/src/solvers/cartesian_path.cpp @@ -40,11 +40,13 @@ #include #include -#include +#include #if MOVEIT_HAS_CARTESIAN_INTERPOLATOR #include #endif +using namespace trajectory_processing; + namespace moveit { namespace task_constructor { namespace solvers { @@ -107,9 +109,9 @@ bool CartesianPath::plan(const planning_scene::PlanningSceneConstPtr& from, cons for (const auto& waypoint : trajectory) result->addSuffixWayPoint(waypoint, 0.0); - trajectory_processing::IterativeParabolicTimeParameterization timing; - timing.computeTimeStamps(*result, props.get("max_velocity_scaling_factor"), - props.get("max_acceleration_scaling_factor")); + auto timing = props.get("time_parameterization"); + timing->computeTimeStamps(*result, props.get("max_velocity_scaling_factor"), + props.get("max_acceleration_scaling_factor")); return achieved_fraction >= props.get("min_fraction"); } diff --git a/core/src/solvers/joint_interpolation.cpp b/core/src/solvers/joint_interpolation.cpp index 35b3935f..61e640c2 100644 --- a/core/src/solvers/joint_interpolation.cpp +++ b/core/src/solvers/joint_interpolation.cpp @@ -37,8 +37,9 @@ */ #include +#include #include -#include +#include #include @@ -46,6 +47,8 @@ namespace moveit { namespace task_constructor { namespace solvers { +using namespace trajectory_processing; + JointInterpolationPlanner::JointInterpolationPlanner() { auto& p = properties(); p.declare("max_step", 0.1, "max joint step"); @@ -89,10 +92,9 @@ bool JointInterpolationPlanner::plan(const planning_scene::PlanningSceneConstPtr if (from->isStateColliding(to_state, jmg->getName())) return false; - // add timing, TODO: use a generic method to add timing via plugins - trajectory_processing::IterativeParabolicTimeParameterization timing; - timing.computeTimeStamps(*result, props.get("max_velocity_scaling_factor"), - props.get("max_acceleration_scaling_factor")); + auto timing = props.get("time_parameterization"); + timing->computeTimeStamps(*result, props.get("max_velocity_scaling_factor"), + props.get("max_acceleration_scaling_factor")); return true; } diff --git a/core/src/solvers/planner_interface.cpp b/core/src/solvers/planner_interface.cpp index 23e31473..bb2a49e7 100644 --- a/core/src/solvers/planner_interface.cpp +++ b/core/src/solvers/planner_interface.cpp @@ -37,6 +37,10 @@ */ #include +#include +#include + +using namespace trajectory_processing; namespace moveit { namespace task_constructor { @@ -46,6 +50,7 @@ PlannerInterface::PlannerInterface() { auto& p = properties(); p.declare("max_velocity_scaling_factor", 1.0, "scale down max velocity by this factor"); p.declare("max_acceleration_scaling_factor", 1.0, "scale down max acceleration by this factor"); + p.declare("time_parameterization", std::make_shared()); } } // namespace solvers } // namespace task_constructor diff --git a/core/src/stages/connect.cpp b/core/src/stages/connect.cpp index 9aadd4b8..ba6483e4 100644 --- a/core/src/stages/connect.cpp +++ b/core/src/stages/connect.cpp @@ -38,10 +38,14 @@ #include #include -#include - +#include #include +#include +#include + +using namespace trajectory_processing; + namespace moveit { namespace task_constructor { namespace stages { @@ -54,6 +58,8 @@ Connect::Connect(const std::string& name, const GroupPlannerVector& planners) : p.declare("merge_mode", WAYPOINTS, "merge mode"); p.declare("path_constraints", moveit_msgs::Constraints(), "constraints to maintain during trajectory"); + properties().declare("merge_time_parameterization", + std::make_shared()); } void Connect::reset() { @@ -219,7 +225,8 @@ SubTrajectoryPtr Connect::merge(const std::vector("merge_time_parameterization"); + robot_trajectory::RobotTrajectoryPtr trajectory = task_constructor::merge(sub_trajectories, state, jmg, *timing); if (!trajectory) return SubTrajectoryPtr();