Add ability to preempt execution and add test

capabilities/CMakeLists.txt

@@ -34,6 +34,8 @@ install(TARGETS ${PROJECT_NAME}
   LIBRARY DESTINATION lib
 )
 
+add_subdirectory(test)
+
 pluginlib_export_plugin_description_file(moveit_ros_move_group capabilities_plugin_description.xml)
 ament_export_dependencies(moveit_core)
 ament_export_dependencies(moveit_ros_move_group)

capabilities/test/CMakeLists.txt

@@ -0,0 +1,15 @@
+#############
+## Testing ##
+#############
+
+## Add gtest based cpp test target
+if (BUILD_TESTING)
+	find_package(ament_cmake_gtest REQUIRED)
+	find_package(launch_testing_ament_cmake REQUIRED)
+	find_package(moveit_task_constructor_core REQUIRED)
+
+	ament_add_gtest_executable(test_execution test_task_execution.cpp)
+	add_launch_test(test_execution.launch.py TARGET test_execution
+	                ARGS "test_binary:=$<TARGET_FILE:test_execution>")
+	ament_target_dependencies(test_execution moveit_task_constructor_core)
+endif()

capabilities/test/test_execution.launch.py

@@ -0,0 +1,77 @@
+import unittest
+import os
+
+from ament_index_python.packages import get_package_share_directory
+import launch_testing
+import pytest
+from launch import LaunchDescription
+from launch.actions import DeclareLaunchArgument, IncludeLaunchDescription
+from launch.launch_description_sources import PythonLaunchDescriptionSource
+from launch.substitutions import LaunchConfiguration
+from launch_ros.actions import Node
+from launch_testing.actions import ReadyToTest
+from launch_testing.util import KeepAliveProc
+from moveit_configs_utils import MoveItConfigsBuilder
+
+
+@pytest.mark.launch_test
+def generate_test_description():
+    moveit_config = (
+        MoveItConfigsBuilder("moveit_resources_panda")
+        .robot_description(file_path="config/panda.urdf.xacro")
+        .to_moveit_configs()
+    )
+
+    demo_bringup = IncludeLaunchDescription(
+        PythonLaunchDescriptionSource(
+            os.path.join(
+                get_package_share_directory("moveit_task_constructor_demo"), "launch/demo.launch.py"
+            )
+        ),
+        launch_arguments={"rviz": "false"}.items(),
+    )
+
+    # increase the timeout for the kinematics solver to fix flaky IK for asan
+    k = moveit_config.robot_description_kinematics["robot_description_kinematics"]
+    k["panda_arm"]["kinematics_solver_timeout"] = 5.0
+
+    test_exec = Node(
+        executable=[
+            LaunchConfiguration("test_binary"),
+        ],
+        output="screen",
+        parameters=[
+            moveit_config.robot_description,
+            moveit_config.robot_description_semantic,
+            moveit_config.robot_description_kinematics,
+            moveit_config.joint_limits,
+        ],
+    )
+
+    return (
+        LaunchDescription(
+            [
+                demo_bringup,
+                DeclareLaunchArgument(
+                    name="test_binary",
+                    description="Test executable",
+                ),
+                test_exec,
+                KeepAliveProc(),
+                ReadyToTest(),
+            ]
+        ),
+        {"test_exec": test_exec},
+    )
+
+
+class TestTerminatingProcessStops(unittest.TestCase):
+    def test_gtest_run_complete(self, proc_info, test_exec):
+        proc_info.assertWaitForShutdown(process=test_exec, timeout=4000.0)
+
+
+@launch_testing.post_shutdown_test()
+class TaskModelTestAfterShutdown(unittest.TestCase):
+    def test_exit_code(self, proc_info):
+        # Check that all processes in the launch exit with code 0
+        launch_testing.asserts.assertExitCodes(proc_info)

capabilities/test/test_task_execution.cpp

