Merge branch 'master' into ros2

rhaschke · rhaschke · commit cc7f9f0585e4 · 2024-09-17T15:00:39.000+02:00
diff --git a/.github/workflows/ci.yaml b/.github/workflows/ci.yaml
@@ -59,15 +59,14 @@ jobs:
           submodules: recursive
 
       - name: Cache ccache
-        uses: rhaschke/cache@main
+        uses: actions/cache@v4
         with:
+          save-always: true
           path: ${{ env.CCACHE_DIR }}
           key: ccache-${{ env.CACHE_PREFIX }}-${{ github.sha }}-${{ github.run_id }}
           restore-keys: |
             ccache-${{ env.CACHE_PREFIX }}-${{ github.sha }}
             ccache-${{ env.CACHE_PREFIX }}
-        env:
-          GHA_CACHE_SAVE: always
 
       - id: ici
         name: Run industrial_ci
diff --git a/.gitmodules b/.gitmodules
@@ -3,3 +3,6 @@
 	url = https://github.com/pybind/pybind11
 	branch = smart_holder
 	shallow = true
+[submodule "core/src/scope_guard"]
+	path = core/src/scope_guard
+	url = https://github.com/ricab/scope_guard
diff --git a/.pre-commit-config.yaml b/.pre-commit-config.yaml
@@ -29,7 +29,7 @@ repos:
       - id: trailing-whitespace
 
   - repo: https://github.com/psf/black
-    rev: 22.3.0
+    rev: 24.8.0
     hooks:
       - id: black
         args: ["--line-length", "100"]
