Merge #156: Rework interface resolution

Author: rhaschke

core/include/moveit/task_constructor/container.h

@@ -91,8 +91,6 @@ class SerialContainer : public ContainerBase
 	PRIVATE_CLASS(SerialContainer)
 	SerialContainer(const std::string& name = "serial container");
 
-	void init(const moveit::core::RobotModelConstPtr& robot_model) override;
-
 	bool canCompute() const override;
 	void compute() override;
 
@@ -130,8 +128,6 @@ class ParallelContainerBase : public ContainerBase
 	PRIVATE_CLASS(ParallelContainerBase)
 	ParallelContainerBase(const std::string& name = "parallel container");
 
-	void init(const moveit::core::RobotModelConstPtr& robot_model) override;
-
 protected:
 	ParallelContainerBase(ParallelContainerBasePrivate* impl);
 

core/include/moveit/task_constructor/container_p.h

@@ -105,6 +105,10 @@ class ContainerBasePrivate : public StagePrivate
 
 	void validateConnectivity() const override;
 
+	// Containers derive their required interface from their children
+	// UNKNOWN until resolveInterface was called
+	InterfaceFlags requiredInterface() const override { return required_interface_; }
+
 	// forward these methods to the public interface for containers
 	bool canCompute() const override;
 	void compute() override;
@@ -115,21 +119,17 @@ class ContainerBasePrivate : public StagePrivate
 protected:
 	ContainerBasePrivate(ContainerBase* me, const std::string& name);
 
-	// Set child's push interfaces: allow pushing if child requires it or
-	// if the interface is unknown: in this case greedily assume a push interface.
-	// 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);
-		child.pimpl()->setPrevEnds(allowed ? pending_backward_ : InterfacePtr());
+	// Set child's push interfaces: allow pushing if child requires it.
+	inline void setChildsPushBackwardInterface(StagePrivate* child) {
+		InterfaceFlags required = child->requiredInterface();
+		bool allowed = (required & WRITES_PREV_END);
+		child->setPrevEnds(allowed ? pending_backward_ : InterfacePtr());
 	}
-	inline void setChildsPushForwardInterface(Stage& child) {
-		InterfaceFlags required = child.pimpl()->requiredInterface();
-		bool allowed = (required & WRITES_NEXT_START) || (required == UNKNOWN);
-		child.pimpl()->setNextStarts(allowed ? pending_forward_ : InterfacePtr());
+	inline void setChildsPushForwardInterface(StagePrivate* child) {
+		InterfaceFlags required = child->requiredInterface();
+		bool allowed = (required & WRITES_NEXT_START);
+		child->setNextStarts(allowed ? pending_forward_ : InterfacePtr());
 	}
-	// report error about mismatching interface (start or end as determined by mask)
-	void mismatchingInterface(InitStageException& errors, const StagePrivate& child, const InterfaceFlags mask) const;
 
 	/// copy external_state to a child's interface and remember the link in internal_to map
 	void copyState(Interface::iterator external, const InterfacePtr& target, bool updated);
@@ -139,6 +139,9 @@ class ContainerBasePrivate : public StagePrivate
 	auto& internalToExternalMap() { return internal_to_external_; }
 	const auto& internalToExternalMap() const { return internal_to_external_; }
 
+	// set in resolveInterface()
+	InterfaceFlags required_interface_;
+
 private:
 	container_type children_;
 
@@ -166,28 +169,20 @@ class SerialContainerPrivate : public ContainerBasePrivate
 public:
 	SerialContainerPrivate(SerialContainer* me, const std::string& name);
 
-	// containers derive their required interface from their children
-	InterfaceFlags requiredInterface() const override;
-
 	// called by parent asking for pruning of this' interface
-	void pruneInterface(InterfaceFlags accepted) override;
+	void resolveInterface(InterfaceFlags expected) override;
 	// validate connectivity of chain
 	void validateConnectivity() const override;
 
-private:
-	// connect cur stage to its predecessor and successor
-	bool connect(container_type::const_iterator cur);
+	void reset();
 
-	// called by init() to prune interfaces for children in range [first, last)
-	void pruneInterfaces(container_type::const_iterator first, container_type::const_iterator end);
-	// prune interface for children in range [first, last) to given direction
-	void pruneInterfaces(container_type::const_iterator first, container_type::const_iterator end,
-	                     InterfaceFlags accepted);
-	// store first/last child's interface as required for this container
-	void storeRequiredInterface(container_type::const_iterator first, container_type::const_iterator end);
+protected:
+	// connect two neighbors
+	void connect(StagePrivate& stage1, StagePrivate& stage2);
 
-private:
-	InterfaceFlags required_interface_;
+	// validate that child's interface matches mine (considering start or end only as determined by mask)
+	template <unsigned int mask>
+	void validateInterface(const StagePrivate& child, InterfaceFlags required) const;
 };
 PIMPL_FUNCTIONS(SerialContainer)
 
@@ -219,14 +214,14 @@ class ParallelContainerBasePrivate : public ContainerBasePrivate
 public:
 	ParallelContainerBasePrivate(ParallelContainerBase* me, const std::string& name);
 
-	// containers derive their required interface from their children
-	InterfaceFlags requiredInterface() const override;
-
 	// called by parent asking for pruning of this' interface
-	void pruneInterface(InterfaceFlags accepted) override;
+	void resolveInterface(InterfaceFlags expected) override;
 
 	void validateConnectivity() const override;
 
