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());
|
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);
|
||||||
|
|||||||
@ -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); }
|
||||||
|
|||||||
@ -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);
|
||||||
|
|||||||
@ -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));
|
||||||
|
|||||||
@ -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) {
|
||||||
|
|||||||
@ -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));
|
||||||
|
|||||||
Loading…
Reference in New Issue
Block a user