diff --git a/capabilities/src/execute_task_solution_capability.cpp b/capabilities/src/execute_task_solution_capability.cpp
@@ -185,8 +185,10 @@ bool ExecuteTaskSolutionCapability::constructMotionPlan(const moveit_task_constr
 		exec_traj.effect_on_success = [this,
 		                               &scene_diff = const_cast<::moveit_msgs::msg::PlanningScene&>(sub_traj.scene_diff),
 		                               description](const plan_execution::ExecutableMotionPlan* /*plan*/) {
+			// Never modify joint state directly (only via robot trajectories)
 			scene_diff.robot_state.joint_state = sensor_msgs::msg::JointState();
 			scene_diff.robot_state.multi_dof_joint_state = sensor_msgs::msg::MultiDOFJointState();
+			scene_diff.robot_state.is_diff = true;  // silent empty JointState msg error
 
 			if (!moveit::core::isEmpty(scene_diff)) {
 				RCLCPP_DEBUG_STREAM(LOGGER, "apply effect of " << description);
diff --git a/core/include/moveit/python/task_constructor/properties.h b/core/include/moveit/python/task_constructor/properties.h
@@ -64,9 +64,9 @@ class class_ : public pybind11::classh<type_, options...>  // NOLINT(readability
 	template <typename PropertyType, typename... Extra>
 	class_& property(const char* name, const Extra&... extra) {
 		PropertyConverter<PropertyType>();  // register corresponding property converter
-		auto getter = [name](const type_& self) {
-			const moveit::task_constructor::PropertyMap& props = self.properties();
-			return props.get<PropertyType>(name);
+		auto getter = [name](type_& self) -> PropertyType& {
+			moveit::task_constructor::PropertyMap& props = self.properties();
+			return const_cast<PropertyType&>(props.get<PropertyType>(name));
 		};
 		auto setter = [name](type_& self, const PropertyType& value) {
 			moveit::task_constructor::PropertyMap& props = self.properties();
diff --git a/core/include/moveit/task_constructor/solvers/cartesian_path.h b/core/include/moveit/task_constructor/solvers/cartesian_path.h
@@ -39,6 +39,7 @@
 #pragma once
 
 #include <moveit/task_constructor/solvers/planner_interface.h>
+#include <moveit/robot_state/cartesian_interpolator.h>
 
 namespace moveit {
 namespace task_constructor {
@@ -57,13 +58,17 @@ class CartesianPath : public PlannerInterface
 	void setIKFrame(const std::string& link) { setIKFrame(Eigen::Isometry3d::Identity(), link); }
 
 	void setStepSize(double step_size) { setProperty("step_size", step_size); }
-	void setJumpThreshold(double jump_threshold) { setProperty("jump_threshold", jump_threshold); }
+	void setPrecision(const moveit::core::CartesianPrecision& precision) { setProperty("precision", precision); }
+	template <typename T = float>
+	void setJumpThreshold(double) {
+		static_assert(std::is_integral<T>::value, "setJumpThreshold() is deprecated. Replace with setPrecision.");
+	}
 	void setMinFraction(double min_fraction) { setProperty("min_fraction", min_fraction); }
 
 	[[deprecated("Replace with setMaxVelocityScalingFactor")]]  // clang-format off
-	void setMaxVelocityScaling(double factor) { setMaxVelocityScalingFactor(factor); }
+	void setMaxVelocityScaling(double factor) { setMaxVelocityScalingFactor(factor); }  // clang-format on
 	[[deprecated("Replace with setMaxAccelerationScalingFactor")]]  // clang-format off
-	void setMaxAccelerationScaling(double factor) { setMaxAccelerationScalingFactor(factor); }
+	void setMaxAccelerationScaling(double factor) { setMaxAccelerationScalingFactor(factor); }  // clang-format on
 
 	void init(const moveit::core::RobotModelConstPtr& robot_model) override;
 
@@ -72,8 +77,8 @@ class CartesianPath : public PlannerInterface
 	            const moveit_msgs::msg::Constraints& path_constraints = moveit_msgs::msg::Constraints()) override;
 
 	Result plan(const planning_scene::PlanningSceneConstPtr& from, const moveit::core::LinkModel& link,
-	            const Eigen::Isometry3d& offset, const Eigen::Isometry3d& target, const moveit::core::JointModelGroup* jmg,
-	            double timeout, robot_trajectory::RobotTrajectoryPtr& result,
+	            const Eigen::Isometry3d& offset, const Eigen::Isometry3d& target,
+	            const moveit::core::JointModelGroup* jmg, double timeout, robot_trajectory::RobotTrajectoryPtr& result,
 	            const moveit_msgs::msg::Constraints& path_constraints = moveit_msgs::msg::Constraints()) override;
 
 	std::string getPlannerId() const override { return "CartesianPath"; }
diff --git a/core/include/moveit/task_constructor/stage_p.h b/core/include/moveit/task_constructor/stage_p.h
@@ -57,6 +57,13 @@
 namespace moveit {
 namespace task_constructor {
 
+/// exception thrown by StagePrivate::runCompute()
+class PreemptStageException : public std::exception
+{
+public:
+	explicit PreemptStageException() {}
+};
+
 class ContainerBase;
 class StagePrivate
 {
@@ -146,6 +153,10 @@ class StagePrivate
 	bool storeFailures() const { return introspection_ != nullptr; }
 	void runCompute() {
 		RCLCPP_DEBUG_STREAM(LOGGER, fmt::format("Computing stage '{}'", name()));
+
+		if (preempted())
+			throw PreemptStageException();
+
 		auto compute_start_time = std::chrono::steady_clock::now();
 		try {
 			compute();
@@ -159,6 +170,11 @@ class StagePrivate
 	/** compute cost for solution through configured CostTerm */
 	void computeCost(const InterfaceState& from, const InterfaceState& to, SolutionBase& solution);
 
+	void setPreemptRequestedMember(const std::atomic<bool>* preempt_requested) {
+		preempt_requested_ = preempt_requested;
+	}
+	bool preempted() const { return preempt_requested_ != nullptr && *preempt_requested_; }
+
 protected:
 	StagePrivate& operator=(StagePrivate&& other);
 
@@ -197,6 +213,8 @@ class StagePrivate
 	InterfaceWeakPtr next_starts_;  // interface to be used for sendForward()
 
 	Introspection* introspection_;  // task's introspection instance
+	const std::atomic<bool>* preempt_requested_;
+
 	inline static const rclcpp::Logger LOGGER = rclcpp::get_logger("stage");
 };
 PIMPL_FUNCTIONS(Stage)
diff --git a/core/include/moveit/task_constructor/task.h b/core/include/moveit/task_constructor/task.h
@@ -129,8 +129,9 @@ class Task : protected WrapperBase
 
 	/// reset, init scene (if not yet done), and init all stages, then start planning
 	moveit::core::MoveItErrorCode plan(size_t max_solutions = 0);
-	/// interrupt current planning (or execution)
+	/// interrupt current planning
 	void preempt();
+	void resetPreemptRequest();
 	/// execute solution, return the result
 	moveit::core::MoveItErrorCode execute(const SolutionBase& s);
 
diff --git a/core/include/moveit/task_constructor/task_p.h b/core/include/moveit/task_constructor/task_p.h
@@ -63,7 +63,7 @@ class TaskPrivate : public WrapperBasePrivate
 	std::string ns_;
 	robot_model_loader::RobotModelLoaderPtr robot_model_loader_;
 	moveit::core::RobotModelConstPtr robot_model_;
-	bool preempt_requested_;
+	std::atomic<bool> preempt_requested_;
 
 	// introspection and monitoring
 	std::unique_ptr<Introspection> introspection_;
diff --git a/core/python/bindings/src/solvers.cpp b/core/python/bindings/src/solvers.cpp
@@ -38,6 +38,7 @@
 #include <moveit/task_constructor/solvers/joint_interpolation.h>
 #include <moveit/task_constructor/solvers/multi_planner.h>
 #include <moveit_msgs/msg/workspace_parameters.hpp>
+#include <fmt/core.h>
 #include "utils.h"
 
 namespace py = pybind11;
@@ -96,6 +97,22 @@ void export_solvers(py::module& m) {
 	    .property<double>("max_step", "float: Limit any (single) joint change between two waypoints to this amount")
 	    .def(py::init<>());
 
+	const moveit::core::CartesianPrecision default_precision;
+	py::class_<moveit::core::CartesianPrecision>(m, "CartesianPrecision", "precision for Cartesian interpolation")
+	    .def(py::init([](double translational, double rotational, double max_resolution) {
+		         return new moveit::core::CartesianPrecision{ translational, rotational, max_resolution };
+	         }),
+	         py::arg("translational") = default_precision.translational,
+	         py::arg("rotational") = default_precision.rotational,
+	         py::arg("max_resolution") = default_precision.max_resolution)
+	    .def_readwrite("translational", &moveit::core::CartesianPrecision::translational)
+	    .def_readwrite("rotational", &moveit::core::CartesianPrecision::rotational)
+	    .def_readwrite("max_resolution", &moveit::core::CartesianPrecision::max_resolution)
+	    .def("__str__", [](const moveit::core::CartesianPrecision& self) {
+		    return fmt::format("CartesianPrecision(translational={}, rotational={}, max_resolution={}",
+		                       self.translational, self.rotational, self.max_resolution);
+	    });
+
 	properties::class_<CartesianPath, PlannerInterface>(m, "CartesianPath", R"(
 			Perform linear interpolation between Cartesian poses.
 		 	Fails on collision along the interpolation path. There is no obstacle avoidance. ::
@@ -105,15 +122,12 @@ void export_solvers(py::module& m) {
 				# Instantiate Cartesian-space interpolation planner
 				cartesianPlanner = core.CartesianPath()
 				cartesianPlanner.step_size = 0.01
-				cartesianPlanner.jump_threshold = 0.0  # effectively disable jump threshold.
+				cartesianPlanner.precision.translational = 0.001
 		)")
 	    .property<double>("step_size", "float: Limit the Cartesian displacement between consecutive waypoints "
 	                                   "In contrast to joint-space interpolation, the Cartesian planner can also "
 	                                   "succeed when only a fraction of the linear path was feasible.")
-	    .property<double>(
-	        "jump_threshold",
-	        "float: Limit joint displacement between consecutive waypoints, thus preventing jumps in joint space. "
-	        "This values specifies the fraction of mean acceptable joint motion per step.")
+	    .property<moveit::core::CartesianPrecision>("precision", "Cartesian interpolation precision")
 	    .property<double>("min_fraction", "float: Fraction of overall distance required to succeed.")
 	    .def(py::init<>());
 
diff --git a/core/python/test/test_mtc.py b/core/python/test/test_mtc.py
@@ -14,10 +14,14 @@ def setUpModule():
     rclcpp.init()
 
 
+def tearDownModule():
+    rclcpp.shutdown()
+
+
 # When py_binding_tools and MTC are compiled with different pybind11 versions,
 # the corresponding classes are not interoperable.
 def check_pybind11_incompatibility():
-    rclcpp.init([])
+    rclcpp.init()
     node = rclcpp.Node("dummy")
     try:
         core.PipelinePlanner(node)
@@ -33,8 +37,8 @@ def check_pybind11_incompatibility():
 
 
 class TestPropertyMap(unittest.TestCase):
+
     def setUp(self):
-        self.node = rclcpp.Node("test_mtc_props")
         self.props = core.PropertyMap()
 
     def _check(self, name, value):
@@ -54,7 +58,8 @@ def test_assign(self):
 
     @unittest.skipIf(incompatible_pybind11, incompatible_pybind11_msg)
     def test_assign_in_reference(self):
-        planner = core.PipelinePlanner(self.node)
+        node = rclcpp.Node("test_mtc_props")
+        planner = core.PipelinePlanner(node)
         props = planner.properties
 
         props["goal_joint_tolerance"] = 3.14
diff --git a/core/src/scope_guard b/core/src/scope_guard
@@ -0,0 +1 @@
+Subproject commit 71a04528184db1749dd08ebbbf4daf3b5dca21fd
diff --git a/core/src/solvers/cartesian_path.cpp b/core/src/solvers/cartesian_path.cpp
@@ -60,7 +60,8 @@ CartesianPath::CartesianPath() {
 	auto& p = properties();
 	p.declare<geometry_msgs::msg::PoseStamped>("ik_frame", "frame to move linearly (use for joint-space target)");
 	p.declare<double>("step_size", 0.01, "step size between consecutive waypoints");
-	p.declare<double>("jump_threshold", 1.5, "acceptable fraction of mean joint motion per step");
+	p.declare<moveit::core::CartesianPrecision>("precision", moveit::core::CartesianPrecision(),
+	                                            "precision of linear path");
 	p.declare<double>("min_fraction", 1.0, "fraction of motion required for success");
 	p.declare<kinematics::KinematicsQueryOptions>("kinematics_options", kinematics::KinematicsQueryOptions(),
 	                                              "KinematicsQueryOptions to pass to CartesianInterpolator");
@@ -120,7 +121,7 @@ PlannerInterface::Result CartesianPath::plan(const planning_scene::PlanningScene
 	double achieved_fraction = moveit::core::CartesianInterpolator::computeCartesianPath(
 	    &(sandbox_scene->getCurrentStateNonConst()), jmg, trajectory, &link, target, true,
 	    moveit::core::MaxEEFStep(props.get<double>("step_size")),
-	    moveit::core::JumpThreshold::relative(props.get<double>("jump_threshold")), is_valid,
+	    props.get<moveit::core::CartesianPrecision>("precision"), is_valid,
 	    props.get<kinematics::KinematicsQueryOptions>("kinematics_options"),
 	    props.get<kinematics::KinematicsBase::IKCostFn>("kinematics_cost_fn"), offset);
 
diff --git a/core/src/stage.cpp b/core/src/stage.cpp
@@ -102,7 +102,8 @@ StagePrivate::StagePrivate(Stage* me, const std::string& name)
   , cost_term_{ std::make_unique<CostTerm>() }
   , total_compute_time_{}
   , parent_{ nullptr }
-  , introspection_{ nullptr } {}
+  , introspection_{ nullptr }
+  , preempt_requested_{ nullptr } {}
 
 StagePrivate& StagePrivate::operator=(StagePrivate&& other) {
 	assert(typeid(*this) == typeid(other));
diff --git a/core/src/stages/move_relative.cpp b/core/src/stages/move_relative.cpp
@@ -191,6 +191,11 @@ bool MoveRelative::compute(const InterfaceState& state, planning_scene::Planning
 
 	double max_distance = props.get<double>("max_distance");
 	double min_distance = props.get<double>("min_distance");
+
+	// if min_distance == max_distance, resort to moving full distance (negative min_distance)
+	if (max_distance > 0.0 && std::fabs(max_distance - min_distance) < std::numeric_limits<double>::epsilon())
+		min_distance = -1.0;
+
 	const auto& path_constraints = props.get<moveit_msgs::msg::Constraints>("path_constraints");
 
 	robot_trajectory::RobotTrajectoryPtr robot_trajectory;
@@ -300,7 +305,8 @@ bool MoveRelative::compute(const InterfaceState& state, planning_scene::Planning
 			comment = result.message;
 		solution.setPlannerId(planner_->getPlannerId());
 
-		if (robot_trajectory) {  // the following requires a robot_trajectory returned from planning
+		if (robot_trajectory && robot_trajectory->getWayPointCount() > 0) {  // the following requires a robot_trajectory
+			                                                                  // returned from planning
 			moveit::core::RobotStatePtr& reached_state = robot_trajectory->getLastWayPointPtr();
 			reached_state->updateLinkTransforms();
 			const Eigen::Isometry3d& reached_pose = reached_state->getGlobalLinkTransform(link) * offset;
@@ -335,7 +341,7 @@ bool MoveRelative::compute(const InterfaceState& state, planning_scene::Planning
 	}
 
 	// store result
-	if (robot_trajectory) {
+	if (robot_trajectory && robot_trajectory->getWayPointCount() > 0) {
 		scene->setCurrentState(robot_trajectory->getLastWayPoint());
 		if (dir == Interface::BACKWARD)
 			robot_trajectory->reverse();
diff --git a/core/src/task.cpp b/core/src/task.cpp
@@ -46,6 +46,8 @@
 #include <moveit/robot_model_loader/robot_model_loader.h>
 #include <moveit/planning_pipeline/planning_pipeline.h>
 
+#include <scope_guard/scope_guard.hpp>
+
 #include <functional>
 
 using namespace std::chrono_literals;
@@ -214,11 +216,12 @@ void Task::init() {
 	// task expects its wrapped child to push to both ends, this triggers interface resolution
 	stages()->pimpl()->resolveInterface(InterfaceFlags({ GENERATE }));
 
-	// provide introspection instance to all stages
+	// provide introspection instance and preempt_requested to all stages
 	auto* introspection = impl->introspection_.get();
 	impl->traverseStages(
-	    [introspection](Stage& stage, int /*depth*/) {
+	    [introspection, impl](Stage& stage, int /*depth*/) {
 		    stage.pimpl()->setIntrospection(introspection);
+		    stage.pimpl()->setPreemptRequestedMember(&impl->preempt_requested_);
 		    return true;
 	    },
 	    1, UINT_MAX);
@@ -233,10 +236,17 @@ bool Task::canCompute() const {
 }
 
 void Task::compute() {
-	stages()->pimpl()->runCompute();
+	try {
+		stages()->pimpl()->runCompute();
+	} catch (const PreemptStageException& e) {
+		// do nothing, needed for early stop
+	}
 }
 
 moveit::core::MoveItErrorCode Task::plan(size_t max_solutions) {
+	// ensure the preempt request is resetted once this method exits
+	auto guard = sg::make_scope_guard([this]() noexcept { this->resetPreemptRequest(); });
+
 	auto impl = pimpl();
 	init();
 
@@ -248,7 +258,6 @@ moveit::core::MoveItErrorCode Task::plan(size_t max_solutions) {
 		explainFailure();
 		return error_code;
 	};
-	impl->preempt_requested_ = false;
 	const double available_time = timeout();
 	const auto start_time = std::chrono::steady_clock::now();
 	while (canCompute() && (max_solutions == 0 || numSolutions() < max_solutions)) {
@@ -269,6 +278,10 @@ void Task::preempt() {
 	pimpl()->preempt_requested_ = true;
 }
 
+void Task::resetPreemptRequest() {
+	pimpl()->preempt_requested_ = false;
+}
+
 moveit::core::MoveItErrorCode Task::execute(const SolutionBase& s) {
 	// Add random ID to prevent warnings about multiple publishers within the same node
 	auto node = rclcpp::Node::make_shared("moveit_task_constructor_executor_" +
diff --git a/core/test/stage_mockups.h b/core/test/stage_mockups.h
diff --git a/core/test/test_container.cpp b/core/test/test_container.cpp
diff --git a/core/test/test_fallback.cpp b/core/test/test_fallback.cpp
diff --git a/demo/src/fallbacks_move_to.cpp b/demo/src/fallbacks_move_to.cpp
diff --git a/rviz_marker_tools/CMakeLists.txt b/rviz_marker_tools/CMakeLists.txt
diff --git a/rviz_marker_tools/include/rviz_marker_tools/marker_creation.h b/rviz_marker_tools/include/rviz_marker_tools/marker_creation.h
diff --git a/rviz_marker_tools/package.xml b/rviz_marker_tools/package.xml
