diff --git a/core/include/moveit/task_constructor/storage.h b/core/include/moveit/task_constructor/storage.h index 2b52064b..a9f07dff 100644 --- a/core/include/moveit/task_constructor/storage.h +++ b/core/include/moveit/task_constructor/storage.h @@ -79,9 +79,9 @@ class InterfaceState public: enum Status { - ENABLED, - DISABLED_START, // state that was the root cause for disabling due to failure - DISABLED_END // state on the other end of a disabled solution sequence + ENABLED, // state is actively considered during planning + DISABLED, // state is disabled because a required connected state failed + DISABLED_FAILED, // state that failed, causing the whole partial solution to be disabled }; /** InterfaceStates are ordered according to two values: * Depth of interlinked trajectory parts and accumulated trajectory costs along that path. diff --git a/core/src/container.cpp b/core/src/container.cpp index f3fcf691..c0adf36b 100644 --- a/core/src/container.cpp +++ b/core/src/container.cpp @@ -408,23 +408,24 @@ void SerialContainer::onNewSolution(const SolutionBase& current) { void SerialContainer::onNewFailure(const Stage& child, const InterfaceState* from, const InterfaceState* to) { switch (child.pimpl()->interfaceFlags()) { case GENERATE: - break; // just ignore: the pair of (new) states isn't known to us anyway + // 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! - - case PROPAGATE_FORWARDS: // mark from as dead (backwards) - StagePrivate::setStatus(from, InterfaceState::Status::DISABLED_START); break; - case PROPAGATE_BACKWARDS: // mark to as dead (forwards) - StagePrivate::setStatus(to, InterfaceState::Status::DISABLED_START); + + case PROPAGATE_FORWARDS: // mark from as failed (backwards) + StagePrivate::setStatus(from, InterfaceState::Status::DISABLED_FAILED); + break; + case PROPAGATE_BACKWARDS: // mark to as failed (forwards) + StagePrivate::setStatus(to, InterfaceState::Status::DISABLED_FAILED); break; case CONNECT: if (const Connecting* conn = dynamic_cast(&child)) { auto cimpl = conn->pimpl(); if (!cimpl->hasPendingOpposites(from)) - StagePrivate::setStatus(from, InterfaceState::Status::DISABLED_START); + StagePrivate::setStatus(from, InterfaceState::Status::DISABLED_FAILED); if (!cimpl->hasPendingOpposites(to)) - StagePrivate::setStatus(to, InterfaceState::Status::DISABLED_START); + StagePrivate::setStatus(to, InterfaceState::Status::DISABLED_FAILED); } break; } diff --git a/core/src/stage.cpp b/core/src/stage.cpp index b3b3fd3c..7d73cbad 100644 --- a/core/src/stage.cpp +++ b/core/src/stage.cpp @@ -158,12 +158,12 @@ void StagePrivate::setStatus(const InterfaceState* s, InterfaceState::Status sta s->owner()->updatePriority(const_cast(s), InterfaceState::Priority(s->priority(), status)); // To break symmetry between both ends of a partial solution sequence that gets disabled, - // we mark the start state with START and all other states down the tree with END. - // This allows us to re-enable the START side, while not (yet) consider the END side, - // when a new states arrives in a Connecting stage. - // The END side is only re-enabled if the START side actually was connected with a solution. - if (status == InterfaceState::Status::DISABLED_START) - status = InterfaceState::Status::DISABLED_END; // ensure that only the first state is marked as START + // we mark the first state with DISABLED_FAILED and all other states down the tree only with DISABLED. + // This allows us to re-enable the FAILED side, while not (yet) consider the DISABLED states again, + // when new states arrive in a Connecting stage. + // All DISABLED states are only re-enabled if the FAILED state actually gets connected. + if (status == InterfaceState::DISABLED_FAILED) + status = InterfaceState::DISABLED; // only the first state is marked as FAILED // traverse solution tree for (const SolutionBase* successor : trajectories(s)) @@ -729,7 +729,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().status() == InterfaceState::Status::DISABLED_END) + if (it->priority().status() == InterfaceState::DISABLED) // remove all pending pairs involving this state pending.remove_if([it](const StatePair& p) { return std::get()>(p) == it; }); else @@ -738,10 +738,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_END + // 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 DISABLED_START - if (oit->priority().status() == InterfaceState::DISABLED_START) + // re-enable the opposing state oit if its status is DISABLED_FAILED + if (oit->priority().status() == InterfaceState::DISABLED_FAILED) oit->owner()->updatePriority(&*oit, InterfaceState::Priority(oit->priority(), InterfaceState::ENABLED)); pending.insert(make_pair(it, oit)); } diff --git a/core/test/test_interface_state.cpp b/core/test/test_interface_state.cpp index 1de95a75..73d6fdae 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::DISABLED_START; + auto dstart = InterfaceState::DISABLED_FAILED; 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::DISABLED_START)); + i.updatePriority(*i.begin(), Prio(6, 0, InterfaceState::DISABLED_FAILED)); EXPECT_THAT(i.depths(), ::testing::ElementsAreArray({ 3, 6 })); } @@ -87,7 +87,7 @@ TEST(StatePairs, compare) { EXPECT_TRUE(pair(Prio(1, 1), Prio(1, 1)) < pair(Prio(1, 0), Prio(0, 0))); auto good = InterfaceState::ENABLED; - auto bad = InterfaceState::DISABLED_START; + auto bad = InterfaceState::DISABLED_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));