Further simplify interface resolution

* Do not modify explicitly configured direction of Propagator
* Avoid code duplication in pruneInterface() and validateConnectivity()
  Only implement pruneInterface() for stages that actually need to adapt their interface.

Author: rhaschke

core/include/moveit/task_constructor/container_p.h

@@ -105,7 +105,7 @@ class ContainerBasePrivate : public StagePrivate
 
 	void validateConnectivity() const override;
 
-	// containers derive their required interface from their children
+	// Containers derive their required interface from their children
 	// UNKNOWN until pruneInterface was called
 	InterfaceFlags requiredInterface() const override { return required_interface_; }
 
@@ -124,12 +124,12 @@ class ContainerBasePrivate : public StagePrivate
 	// If, during pruneInterface() later, we notice that it's not needed, prune there.
 	inline void setChildsPushBackwardInterface(Stage& child) {
 		InterfaceFlags required = child.pimpl()->requiredInterface();
-		bool allowed = (required & WRITES_PREV_END) || (required == UNKNOWN);
+		bool allowed = (required & WRITES_PREV_END);
 		child.pimpl()->setPrevEnds(allowed ? pending_backward_ : InterfacePtr());
 	}
 	inline void setChildsPushForwardInterface(Stage& child) {
 		InterfaceFlags required = child.pimpl()->requiredInterface();
-		bool allowed = (required & WRITES_NEXT_START) || (required == UNKNOWN);
+		bool allowed = (required & WRITES_NEXT_START);
 		child.pimpl()->setNextStarts(allowed ? pending_forward_ : InterfacePtr());
 	}
 	// report error about mismatching interface (start or end as determined by mask)
@@ -144,7 +144,7 @@ class ContainerBasePrivate : public StagePrivate
 	auto& internalToExternalMap() { return internal_to_external_; }
 	const auto& internalToExternalMap() const { return internal_to_external_; }
 
-	// set by containers in pruneInterface()
+	// set in pruneInterface()
 	InterfaceFlags required_interface_;
 
 private:
@@ -249,7 +249,7 @@ class MergerPrivate : public ParallelContainerBasePrivate
 	typedef std::function<void(SubTrajectory&&)> Spawner;
 	MergerPrivate(Merger* me, const std::string& name);
 
-	InterfaceFlags requiredInterface() const override;
+	void pruneInterface(InterfaceFlags accepted) override;
 
 	void onNewPropagateSolution(const SolutionBase& s);
 	void onNewGeneratorSolution(const SolutionBase& s);

core/include/moveit/task_constructor/stage_p.h

@@ -76,11 +76,10 @@ class StagePrivate
 	 * If the interface is unknown (because it is auto-detected from context), return UNKNOWN */
 	virtual InterfaceFlags requiredInterface() const = 0;
 
-	/** validate correct interface usage and resolve ambiguous interfaces */
-	virtual void pruneInterface(InterfaceFlags accepted);
-
-	void pruneStartInterface(InterfaceFlags start_flags) { pruneInterface(start_flags & START_IF_MASK); }
-	void pruneEndInterface(InterfaceFlags end_flags) { pruneInterface(end_flags & END_IF_MASK); }
+	/** Prune interface to comply with the given propagation direction
+	 *
+	 * PropagatingEitherWay uses this in restrictDirection() */
+	virtual void pruneInterface(InterfaceFlags /* accepted */) {}
 
 	/// validate connectivity of children (after init() was done)
 	virtual void validateConnectivity() const;
@@ -212,10 +211,10 @@ class PropagatingEitherWayPrivate : public ComputeBasePrivate
 	friend class PropagatingEitherWay;
 
 public:
-	PropagatingEitherWay::Direction required_interface_dirs_;
+	PropagatingEitherWay::Direction configured_dir_;
+	InterfaceFlags required_interface_;
 
