From 19484cec64ccd2578fda3bb870a10f3eb22a03a1 Mon Sep 17 00:00:00 2001 From: Robert Haschke Date: Sun, 6 Dec 2020 23:01:28 +0100 Subject: [PATCH] Restore pruning If a stage fails to find a solution, this often implies that further planning on the open end(s) of connected InterfaceStates is not needed anymore. Thus the InterfaceStates along all connected solution paths will be marked as disabled. They are not removed from the pending state lists though, because they might get reactivated by solutions found in future. To this end, we introduced the method ContainerBase::onNewFailure(). --- .../include/moveit/task_constructor/container.h | 5 ++++- core/include/moveit/task_constructor/stage_p.h | 2 +- core/include/moveit/task_constructor/storage.h | 12 ++++++++++++ core/src/container.cpp | 11 ++++++++--- core/src/stage.cpp | 17 +++++++++++------ core/src/storage.cpp | 17 +++++++++-------- core/test/test_interface_state.cpp | 2 +- core/test/test_serial.cpp | 2 +- 8 files changed, 47 insertions(+), 21 deletions(-) diff --git a/core/include/moveit/task_constructor/container.h b/core/include/moveit/task_constructor/container.h index b58f5abc..13c5262a 100644 --- a/core/include/moveit/task_constructor/container.h +++ b/core/include/moveit/task_constructor/container.h @@ -77,6 +77,8 @@ public: /// called by a (direct) child when a new solution becomes available virtual void onNewSolution(const SolutionBase& s) = 0; + /// called by a (direct) child when a solution failed + virtual void onNewFailure(const Stage& child, const InterfaceState* from, const InterfaceState* to) = 0; protected: ContainerBase(ContainerBasePrivate* impl); @@ -95,8 +97,8 @@ public: void compute() override; protected: - /// called by a (direct) child when a new solution becomes available void onNewSolution(const SolutionBase& s) override; + void onNewFailure(const Stage& child, const InterfaceState* from, const InterfaceState* to) override; protected: SerialContainer(SerialContainerPrivate* impl); @@ -116,6 +118,7 @@ class ParallelContainerBase : public ContainerBase public: PRIVATE_CLASS(ParallelContainerBase) ParallelContainerBase(const std::string& name = "parallel container"); + void onNewFailure(const Stage& child, const InterfaceState* from, const InterfaceState* to) override; protected: ParallelContainerBase(ParallelContainerBasePrivate* impl); diff --git a/core/include/moveit/task_constructor/stage_p.h b/core/include/moveit/task_constructor/stage_p.h index 43e53174..872d5c33 100644 --- a/core/include/moveit/task_constructor/stage_p.h +++ b/core/include/moveit/task_constructor/stage_p.h @@ -144,7 +144,7 @@ public: void spawn(InterfaceState&& state, const SolutionBasePtr& solution); void connect(const InterfaceState& from, const InterfaceState& to, const SolutionBasePtr& solution); - bool storeSolution(const SolutionBasePtr& solution); + bool storeSolution(const SolutionBasePtr& solution, const InterfaceState* from, const InterfaceState* to); void newSolution(const SolutionBasePtr& solution); bool storeFailures() const { return introspection_ != nullptr; } void runCompute() { diff --git a/core/include/moveit/task_constructor/storage.h b/core/include/moveit/task_constructor/storage.h index 5f817b2d..249a7729 100644 --- a/core/include/moveit/task_constructor/storage.h +++ b/core/include/moveit/task_constructor/storage.h @@ -390,6 +390,18 @@ template <> inline const InterfaceState::Solutions& trajectories(const InterfaceState* state) { return state->incomingTrajectories(); } + +/// Trait to retrieve opposite direction (FORWARD <-> BACKWARD) +template +constexpr Interface::Direction opposite(); +template <> +inline constexpr Interface::Direction opposite() { + return Interface::BACKWARD; +} +template <> +inline constexpr Interface::Direction opposite() { + return Interface::FORWARD; +} } // namespace task_constructor } // namespace moveit diff --git a/core/src/container.cpp b/core/src/container.cpp index 14a4f1c7..4ea046c0 100644 --- a/core/src/container.cpp +++ b/core/src/container.cpp @@ -122,9 +122,7 @@ void ContainerBasePrivate::liftSolution(const SolutionBasePtr& solution, const I const InterfaceState* internal_to) { computeCost(*internal_from, *internal_to, *solution); - if (!storeSolution(solution)) - return; - + // map internal to external states auto find_or_create_external = [this](const InterfaceState* internal, bool& created) -> InterfaceState* { auto it = internal_to_external_.find(internal); if (it != internal_to_external_.end()) @@ -140,6 +138,9 @@ void ContainerBasePrivate::liftSolution(const SolutionBasePtr& solution, const I InterfaceState* external_from = find_or_create_external(internal_from, created_from); InterfaceState* external_to = find_or_create_external(internal_to, created_to); + if (!storeSolution(solution, external_from, external_to)) + return; + // connect solution to start/end state solution->setStartState(*external_from); solution->setEndState(*external_to); @@ -368,6 +369,8 @@ void SerialContainer::onNewSolution(const SolutionBase& current) { impl->liftSolution(solution, solution->internalStart(), solution->internalEnd()); } +void SerialContainer::onNewFailure(const Stage& child, const InterfaceState* from, const InterfaceState* to) {} + SerialContainer::SerialContainer(SerialContainerPrivate* impl) : ContainerBase(impl) {} SerialContainer::SerialContainer(const std::string& name) : SerialContainer(new SerialContainerPrivate(this, name)) {} @@ -602,6 +605,8 @@ ParallelContainerBase::ParallelContainerBase(ParallelContainerBasePrivate* impl) ParallelContainerBase::ParallelContainerBase(const std::string& name) : ParallelContainerBase(new ParallelContainerBasePrivate(this, name)) {} +void ParallelContainerBase::onNewFailure(const Stage& child, const InterfaceState* from, const InterfaceState* to) {} + void ParallelContainerBase::liftSolution(const SolutionBase& solution, double cost, std::string comment) { pimpl()->liftSolution(std::make_shared(this, &solution, cost, std::move(comment)), solution.start(), solution.end()); diff --git a/core/src/stage.cpp b/core/src/stage.cpp index 2f071763..6dc43749 100644 --- a/core/src/stage.cpp +++ b/core/src/stage.cpp @@ -129,13 +129,16 @@ void StagePrivate::validateConnectivity() const { } } -bool StagePrivate::storeSolution(const SolutionBasePtr& solution) { +bool StagePrivate::storeSolution(const SolutionBasePtr& solution, const InterfaceState* from, + const InterfaceState* to) { solution->setCreator(me()); if (introspection_) introspection_->registerSolution(*solution); if (solution->isFailure()) { ++num_failures_; + if (parent()) + parent()->onNewFailure(*me(), from, to); if (!storeFailures()) return false; // drop solution failures_.push_back(solution); @@ -150,7 +153,7 @@ void StagePrivate::sendForward(const InterfaceState& from, InterfaceState&& to, computeCost(from, to, *solution); - if (!storeSolution(solution)) + if (!storeSolution(solution, &from, nullptr)) return; // solution dropped me()->forwardProperties(from, to); @@ -172,7 +175,7 @@ void StagePrivate::sendBackward(InterfaceState&& from, const InterfaceState& to, computeCost(from, to, *solution); - if (!storeSolution(solution)) + if (!storeSolution(solution, nullptr, &to)) return; // solution dropped me()->forwardProperties(to, from); @@ -193,7 +196,7 @@ void StagePrivate::spawn(InterfaceState&& state, const SolutionBasePtr& solution computeCost(state, state, *solution); - if (!storeSolution(solution)) + if (!storeSolution(solution, nullptr, nullptr)) return; // solution dropped auto from = states_.insert(states_.end(), InterfaceState(state)); // copy @@ -213,7 +216,7 @@ void StagePrivate::spawn(InterfaceState&& state, const SolutionBasePtr& solution void StagePrivate::connect(const InterfaceState& from, const InterfaceState& to, const SolutionBasePtr& solution) { computeCost(from, to, *solution); - if (!storeSolution(solution)) + if (!storeSolution(solution, &from, &to)) return; // solution dropped solution->setStartState(from); @@ -714,7 +717,9 @@ void ConnectingPrivate::newState(Interface::iterator it, bool updated) { } bool ConnectingPrivate::canCompute() const { - return !pending.empty(); + // Do we still have feasible pending state pairs? + return !pending.empty() && pending.front().first->priority().enabled() && + pending.front().second->priority().enabled(); } void ConnectingPrivate::compute() { diff --git a/core/src/storage.cpp b/core/src/storage.cpp index f79ff86b..5a3b1bfb 100644 --- a/core/src/storage.cpp +++ b/core/src/storage.cpp @@ -127,14 +127,15 @@ Interface::container_type Interface::remove(iterator it) { } void Interface::updatePriority(InterfaceState* state, const InterfaceState::Priority& priority) { - if (priority < state->priority()) { // only allow decreasing of priority (smaller is better) - 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 - update(it); - if (notify_) - notify_(it, true); - } + if (priority == state->priority()) + return; // nothing to do + + 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 + update(it); // update position in ordered list + if (notify_) + notify_(it, true); // notify callback } void SolutionBase::setCreator(Stage* creator) { diff --git a/core/test/test_interface_state.cpp b/core/test/test_interface_state.cpp index e86640bf..4e88f996 100644 --- a/core/test/test_interface_state.cpp +++ b/core/test/test_interface_state.cpp @@ -68,7 +68,7 @@ TEST(Interface, update) { EXPECT_THAT(i.depths(), ::testing::ElementsAreArray({ 5, 3 })); i.updatePriority(*i.begin(), Prio(6, 0, false)); - EXPECT_THAT(i.depths(), ::testing::ElementsAreArray({ 5, 3 })); // larger priority is ignored + EXPECT_THAT(i.depths(), ::testing::ElementsAreArray({ 3, 6 })); } using PrioPair = std::pair; diff --git a/core/test/test_serial.cpp b/core/test/test_serial.cpp index 526367ff..b7a0f900 100644 --- a/core/test/test_serial.cpp +++ b/core/test/test_serial.cpp @@ -172,7 +172,7 @@ TEST(ConnectConnect, Pruning) { ++expected_cost; } EXPECT_EQ(c2->calls_, 3u); - // EXPECT_EQ(c1->calls_, 6u); // TODO: avoid compute() calls on failure of remaining part + EXPECT_EQ(c1->calls_, 6u); // TODO: avoid compute() calls on failure of remaining part } // https://github.com/ros-planning/moveit_task_constructor/issues/218