From bca01e8aa7d383446d167a4b2a8ea4e9eea0a51f Mon Sep 17 00:00:00 2001 From: v4hn Date: Tue, 16 Mar 2021 23:54:37 +0100 Subject: [PATCH] rename Status values Start and End are already used for an entirely different concept, so if anyone ever wants to read this code, we should use new terms instead. Because the source state is the disabled state that *failed* to extend, triggering the whole subtree to be disabled, I went for the new terms DISABLED and DISABLED_FAILED. --- .../include/moveit/task_constructor/storage.h | 6 +++--- core/src/container.cpp | 17 ++++++++-------- core/src/stage.cpp | 20 +++++++++---------- core/test/test_interface_state.cpp | 6 +++--- 4 files changed, 25 insertions(+), 24 deletions(-) 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));