Merge PR #311: fix Fallbacks

Author: rhaschke

.github/workflows/ci.yaml

@@ -32,7 +32,7 @@ jobs:
             DOCKER_RUN_OPTS: >-
               -e PRELOAD=libasan.so.5
               -e LSAN_OPTIONS="suppressions=$PWD/.github/workflows/lsan.suppressions"
-            TARGET_CMAKE_ARGS: -DCMAKE_CXX_FLAGS="-fsanitize=address -fno-omit-frame-pointer -O1"
+            TARGET_CMAKE_ARGS: -DCMAKE_CXX_FLAGS="-fsanitize=address -fno-omit-frame-pointer -O1 -g"
 
     env:
       CATKIN_LINT: true

.pre-commit-config.yaml

@@ -15,7 +15,7 @@
 repos:
   # Standard hooks
   - repo: https://github.com/pre-commit/pre-commit-hooks
-    rev: v3.4.0
+    rev: v4.0.1
     hooks:
       - id: check-added-large-files
       - id: check-case-conflict
@@ -29,7 +29,7 @@ repos:
       - id: trailing-whitespace
 
   - repo: https://github.com/psf/black
-    rev: 20.8b1
+    rev: 21.11b1
     hooks:
       - id: black
 

core/include/moveit/task_constructor/container.h

@@ -150,6 +150,7 @@ class Alternatives : public ParallelContainerBase
 	void onNewSolution(const SolutionBase& s) override;
 };
 
+class FallbacksPrivate;
 /** Plan for different alternatives in sequence.
  *
  * Try to find feasible solutions using first child. Only if this fails,
@@ -158,17 +159,23 @@ class Alternatives : public ParallelContainerBase
  */
 class Fallbacks : public ParallelContainerBase
 {
-	mutable Stage* active_child_ = nullptr;
+	inline void replaceImpl();
 
 public:
-	Fallbacks(const std::string& name = "fallbacks") : ParallelContainerBase(name) {}
+	PRIVATE_CLASS(Fallbacks);
+	Fallbacks(const std::string& name = "fallbacks");
 
 	void reset() override;
 	void init(const moveit::core::RobotModelConstPtr& robot_model) override;
-	bool canCompute() const override;
-	void compute() override;
 
+protected:
+	Fallbacks(FallbacksPrivate* impl);
 	void onNewSolution(const SolutionBase& s) override;
+
+private:
+	// not needed, we directly use corresponding virtual methods of FallbacksPrivate
+	bool canCompute() const final { return false; }
+	void compute() final {}
 };
 
 class MergerPrivate;

core/include/moveit/task_constructor/container_p.h

