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());
}
/// 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>
void setStatus(const Stage* creator, const InterfaceState* source, const InterfaceState* target,
InterfaceState::Status status);

View File

@ -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); }

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,
// 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<dir>(*target))
@ -175,15 +174,15 @@ void ContainerBasePrivate::onNewFailure(const Stage& child, const InterfaceState
break;
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;
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;
case CONNECT:
setStatus<Interface::BACKWARD>(&child, to, from, InterfaceState::Status::FAILED);
setStatus<Interface::FORWARD>(&child, from, to, InterfaceState::Status::FAILED);
setStatus<Interface::BACKWARD>(&child, to, from, InterfaceState::Status::ARMED);
setStatus<Interface::FORWARD>(&child, from, to, InterfaceState::Status::ARMED);
break;
}
// printChildrenInterfaces(*this, false, child);

View File

@ -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<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
// 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<other>(it, oit));

View File

@ -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) {

View File

@ -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));