Fallbacks per solution

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,16 +159,17 @@ class Alternatives : public ParallelContainerBase
  */
 class Fallbacks : public ParallelContainerBase
 {
-	mutable Stage* active_child_ = nullptr;
-
 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;
 };
 

core/include/moveit/task_constructor/container_p.h

@@ -76,6 +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);
 
 public:
@@ -131,7 +132,7 @@ 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);
@@ -148,13 +149,21 @@ class ContainerBasePrivate : public StagePrivate
 		child->setNextStarts(allowed ? pending_forward_ : InterfacePtr());
 	}
 
-	/// Set ENABLED / PRUNED status of the solution tree starting from s into given direction
+	/// Set ENABLED/PRUNED status of a solution branch starting from target into the given direction
 	template <Interface::Direction dir>
-	void setStatus(const InterfaceState* s, InterfaceState::Status status);
+	void setStatus(const Stage* creator, const InterfaceState* source, const InterfaceState* target,
+	               InterfaceState::Status status);
+	/// non-template version
+	void setStatus(Interface::Direction dir, 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
 	template <Interface::Direction>
-	void copyState(Interface::iterator external, const InterfacePtr& target, bool updated);
+	void copyState(Interface::iterator external, const InterfacePtr& target, Interface::UpdateFlags updated);
+	/// 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);
@@ -227,13 +236,85 @@ class ParallelContainerBasePrivate : public ContainerBasePrivate
 protected:
 	void validateInterfaces(const StagePrivate& child, InterfaceFlags& external, bool first = false) const;
 
-private:
-	/// callback for new externally received states
+	/// notify callback for new states received on external interfaces
 	template <typename Interface::Direction>
-	void onNewExternalState(Interface::iterator external, bool updated);
+	void propagateStateToChildren(Interface::iterator external, Interface::UpdateFlags updated);
+
+private:
+	// override for custom behavior on received interface states
+	virtual void initializeExternalInterfaces();
 };
 PIMPL_FUNCTIONS(ParallelContainerBase)
 
