mirror of
https://github.com/moveit/moveit_task_constructor.git
synced 2025-11-04 14:49:57 +08:00
Rename Interface::Status FAILED -> ARMED
... to better indicate that such a state can be immediately re-enabled.
This commit is contained in:
parent
1dda3d14d8
commit
3c4ef68dbe
@ -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);
|
||||
|
||||
@ -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); }
|
||||
|
||||
@ -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);
|
||||
|
||||
@ -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));
|
||||
|
||||
@ -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) {
|
||||
|
||||
@ -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));
|
||||
|
||||
Loading…
Reference in New Issue
Block a user