From 3c4ef68dbefd2b32949b99e34257441b85070265 Mon Sep 17 00:00:00 2001 From: Robert Haschke Date: Sat, 20 Nov 2021 14:29:39 +0100 Subject: [PATCH] Rename Interface::Status FAILED -> ARMED ... to better indicate that such a state can be immediately re-enabled. --- .../moveit/task_constructor/container_p.h | 2 +- .../include/moveit/task_constructor/storage.h | 6 ++---- core/src/container.cpp | 19 +++++++++---------- core/src/stage.cpp | 4 ++-- core/src/storage.cpp | 4 ++-- core/test/test_interface_state.cpp | 6 +++--- 6 files changed, 19 insertions(+), 22 deletions(-) diff --git a/core/include/moveit/task_constructor/container_p.h b/core/include/moveit/task_constructor/container_p.h index 3f73ee02..6202c141 100644 --- a/core/include/moveit/task_constructor/container_p.h +++ b/core/include/moveit/task_constructor/container_p.h @@ -148,7 +148,7 @@ protected: child->setNextStarts(allowed ? pending_forward_ : InterfacePtr()); } - /// Set ENABLED/PRUNED/FAILED status of a solution branch starting from target into the given direction + /// Set ENABLED/PRUNED status of a solution branch starting from target into the given direction template void setStatus(const Stage* creator, const InterfaceState* source, const InterfaceState* target, InterfaceState::Status status); diff --git a/core/include/moveit/task_constructor/storage.h b/core/include/moveit/task_constructor/storage.h index 22322a97..893dc09d 100644 --- a/core/include/moveit/task_constructor/storage.h +++ b/core/include/moveit/task_constructor/storage.h @@ -82,8 +82,8 @@ public: enum Status { ENABLED, // state is actively considered during planning - PRUNED, // state is disabled because a required connected state failed - FAILED, // state that failed, causing the whole partial solution to be disabled + ARMED, // disabled state in a Connecting interface that will become re-enabled with a new opposite state + PRUNED, // disabled state on a pruned solution branch }; static const char* STATUS_COLOR[]; @@ -102,8 +102,6 @@ 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 bool pruned() const { return std::get<0>(*this) == PRUNED; } 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 4c556806..adb9f841 100644 --- a/core/src/container.cpp +++ b/core/src/container.cpp @@ -153,13 +153,12 @@ void ContainerBasePrivate::setStatus(const Stage* creator, const InterfaceState* } // To break symmetry between both ends of a partial solution sequence that gets disabled, - // we mark the first state with FAILED and all other states down the tree only with PRUNED. - // This allows us to re-enable the FAILED side, while not (yet) consider the PRUNED states again, + // we mark the first state with ARMED and all other states down the tree with PRUNED. + // This allows us to re-enable the ARMED state, but not the PRUNED states, // when new states arrive in a Connecting stage. - // All PRUNED 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::Status::FAILED) - status = InterfaceState::Status::PRUNED; // only the first state is marked as FAILED + // For details, https://github.com/ros-planning/moveit_task_constructor/pull/309#issuecomment-974636202 + if (status == InterfaceState::Status::ARMED) + status = InterfaceState::Status::PRUNED; // only the first state is marked as ARMED // traverse solution tree for (const SolutionBase* successor : trajectories(*target)) @@ -175,15 +174,15 @@ void ContainerBasePrivate::onNewFailure(const Stage& child, const InterfaceState break; case PROPAGATE_FORWARDS: // mark from as failed (backwards) - setStatus(nullptr, nullptr, from, InterfaceState::Status::FAILED); + setStatus(nullptr, nullptr, from, InterfaceState::Status::PRUNED); break; case PROPAGATE_BACKWARDS: // mark to as failed (forwards) - setStatus(nullptr, nullptr, to, InterfaceState::Status::FAILED); + setStatus(nullptr, nullptr, to, InterfaceState::Status::PRUNED); break; case CONNECT: - setStatus(&child, to, from, InterfaceState::Status::FAILED); - setStatus(&child, from, to, InterfaceState::Status::FAILED); + setStatus(&child, to, from, InterfaceState::Status::ARMED); + setStatus(&child, from, to, InterfaceState::Status::ARMED); break; } // printChildrenInterfaces(*this, false, child); diff --git a/core/src/stage.cpp b/core/src/stage.cpp index ed1e97c4..667e9352 100644 --- a/core/src/stage.cpp +++ b/core/src/stage.cpp @@ -718,10 +718,10 @@ void ConnectingPrivate::newState(Interface::iterator it, bool updated) { InterfacePtr other_interface = pullInterface(other); for (Interface::iterator oit = other_interface->begin(), oend = other_interface->end(); oit != oend; ++oit) { if (static_cast(me_)->compatible(*it, *oit)) { - // re-enable the opposing state oit if its status is FAILED, + // re-enable the opposing state oit if its status is ARMED, // but don't re-enable states that are marked DISABLED // https://github.com/ros-planning/moveit_task_constructor/pull/221 - if (oit->priority().status() == InterfaceState::Status::FAILED) + if (oit->priority().status() == InterfaceState::Status::ARMED) 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 f88ac960..54f3a846 100644 --- a/core/src/storage.cpp +++ b/core/src/storage.cpp @@ -161,8 +161,8 @@ std::ostream& operator<<(std::ostream& os, const Interface& interface) { } const char* InterfaceState::STATUS_COLOR[] = { "\033[32m", // ENABLED - green - "\033[33m", // PRUNED - yellow - "\033[31m", // FAILED - red + "\033[33m", // ARMED - yellow + "\033[31m", // PRUNED - red "\033[m" // reset }; std::ostream& operator<<(std::ostream& os, const InterfaceState::Priority& prio) { diff --git a/core/test/test_interface_state.cpp b/core/test/test_interface_state.cpp index ec4261a2..65aeee36 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::Status::FAILED; + auto dstart = InterfaceState::Status::ARMED; 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::Status::FAILED)); + i.updatePriority(*i.begin(), Prio(6, 0, InterfaceState::Status::ARMED)); EXPECT_THAT(i.depths(), ::testing::ElementsAreArray({ 3, 6 })); } @@ -89,7 +89,7 @@ TEST(StatePairs, compare) { auto good = InterfaceState::Status::ENABLED; auto good_good = pair(Prio(0, 10, good), Prio(0, 0, good)); ASSERT_TRUE(good_good > pair(good, good)); // a bad status should reverse this relation - for (auto bad : { InterfaceState::Status::FAILED, InterfaceState::Status::PRUNED }) { + for (auto bad : { InterfaceState::Status::ARMED, InterfaceState::Status::PRUNED }) { EXPECT_TRUE(good_good < pair(bad, good)); EXPECT_TRUE(good_good < pair(good, bad)); EXPECT_TRUE(good_good < pair(bad, bad));