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:
|
||||
enum Status
|
||||
{
|
||||
ENABLED,
|
||||
DISABLED_START, // state that was the root cause for disabling due to failure
|
||||
DISABLED_END // state on the other end of a disabled solution sequence
|
||||
ENABLED, // state is actively considered during planning
|
||||
DISABLED, // state is disabled because a required connected state failed
|
||||
DISABLED_FAILED, // state that failed, causing the whole partial solution to be disabled
|
||||
};
|
||||
/** InterfaceStates are ordered according to two values:
|
||||
* 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) {
|
||||
switch (child.pimpl()->interfaceFlags()) {
|
||||
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!
|
||||
|
||||
case PROPAGATE_FORWARDS: // mark from as dead (backwards)
|
||||
StagePrivate::setStatus<Interface::BACKWARD>(from, InterfaceState::Status::DISABLED_START);
|
||||
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;
|
||||
|
||||
case CONNECT:
|
||||
if (const Connecting* conn = dynamic_cast<const Connecting*>(&child)) {
|
||||
auto cimpl = conn->pimpl();
|
||||
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))
|
||||
StagePrivate::setStatus<Interface::FORWARD>(to, InterfaceState::Status::DISABLED_START);
|
||||
StagePrivate::setStatus<Interface::FORWARD>(to, InterfaceState::Status::DISABLED_FAILED);
|
||||
}
|
||||
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));
|
||||
|
||||
// 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.
|
||||
// This allows us to re-enable the START side, while not (yet) consider the END side,
|
||||
// when a new states arrives in a Connecting stage.
|
||||
// The END side is only re-enabled if the START side actually was connected with a solution.
|
||||
if (status == InterfaceState::Status::DISABLED_START)
|
||||
status = InterfaceState::Status::DISABLED_END; // ensure that only the first state is marked as START
|
||||
// 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 FAILED side, while not (yet) consider the DISABLED states again,
|
||||
// when new states arrive in a Connecting stage.
|
||||
// All DISABLED states are only re-enabled if the FAILED state actually gets connected.
|
||||
if (status == InterfaceState::DISABLED_FAILED)
|
||||
status = InterfaceState::DISABLED; // only the first state is marked as FAILED
|
||||
|
||||
// traverse solution tree
|
||||
for (const SolutionBase* successor : trajectories<dir>(s))
|
||||
@ -729,7 +729,7 @@ ConnectingPrivate::StatePair ConnectingPrivate::make_pair<Interface::FORWARD>(In
|
||||
template <Interface::Direction other>
|
||||
void ConnectingPrivate::newState(Interface::iterator it, bool updated) {
|
||||
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
|
||||
pending.remove_if([it](const StatePair& p) { return std::get<opposite<other>()>(p) == it; });
|
||||
else
|
||||
@ -738,10 +738,10 @@ void ConnectingPrivate::newState(Interface::iterator it, bool updated) {
|
||||
assert(it->priority().enabled()); // new solutions are feasible, aren't they?
|
||||
InterfacePtr other_interface = pullInterface(other);
|
||||
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)) {
|
||||
// re-enable the opposing state oit if its status is DISABLED_START
|
||||
if (oit->priority().status() == InterfaceState::DISABLED_START)
|
||||
// re-enable the opposing state oit if its status is DISABLED_FAILED
|
||||
if (oit->priority().status() == InterfaceState::DISABLED_FAILED)
|
||||
oit->owner()->updatePriority(&*oit, InterfaceState::Priority(oit->priority(), InterfaceState::ENABLED));
|
||||
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(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(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::DISABLED_START));
|
||||
i.updatePriority(*i.begin(), Prio(6, 0, InterfaceState::DISABLED_FAILED));
|
||||
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)));
|
||||
|
||||
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(bad, good));
|
||||
EXPECT_TRUE(pair(bad, good) < pair(good, bad));
|
||||
|
||||
Loading…
Reference in New Issue
Block a user