mirror of
https://github.com/moveit/moveit_task_constructor.git
synced 2025-11-04 14:49:57 +08:00
rename Status values
Start and End are already used for an entirely different concept, so if anyone ever wants to read this code, we should use new terms instead. Because the source state is the disabled state that *failed* to extend, triggering the whole subtree to be disabled, I went for the new terms DISABLED and DISABLED_FAILED.
This commit is contained in:
parent
c0100ff775
commit
bca01e8aa7
@ -79,9 +79,9 @@ class InterfaceState
|
|||||||
public:
|
public:
|
||||||
enum Status
|
enum Status
|
||||||
{
|
{
|
||||||
ENABLED,
|
ENABLED, // state is actively considered during planning
|
||||||
DISABLED_START, // state that was the root cause for disabling due to failure
|
DISABLED, // state is disabled because a required connected state failed
|
||||||
DISABLED_END // state on the other end of a disabled solution sequence
|
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.
|
||||||
|
|||||||
@ -408,23 +408,24 @@ void SerialContainer::onNewSolution(const SolutionBase& current) {
|
|||||||
void SerialContainer::onNewFailure(const Stage& child, const InterfaceState* from, const InterfaceState* to) {
|
void SerialContainer::onNewFailure(const Stage& child, const InterfaceState* from, const InterfaceState* to) {
|
||||||
switch (child.pimpl()->interfaceFlags()) {
|
switch (child.pimpl()->interfaceFlags()) {
|
||||||
case GENERATE:
|
case GENERATE:
|
||||||
break; // just ignore: the pair of (new) states isn't known to us anyway
|
// just ignore: the pair of (new) states isn't known to us anyway
|
||||||
// TODO: If child is a container, from and to might have associated solutions already!
|
// TODO: If child is a container, from and to might have associated solutions already!
|
||||||
|
|
||||||
case PROPAGATE_FORWARDS: // mark from as dead (backwards)
|
|
||||||
StagePrivate::setStatus<Interface::BACKWARD>(from, InterfaceState::Status::DISABLED_START);
|
|
||||||
break;
|
break;
|
||||||
case PROPAGATE_BACKWARDS: // mark to as dead (forwards)
|
|
||||||
StagePrivate::setStatus<Interface::FORWARD>(to, InterfaceState::Status::DISABLED_START);
|
case PROPAGATE_FORWARDS: // mark from as failed (backwards)
|
||||||
|
StagePrivate::setStatus<Interface::BACKWARD>(from, InterfaceState::Status::DISABLED_FAILED);
|
||||||
|
break;
|
||||||
|
case PROPAGATE_BACKWARDS: // mark to as failed (forwards)
|
||||||
|
StagePrivate::setStatus<Interface::FORWARD>(to, InterfaceState::Status::DISABLED_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))
|
||||||
StagePrivate::setStatus<Interface::BACKWARD>(from, InterfaceState::Status::DISABLED_START);
|
StagePrivate::setStatus<Interface::BACKWARD>(from, InterfaceState::Status::DISABLED_FAILED);
|
||||||
if (!cimpl->hasPendingOpposites<Interface::BACKWARD>(to))
|
if (!cimpl->hasPendingOpposites<Interface::BACKWARD>(to))
|
||||||
StagePrivate::setStatus<Interface::FORWARD>(to, InterfaceState::Status::DISABLED_START);
|
StagePrivate::setStatus<Interface::FORWARD>(to, InterfaceState::Status::DISABLED_FAILED);
|
||||||
}
|
}
|
||||||
break;
|
break;
|
||||||
}
|
}
|
||||||
|
|||||||
@ -158,12 +158,12 @@ void StagePrivate::setStatus(const InterfaceState* s, InterfaceState::Status sta
|
|||||||
s->owner()->updatePriority(const_cast<InterfaceState*>(s), InterfaceState::Priority(s->priority(), status));
|
s->owner()->updatePriority(const_cast<InterfaceState*>(s), InterfaceState::Priority(s->priority(), status));
|
||||||
|
|
||||||
// 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 start state with START and all other states down the tree with END.
|
// we mark the first state with DISABLED_FAILED and all other states down the tree only with DISABLED.
|
||||||
// This allows us to re-enable the START side, while not (yet) consider the END side,
|
// This allows us to re-enable the FAILED side, while not (yet) consider the DISABLED states again,
|
||||||
// when a new states arrives in a Connecting stage.
|
// when new states arrive in a Connecting stage.
|
||||||
// The END side is only re-enabled if the START side actually was connected with a solution.
|
// All DISABLED states are only re-enabled if the FAILED state actually gets connected.
|
||||||
if (status == InterfaceState::Status::DISABLED_START)
|
if (status == InterfaceState::DISABLED_FAILED)
|
||||||
status = InterfaceState::Status::DISABLED_END; // ensure that only the first state is marked as START
|
status = InterfaceState::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))
|
||||||
@ -729,7 +729,7 @@ ConnectingPrivate::StatePair ConnectingPrivate::make_pair<Interface::FORWARD>(In
|
|||||||
template <Interface::Direction other>
|
template <Interface::Direction other>
|
||||||
void ConnectingPrivate::newState(Interface::iterator it, bool updated) {
|
void ConnectingPrivate::newState(Interface::iterator it, bool updated) {
|
||||||
if (updated) { // many pairs might be affected: resort
|
if (updated) { // many pairs might be affected: resort
|
||||||
if (it->priority().status() == InterfaceState::Status::DISABLED_END)
|
if (it->priority().status() == InterfaceState::DISABLED)
|
||||||
// remove all pending pairs involving this state
|
// remove all pending pairs involving this state
|
||||||
pending.remove_if([it](const StatePair& p) { return std::get<opposite<other>()>(p) == it; });
|
pending.remove_if([it](const StatePair& p) { return std::get<opposite<other>()>(p) == it; });
|
||||||
else
|
else
|
||||||
@ -738,10 +738,10 @@ void ConnectingPrivate::newState(Interface::iterator it, bool updated) {
|
|||||||
assert(it->priority().enabled()); // new solutions are feasible, aren't they?
|
assert(it->priority().enabled()); // new solutions are feasible, aren't they?
|
||||||
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) {
|
||||||
// Don't re-enable states that are marked DISABLED_END
|
// 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_START
|
// re-enable the opposing state oit if its status is DISABLED_FAILED
|
||||||
if (oit->priority().status() == InterfaceState::DISABLED_START)
|
if (oit->priority().status() == InterfaceState::DISABLED_FAILED)
|
||||||
oit->owner()->updatePriority(&*oit, InterfaceState::Priority(oit->priority(), InterfaceState::ENABLED));
|
oit->owner()->updatePriority(&*oit, InterfaceState::Priority(oit->priority(), InterfaceState::ENABLED));
|
||||||
pending.insert(make_pair<other>(it, oit));
|
pending.insert(make_pair<other>(it, oit));
|
||||||
}
|
}
|
||||||
|
|||||||
@ -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_START;
|
auto dstart = InterfaceState::DISABLED_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_START));
|
i.updatePriority(*i.begin(), Prio(6, 0, InterfaceState::DISABLED_FAILED));
|
||||||
EXPECT_THAT(i.depths(), ::testing::ElementsAreArray({ 3, 6 }));
|
EXPECT_THAT(i.depths(), ::testing::ElementsAreArray({ 3, 6 }));
|
||||||
}
|
}
|
||||||
|
|
||||||
@ -87,7 +87,7 @@ TEST(StatePairs, compare) {
|
|||||||
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::ENABLED;
|
||||||
auto bad = InterfaceState::DISABLED_START;
|
auto bad = InterfaceState::DISABLED_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