+protected:
+	void validateInterfaces(const StagePrivate& child, InterfaceFlags& external, bool first = false) const;
+
 private:
 	/// callback for new externally received states
 	void onNewExternalState(Interface::Direction dir, Interface::iterator external, bool updated);
@@ -256,7 +251,7 @@ class MergerPrivate : public ParallelContainerBasePrivate
 	typedef std::function<void(SubTrajectory&&)> Spawner;
 	MergerPrivate(Merger* me, const std::string& name);
 
-	InterfaceFlags requiredInterface() const override;
+	void resolveInterface(InterfaceFlags expected) override;
 
 	void onNewPropagateSolution(const SolutionBase& s);
 	void onNewGeneratorSolution(const SolutionBase& s);

core/include/moveit/task_constructor/stage.h

@@ -83,13 +83,30 @@ enum InterfaceFlag
 	PROPAGATE_BACKWARDS = READS_END | WRITES_PREV_END,
 	GENERATE = WRITES_PREV_END | WRITES_NEXT_START,
 };
+
 typedef Flags<InterfaceFlag> InterfaceFlags;
 
+/** invert interface such that
+ * - new end can connect to old start
+ * - new start can connect to old end
+ */
+constexpr InterfaceFlags invert(InterfaceFlags f) {
+	InterfaceFlags inv;
+	if (f & READS_START)
+		inv |= WRITES_NEXT_START;
+	if (f & WRITES_PREV_END)
+		inv |= READS_END;
+	if (f & READS_END)
+		inv |= WRITES_PREV_END;
+	if (f & WRITES_NEXT_START)
+		inv |= READS_START;
+	return inv;
+};
+
 // some useful constants
 constexpr InterfaceFlags UNKNOWN;
 constexpr InterfaceFlags START_IF_MASK({ READS_START, WRITES_PREV_END });
 constexpr InterfaceFlags END_IF_MASK({ READS_END, WRITES_NEXT_START });
-constexpr InterfaceFlags PROPAGATE_BOTHWAYS({ PROPAGATE_FORWARDS, PROPAGATE_BACKWARDS });
 
 MOVEIT_CLASS_FORWARD(Interface)
 MOVEIT_CLASS_FORWARD(Stage)
@@ -253,12 +270,9 @@ class PropagatingEitherWay : public ComputeBase
 		AUTO = 0x00,  // auto-derive direction from context
 		FORWARD = 0x01,  // propagate forward only
 		BACKWARD = 0x02,  // propagate backward only
-		BOTHWAY = FORWARD | BACKWARD,  // propagate both ways
 	};
 	void restrictDirection(Direction dir);
 
-	void init(const moveit::core::RobotModelConstPtr& robot_model) override;
-
 	virtual void computeForward(const InterfaceState& from) = 0;
 	virtual void computeBackward(const InterfaceState& to) = 0;
 
@@ -368,6 +382,8 @@ class Connecting : public ComputeBase
 	}
 };
 
-const char* flowSymbol(moveit::task_constructor::InterfaceFlags f);
+/** Return (horizontal) flow symbol for start or end interface (specified by mask) */
+template <unsigned int mask>
+const char* flowSymbol(InterfaceFlags f);
 }
 }

core/include/moveit/task_constructor/stage_p.h

@@ -71,15 +71,11 @@ class StagePrivate
 	/// actually configured interface of this stage (only valid after init())
 	InterfaceFlags interfaceFlags() const;
 
-	/** Interface required by this stage
-	 *
-	 * If the interface is unknown (because it is auto-detected from context), return 0 */
+	/// Retrieve interface required by this stage, UNKNOWN if auto-detected from context
 	virtual InterfaceFlags requiredInterface() const = 0;
 
-	/** Prune interface to comply with the given propagation direction
-	 *
-	 * PropagatingEitherWay uses this in restrictDirection() */
-	virtual void pruneInterface(InterfaceFlags accepted) {}
+	/// Resolve interface/propagation direction to comply with the given external interface
+	virtual void resolveInterface(InterfaceFlags /* expected */) {}
 
 	/// validate connectivity of children (after init() was done)
 	virtual void validateConnectivity() const;
@@ -163,6 +159,7 @@ class StagePrivate
 	std::string name_;
 	PropertyMap properties_;
 
+	// pull interfaces, created by the stage as required
 	InterfacePtr starts_;
 	InterfacePtr ends_;
 
@@ -182,6 +179,8 @@ class StagePrivate
 	ContainerBase* parent_;  // owning parent
 	container_type::iterator it_;  // iterator into parent's children_ list referring to this
 
+	// push interfaces, assigned by the parent container
+	// linking to previous/next sibling's pull interfaces
 	InterfaceWeakPtr prev_ends_;  // interface to be used for sendBackward()
 	InterfaceWeakPtr next_starts_;  // interface to be used for sendForward()
 
@@ -208,19 +207,17 @@ 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;
 	// initialize pull interfaces for given propagation directions
 	void initInterface(PropagatingEitherWay::Direction dir);
-	// prune interface to the given propagation direction
-	void pruneInterface(InterfaceFlags accepted) override;
-	// validate that we can propagate in one direction at least
-	void validateConnectivity() const override;
+	// resolve interface to the given propagation direction
+	void resolveInterface(InterfaceFlags expected) override;
 
 	bool canCompute() const override;
 	void compute() override;