@@ -0,0 +1,71 @@
+#include <moveit/task_constructor/task.h>
+#include <moveit/task_constructor/stages/current_state.h>
+#include <moveit/task_constructor/stages/move_to.h>
+#include <moveit/task_constructor/solvers/joint_interpolation.h>
+#include <moveit_task_constructor_msgs/msg/solution.hpp>
+#include <moveit/robot_model/robot_model.hpp>
+
+#include <gtest/gtest.h>
+
+using namespace moveit::task_constructor;
+
+// provide a basic test fixture that prepares a Task
+struct PandaMoveTo : public testing::Test
+{
+	Task t;
+	stages::MoveTo* move_to;
+	rclcpp::Node::SharedPtr node;
+
+	PandaMoveTo() {
+		node = rclcpp::Node::make_shared("panda_move_to");
+		t.loadRobotModel(node);
+
+		auto group = t.getRobotModel()->getJointModelGroup("panda_arm");
+
+		auto initial = std::make_unique<stages::CurrentState>("current state");
+		t.add(std::move(initial));
+
+		auto move = std::make_unique<stages::MoveTo>("move", std::make_shared<solvers::JointInterpolationPlanner>());
+		move_to = move.get();
+		move_to->setGroup(group->getName());
+		t.add(std::move(move));
+	}
+};
+
+// The arm starts in the "ready" pose so make sure we can move to a different known location
+TEST_F(PandaMoveTo, successExecution) {
+	move_to->setGoal("extended");
+	EXPECT_TRUE(t.plan());
+	auto result = t.execute(*t.solutions().front());
+	EXPECT_TRUE(result.val == moveit_msgs::msg::MoveItErrorCodes::SUCCESS);
+}
+
+// After the arm successfully moved to "extended", move back to "ready" and make sure preempt() works as expected
+TEST_F(PandaMoveTo, preemptExecution) {
+	move_to->setGoal("ready");
+	EXPECT_TRUE(t.plan());
+	// extract the expected execution time (for this task its in the last sub_trajectory)
+	moveit_task_constructor_msgs::msg::Solution s;
+	t.solutions().front()->toMsg(s, nullptr);
+	rclcpp::Duration exec_duration{ s.sub_trajectory.back().trajectory.joint_trajectory.points.back().time_from_start };
+
+	moveit::core::MoveItErrorCode result;
+	std::thread execute_thread{ [this, &result]() { result = t.execute(*t.solutions().front()); } };
+
+	// cancel the trajectory half way through the expected execution time
+	rclcpp::sleep_for(exec_duration.to_chrono<std::chrono::milliseconds>() / 2);
+	t.preempt();
+	execute_thread.join();
+
+	EXPECT_TRUE(result.val == moveit_msgs::msg::MoveItErrorCodes::PREEMPTED);
+}
+
+int main(int argc, char** argv) {
+	testing::InitGoogleTest(&argc, argv);
+	rclcpp::init(argc, argv);
+
+	// wait some time for move_group to come up
+	rclcpp::sleep_for(std::chrono::seconds(2));
+
+	return RUN_ALL_TESTS();
+}

core/include/moveit/task_constructor/task.h

@@ -43,13 +43,15 @@
 
 #include <moveit/task_constructor/introspection.h>
 #include <moveit_task_constructor_msgs/msg/solution.hpp>
+#include <moveit_task_constructor_msgs/action/execute_task_solution.hpp>
 
 #include <moveit/macros/class_forward.hpp>
 
 #include <moveit_msgs/msg/move_it_error_codes.hpp>
 #include <moveit/utils/moveit_error_code.hpp>
 
 #include <rclcpp/node.hpp>
