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

This commit is contained in:
Marq Rasmussen 2025-05-29 12:31:15 -06:00 committed by marqrazz
parent bf6c6496b7
commit fd9044a305
7 changed files with 255 additions and 18 deletions

View File

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

View File

@ -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>

View File

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

View File

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

View File

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

View File

@ -43,6 +43,7 @@
#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>
@ -50,6 +51,7 @@
#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 @@ protected:
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) {

View File

@ -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;
}