Rename Interface::Status FAILED -> ARMED

... to better indicate that such a state can be immediately re-enabled.
This commit is contained in:
Robert Haschke 2021-11-20 14:29:39 +01:00
parent 1dda3d14d8
commit 3c4ef68dbe
6 changed files with 19 additions and 22 deletions

View File

@ -148,7 +148,7 @@ protected:
child->setNextStarts(allowed ? pending_forward_ : InterfacePtr()); 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 <Interface::Direction dir> template <Interface::Direction dir>
void setStatus(const Stage* creator, const InterfaceState* source, const InterfaceState* target, void setStatus(const Stage* creator, const InterfaceState* source, const InterfaceState* target,
InterfaceState::Status status); InterfaceState::Status status);

View File

@ -82,8 +82,8 @@ public:
enum Status enum Status
{ {
ENABLED, // state is actively considered during planning ENABLED, // state is actively considered during planning
PRUNED, // state is disabled because a required connected state failed ARMED, // disabled state in a Connecting interface that will become re-enabled with a new opposite state
FAILED, // state that failed, causing the whole partial solution to be disabled PRUNED, // disabled state on a pruned solution branch
}; };
static const char* STATUS_COLOR[]; static const char* STATUS_COLOR[];
@ -102,8 +102,6 @@ public:
inline Status status() const { return std::get<0>(*this); } inline Status status() const { return std::get<0>(*this); }
inline bool enabled() const { return std::get<0>(*this) == ENABLED; } 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 unsigned int depth() const { return std::get<1>(*this); }
inline double cost() const { return std::get<2>(*this); } inline double cost() const { return std::get<2>(*this); }

View File

@ -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, // 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. // we mark the first state with ARMED and all other states down the tree with PRUNED.
// This allows us to re-enable the FAILED side, while not (yet) consider the PRUNED states again, // This allows us to re-enable the ARMED state, but not the PRUNED states,
// when new states arrive in a Connecting stage. // when new states arrive in a Connecting stage.
// All PRUNED states are only re-enabled if the FAILED state actually gets connected. // For details, https://github.com/ros-planning/moveit_task_constructor/pull/309#issuecomment-974636202
// For details, see: https://github.com/ros-planning/moveit_task_constructor/pull/221 if (status == InterfaceState::Status::ARMED)
if (status == InterfaceState::Status::FAILED) status = InterfaceState::Status::PRUNED; // only the first state is marked as ARMED
status = InterfaceState::Status::PRUNED; // only the first state is marked as FAILED
// traverse solution tree // traverse solution tree
for (const SolutionBase* successor : trajectories<dir>(*target)) for (const SolutionBase* successor : trajectories<dir>(*target))
@ -175,15 +174,15 @@ void ContainerBasePrivate::onNewFailure(const Stage& child, const InterfaceState
break; break;
case PROPAGATE_FORWARDS: // mark from as failed (backwards) case PROPAGATE_FORWARDS: // mark from as failed (backwards)
setStatus<Interface::BACKWARD>(nullptr, nullptr, from, InterfaceState::Status::FAILED); setStatus<Interface::BACKWARD>(nullptr, nullptr, from, InterfaceState::Status::PRUNED);
break; break;
case PROPAGATE_BACKWARDS: // mark to as failed (forwards) case PROPAGATE_BACKWARDS: // mark to as failed (forwards)
setStatus<Interface::FORWARD>(nullptr, nullptr, to, InterfaceState::Status::FAILED); setStatus<Interface::FORWARD>(nullptr, nullptr, to, InterfaceState::Status::PRUNED);
break; break;
case CONNECT: case CONNECT:
setStatus<Interface::BACKWARD>(&child, to, from, InterfaceState::Status::FAILED); setStatus<Interface::BACKWARD>(&child, to, from, InterfaceState::Status::ARMED);
setStatus<Interface::FORWARD>(&child, from, to, InterfaceState::Status::FAILED); setStatus<Interface::FORWARD>(&child, from, to, InterfaceState::Status::ARMED);
break; break;
} }
// printChildrenInterfaces(*this, false, child); // printChildrenInterfaces(*this, false, child);

View File

@ -718,10 +718,10 @@ void ConnectingPrivate::newState(Interface::iterator it, bool updated) {
InterfacePtr other_interface = pullInterface(other); InterfacePtr other_interface = pullInterface(other);
for (Interface::iterator oit = other_interface->begin(), oend = other_interface->end(); oit != oend; ++oit) { for (Interface::iterator oit = other_interface->begin(), oend = other_interface->end(); oit != oend; ++oit) {
if (static_cast<Connecting*>(me_)->compatible(*it, *oit)) { if (static_cast<Connecting*>(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 // but don't re-enable states that are marked DISABLED
// https://github.com/ros-planning/moveit_task_constructor/pull/221 // 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, oit->owner()->updatePriority(oit,
InterfaceState::Priority(oit->priority(), InterfaceState::Status::ENABLED)); InterfaceState::Priority(oit->priority(), InterfaceState::Status::ENABLED));
pending.insert(make_pair<other>(it, oit)); pending.insert(make_pair<other>(it, oit));

View File

@ -161,8 +161,8 @@ std::ostream& operator<<(std::ostream& os, const Interface& interface) {
} }
const char* InterfaceState::STATUS_COLOR[] = { const char* InterfaceState::STATUS_COLOR[] = {
"\033[32m", // ENABLED - green "\033[32m", // ENABLED - green
"\033[33m", // PRUNED - yellow "\033[33m", // ARMED - yellow
"\033[31m", // FAILED - red "\033[31m", // PRUNED - red
"\033[m" // reset "\033[m" // reset
}; };
std::ostream& operator<<(std::ostream& os, const InterfaceState::Priority& prio) { std::ostream& operator<<(std::ostream& os, const InterfaceState::Priority& prio) {

View File

@ -19,7 +19,7 @@ TEST(InterfaceStatePriority, compare) {
EXPECT_TRUE(Prio(1, 42) < Prio(0, 0)); EXPECT_TRUE(Prio(1, 42) < Prio(0, 0));
EXPECT_TRUE(Prio(0, 0) < Prio(0, 42)); // at same depth, higher cost is larger 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(0, 0, dstart) == Prio(0, 0, dstart));
EXPECT_TRUE(Prio(1, 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)); 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)); i.updatePriority(*i.rbegin(), Prio(5, 0.0));
EXPECT_THAT(i.depths(), ::testing::ElementsAreArray({ 5, 3 })); 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 })); EXPECT_THAT(i.depths(), ::testing::ElementsAreArray({ 3, 6 }));
} }
@ -89,7 +89,7 @@ TEST(StatePairs, compare) {
auto good = InterfaceState::Status::ENABLED; auto good = InterfaceState::Status::ENABLED;
auto good_good = pair(Prio(0, 10, good), Prio(0, 0, good)); 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 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(bad, good));
EXPECT_TRUE(good_good < pair(good, bad)); EXPECT_TRUE(good_good < pair(good, bad));
EXPECT_TRUE(good_good < pair(bad, bad)); EXPECT_TRUE(good_good < pair(bad, bad));