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