Implement pruning inside serial container

By inefficient inverse lookup.

Also add disabled test for the inverted inference (failure inside should prune outside)

Author: v4hn

core/include/moveit/task_constructor/container_p.h

@@ -136,6 +136,7 @@ class ContainerBasePrivate : public StagePrivate
 	}
 
 	/// copy external_state to a child's interface and remember the link in internal_to map
+	template <Interface::Direction>
 	void copyState(Interface::iterator external, const InterfacePtr& target, bool updated);
 	/// lift solution from internal to external level
 	void liftSolution(const SolutionBasePtr& solution, const InterfaceState* internal_from,
@@ -207,7 +208,8 @@ class ParallelContainerBasePrivate : public ContainerBasePrivate
 
 private:
 	/// callback for new externally received states
-	void onNewExternalState(Interface::Direction dir, Interface::iterator external, bool updated);
+	template <typename Interface::Direction>
+	void onNewExternalState(Interface::iterator external, bool updated);
 };
 PIMPL_FUNCTIONS(ParallelContainerBase)
 

core/include/moveit/task_constructor/storage.h

@@ -146,7 +146,9 @@ class InterfaceState
 private:
 	planning_scene::PlanningSceneConstPtr scene_;
 	PropertyMap properties_;
+	/// trajectories which are *timewise before* this state
 	Solutions incoming_trajectories_;
+	/// trajectories which are *timewise after* this state
 	Solutions outgoing_trajectories_;
 
 	// members needed for priority scheduling in Interface list

core/src/container.cpp

@@ -106,10 +106,17 @@ void ContainerBasePrivate::compute() {
 	static_cast<ContainerBase*>(me_)->compute();
 }
 