+class FallbacksPrivate : public ParallelContainerBasePrivate
+{
+	friend class Fallbacks;
+
+	struct ExternalState
+	{
+		ExternalState() = default;
+		ExternalState(Interface::iterator state, container_type::const_iterator child)
+		  : external_state(state), stage(child) {}
+
+		Interface::iterator external_state;
+		container_type::const_iterator stage;
+
+		inline bool operator<(const ExternalState& other) const { return *external_state < *other.external_state; }
+	};
+
+	struct PendingStates
+	{
+		std::pair<ExternalState, Interface::Direction> pop();
+		inline void push(const ExternalState& job, Interface::Direction dir);
+		inline bool empty() const { return pending_states_[0].empty() && pending_states_[1].empty(); }
+
+		/// get pending states queue for a given direction
+		template <Interface::Direction dir>
+		inline ordered<ExternalState>& queue() {
+			static_assert(static_cast<size_t>(dir) < 2, "Expecting dir = FORWARD | BACKWARD");
+			return pending_states_[dir];
+		}
+
+		inline auto& current() { return current_; }
+
+		ordered<ExternalState> pending_states_[2];  ///< separate queues for start / end states
+		Interface::Direction current_queue_ = Interface::FORWARD;  ///< which queue to check on next pop
+		struct
+		{
+			bool valid = false;
+			ExternalState state;
+			Interface::Direction dir = Interface::FORWARD;
+		} current_;
+
+	} pending_;
+
+	friend std::ostream& operator<<(std::ostream& os, const FallbacksPrivate::PendingStates& pending);
+
+public:
+	FallbacksPrivate(Fallbacks* me, const std::string& name);
+
+protected:
+	inline void computeGenerate();
+	void computeFromExternal();
+
+	bool seekToNextPending();
+
+	container_type::const_iterator current_generator_;
+	bool seekToNextGenerator();
+
+private:
+	void initializeExternalInterfaces() override;
+	template <typename Interface::Direction>
+	void onNewExternalState(Interface::iterator external, Interface::UpdateFlags updated);
+	void onNewFailure(const Stage& child, const InterfaceState* from, const InterfaceState* to) override;
+
+	void advanceCurrentStateToNextChild();
+	// print pending states for debugging
+	void printPending(const char* comment = "pending: ") const;
+};
+PIMPL_FUNCTIONS(Fallbacks)
+std::ostream& operator<<(std::ostream& os, const FallbacksPrivate::PendingStates& pending);
+
 class WrapperBasePrivate : public ParallelContainerBasePrivate
 {
 	friend class WrapperBase;

core/include/moveit/task_constructor/stage_p.h

@@ -100,17 +100,11 @@ class StagePrivate
 	inline InterfaceConstPtr prevEnds() const { return prev_ends_.lock(); }
 	inline InterfaceConstPtr nextStarts() const { return next_starts_.lock(); }
 
-	/// direction-based access to pull/push interfaces
-	inline InterfacePtr& pullInterface(Interface::Direction dir) { return dir == Interface::FORWARD ? starts_ : ends_; }
-	inline InterfacePtr pushInterface(Interface::Direction dir) {
-		return dir == Interface::FORWARD ? next_starts_.lock() : prev_ends_.lock();
-	}
-	inline InterfaceConstPtr pullInterface(Interface::Direction dir) const {
-		return dir == Interface::FORWARD ? starts_ : ends_;
-	}
-	inline InterfaceConstPtr pushInterface(Interface::Direction dir) const {
-		return dir == Interface::FORWARD ? next_starts_.lock() : prev_ends_.lock();
-	}
+	/// direction-based access to pull interface
+	template <Interface::Direction dir>
+	inline InterfacePtr pullInterface();
+	/// non-template version
+	inline InterfacePtr pullInterface(Interface::Direction dir);
 
 	/// set parent of stage
 	/// enforce only one parent exists
@@ -204,6 +198,19 @@ class StagePrivate
 PIMPL_FUNCTIONS(Stage)
 std::ostream& operator<<(std::ostream& os, const StagePrivate& stage);
 
+template <>
+inline InterfacePtr StagePrivate::pullInterface<Interface::FORWARD>() {
+	return starts_;
+}
+template <>
+inline InterfacePtr StagePrivate::pullInterface<Interface::BACKWARD>() {
+	return ends_;
+}
+inline InterfacePtr StagePrivate::pullInterface(Interface::Direction dir) {
+	return dir == Interface::Direction::FORWARD ? pullInterface<Interface::Direction::FORWARD>() :
+	                                              pullInterface<Interface::Direction::BACKWARD>();
+}
+
 template <>
 inline void StagePrivate::send<Interface::FORWARD>(const InterfaceState& start, InterfaceState&& end,
                                                    const SolutionBasePtr& solution) {
@@ -310,18 +317,15 @@ class ConnectingPrivate : public ComputeBasePrivate
 		}
 		static inline bool less(const InterfaceState::Priority& lhsA, const InterfaceState::Priority& lhsB,
 		                        const InterfaceState::Priority& rhsA, const InterfaceState::Priority& rhsB) {
-			unsigned char lhs = (lhsA.enabled() << 1) | lhsB.enabled();  // combine bits into two-digit binary number
-			unsigned char rhs = (rhsA.enabled() << 1) | rhsB.enabled();
+			bool lhs = lhsA.enabled() && lhsB.enabled();
+			bool rhs = rhsA.enabled() && rhsB.enabled();
+
 			if (lhs == rhs)  // if enabled status is identical
 				return lhsA + lhsB < rhsA + rhsB;  // compare the sums of both contributions
-			// one of the states in each pair should be enabled
-			assert(lhs != 0b00 && rhs != 0b00);
-			// both states valid (b11)
-			if (lhs == 0b11)
-				return true;
-			if (rhs == 0b11)
-				return false;
-			return lhs < rhs;  // disabled states in 1st component go before disabled states in 2nd component
+
+			// sort both-enabled pairs first
+			static_assert(true > false, "Comparing enabled states requires true > false");
+			return lhs > rhs;
 		}
 	};
 
@@ -333,7 +337,7 @@ class ConnectingPrivate : public ComputeBasePrivate
 
 	// Check whether there are pending feasible states that could connect to source
 	template <Interface::Direction dir>
-	bool hasPendingOpposites(const InterfaceState* source) const;
+	bool hasPendingOpposites(const InterfaceState* source, const InterfaceState* target) const;
 
 	std::ostream& printPendingPairs(std::ostream& os = std::cerr) const;
 
@@ -342,9 +346,9 @@ class ConnectingPrivate : public ComputeBasePrivate
 	template <Interface::Direction other>
 	inline StatePair make_pair(Interface::const_iterator first, Interface::const_iterator second);
 
-	// get informed when new start or end state becomes available
+	// notify callback to get informed about newly inserted (or updated) start or end states
 	template <Interface::Direction other>
-	void newState(Interface::iterator it, bool updated);
+	void newState(Interface::iterator it, Interface::UpdateFlags updated);
 
 	// ordered list of pending state pairs
 	ordered<StatePair> pending;