+#include <rclcpp_action/rclcpp_action.hpp>
 
 namespace moveit {
 namespace core {
@@ -167,6 +169,9 @@ class Task : protected WrapperBase
 
 private:
 	using WrapperBase::init;
+	// persistent node and client to call the ExecuteTaskSolution action and is only created if execute() is called
+	rclcpp::Node::SharedPtr execute_solution_node_;
+	std::shared_ptr<rclcpp_action::Client<moveit_task_constructor_msgs::action::ExecuteTaskSolution>> execute_ac_;
 };
 
 inline std::ostream& operator<<(std::ostream& os, const Task& task) {

core/src/task.cpp

@@ -38,10 +38,8 @@
 #include <moveit/task_constructor/container_p.h>
 #include <moveit/task_constructor/task_p.h>
 #include <moveit/task_constructor/introspection.h>
-#include <moveit_task_constructor_msgs/action/execute_task_solution.hpp>
 
 #include <rclcpp/rclcpp.hpp>
-#include <rclcpp_action/rclcpp_action.hpp>
 
 #include <moveit/robot_model_loader/robot_model_loader.hpp>
 #include <moveit/planning_pipeline/planning_pipeline.hpp>
@@ -283,13 +281,15 @@ void Task::resetPreemptRequest() {
 }
 
 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_" +
-	                                      std::to_string(reinterpret_cast<std::size_t>(this)));
-	auto ac = rclcpp_action::create_client<moveit_task_constructor_msgs::action::ExecuteTaskSolution>(
-	    node, "execute_task_solution");
-	if (!ac->wait_for_action_server(0.5s)) {
-		RCLCPP_ERROR(node->get_logger(), "Failed to connect to the 'execute_task_solution' action server");
+	// If this is the first call to execute create a persistent node that can be used to call the action server
+	if (!execute_solution_node_) {
+		execute_solution_node_ = rclcpp::Node::make_shared("moveit_task_constructor_executor");
+		execute_ac_ = rclcpp_action::create_client<moveit_task_constructor_msgs::action::ExecuteTaskSolution>(
+		    execute_solution_node_, "execute_task_solution");
+	}
+	if (!execute_ac_->wait_for_action_server(0.5s)) {
+		RCLCPP_ERROR(execute_solution_node_->get_logger(),
+		             "Failed to connect to the 'execute_task_solution' action server");
 		return moveit::core::MoveItErrorCode::FAILURE;
 	}
 
@@ -298,27 +298,38 @@ moveit::core::MoveItErrorCode Task::execute(const SolutionBase& s) {
 
 	moveit_msgs::msg::MoveItErrorCodes error_code;
 	error_code.val = moveit_msgs::msg::MoveItErrorCodes::FAILURE;
-	auto goal_handle_future = ac->async_send_goal(goal);
-	if (rclcpp::spin_until_future_complete(node, goal_handle_future) != rclcpp::FutureReturnCode::SUCCESS) {
-		RCLCPP_ERROR(node->get_logger(), "Send goal call failed");
+	auto goal_handle_future = execute_ac_->async_send_goal(goal);
+	if (rclcpp::spin_until_future_complete(execute_solution_node_, goal_handle_future) !=
+	    rclcpp::FutureReturnCode::SUCCESS) {
+		RCLCPP_ERROR(execute_solution_node_->get_logger(), "Send goal call failed");
 		return error_code;
 	}
 
 	const auto& goal_handle = goal_handle_future.get();
 	if (!goal_handle) {
-		RCLCPP_ERROR(node->get_logger(), "Goal was rejected by server");
+		RCLCPP_ERROR(execute_solution_node_->get_logger(), "Goal was rejected by server");
 		return error_code;
 	}
 
-	auto result_future = ac->async_get_result(goal_handle);
-	if (rclcpp::spin_until_future_complete(node, result_future) != rclcpp::FutureReturnCode::SUCCESS) {
-		RCLCPP_ERROR(node->get_logger(), "Get result call failed");
-		return error_code;
+	auto result_future = execute_ac_->async_get_result(goal_handle);
+	while (result_future.wait_for(std::chrono::milliseconds(10)) != std::future_status::ready) {
+		if (pimpl()->preempt_requested_) {
+			auto cancel_future = execute_ac_->async_cancel_goal(goal_handle);
+			if (rclcpp::spin_until_future_complete(execute_solution_node_, cancel_future) !=
+			    rclcpp::FutureReturnCode::SUCCESS) {
+				RCLCPP_ERROR(execute_solution_node_->get_logger(), "Could not preempt execution");
+				return error_code;
+			} else {
+				error_code.val = moveit_msgs::msg::MoveItErrorCodes::PREEMPTED;
+				return error_code;
+			}
+		}
+		rclcpp::spin_some(execute_solution_node_);
 	}
 
 	auto result = result_future.get();
 	if (result.code != rclcpp_action::ResultCode::SUCCEEDED) {
-		RCLCPP_ERROR(node->get_logger(), "Goal was aborted or canceled");
+		RCLCPP_ERROR(execute_solution_node_->get_logger(), "Goal was aborted or canceled");
 		return error_code;
 	}
 

demo/launch/demo.launch.py

@@ -2,12 +2,18 @@
 
 from ament_index_python.packages import get_package_share_directory
 from launch import LaunchDescription
-from launch.actions import ExecuteProcess
+from launch.actions import ExecuteProcess, DeclareLaunchArgument
+from launch.conditions import IfCondition
+from launch.substitutions import LaunchConfiguration
 from launch_ros.actions import Node
 from moveit_configs_utils import MoveItConfigsBuilder
 
 
 def generate_launch_description():
+    declared_arguments = []
+    declared_arguments.append(
+        DeclareLaunchArgument("rviz", default_value="true", description="Launch RViz?")
+    )
     moveit_config = (
         MoveItConfigsBuilder("moveit_resources_panda")
         .planning_pipelines(pipelines=["ompl"])
@@ -31,6 +37,7 @@ def generate_launch_description():
     )
 
     # RViz
+    launch_rviz = LaunchConfiguration("rviz")
     rviz_config_file = (
         get_package_share_directory("moveit_task_constructor_demo") + "/config/mtc.rviz"
     )
@@ -45,6 +52,7 @@ def generate_launch_description():
             moveit_config.robot_description_kinematics,
             moveit_config.planning_pipelines,
         ],
+        condition=IfCondition(launch_rviz),
     )
 
     # Static TF
@@ -94,7 +102,8 @@ def generate_launch_description():
         ]
 
     return LaunchDescription(
-        [
+        declared_arguments
+        + [
             rviz_node,
             static_tf,
             robot_state_publisher,