mirror of
https://github.com/moveit/moveit_task_constructor.git
synced 2025-09-27 00:29:13 +08:00
Let Task::preempt() cancel execute() (#684)
This commit is contained in:
parent
bf6c6496b7
commit
fd9044a305
@ -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)
|
||||
|
@ -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>
|
||||
|
15
capabilities/test/CMakeLists.txt
Normal file
15
capabilities/test/CMakeLists.txt
Normal 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()
|
124
capabilities/test/test_execution.launch.py
Normal file
124
capabilities/test/test_execution.launch.py
Normal 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)
|
73
capabilities/test/test_task_execution.cpp
Normal file
73
capabilities/test/test_task_execution.cpp
Normal 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();
|
||||
}
|
@ -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) {
|
||||
|
@ -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;
|
||||
}
|
||||
|
||||
|
Loading…
Reference in New Issue
Block a user