core/include/moveit/task_constructor/storage.h

@@ -42,6 +42,7 @@
 #include <moveit/macros/class_forward.h>
 #include <moveit/task_constructor/properties.h>
 #include <moveit/task_constructor/cost_queue.h>
+#include <moveit/task_constructor/utils.h>
 #include <moveit_task_constructor_msgs/Solution.h>
 #include <visualization_msgs/MarkerArray.h>
 
@@ -82,9 +83,11 @@ class InterfaceState
 	enum Status
 	{
 		ENABLED,  // state is actively considered during planning
-		PRUNED,  // state is disabled because a required connected state failed
-		FAILED,  // state that failed, causing the whole partial solution to be disabled
+		ARMED,  // disabled state in a Connecting interface that will become re-enabled with a new opposite state
+		PRUNED,  // disabled state on a pruned solution branch
 	};
+	static const char* STATUS_COLOR[];
+
 	/** InterfaceStates are ordered according to two values:
 	 *  Depth of interlinked trajectory parts and accumulated trajectory costs along that path.
 	 *  Preference ordering considers high-depth first and within same depth, minimal cost paths.
@@ -100,8 +103,6 @@ class InterfaceState
 
 		inline Status status() const { return std::get<0>(*this); }
 		inline bool enabled() const { return std::get<0>(*this) == ENABLED; }
-		inline bool failed() const { return std::get<0>(*this) == FAILED; }
-		inline bool pruned() const { return std::get<0>(*this) == PRUNED; }
 
 		inline unsigned int depth() const { return std::get<1>(*this); }
 		inline double cost() const { return std::get<2>(*this); }
@@ -138,13 +139,21 @@ class InterfaceState
 
 	/// states are ordered by priority
 	inline bool operator<(const InterfaceState& other) const { return this->priority_ < other.priority_; }
+
 	inline const Priority& priority() const { return priority_; }
+	/// Update priority and call owner's notify() if possible
+	void updatePriority(const InterfaceState::Priority& priority);
+	/// Update status, but keep current priority
+	void updateStatus(Status status);
+
 	Interface* owner() const { return owner_; }
 
 private:
 	// these methods should be only called by SolutionBase::set[Start|End]State()
 	inline void addIncoming(SolutionBase* t) { incoming_trajectories_.push_back(t); }
 	inline void addOutgoing(SolutionBase* t) { outgoing_trajectories_.push_back(t); }
+	// Set new priority without updating the owning interface (USE WITH CARE)
+	inline void setPriority(const Priority& prio) { priority_ = prio; }
 
 private:
 	planning_scene::PlanningSceneConstPtr scene_;
@@ -169,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*(); }
@@ -190,10 +200,27 @@ class Interface : public ordered<InterfaceState*>
 	{
 		FORWARD,
 		BACKWARD,
-		START = FORWARD,
-		END = BACKWARD
 	};
-	using NotifyFunction = std::function<void(iterator, bool)>;
+	enum Update
+	{
+		STATUS = 1 << 0,
+		PRIORITY = 1 << 1,
+		ALL = STATUS | PRIORITY,
+	};
+	using UpdateFlags = utils::Flags<Update>;
+	using NotifyFunction = std::function<void(iterator, UpdateFlags)>;
+
+	class DisableNotify
+	{
+		Interface& if_;
+		Interface::NotifyFunction old_;
+
+	public:
+		DisableNotify(Interface& i) : if_(i) { old_.swap(if_.notify_); }
+		~DisableNotify() { old_.swap(if_.notify_); }
+	};
+	friend class DisableNotify;
+
 	Interface(const NotifyFunction& notify = NotifyFunction());
 
 	/// add a new InterfaceState
@@ -204,9 +231,10 @@ class Interface : public ordered<InterfaceState*>
 
 	/// update state's priority (and call notify_ if it really has changed)
 	void updatePriority(InterfaceState* state, const InterfaceState::Priority& priority);
+	inline bool notifyEnabled() const { return static_cast<bool>(notify_); }
 
 private:
-	const NotifyFunction notify_;
+	NotifyFunction notify_;
 
 	// restrict access to some functions to ensure consistency
 	// (we need to set/unset InterfaceState::owner_)

core/include/moveit/task_constructor/utils.h

@@ -140,8 +140,6 @@ class Flags
 	Int i;
 };
 
-#define DECLARE_FLAGS(Flags, Enum) using Flags = QFlags<Enum>;
-
 const moveit::core::LinkModel* getRigidlyConnectedParentLinkModel(const moveit::core::RobotState& state,
                                                                   std::string frame);