Replace Priority::enabled() with status()

The key to pruning in the Connecting stage was the following:
- Don't remove states during pruning, but only disable them.
  They might become re-enabled due to further input.
- Distinguish START and END sides of a disabled solution tree to break their symmetry.
  The START side from where we started disabling, can be re-enabled by a new partner state in
  Connecting, the END side must not. This was important as, otherwise, the states would simply
  get re-enabled immediately. The END side only gets re-enabled if the START side actually
  connects the whole solution branch.

Author: rhaschke

core/include/moveit/task_constructor/stage_p.h

@@ -157,6 +157,10 @@ class StagePrivate
 	/** compute cost for solution through configured CostTerm */
 	void computeCost(const InterfaceState& from, const InterfaceState& to, SolutionBase& solution);
 
+	/// Set ENABLED / DISABLED status of the solution tree starting from s into given direction
+	template <Interface::Direction dir>
+	static void setStatus(const InterfaceState* s, InterfaceState::Status status);
+
 protected:
 	// associated/owning Stage instance
 	Stage* me_;

core/include/moveit/task_constructor/storage.h

@@ -77,26 +77,33 @@ class InterfaceState
 	friend class SolutionBase;  // addIncoming() / addOutgoing() should be called only by SolutionBase
 	friend class Interface;  // allow Interface to set owner_ and priority_
 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
+	};
 	/** InterfaceStates are ordered according to two values:
 	 *  Depth of interlinked trajectory parts and accumulated trajectory costs along that path.
 	 *  Preference ordering considers high-depth first and within same depth, minimal cost paths.
 	 */
-	struct Priority : std::tuple<bool, unsigned int, double>
+	struct Priority : std::tuple<Status, unsigned int, double>
 	{
-		Priority(unsigned int depth, double cost, bool enabled = true)
-		  : std::tuple<bool, unsigned int, double>(enabled, depth, cost) {
+		Priority(unsigned int depth, double cost, Status status = ENABLED)
+		  : std::tuple<Status, unsigned int, double>(status, depth, cost) {
 			assert(std::isfinite(cost));
 		}
-		// Constructor copying depth and cost, but modifying enabled status
-		Priority(const Priority& other, bool enabled) : Priority(other.depth(), other.cost(), enabled) {}
+		// Constructor copying depth and cost, but modifying its status
+		Priority(const Priority& other, Status status) : Priority(other.depth(), other.cost(), status) {}
 
-		inline bool enabled() 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 unsigned int depth() const { return std::get<1>(*this); }
 		inline double cost() const { return std::get<2>(*this); }
 
 		// add priorities
 		Priority operator+(const Priority& other) const {
-			return Priority(depth() + other.depth(), cost() + other.cost(), enabled() || other.enabled());
+			return Priority(depth() + other.depth(), cost() + other.cost(), std::min(status(), other.status()));
 		}
 		// comparison operators
 		bool operator<(const Priority& rhs) const;

core/src/container.cpp

@@ -341,8 +341,8 @@ struct SolutionCollector
 inline void updateStatePrio(const InterfaceState* state, const InterfaceState::Priority& prio) {
 	if (state->owner())  // owner becomes NULL if state is removed from (pending) Interface list
 		state->owner()->updatePriority(const_cast<InterfaceState*>(state),
-		                               // update depth + cost, but keep current enabled status
-		                               InterfaceState::Priority(prio, state->priority().enabled()));
+		                               // update depth + cost, but keep current status
+		                               InterfaceState::Priority(prio, state->priority().status()));
 }
 
 template <Interface::Direction dir>
@@ -406,40 +406,26 @@ void SerialContainer::onNewSolution(const SolutionBase& current) {
 		impl->liftSolution(solution, solution->internalStart(), solution->internalEnd());
 }
 
-template <Interface::Direction dir>
-void markAsEnabled(const InterfaceState* s, bool enabled) {
-	if (s->priority().enabled() == enabled)
-		return;  // nothing changing
-
-	// actually enable/disable the state
-	if (s->owner())
-		s->owner()->updatePriority(const_cast<InterfaceState*>(s), InterfaceState::Priority(s->priority(), enabled));
-
-	// traverse solution tree
-	for (const SolutionBase* successor : trajectories<dir>(s))
-		markAsEnabled<dir>(state<dir>(*successor), enabled);
-}
-
 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
 			// TODO: If child is a container, from and to might have associated solutions already!
 
 		case PROPAGATE_FORWARDS:  // mark from as dead (backwards)
-			markAsEnabled<Interface::BACKWARD>(from, false);
+			StagePrivate::setStatus<Interface::BACKWARD>(from, InterfaceState::Status::DISABLED_START);
 			break;
 		case PROPAGATE_BACKWARDS:  // mark to as dead (forwards)
-			markAsEnabled<Interface::FORWARD>(to, false);
+			StagePrivate::setStatus<Interface::FORWARD>(to, InterfaceState::Status::DISABLED_START);
 			break;
 
 		case CONNECT:
 			if (const Connecting* conn = dynamic_cast<const Connecting*>(&child)) {
 				auto cimpl = conn->pimpl();
 				if (!cimpl->hasPendingOpposites<Interface::FORWARD>(from))
-					markAsEnabled<Interface::BACKWARD>(from, false);
+					StagePrivate::setStatus<Interface::BACKWARD>(from, InterfaceState::Status::DISABLED_START);
 				if (!cimpl->hasPendingOpposites<Interface::BACKWARD>(to))
-					markAsEnabled<Interface::FORWARD>(to, false);
+					StagePrivate::setStatus<Interface::FORWARD>(to, InterfaceState::Status::DISABLED_START);
 			}
 			break;
 	}

