Propagate status across Connecting gap

Not only propagate updates along solution paths, but also bridge
the gap of a `Connecting` stage.

- If a state becomes enabled, re-enable opposite `ARMED` states as well.
- If a state becomes pruned, also prune opposite states if they don't have alternatives.
- Make sure that we don't run into a recursive update loop by disabling notify() callbacks.

Author: rhaschke

core/include/moveit/task_constructor/storage.h

@@ -200,6 +200,18 @@ class Interface : public ordered<InterfaceState*>
 		BACKWARD,
 	};
 	using NotifyFunction = std::function<void(iterator, bool)>;
+
+	class DisableNotify
+	{
+		Interface& if_;
+		Interface::NotifyFunction old_;
+
+	public:
+		DisableNotify(Interface& i) : if_(i) { old_.swap(if_.notify_); }
+		~DisableNotify() { old_.swap(if_.notify_); }
+	};
+	friend class DisableNotify;
+
 	Interface(const NotifyFunction& notify = NotifyFunction());
 
 	/// add a new InterfaceState
@@ -210,9 +222,10 @@ class Interface : public ordered<InterfaceState*>
 
 	/// update state's priority (and call notify_ if it really has changed)
 	void updatePriority(InterfaceState* state, const InterfaceState::Priority& priority);
+	inline bool notifyEnabled() const { return static_cast<bool>(notify_); }
 
 private:
-	const NotifyFunction notify_;
+	NotifyFunction notify_;
 
 	// restrict access to some functions to ensure consistency
 	// (we need to set/unset InterfaceState::owner_)

core/src/stage.cpp

@@ -709,13 +709,44 @@ ConnectingPrivate::StatePair ConnectingPrivate::make_pair<Interface::FORWARD>(In
 	return StatePair(second, first);
 }
 
+// TODO: bool updated -> uint_8 updated (bitfield of PRIORITY | STATUS)
 template <Interface::Direction dir>
 void ConnectingPrivate::newState(Interface::iterator it, bool updated) {
-	if (updated) {  // many pairs might be affected: resort
+	auto parent_pimpl = parent()->pimpl();
+	Interface::DisableNotify disable_source_interface(*pullInterface(dir));
+	if (updated) {
+		if (pullInterface(opposite<dir>())->notifyEnabled())  // suppress recursive loop
+		{
+			// If status has changed, propagate the update to the opposite side
+			auto status = it->priority().status();
+			if (status == InterfaceState::Status::PRUNED)  // PRUNED becomes ARMED on opposite side
+				status = InterfaceState::Status::ARMED;  // (only for pending state pairs)
+
+			for (const auto& candidate : this->pending) {
+				if (std::get<opposite<dir>()>(candidate) != it)  // only consider pairs with source state == state
+					continue;
+				auto oit = std::get<dir>(candidate);  // opposite target state
+				auto ostatus = oit->priority().status();
+				if (ostatus != status) {
+					if (status != InterfaceState::Status::ENABLED) {
+						// quicker check for hasPendingOpposites(): search in it->owner() for an enabled alternative
+						bool cancel = false;  // if found, cancel propagation of new status
+						for (const auto alternative : *it->owner())
+							if ((cancel = alternative->priority().enabled()))
+								break;
+						if (cancel)
+							continue;
+					}
+					// pass creator=nullptr to skip hasPendingOpposites() check
+					parent_pimpl->setStatus<opposite<dir>()>(nullptr, nullptr, &*oit, status);
+				}
+			}
+		}
+
+		// many pairs will have changed priorities: resort pending list
 		pending.sort();
 	} else {  // new state: insert all pairs with other interface
 		assert(it->priority().enabled());  // new solutions are feasible, aren't they?
-		auto parent_pimpl = parent()->pimpl();
 		InterfacePtr other_interface = pullInterface(dir);
 		bool have_enabled_opposites = false;
 		for (Interface::iterator oit = other_interface->begin(), oend = other_interface->end(); oit != oend; ++oit) {

core/test/test_pruning.cpp

@@ -91,6 +91,7 @@ TYPED_TEST(PruningContainerTests, ConnectReactivatesPrunedPaths) {
 }
 
 TEST_F(Pruning, ConnectConnectForward) {
+	add(t, new BackwardMockup());
 	add(t, new GeneratorMockup());
 	auto c1 = add(t, new ConnectMockup({ INF, 0, 0 }));  // 1st attempt is a failue
 	add(t, new GeneratorMockup({ 0, 10, 20 }));
@@ -112,6 +113,7 @@ TEST_F(Pruning, ConnectConnectForward) {
 }
 
 TEST_F(Pruning, ConnectConnectBackward) {
+	add(t, new BackwardMockup());
 	add(t, new GeneratorMockup({ 1, 2, 3 }));
 	auto c1 = add(t, new ConnectMockup());
 	add(t, new BackwardMockup());