@@ -76,8 +76,7 @@ namespace task_constructor {
 class ContainerBasePrivate : public StagePrivate
 {
 	friend class ContainerBase;
-	friend class ConnectingPrivate;  // needs to call protected setStatus()
-	friend void swap(StagePrivate*& lhs, StagePrivate*& rhs);
+	friend class ConnectingPrivate;  // needed propagate setStatus() in newState()
 
 public:
 	using container_type = StagePrivate::container_type;
@@ -132,10 +131,11 @@ class ContainerBasePrivate : public StagePrivate
 	inline const auto& externalToInternalMap() const { return internal_external_.by<EXTERNAL>(); }
 
 	/// called by a (direct) child when a solution failed
-	void onNewFailure(const Stage& child, const InterfaceState* from, const InterfaceState* to);
+	virtual void onNewFailure(const Stage& child, const InterfaceState* from, const InterfaceState* to);
 
 protected:
 	ContainerBasePrivate(ContainerBase* me, const std::string& name);
+	ContainerBasePrivate& operator=(ContainerBasePrivate&& other);
 
 	// Set child's push interfaces: allow pushing if child requires it.
 	inline void setChildsPushBackwardInterface(StagePrivate* child) {
@@ -154,10 +154,14 @@ class ContainerBasePrivate : public StagePrivate
 	void setStatus(const Stage* creator, const InterfaceState* source, const InterfaceState* target,
 	               InterfaceState::Status status);
 
-	/// copy external_state to a child's interface and remember the link in internal_external map
+	/// Copy external_state to a child's interface and remember the link in internal_external map
 	template <Interface::Direction>
 	void copyState(Interface::iterator external, const InterfacePtr& target, Interface::UpdateFlags updated);
-	/// lift solution from internal to external level
+	// non-template version
+	void copyState(Interface::Direction dir, Interface::iterator external, const InterfacePtr& target,
+	               Interface::UpdateFlags updated);
+
+	/// Lift solution from internal to external level
 	void liftSolution(const SolutionBasePtr& solution, const InterfaceState* internal_from,
 	                  const InterfaceState* internal_to);
 
@@ -230,12 +234,91 @@ class ParallelContainerBasePrivate : public ContainerBasePrivate
 	void validateInterfaces(const StagePrivate& child, InterfaceFlags& external, bool first = false) const;
 
 private:
-	/// notify callback for new externally received states
+	/// notify callback for new externally received interface states
 	template <typename Interface::Direction>
-	void onNewExternalState(Interface::iterator external, Interface::UpdateFlags updated);
+	void propagateStateToAllChildren(Interface::iterator external, Interface::UpdateFlags updated);
+
+	// override to customize behavior on received interface states (default: propagateStateToAllChildren())
+	virtual void initializeExternalInterfaces();
 };
 PIMPL_FUNCTIONS(ParallelContainerBase)
 
+/* The Fallbacks container needs to implement different behaviour based on its interface.
+ * Thus, we implement 3 different classes: for Generator, Propagator, and Connect-like interfaces.
+ * FallbacksPrivate is the common base class for all of them, defining the common API
+ * to be used by the Fallbacks container.
+ * The actual interface-specific class is instantiated in initializeExternalInterfaces()
+ * resp. Fallbacks::replaceImpl() when the actual interface is known.
+ * The key difference between the 3 variants is how they advance to the next job. */
+class FallbacksPrivate : public ParallelContainerBasePrivate
+{
+public:
+	FallbacksPrivate(Fallbacks* me, const std::string& name);
+	FallbacksPrivate(FallbacksPrivate&& other);
+
+	void initializeExternalInterfaces() final;
+	void onNewFailure(const Stage& child, const InterfaceState* from, const InterfaceState* to) override;
+
+	// virtual methods specific to each variant
+	virtual void onNewSolution(const SolutionBase& s);
+	virtual void reset() {}
+};
+PIMPL_FUNCTIONS(Fallbacks)
+
+/* Class shared between FallbacksPrivateGenerator and FallbacksPrivatePropagator,
+   which both have the notion of a currently active child stage */
+class FallbacksPrivateCommon : public FallbacksPrivate
+{
+public:
+	FallbacksPrivateCommon(FallbacksPrivate&& other) : FallbacksPrivate(std::move(other)) {}
+
+	/// Advance to next child
+	inline void nextChild();
+	/// Advance to the next job, assuming that the current child is exhausted on the current job.
+	virtual bool nextJob() = 0;
+
+	void reset() override;
+	bool canCompute() const override;
+	void compute() override;
+
+	container_type::const_iterator current_;  // currently active child
+};
+
+/// Fallbacks implementation for GENERATOR interface
+struct FallbacksPrivateGenerator : FallbacksPrivateCommon
+{
+	FallbacksPrivateGenerator(FallbacksPrivate&& old);
+	bool nextJob() override;
+};
+
+/// Fallbacks implementation for FORWARD or BACKWARD interface
+struct FallbacksPrivatePropagator : FallbacksPrivateCommon
+{
+	FallbacksPrivatePropagator(FallbacksPrivate&& old);
+	void reset() override;
+	void onNewSolution(const SolutionBase& s) override;
+	bool nextJob() override;
+
+	Interface::Direction dir_;  // propagation direction
+	Interface::iterator job_;  // pointer to currently processed external state
+	bool job_has_solutions_;  // flag indicating whether the current job generated solutions
+};
+
+/// Fallbacks implementation for CONNECT interface
+struct FallbacksPrivateConnect : FallbacksPrivate
+{
+	FallbacksPrivateConnect(FallbacksPrivate&& old);
+	void reset() override;
+	bool canCompute() const override;
+	void compute() override;
+	void onNewFailure(const Stage& child, const InterfaceState* from, const InterfaceState* to) override;
+
+	template <Interface::Direction dir>
+	void propagateStateUpdate(Interface::iterator external, Interface::UpdateFlags updated);
+
+	mutable container_type::const_iterator active_;  // child picked for compute()
+};
+
 class WrapperBasePrivate : public ParallelContainerBasePrivate
 {
 	friend class WrapperBase;

core/include/moveit/task_constructor/stage_p.h

@@ -61,7 +61,6 @@ class StagePrivate
 {
 	friend class Stage;
 	friend std::ostream& operator<<(std::ostream& os, const StagePrivate& stage);
-	friend void swap(StagePrivate*& lhs, StagePrivate*& rhs);
 
 public:
 	/// container type used to store children
@@ -103,6 +102,8 @@ class StagePrivate
 	/// direction-based access to pull interface
 	template <Interface::Direction dir>
 	inline InterfacePtr pullInterface();
+	// non-template version
+	inline InterfacePtr pullInterface(Interface::Direction dir) { return dir == Interface::FORWARD ? starts_ : ends_; }
 
 	/// set parent of stage
 	/// enforce only one parent exists
@@ -157,6 +158,8 @@ class StagePrivate
 	void computeCost(const InterfaceState& from, const InterfaceState& to, SolutionBase& solution);
 
 protected:
+	StagePrivate& operator=(StagePrivate&& other);
+
 	// associated/owning Stage instance
 	Stage* me_;
 
@@ -301,6 +304,7 @@ PIMPL_FUNCTIONS(MonitoringGenerator)
 class ConnectingPrivate : public ComputeBasePrivate
 {
 	friend class Connecting;
+	friend struct FallbacksPrivateConnect;
 
 public:
 	struct StatePair : std::pair<Interface::const_iterator, Interface::const_iterator>

core/include/moveit/task_constructor/storage.h

@@ -178,6 +178,7 @@ class Interface : public ordered<InterfaceState*>
 	class iterator : public base_type::iterator
 	{
 	public:
+		iterator() = default;
 		iterator(base_type::iterator other) : base_type::iterator(other) {}
 
 		InterfaceState& operator*() const noexcept { return *base_type::iterator::operator*(); }
@@ -247,6 +248,16 @@ class Interface : public ordered<InterfaceState*>
 std::ostream& operator<<(std::ostream& os, const InterfaceState::Priority& prio);
 std::ostream& operator<<(std::ostream& os, const Interface& interface);
 
+/// Find index of the iterator in the container. Counting starts at 1. Zero corresponds to not found.
+template <typename T>
+size_t getIndex(const T& container, typename T::const_iterator search) {
+	size_t index = 1;
+	for (typename T::const_iterator it = container.begin(), end = container.end(); it != end; ++it, ++index)
+		if (it == search)
+			return index;
+	return 0;
+}
+
 class CostTerm;
 class StagePrivate;
 class ContainerBasePrivate;

core/include/moveit/task_constructor/task_p.h

@@ -51,15 +51,14 @@ namespace task_constructor {
 class TaskPrivate : public WrapperBasePrivate
 {
 	friend class Task;
+	TaskPrivate& operator=(TaskPrivate&& other);
 
 public:
 	TaskPrivate(Task* me, const std::string& ns);
+
 	const std::string& ns() const { return ns_; }
 	const ContainerBase* stages() const;
 
-protected:
-	static void swap(StagePrivate*& lhs, StagePrivate*& rhs);
-
 private:
 	std::string ns_;
 	robot_model_loader::RobotModelLoaderPtr robot_model_loader_;