core/src/stage.cpp

@@ -148,6 +148,30 @@ bool StagePrivate::storeSolution(const SolutionBasePtr& solution, const Interfac
 	return true;
 }
 
+template <Interface::Direction dir>
+void StagePrivate::setStatus(const InterfaceState* s, InterfaceState::Status status) {
+	if (s->priority().status() == status)
+		return;  // nothing changing
+
+	// actually enable/disable the state
+	if (s->owner())
+		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
+
+	// traverse solution tree
+	for (const SolutionBase* successor : trajectories<dir>(s))
+		setStatus<dir>(state<dir>(*successor), status);
+}
+template void StagePrivate::setStatus<Interface::FORWARD>(const InterfaceState* s, InterfaceState::Status status);
+template void StagePrivate::setStatus<Interface::BACKWARD>(const InterfaceState* s, InterfaceState::Status status);
+
 void StagePrivate::sendForward(const InterfaceState& from, InterfaceState&& to, const SolutionBasePtr& solution) {
 	assert(nextStarts());
 
@@ -705,17 +729,20 @@ 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().enabled())  // remove all pending pairs involving this state
+		if (it->priority().status() == InterfaceState::Status::DISABLED_END)
+			// remove all pending pairs involving this state
 			pending.remove_if([it](const StatePair& p) { return std::get<opposite<other>()>(p) == it; });
 		else
 			pending.sort();
 	} else {  // new state: insert all pairs with other interface
 		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
 			if (static_cast<Connecting*>(me_)->compatible(*it, *oit)) {
-				// if needed, re-enable the opposing state oit
-				oit->owner()->updatePriority(&*oit, InterfaceState::Priority(oit->priority(), true));
+				// re-enable the opposing state oit if its status is DISABLED_START
+				if (oit->priority().status() == InterfaceState::DISABLED_START)
+					oit->owner()->updatePriority(&*oit, InterfaceState::Priority(oit->priority(), InterfaceState::ENABLED));
 				pending.insert(make_pair<other>(it, oit));
 			}
 		}

core/src/storage.cpp

