mirror of
https://github.com/moveit/moveit_task_constructor.git
synced 2025-11-04 14:49:57 +08:00
DISABLED_FAILED -> FAILED
Failed states are *not* disabled, they just failed connecting (for now).
This commit is contained in:
parent
3b9f6ee519
commit
40b00c61d2
@ -83,7 +83,7 @@ public:
|
|||||||
{
|
{
|
||||||
ENABLED, // state is actively considered during planning
|
ENABLED, // state is actively considered during planning
|
||||||
DISABLED, // state is disabled because a required connected state failed
|
DISABLED, // state is disabled because a required connected state failed
|
||||||
DISABLED_FAILED, // state that failed, causing the whole partial solution to be disabled
|
FAILED, // state that failed, causing the whole partial solution to be disabled
|
||||||
};
|
};
|
||||||
/** InterfaceStates are ordered according to two values:
|
/** InterfaceStates are ordered according to two values:
|
||||||
* Depth of interlinked trajectory parts and accumulated trajectory costs along that path.
|
* Depth of interlinked trajectory parts and accumulated trajectory costs along that path.
|
||||||
@ -100,6 +100,8 @@ 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 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); }
|
||||||
|
|
||||||
|
|||||||
@ -114,19 +114,19 @@ 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>(from, InterfaceState::Status::DISABLED_FAILED);
|
setStatus<Interface::BACKWARD>(from, InterfaceState::Status::FAILED);
|
||||||
break;
|
break;
|
||||||
case PROPAGATE_BACKWARDS: // mark to as failed (forwards)
|
case PROPAGATE_BACKWARDS: // mark to as failed (forwards)
|
||||||
setStatus<Interface::FORWARD>(to, InterfaceState::Status::DISABLED_FAILED);
|
setStatus<Interface::FORWARD>(to, InterfaceState::Status::FAILED);
|
||||||
break;
|
break;
|
||||||
|
|
||||||
case CONNECT:
|
case CONNECT:
|
||||||
if (const Connecting* conn = dynamic_cast<const Connecting*>(&child)) {
|
if (const Connecting* conn = dynamic_cast<const Connecting*>(&child)) {
|
||||||
auto cimpl = conn->pimpl();
|
auto cimpl = conn->pimpl();
|
||||||
if (!cimpl->hasPendingOpposites<Interface::FORWARD>(from))
|
if (!cimpl->hasPendingOpposites<Interface::FORWARD>(from))
|
||||||
setStatus<Interface::BACKWARD>(from, InterfaceState::Status::DISABLED_FAILED);
|
setStatus<Interface::BACKWARD>(from, InterfaceState::Status::FAILED);
|
||||||
if (!cimpl->hasPendingOpposites<Interface::BACKWARD>(to))
|
if (!cimpl->hasPendingOpposites<Interface::BACKWARD>(to))
|
||||||
setStatus<Interface::FORWARD>(to, InterfaceState::Status::DISABLED_FAILED);
|
setStatus<Interface::FORWARD>(to, InterfaceState::Status::FAILED);
|
||||||
}
|
}
|
||||||
break;
|
break;
|
||||||
}
|
}
|
||||||
@ -172,13 +172,13 @@ void ContainerBasePrivate::setStatus(const InterfaceState* s, InterfaceState::St
|
|||||||
}
|
}
|
||||||
|
|
||||||
// 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 DISABLED_FAILED and all other states down the tree only with DISABLED.
|
// we mark the first state with FAILED and all other states down the tree only with DISABLED.
|
||||||
// This allows us to re-enable the FAILED side, while not (yet) consider the DISABLED states again,
|
// This allows us to re-enable the FAILED side, while not (yet) consider the DISABLED states again,
|
||||||
// when new states arrive in a Connecting stage.
|
// when new states arrive in a Connecting stage.
|
||||||
// All DISABLED states are only re-enabled if the FAILED state actually gets connected.
|
// All DISABLED 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
|
// For details, see: https://github.com/ros-planning/moveit_task_constructor/pull/221
|
||||||
if (status == InterfaceState::DISABLED_FAILED)
|
if (status == InterfaceState::Status::FAILED)
|
||||||
status = InterfaceState::DISABLED; // only the first state is marked as FAILED
|
status = InterfaceState::Status::DISABLED; // only the first state is marked as FAILED
|
||||||
|
|
||||||
// traverse solution tree
|
// traverse solution tree
|
||||||
for (const SolutionBase* successor : trajectories<dir>(*s))
|
for (const SolutionBase* successor : trajectories<dir>(*s))
|
||||||
|
|||||||
@ -724,9 +724,10 @@ void ConnectingPrivate::newState(Interface::iterator it, bool updated) {
|
|||||||
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) {
|
||||||
// Don't re-enable states that are marked DISABLED
|
// Don't re-enable states that are marked DISABLED
|
||||||
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 DISABLED_FAILED
|
// re-enable the opposing state oit if its status is FAILED
|
||||||
if (oit->priority().status() == InterfaceState::DISABLED_FAILED)
|
if (oit->priority().status() == InterfaceState::Status::FAILED)
|
||||||
oit->owner()->updatePriority(&*oit, InterfaceState::Priority(oit->priority(), InterfaceState::ENABLED));
|
oit->owner()->updatePriority(&*oit,
|
||||||
|
InterfaceState::Priority(oit->priority(), InterfaceState::Status::ENABLED));
|
||||||
pending.insert(make_pair<other>(it, oit));
|
pending.insert(make_pair<other>(it, oit));
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
|||||||
@ -149,7 +149,7 @@ std::ostream& operator<<(std::ostream& os, const InterfaceState::Priority& prio)
|
|||||||
static const char* prefix[] = {
|
static const char* prefix[] = {
|
||||||
"\033[32me:", // ENABLED - green
|
"\033[32me:", // ENABLED - green
|
||||||
"\033[33md:", // DISABLED - yellow
|
"\033[33md:", // DISABLED - yellow
|
||||||
"\033[31mf:", // DISABLED_FAILED - red
|
"\033[31mf:", // FAILED - red
|
||||||
};
|
};
|
||||||
static const char* color_reset = "\033[m";
|
static const char* color_reset = "\033[m";
|
||||||
os << prefix[prio.status()] << prio.depth() << ":" << prio.cost() << color_reset;
|
os << prefix[prio.status()] << prio.depth() << ":" << prio.cost() << color_reset;
|
||||||
|
|||||||
@ -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::DISABLED_FAILED;
|
auto dstart = InterfaceState::Status::FAILED;
|
||||||
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::DISABLED_FAILED));
|
i.updatePriority(*i.begin(), Prio(6, 0, InterfaceState::Status::FAILED));
|
||||||
EXPECT_THAT(i.depths(), ::testing::ElementsAreArray({ 3, 6 }));
|
EXPECT_THAT(i.depths(), ::testing::ElementsAreArray({ 3, 6 }));
|
||||||
}
|
}
|
||||||
|
|
||||||
@ -86,8 +86,8 @@ TEST(StatePairs, compare) {
|
|||||||
EXPECT_TRUE(pair(Prio(1, 0), Prio(0, 1)) < pair(Prio(1, 1), Prio(0, 1)));
|
EXPECT_TRUE(pair(Prio(1, 0), Prio(0, 1)) < pair(Prio(1, 1), Prio(0, 1)));
|
||||||
EXPECT_TRUE(pair(Prio(1, 1), Prio(1, 1)) < pair(Prio(1, 0), Prio(0, 0)));
|
EXPECT_TRUE(pair(Prio(1, 1), Prio(1, 1)) < pair(Prio(1, 0), Prio(0, 0)));
|
||||||
|
|
||||||
auto good = InterfaceState::ENABLED;
|
auto good = InterfaceState::Status::ENABLED;
|
||||||
auto bad = InterfaceState::DISABLED_FAILED;
|
auto bad = InterfaceState::Status::FAILED;
|
||||||
EXPECT_TRUE(pair(good, good) < pair(good, bad));
|
EXPECT_TRUE(pair(good, good) < pair(good, bad));
|
||||||
EXPECT_TRUE(pair(good, good) < pair(bad, good));
|
EXPECT_TRUE(pair(good, good) < pair(bad, good));
|
||||||
EXPECT_TRUE(pair(bad, good) < pair(good, bad));
|
EXPECT_TRUE(pair(bad, good) < pair(good, bad));
|
||||||
|
|||||||
Loading…
Reference in New Issue
Block a user