Let Task::preempt() cancel execute() (moveit#684)

Author: MarqRazz

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/package.xml

@@ -22,6 +22,12 @@
 	<depend>rclcpp_action</depend>
 	<depend>std_msgs</depend>
 
+	<test_depend>ament_cmake_gtest</test_depend>
+	<test_depend>moveit_configs_utils</test_depend>
+	<test_depend>launch_testing</test_depend>
+	<test_depend>launch_testing_ament_cmake</test_depend>
+	<test_depend>moveit_resources_panda_moveit_config</test_depend>
+
 	<export>
 		<build_type>ament_cmake</build_type>
 	</export>

capabilities/test/CMakeLists.txt

@@ -0,0 +1,15 @@
+#############
+## Testing ##
+#############
+
+## Add gtest based cpp test target
+if (BUILD_TESTING AND NOT (CMAKE_CXX_FLAGS MATCHES "-fsanitize"))
+	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,124 @@
+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
+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")
+        .planning_pipelines(pipelines=["ompl"])
+        .robot_description(file_path="config/panda.urdf.xacro")
+        .to_moveit_configs()
+    )
+
+    # Load  ExecuteTaskSolutionCapability so we can test executing solutions
+    move_group_capabilities = {"capabilities": "move_group/ExecuteTaskSolutionCapability"}
+
+    # Start the actual move_group node/action server
+    move_group_node = Node(
+        package="moveit_ros_move_group",
+        executable="move_group",
+        output="screen",
+        parameters=[
+            moveit_config.to_dict(),
+            move_group_capabilities,
+        ],
+    )
+
+    # Static TF
+    static_tf = Node(
+        package="tf2_ros",
+        executable="static_transform_publisher",
+        name="static_transform_publisher",
+        output="log",
+        arguments=["0.0", "0.0", "0.0", "0.0", "0.0", "0.0", "world", "panda_link0"],
+    )
+
+    # Publish joint states to TF
+    robot_state_publisher = Node(
+        package="robot_state_publisher",
+        executable="robot_state_publisher",
+        name="robot_state_publisher",
+        output="both",
+        parameters=[moveit_config.robot_description],
+    )
+
+    # ros2_control using FakeSystem as hardware
+    ros2_controllers_path = os.path.join(
+        get_package_share_directory("moveit_resources_panda_moveit_config"),
+        "config",
+        "ros2_controllers.yaml",
+    )
+    ros2_control_node = Node(
+        package="controller_manager",
+        executable="ros2_control_node",
+        parameters=[moveit_config.robot_description, ros2_controllers_path],
+        output="both",
+    )
+
+    controller_spawner = Node(
+        package="controller_manager",
+        executable="spawner",
+        arguments=[
+            "panda_arm_controller",
+            "joint_state_broadcaster",
+            "--controller-manager",
+            "/controller_manager",
+        ],
+    )
+
+    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(
+            [
+                static_tf,
+                robot_state_publisher,
+                move_group_node,
+                ros2_control_node,
+                controller_spawner,
+                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,73 @@
+#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("test_task_execution");
+		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");
+	ASSERT_TRUE(t.plan());
+	auto result = t.execute(*t.solutions().front());
+	EXPECT_EQ(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");
+	ASSERT_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();
+	if (execute_thread.joinable()) {
+		execute_thread.join();
+	}
+
+	EXPECT_EQ(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(5));
+
+	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,16 @@ 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_" +
+		                                                   std::to_string(reinterpret_cast<std::size_t>(this)));
+		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 +299,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;
 	}