@@ -70,15 +70,16 @@ InterfaceState::InterfaceState(const InterfaceState& other)
   : scene_(other.scene_), properties_(other.properties_), priority_(other.priority_) {}
 
 bool InterfaceState::Priority::operator<(const InterfaceState::Priority& other) const {
-	// disabled states always after enabled ones
-	if (enabled() != other.enabled())
-		return enabled();
-
-	// both prios have same enabled_ state
-	if (depth() == other.depth())
-		return cost() < other.cost();
-	else
+	// first order by status if that differs
+	if (status() != other.status())
+		return status() < other.status();
+
+	// then by depth if that differs
+	if (depth() != other.depth())
 		return depth() > other.depth();  // larger depth = smaller prio!
+
+	// finally by cost
+	return cost() < other.cost();
 }
 
 Interface::Interface(const Interface::NotifyFunction& notify) : notify_(notify) {}
@@ -142,10 +143,14 @@ std::ostream& operator<<(std::ostream& os, const Interface& interface) {
 	return os;
 }
 std::ostream& operator<<(std::ostream& os, const InterfaceState::Priority& prio) {
-	static const char* red = "\033[31m";
-	static const char* green = "\033[32m";
+	// maps InterfaceState::Status values to output (color-changing) prefix
+	static const char* prefix[] = {
+		"\033[32me:",  // ENABLED - green
+		"\033[33md:",  // DISABLED - yellow
+		"\033[31mf:",  // DISABLED_FAILED - red
+	};
 	static const char* color_reset = "\033[m";
-	os << (prio.enabled() ? green : red) << prio.depth() << ":" << prio.cost() << color_reset;
+	os << prefix[prio.status()] << prio.depth() << ":" << prio.cost() << color_reset;
 	return os;
 }
 

core/test/test_interface_state.cpp

@@ -19,14 +19,15 @@ 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
 
-	EXPECT_TRUE(Prio(0, 0, false) == Prio(0, 0, false));
-	EXPECT_TRUE(Prio(1, 0, false) < Prio(0, 0, false));
-	EXPECT_TRUE(Prio(1, 42, false) < Prio(0, 0, false));
-	EXPECT_TRUE(Prio(0, 0, false) < Prio(0, 42, false));
+	auto dstart = InterfaceState::DISABLED_START;
+	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));
+	EXPECT_TRUE(Prio(0, 0, dstart) < Prio(0, 42, dstart));
 
 	// disabled prios are always larger than enabled ones
-	EXPECT_TRUE(Prio(0, 42) < Prio(1, 0, false));
-	EXPECT_TRUE(Prio(1, 0) < Prio(0, 42, false));
+	EXPECT_TRUE(Prio(0, 42) < Prio(1, 0, dstart));
+	EXPECT_TRUE(Prio(1, 0) < Prio(0, 42, dstart));
 
 	// other comparison operators
 	EXPECT_TRUE(Prio(0, 0) <= Prio(0, 0));
@@ -67,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, false));
+	i.updatePriority(*i.begin(), Prio(6, 0, InterfaceState::DISABLED_START));
 	EXPECT_THAT(i.depths(), ::testing::ElementsAreArray({ 3, 6 }));
 }
 
@@ -78,14 +79,16 @@ inline bool operator<(const PrioPair& lhs, const PrioPair& rhs) {
 PrioPair pair(Prio&& p1, Prio&& p2) {
 	return std::make_pair(std::move(p1), std::move(p2));
 }
-PrioPair pair(bool e1, bool e2) {
-	return pair(Prio(0, 0, e1), Prio(0, 0, e2));
+PrioPair pair(InterfaceState::Status s1, InterfaceState::Status s2) {
+	return pair(Prio(0, 0, s1), Prio(0, 0, s2));
 }
 TEST(StatePairs, compare) {
 	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(true, true) < pair(true, false));
-	EXPECT_TRUE(pair(true, true) < pair(false, true));
-	EXPECT_TRUE(pair(false, true) < pair(true, false));
+	auto good = InterfaceState::ENABLED;
+	auto bad = InterfaceState::DISABLED_START;
+	EXPECT_TRUE(pair(good, good) < pair(good, bad));
+	EXPECT_TRUE(pair(good, good) < pair(bad, good));
+	EXPECT_TRUE(pair(bad, good) < pair(good, bad));
 }