-	inline PropagatingEitherWayPrivate(PropagatingEitherWay* me,
-	                                   PropagatingEitherWay::Direction required_interface_dirs_,
+	inline PropagatingEitherWayPrivate(PropagatingEitherWay* me, PropagatingEitherWay::Direction configured_dir_,
 	                                   const std::string& name);
 
 	InterfaceFlags requiredInterface() const override;

core/src/container.cpp

@@ -425,51 +425,39 @@ void SerialContainerPrivate::connect(StagePrivate& stage1, StagePrivate& stage2)
 
 // called by parent asking for pruning of this' interface
 void SerialContainerPrivate::pruneInterface(InterfaceFlags accepted) {
+	// we need to have some children to do the actual work
 	if (children().empty())
-		throw InitStageException(*me(), "container is empty");
+		throw InitStageException(*me(), "no children");
 
-	// TODO(v4hn): if ever there is a use case to start pruning
-	// with a specified end interface, this would need to be extended
-	if (!(accepted & (READS_START | WRITES_PREV_END)))
-		return;  // The start interface direction is not decided
+	if (!(accepted & START_IF_MASK))
+		throw InitStageException(*me(), "unknown start interface");
 
 	Stage& first = *children().front();
 	Stage& last = *children().back();
 
-	// sweep through children once: infer and connect interfaces
-	first.pimpl()->pruneStartInterface(accepted);
+	// FIRST child
+	first.pimpl()->pruneInterface(accepted & START_IF_MASK);
+	// connect first child's (start) push interface
 	setChildsPushBackwardInterface(first);
+	// 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); }));
 
+	// process all children and connect them
 	for (auto it = ++children().begin(), previous_it = children().begin(); it != children().end(); ++it, ++previous_it) {
 		StagePrivate* child_impl = (**it).pimpl();
 		StagePrivate* previous_impl = (**previous_it).pimpl();
-		child_impl->pruneStartInterface(invert(previous_impl->requiredInterface()));
+		child_impl->pruneInterface(invert(previous_impl->requiredInterface()) & START_IF_MASK);
 		connect(*previous_impl, *child_impl);
 	}
 
-	// potentially connect outmost push interface to pending_ buffer
+	// connect last child's (end) push interface
 	setChildsPushForwardInterface(last);
-
-	if ((accepted & END_IF_MASK) != UNKNOWN &&
-	    (last.pimpl()->requiredInterface() & END_IF_MASK) != (accepted & END_IF_MASK)) {
-		boost::format desc(
-		    "requested end interface for container (%1%) does not agree with inferred end interface of last child (%2%)");
-		desc % flowSymbol<END_IF_MASK>(accepted) % flowSymbol<END_IF_MASK>(last.pimpl()->requiredInterface());
-		throw InitStageException(*me(), desc.str());
-	}
-
-	// if first/last pull, this needs to pull to and forward to the children
-	if (const InterfacePtr& target = first.pimpl()->starts())
-		starts_.reset(new Interface(
-		    [this, target](Interface::iterator it, bool updated) { this->copyState(it, target, updated); }));
-	else
-		starts_.reset();
-
+	// 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); }));
-	else
-		ends_.reset();
 
 	required_interface_ = first.pimpl()->interfaceFlags() & START_IF_MASK | last.pimpl()->interfaceFlags() & END_IF_MASK;
 }
@@ -582,11 +570,9 @@ ParallelContainerBasePrivate::ParallelContainerBasePrivate(ParallelContainerBase
   : ContainerBasePrivate(me, name) {}
 
 void ParallelContainerBasePrivate::pruneInterface(InterfaceFlags accepted) {
+	// we need to have some children to do the actual work
 	if (children().empty())
-		throw InitStageException(*me(), "trying to prune empty container");
-
-	if (accepted == UNKNOWN)
-		return;  // nothing to prune
+		throw InitStageException(*me(), "no children");
 
 	InitStageException exceptions;
 	InterfaceFlags interface;
@@ -789,32 +775,21 @@ void Fallbacks::onNewSolution(const SolutionBase& s) {
 
 MergerPrivate::MergerPrivate(Merger* me, const std::string& name) : ParallelContainerBasePrivate(me, name) {}
 
-InterfaceFlags MergerPrivate::requiredInterface() const {
-	if (children().size() < 2)
-		throw InitStageException(*me_, "Need 2 children at least.");
-
-	InterfaceFlags required = ParallelContainerBasePrivate::requiredInterface();
-
-	// all children need to share a common interface
-	for (const Stage::pointer& stage : children()) {
-		InterfaceFlags current = stage->pimpl()->requiredInterface();
-		if (current != required)
-			throw InitStageException(*stage, "Interface does not match the common one.");
-	}
-
-	switch (required) {
+void MergerPrivate::pruneInterface(InterfaceFlags accepted) {
+	ContainerBasePrivate::pruneInterface(accepted);
+	switch (interfaceFlags()) {
 		case PROPAGATE_FORWARDS:
 		case PROPAGATE_BACKWARDS:
 		case UNKNOWN:
 			break;  // these are supported
+
 		case GENERATE:
 			throw InitStageException(*me_, "Generator stages not yet supported.");
 		case CONNECT:
 			throw InitStageException(*me_, "Cannot merge connecting stages. Use Connect.");
 		default:
 			throw InitStageException(*me_, "Children's interface not supported.");
 	}
-	return required;
 }
 
 Merger::Merger(const std::string& name) : Merger(new MergerPrivate(this, name)) {}

core/src/stage.cpp

@@ -112,28 +112,6 @@ InterfaceFlags StagePrivate::interfaceFlags() const {
 	return f;
 }
 
-void StagePrivate::pruneInterface(InterfaceFlags accepted) {
-	const InterfaceFlags accepted_start{ accepted & START_IF_MASK };
-	const InterfaceFlags accepted_end{ accepted & END_IF_MASK };
-
-	const InterfaceFlags required{ requiredInterface() };
-	const InterfaceFlags required_start{ required & START_IF_MASK };
-	const InterfaceFlags required_end{ required & END_IF_MASK };
-
-	if (required == UNKNOWN)
-		throw std::logic_error("stage type does not specify interface, but also does not override pruneInterface. "
-		                       "Who sets up its interfaces?");
-
-	// only one side needs to be specified but the value has to be compatible with this stage
-	if (((accepted_start != UNKNOWN) && (accepted_start & required_start) != required_start) ||
-	    ((accepted_end != UNKNOWN) && (accepted_end & required_end) != required_end)) {
-		boost::format desc("interface %1% %2% does not match inferred interface %3% %4%");
-		desc % flowSymbol<START_IF_MASK>(required_start) % flowSymbol<END_IF_MASK>(required_end);
-		desc % flowSymbol<START_IF_MASK>(accepted_start) % flowSymbol<END_IF_MASK>(accepted_end);
-		throw InitStageException(*me(), desc.str());
-	}
-}
-
 void StagePrivate::validateConnectivity() const {
 	// check that the required interface is provided
 	InterfaceFlags required = requiredInterface();
@@ -416,47 +394,53 @@ ComputeBase::ComputeBase(ComputeBasePrivate* impl) : Stage(impl) {}
 
 PropagatingEitherWayPrivate::PropagatingEitherWayPrivate(PropagatingEitherWay* me, PropagatingEitherWay::Direction dir,
                                                          const std::string& name)
-  : ComputeBasePrivate(me, name), required_interface_dirs_(dir) {}
+  : ComputeBasePrivate(me, name), configured_dir_(dir) {
+	initInterface(dir);
+}
 
 // initialize pull interfaces to match requested propagation directions
 void PropagatingEitherWayPrivate::initInterface(PropagatingEitherWay::Direction dir) {
-	if (required_interface_dirs_ != PropagatingEitherWay::AUTO && dir != required_interface_dirs_) {
-		auto flow = [](PropagatingEitherWay::Direction dir) -> const char* {
-			switch (dir) {
-				case PropagatingEitherWay::AUTO:
-					return flowSymbol<START_IF_MASK>(InterfaceFlags{ READS_START, WRITES_PREV_END });
-				case PropagatingEitherWay::FORWARD:
-					return flowSymbol<START_IF_MASK>(InterfaceFlags{ READS_START });
-				case PropagatingEitherWay::BACKWARD:
-					return flowSymbol<START_IF_MASK>(InterfaceFlags{ WRITES_PREV_END });
-			}
-		};
-
-		boost::format desc("propagation direction %1% incompatible with inferred direction %2%");
-		desc % flow(required_interface_dirs_) % flow(dir);
-		throw InitStageException(*me(), desc.str());
+	switch (dir) {
+		case PropagatingEitherWay::FORWARD:
+			required_interface_ = PROPAGATE_FORWARDS;
+			if (!starts_)  // keep existing interface if possible
+				starts_.reset(
+				    new Interface([this](Interface::iterator it, bool /*updated*/) { this->dropFailedStarts(it); }));
+			ends_.reset();
+			return;
+		case PropagatingEitherWay::BACKWARD:
+			required_interface_ = PROPAGATE_BACKWARDS;
+			starts_.reset();
+			if (!ends_)  // keep existing interface if possible
+				ends_.reset(new Interface([this](Interface::iterator it, bool /*updated*/) { this->dropFailedEnds(it); }));
+			return;
+		case PropagatingEitherWay::AUTO:
+			required_interface_ = UNKNOWN;
+			return;
 	}
-
-	if (dir == PropagatingEitherWay::FORWARD)
-		starts_.reset(new Interface([this](Interface::iterator it, bool /* updated */) { this->dropFailedStarts(it); }));
-	else if (dir == PropagatingEitherWay::BACKWARD)
-		ends_.reset(new Interface([this](Interface::iterator it, bool /* updated */) { this->dropFailedEnds(it); }));
-
-	required_interface_dirs_ = dir;
 }
 
 void PropagatingEitherWayPrivate::pruneInterface(InterfaceFlags accepted) {
 	if (accepted == UNKNOWN)
-		throw InitStageException(*me(), "cannot prune to unknown interface");
+		throw InitStageException(*me(), "cannot initialize to unknown interface");
+
+	auto dir = PropagatingEitherWay::AUTO;
 	if ((accepted & START_IF_MASK) == READS_START || (accepted & END_IF_MASK) == WRITES_NEXT_START)
-		initInterface(PropagatingEitherWay::FORWARD);
+		dir = PropagatingEitherWay::FORWARD;
 	else if ((accepted & START_IF_MASK) == WRITES_PREV_END || (accepted & END_IF_MASK) == READS_END)
-		initInterface(PropagatingEitherWay::BACKWARD);
+		dir = PropagatingEitherWay::BACKWARD;
 	else {
-		boost::format desc("propagator cannot act with interfaces start(%1%), end(%2%)");
+		boost::format desc("propagator cannot handle external interface %1% %2%");
+		desc % flowSymbol<START_IF_MASK>(accepted) % flowSymbol<END_IF_MASK>(accepted);
+		throw InitStageException(*me(), desc.str());
+	}
+	if (configured_dir_ != PropagatingEitherWay::AUTO && dir != configured_dir_) {
+		boost::format desc("configured interface (%1% %2%) does not match external one (%3% %4%)");
+		desc % flowSymbol<START_IF_MASK>(required_interface_) % flowSymbol<END_IF_MASK>(required_interface_);
 		desc % flowSymbol<START_IF_MASK>(accepted) % flowSymbol<END_IF_MASK>(accepted);
 		throw InitStageException(*me(), desc.str());
 	}
+	initInterface(dir);
 }
 
 void PropagatingEitherWayPrivate::validateConnectivity() const {
@@ -475,14 +459,7 @@ void PropagatingEitherWayPrivate::validateConnectivity() const {
 }
 
 InterfaceFlags PropagatingEitherWayPrivate::requiredInterface() const {
-	switch (required_interface_dirs_) {
-		case PropagatingEitherWay::FORWARD:
-			return PROPAGATE_FORWARDS;
-		case PropagatingEitherWay::BACKWARD:
-			return PROPAGATE_BACKWARDS;
-		case PropagatingEitherWay::AUTO:
-			return UNKNOWN;
-	}
+	return required_interface_;
 }
 
 void PropagatingEitherWayPrivate::dropFailedStarts(Interface::iterator state) {
@@ -540,9 +517,11 @@ PropagatingEitherWay::PropagatingEitherWay(PropagatingEitherWayPrivate* impl) :
 
 void PropagatingEitherWay::restrictDirection(PropagatingEitherWay::Direction dir) {
 	auto impl = pimpl();
-	if (impl->required_interface_dirs_ != AUTO)
+	if (impl->configured_dir_ == dir)
+		return;
+	if (impl->configured_dir_ != AUTO)
 		throw std::runtime_error("Cannot change direction after being connected");
-
+	impl->configured_dir_ = dir;
 	impl->initInterface(dir);
 }