Pruning: Relax too strong assertion: PRUNED => !ARMED (#340)

Author: rhaschke

core/include/moveit/task_constructor/storage.h

@@ -248,6 +248,7 @@ class Interface : public ordered<InterfaceState*>
 
 std::ostream& operator<<(std::ostream& os, const InterfaceState::Priority& prio);
 std::ostream& operator<<(std::ostream& os, const Interface& interface);
+std::ostream& operator<<(std::ostream& os, Interface::Direction);
 
 /// Find index of the iterator in the container. Counting starts at 1. Zero corresponds to not found.
 template <typename T>

core/src/stage.cpp

@@ -737,14 +737,15 @@ 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, Interface::UpdateFlags updated) {
 	auto parent_pimpl = parent()->pimpl();
+	// disable current interface to break loop (jumping back and forth between both interfaces)
+	// this will be checked by notifyEnabled() below
 	Interface::DisableNotify disable_source_interface(*pullInterface<dir>());
 	if (updated) {
 		if (updated.testFlag(Interface::STATUS) &&  // only perform these costly operations if needed
-		    pullInterface<opposite<dir>()>()->notifyEnabled())  // suppress recursive loop
+		    pullInterface<opposite<dir>()>()->notifyEnabled())  // suppressing recursive loop?
 		{
 			// If status has changed, propagate the update to the opposite side
 			auto status = it->priority().status();
@@ -799,16 +800,15 @@ void ConnectingPrivate::newState(Interface::iterator it, Interface::UpdateFlags
 #if 0
 	auto& os = std::cerr;
 	for (auto d : { Interface::FORWARD, Interface::BACKWARD }) {
-		bool fw = (d == Interface::FORWARD);
-		if (fw)
+		if (d == Interface::FORWARD)
 			os << "  " << std::setw(10) << std::left << this->name();
 		else
 			os << std::setw(12) << std::right << "";
 		if (dir != d)
 			os << (updated ? " !" : " +");
 		else
 			os << "  ";
-		os << (fw ? "↓ " : "↑ ") << this->pullInterface(d) << ": " << *this->pullInterface(d) << std::endl;
+		os << d << " " << this->pullInterface(d) << ": " << *this->pullInterface(d) << std::endl;
 	}
 	os << std::setw(15) << " ";
 	printPendingPairs(os) << std::endl;

core/src/storage.cpp

@@ -83,8 +83,9 @@ bool InterfaceState::Priority::operator<(const InterfaceState::Priority& other)
 }
 
 void InterfaceState::updatePriority(const InterfaceState::Priority& priority) {
-	// Never overwrite ARMED with PRUNED: PRUNED => !ARMED
-	assert(priority.status() != InterfaceState::Status::PRUNED || priority_.status() != InterfaceState::Status::ARMED);
+	// Never overwrite ARMED with PRUNED
+	if (priority.status() == InterfaceState::Status::PRUNED && priority_.status() == InterfaceState::Status::ARMED)
+		return;
 
 	if (owner()) {
 		owner()->updatePriority(this, priority);
@@ -178,6 +179,10 @@ std::ostream& operator<<(std::ostream& os, const InterfaceState::Priority& prio)
 	   << InterfaceState::STATUS_COLOR[3];
 	return os;
 }
+std::ostream& operator<<(std::ostream& os, Interface::Direction dir) {
+	os << (dir == Interface::FORWARD ? "↓" : "↑");
+	return os;
+}
 
 void SolutionBase::setCreator(Stage* creator) {
 	assert(creator_ == nullptr || creator_ == creator);  // creator must only set once

core/test/test_pruning.cpp

@@ -193,3 +193,17 @@ TEST_F(Pruning, PropagateFromParallelContainerMultiplePaths) {
 	// the failure in one branch of Alternatives must not prune computing back
 	EXPECT_EQ(back->runs_, 1u);
 }
+
+TEST_F(Pruning, TwoConnects) {
+	add(t, new GeneratorMockup({ 0 }));
+	add(t, new ForwardMockup({ INF }));
+	add(t, new ConnectMockup());
+
+	add(t, new GeneratorMockup());
+	add(t, new ConnectMockup());
+
+	add(t, new GeneratorMockup());
+	add(t, new ForwardMockup());
+
+	EXPECT_FALSE(t.plan());
+}