+template <Interface::Direction dir>
 void ContainerBasePrivate::copyState(Interface::iterator external, const InterfacePtr& target, bool updated) {
-	// TODO: update internal's prio from external's new priority
-	if (updated)
+	if (updated) {
+		// TODO(v4hn): This is inefficient, consider storing inverse mapping for each start/end pair in children
+		// for parallel containers there will be multiple internal states
+		std::for_each(internal_to_external_.begin(), internal_to_external_.end(), [&external](auto& p) {
+			if (p.second == &*external)
+				setStatus<dir>(p.first, external->priority().status());
+		});
 		return;
+	}
 
 	// create a clone of external state within target interface (child's starts() or ends())
 	auto internal = states_.insert(states_.end(), InterfaceState(*external));
@@ -497,8 +504,9 @@ void SerialContainerPrivate::resolveInterface(InterfaceFlags expected) {
 		validateInterface<START_IF_MASK>(*first.pimpl(), expected);
 		// connect first child's (start) pull interface
 		if (const InterfacePtr& target = first.pimpl()->starts())
-			starts_.reset(new Interface(
-			    [this, target](Interface::iterator it, bool updated) { this->copyState(it, target, updated); }));
+			starts_.reset(new Interface([this, target](Interface::iterator it, bool updated) {
+				this->copyState<Interface::FORWARD>(it, target, updated);
+			}));
 	} catch (InitStageException& e) {
 		exceptions.append(e);
 	}
@@ -521,8 +529,9 @@ void SerialContainerPrivate::resolveInterface(InterfaceFlags expected) {
 		validateInterface<END_IF_MASK>(*last.pimpl(), expected);
 		// connect last child's (end) pull interface
 		if (const InterfacePtr& target = last.pimpl()->ends())
-			ends_.reset(new Interface(
-			    [this, target](Interface::iterator it, bool updated) { this->copyState(it, target, updated); }));
+			ends_.reset(new Interface([this, target](Interface::iterator it, bool updated) {
+				this->copyState<Interface::BACKWARD>(it, target, updated);
+			}));
 	} catch (InitStageException& e) {
 		exceptions.append(e);
 	}
@@ -622,11 +631,11 @@ void ParallelContainerBasePrivate::resolveInterface(InterfaceFlags expected) {
 	// States received by the container need to be copied to all children's pull interfaces.
 	if (expected & READS_START)
 		starts().reset(new Interface([this](Interface::iterator external, bool updated) {
-			this->onNewExternalState(Interface::FORWARD, external, updated);
+			this->onNewExternalState<Interface::FORWARD>(external, updated);
 		}));
 	if (expected & READS_END)
 		ends().reset(new Interface([this](Interface::iterator external, bool updated) {
-			this->onNewExternalState(Interface::BACKWARD, external, updated);
+			this->onNewExternalState<Interface::BACKWARD>(external, updated);
 		}));
 
 	required_interface_ = expected;
@@ -663,10 +672,10 @@ void ParallelContainerBasePrivate::validateConnectivity() const {
 	ContainerBasePrivate::validateConnectivity();
 }
 
-void ParallelContainerBasePrivate::onNewExternalState(Interface::Direction dir, Interface::iterator external,
-                                                      bool updated) {
+template <Interface::Direction dir>
+void ParallelContainerBasePrivate::onNewExternalState(Interface::iterator external, bool updated) {
 	for (const Stage::pointer& stage : children())
-		copyState(external, stage->pimpl()->pullInterface(dir), updated);
+		copyState<dir>(external, stage->pimpl()->pullInterface(dir), updated);
 }
 
 ParallelContainerBase::ParallelContainerBase(ParallelContainerBasePrivate* impl) : ContainerBase(impl) {}

core/test/test_serial.cpp

@@ -145,6 +145,13 @@ void resetIds() {
 	Connect::id_ = 0;
 }
 
+template <typename C, typename S>
+auto add(C& container, S* stage) -> S* {
+	Stage::pointer ptr{ stage };
+	container.add(std::move(ptr));
+	return stage;
+}
+
 // https://github.com/ros-planning/moveit_task_constructor/issues/182
 TEST(ConnectConnect, SuccSucc) {
 	resetIds();
@@ -208,7 +215,7 @@ TEST(Pruning, PruningMultiForward) {
 	t.add(Stage::pointer(new GeneratorMockup()));
 	// spawn two solutions for the only incoming state
 	t.add(Stage::pointer(new ForwardMockup({ 0, 0 }, 2)));
-	// fail to exten
+	// fail to extend the second solution
 	t.add(Stage::pointer(new ForwardMockup({ 0, inf })));
 
 	t.plan();
@@ -268,3 +275,39 @@ TEST(Pruning, ConnectConnectBackward) {
 	EXPECT_EQ(c2->calls_, 3u);
 	EXPECT_EQ(c1->calls_, 6u);
 }
+
+TEST(Pruning, PropagateInsideContainerBoundaries) {
+	resetIds();
+	Task t;
+	t.setRobotModel(getModel());
+	add(t, new BackwardMockup({ inf }));
+	add(t, new GeneratorMockup({ 0 }));
+	auto c{ std::make_unique<SerialContainer>() };
+	auto con = add(*c, new Connect());
+	add(*c, new GeneratorMockup({ 0 }));
+	t.add(std::move(c));
+
+	t.plan();
+
+	// the failure in the backward stage (outside the container)
+	// should prune the expected computation of con
+	EXPECT_EQ(con->calls_, 0);
+}
+
+TEST(Pruning, DISABLED_PropagateOutsideContainerBoundaries) {
+	resetIds();
+	Task t;
+	t.setRobotModel(getModel());
+	auto back = add(t, new BackwardMockup());
+	add(t, new BackwardMockup());
+	add(t, new GeneratorMockup({ 0 }));
+	auto c{ std::make_unique<SerialContainer>() };
+	add(*c, new ForwardMockup({ inf }));
+	add(*c, new ForwardMockup());
+	t.add(std::move(c));
+
+	t.plan();
+
+	// the failure inside the container should prune computing of back
+	EXPECT_EQ(back->calls_, 0);
+}