From 40b00c61d2c060d1df338a94f0ef4be702d28983 Mon Sep 17 00:00:00 2001 From: v4hn Date: Mon, 20 Sep 2021 16:03:54 +0200 Subject: [PATCH] DISABLED_FAILED -> FAILED Failed states are *not* disabled, they just failed connecting (for now). --- core/include/moveit/task_constructor/storage.h | 4 +++- core/src/container.cpp | 14 +++++++------- core/src/stage.cpp | 7 ++++--- core/src/storage.cpp | 2 +- core/test/test_interface_state.cpp | 8 ++++---- 5 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 08a03926..f115dff6 100644 --- a/core/include/moveit/task_constructor/storage.h +++ b/core/include/moveit/task_constructor/storage.h @@ -83,7 +83,7 @@ public: { 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 + 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. @@ -100,6 +100,8 @@ 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 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 34eb445d..02bec87e 100644 --- a/core/src/container.cpp +++ b/core/src/container.cpp @@ -114,19 +114,19 @@ void ContainerBasePrivate::onNewFailure(const Stage& child, const InterfaceState break; case PROPAGATE_FORWARDS: // mark from as failed (backwards) - setStatus(from, InterfaceState::Status::DISABLED_FAILED); + setStatus(from, InterfaceState::Status::FAILED); break; case PROPAGATE_BACKWARDS: // mark to as failed (forwards) - setStatus(to, InterfaceState::Status::DISABLED_FAILED); + setStatus(to, InterfaceState::Status::FAILED); break; case CONNECT: if (const Connecting* conn = dynamic_cast(&child)) { auto cimpl = conn->pimpl(); if (!cimpl->hasPendingOpposites(from)) - setStatus(from, InterfaceState::Status::DISABLED_FAILED); + setStatus(from, InterfaceState::Status::FAILED); if (!cimpl->hasPendingOpposites(to)) - setStatus(to, InterfaceState::Status::DISABLED_FAILED); + setStatus(to, InterfaceState::Status::FAILED); } break; } @@ -172,13 +172,13 @@ void ContainerBasePrivate::setStatus(const InterfaceState* s, InterfaceState::St } // To break symmetry between both ends of a partial solution sequence that gets disabled, - // we mark the first state with DISABLED_FAILED and all other states down the tree only with DISABLED. + // we mark the first state with 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. // For details, see: https://github.com/ros-planning/moveit_task_constructor/pull/221 - if (status == InterfaceState::DISABLED_FAILED) - status = InterfaceState::DISABLED; // only the first state is marked as FAILED + if (status == InterfaceState::Status::FAILED) + status = InterfaceState::Status::DISABLED; // only the first state is marked as FAILED // traverse solution tree for (const SolutionBase* successor : trajectories(*s)) diff --git a/core/src/stage.cpp b/core/src/stage.cpp index 80972e31..cdda0835 100644 --- a/core/src/stage.cpp +++ b/core/src/stage.cpp @@ -724,9 +724,10 @@ void ConnectingPrivate::newState(Interface::iterator it, bool updated) { 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 DISABLED_FAILED - if (oit->priority().status() == InterfaceState::DISABLED_FAILED) - oit->owner()->updatePriority(&*oit, InterfaceState::Priority(oit->priority(), InterfaceState::ENABLED)); + // re-enable the opposing state oit if its status is FAILED + if (oit->priority().status() == InterfaceState::Status::FAILED) + 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 9fc755d0..372e66a1 100644 --- a/core/src/storage.cpp +++ b/core/src/storage.cpp @@ -149,7 +149,7 @@ std::ostream& operator<<(std::ostream& os, const InterfaceState::Priority& prio) static const char* prefix[] = { "\033[32me:", // ENABLED - green "\033[33md:", // DISABLED - yellow - "\033[31mf:", // DISABLED_FAILED - red + "\033[31mf:", // FAILED - red }; static const char* color_reset = "\033[m"; os << prefix[prio.status()] << prio.depth() << ":" << prio.cost() << color_reset; diff --git a/core/test/test_interface_state.cpp b/core/test/test_interface_state.cpp index 73d6fdae..9ca6af53 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_FAILED; + auto dstart = InterfaceState::Status::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_FAILED)); + i.updatePriority(*i.begin(), Prio(6, 0, InterfaceState::Status::FAILED)); EXPECT_THAT(i.depths(), ::testing::ElementsAreArray({ 3, 6 })); } @@ -86,8 +86,8 @@ TEST(StatePairs, compare) { EXPECT_TRUE(pair(Prio(1, 0), Prio(0, 1)) < pair(Prio(1, 1), Prio(0, 1))); EXPECT_TRUE(pair(Prio(1, 1), Prio(1, 1)) < pair(Prio(1, 0), Prio(0, 0))); - auto good = InterfaceState::ENABLED; - auto bad = InterfaceState::DISABLED_FAILED; + 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));