Merge remote-tracking branch 'upstream/master' into ros2

# Conflicts:
#	capabilities/src/execute_task_solution_capability.cpp
#	core/src/stages/move_relative.cpp

Author: sjahr

.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

capabilities/src/execute_task_solution_capability.cpp

@@ -183,10 +183,12 @@ bool ExecuteTaskSolutionCapability::constructMotionPlan(const moveit_task_constr
 
 		/* TODO add action feedback and markers */
 		exec_traj.effect_on_success = [this,
-		                               &scene_diff = const_cast<::moveit_msgs::msg::PlanningScene&>(sub_traj.scene_diff),
-		                               description](const plan_execution::ExecutableMotionPlan* /*plan*/) {
-			scene_diff.robot_state.joint_state = sensor_msgs::msg::JointState();
-			scene_diff.robot_state.multi_dof_joint_state = sensor_msgs::msg::MultiDOFJointState();
+		                                &scene_diff = const_cast<::moveit_msgs::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::JointState();
+			scene_diff.robot_state.multi_dof_joint_state = sensor_msgs::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);

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);
 

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_;

core/src/scope_guard

@@ -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();

core/src/stages/move_relative.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;
@@ -237,6 +239,9 @@ void Task::compute() {
 }
 
 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 +253,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 +273,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_" +

core/src/task.cpp

@@ -672,3 +672,31 @@ TEST(Task, timeout) {
 	EXPECT_TRUE(t.plan());
 	EXPECT_EQ(t.solutions().size(), 2u);
 }
+
+// https://github.com/moveit/moveit_task_constructor/pull/597
+// start planning in another thread, then preempt it in this thread
+TEST(Task, preempt) {
+	moveit::core::MoveItErrorCode ec;
+	resetMockupIds();
+
+	Task t;
+	t.setRobotModel(getModel());
+
+	auto timeout = std::chrono::milliseconds(10);
+	t.add(std::make_unique<GeneratorMockup>(PredefinedCosts::constant(0.0)));
+	t.add(std::make_unique<TimedForwardMockup>(timeout));
+
+	// preempt before preempt_request_ is reset in plan()
+	{
+		std::thread thread{ [&ec, &t, timeout] {
+			std::this_thread::sleep_for(timeout);
+			ec = t.plan(1);
+		} };
+		t.preempt();
+		thread.join();
+	}
+
+	EXPECT_EQ(ec, moveit::core::MoveItErrorCode::PREEMPTED);
+	EXPECT_EQ(t.solutions().size(), 0u);
+	EXPECT_TRUE(t.plan(1));  // make sure the preempt request has been resetted on the previous call to plan()
+}

core/test/test_container.cpp

@@ -1,7 +1,8 @@
 #include <moveit/task_constructor/container_p.h>
 #include <moveit/task_constructor/stage_p.h>
 #include <moveit/task_constructor/task_p.h>
-#include <moveit/task_constructor/stages/fixed_state.h>
+#include <moveit/task_constructor/stages/predicate_filter.h>
+#include <moveit/task_constructor/stages/noop.h>
 #include <moveit/planning_scene/planning_scene.h>
 
 #include "stage_mockups.h"
@@ -160,6 +161,26 @@ TEST_F(FallbacksFixturePropagate, activeChildReset) {
 	EXPECT_COSTS(fwd2->solutions(), testing::IsEmpty());
 }
 
+// https://github.com/moveit/moveit_task_constructor/issues/581#issuecomment-2147985474
+TEST_F(FallbacksFixturePropagate, filterPropagatesFailures) {
+	t.add(std::make_unique<GeneratorMockup>(PredefinedCosts::single(0.0)));
+
+	auto fallbacks = std::make_unique<Fallbacks>("Fallbacks");
+	auto add_filtered_fwd = [&fallbacks](double cost, bool accept) {
+		auto fwd = std::make_unique<ForwardMockup>(PredefinedCosts::constant(cost));
+		auto filter = std::make_unique<stages::PredicateFilter>("filter", std::move(fwd));
+		filter->setPredicate([accept](const SolutionBase& /*solution*/, std::string& /*comment*/) { return accept; });
+		fallbacks->add(std::move(filter));
+	};
+	add_filtered_fwd(INF, false);  // Propagate fails, filter rejects
+	add_filtered_fwd(2.0, true);  // Propagate succeeds, filter accepts
+	fallbacks->add(std::make_unique<stages::NoOp>());
+	t.add(std::move(fallbacks));
+
+	EXPECT_TRUE(t.plan());
+	EXPECT_COSTS(t.solutions(), testing::ElementsAre(2.));
+}
+
 using FallbacksFixtureConnect = TaskTestBase;
 
 TEST_F(FallbacksFixtureConnect, connectStageInsideFallbacks) {