Merge moveit/moveit@d2918f1

.github/workflows/ci.yaml

@@ -39,7 +39,7 @@ jobs:
               -e PRELOAD=libasan.so.5
               -e LSAN_OPTIONS="suppressions=$PWD/.github/workflows/lsan.suppressions,fast_unwind_on_malloc=0"
               -e ASAN_OPTIONS="new_delete_type_mismatch=0,alloc_dealloc_mismatch=0"
-            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:
       CLANG_TIDY_ARGS: --fix --fix-errors --format-style=file

.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

core/CMakeLists.txt

@@ -14,9 +14,9 @@ find_package(rviz_marker_tools REQUIRED)
 find_package(tf2_eigen REQUIRED)
 find_package(visualization_msgs REQUIRED)
 
-if(NOT CMAKE_CXX_STANDARD)
-  set(CMAKE_CXX_STANDARD 17)
-endif()
+set(CMAKE_CXX_STANDARD 17)
+set(CMAKE_CXX_STANDARD_REQUIRED ON)
+set(CMAKE_CXX_EXTENSIONS OFF)
 
 add_compile_options(-fvisibility-inlines-hidden)
 

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,7 +76,7 @@ namespace task_constructor {
 class ContainerBasePrivate : public StagePrivate
 {
 	friend class ContainerBase;
-	friend void swap(StagePrivate*& lhs, StagePrivate*& rhs);
+	friend class ConnectingPrivate;  // needed propagate setStatus() in newState()
 
 public:
 	using container_type = StagePrivate::container_type;
@@ -131,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) {
@@ -148,14 +149,19 @@ 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);
 
-	/// 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, bool updated);
-	/// lift solution from internal to external level
+	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);
 
@@ -228,12 +234,91 @@ class ParallelContainerBasePrivate : public ContainerBasePrivate
 	void validateInterfaces(const StagePrivate& child, InterfaceFlags& external, bool first = false) const;
 
 private:
-	/// callback for new externally received states
+	/// notify callback for new externally received interface states
 	template <typename Interface::Direction>
-	void onNewExternalState(Interface::iterator external, bool 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/merge.h

@@ -39,6 +39,7 @@
 #include <moveit/task_constructor/storage.h>
 #include <moveit/robot_model/robot_model.h>
 #include <moveit/robot_trajectory/robot_trajectory.h>
+#include <moveit/trajectory_processing/time_parameterization.h>
 
 namespace moveit {
 namespace task_constructor {
@@ -57,6 +58,7 @@ moveit::core::JointModelGroup* merge(const std::vector<const moveit::core::Joint
  */
 robot_trajectory::RobotTrajectoryPtr
 merge(const std::vector<robot_trajectory::RobotTrajectoryConstPtr>& sub_trajectories,
-      const moveit::core::RobotState& base_state, moveit::core::JointModelGroup*& merged_group);
+      const moveit::core::RobotState& base_state, moveit::core::JointModelGroup*& merged_group,
+      const trajectory_processing::TimeParameterization& time_parameterization);
 }  // namespace task_constructor
 }  // namespace moveit

core/include/moveit/task_constructor/moveit_compat.h

@@ -39,6 +39,7 @@
 #pragma once
 
 #include <moveit/version.h>
+#include <moveit/macros/class_forward.h>
 
 #define MOVEIT_VERSION_GE(major, minor, patch)                                                \
 	(MOVEIT_VERSION_MAJOR * 1'000'000 + MOVEIT_VERSION_MINOR * 1'000 + MOVEIT_VERSION_PATCH >= \
@@ -47,4 +48,11 @@
 // use object shape poses relative to a single object pose
 #define MOVEIT_HAS_OBJECT_POSE 1
 
-#define MOVEIT_HAS_STATE_RIGID_PARENT_LINK 0
+#define MOVEIT_HAS_STATE_RIGID_PARENT_LINK 1
+
+#if !MOVEIT_VERSION_GE(3, 0, 0)
+// the pointers are not yet available
+namespace trajectory_processing {
+MOVEIT_CLASS_FORWARD(TimeParameterization);
+}
+#endif

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
@@ -100,17 +99,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) { return dir == Interface::FORWARD ? starts_ : ends_; }
 
 	/// set parent of stage
 	/// enforce only one parent exists
@@ -165,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_;
 
@@ -205,6 +200,15 @@ 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_;
+}
+
 template <>
 inline void StagePrivate::send<Interface::FORWARD>(const InterfaceState& start, InterfaceState&& end,
                                                    const SolutionBasePtr& solution) {
@@ -301,6 +305,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>
@@ -311,18 +316,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;
 		}
 	};
 
@@ -334,7 +336,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;
 
@@ -343,9 +345,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;