diff --git a/doc/how_to_guides/pilz_industrial_motion_planner/CMakeLists.txt b/doc/how_to_guides/pilz_industrial_motion_planner/CMakeLists.txt index 2ef8f5af91..5973acf864 100644 --- a/doc/how_to_guides/pilz_industrial_motion_planner/CMakeLists.txt +++ b/doc/how_to_guides/pilz_industrial_motion_planner/CMakeLists.txt @@ -6,10 +6,15 @@ add_executable(pilz_mtc src/pilz_mtc.cpp) target_include_directories(pilz_mtc PUBLIC include) ament_target_dependencies(pilz_mtc ${THIS_PACKAGE_INCLUDE_DEPENDS}) +add_executable(pilz_sequence src/pilz_sequence.cpp) +target_include_directories(pilz_sequence PUBLIC include) +ament_target_dependencies(pilz_sequence ${THIS_PACKAGE_INCLUDE_DEPENDS}) + install( TARGETS pilz_move_group pilz_mtc + pilz_sequence DESTINATION lib/${PROJECT_NAME} ) diff --git a/doc/how_to_guides/pilz_industrial_motion_planner/launch/pilz_moveit.launch.py b/doc/how_to_guides/pilz_industrial_motion_planner/launch/pilz_moveit.launch.py new file mode 100644 index 0000000000..088fc41903 --- /dev/null +++ b/doc/how_to_guides/pilz_industrial_motion_planner/launch/pilz_moveit.launch.py @@ -0,0 +1,111 @@ +import os +from launch import LaunchDescription +from launch_ros.actions import Node +from launch.actions import ExecuteProcess +from ament_index_python.packages import get_package_share_directory +from moveit_configs_utils import MoveItConfigsBuilder + + +def generate_launch_description(): + moveit_config = ( + MoveItConfigsBuilder("moveit_resources_panda") + .robot_description(file_path="config/panda.urdf.xacro") + .trajectory_execution(file_path="config/gripper_moveit_controllers.yaml") + .planning_scene_monitor( + publish_robot_description=True, publish_robot_description_semantic=True + ) + .planning_pipelines(pipelines=["pilz_industrial_motion_planner"]) + .to_moveit_configs() + ) + + # Starts Pilz Industrial Motion Planner MoveGroupSequenceAction and MoveGroupSequenceService servers + move_group_capabilities = { + "capabilities": "pilz_industrial_motion_planner/MoveGroupSequenceAction pilz_industrial_motion_planner/MoveGroupSequenceService" + } + + # Start the actual move_group node/action server + run_move_group_node = Node( + package="moveit_ros_move_group", + executable="move_group", + output="screen", + parameters=[ + moveit_config.to_dict(), + move_group_capabilities, + ], + ) + + # RViz + rviz_config_file = ( + get_package_share_directory("moveit2_tutorials") + "/launch/move_group.rviz" + ) + rviz_node = Node( + package="rviz2", + executable="rviz2", + name="rviz2", + output="log", + arguments=["-d", rviz_config_file], + parameters=[ + moveit_config.robot_description, + moveit_config.robot_description_semantic, + moveit_config.robot_description_kinematics, + moveit_config.planning_pipelines, + moveit_config.joint_limits, + ], + ) + + # 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 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", + ) + + # Load controllers + load_controllers = [] + for controller in [ + "panda_arm_controller", + "panda_hand_controller", + "joint_state_broadcaster", + ]: + load_controllers += [ + ExecuteProcess( + cmd=["ros2 run controller_manager spawner {}".format(controller)], + shell=True, + output="screen", + ) + ] + + return LaunchDescription( + [ + rviz_node, + static_tf, + robot_state_publisher, + run_move_group_node, + ros2_control_node, + ] + + load_controllers + ) diff --git a/doc/how_to_guides/pilz_industrial_motion_planner/pilz_industrial_motion_planner.rst b/doc/how_to_guides/pilz_industrial_motion_planner/pilz_industrial_motion_planner.rst index 92403787ca..99f70bd9d9 100644 --- a/doc/how_to_guides/pilz_industrial_motion_planner/pilz_industrial_motion_planner.rst +++ b/doc/how_to_guides/pilz_industrial_motion_planner/pilz_industrial_motion_planner.rst @@ -280,7 +280,7 @@ To run this, execute the following commands in separate Terminals: :: - ros2 launch moveit_resources_panda_moveit_config demo.launch.py + ros2 launch moveit2_tutorials pilz_moveit.launch.py ros2 run moveit2_tutorials pilz_move_group @@ -383,6 +383,174 @@ Restrictions for ``MotionSequenceRequest`` ``blend_radius``\ (i) + ``blend_radius``\ (i+1) has to be smaller than the distance between the goals. +Service interface +~~~~~~~~~~~~~~~~~ + +The service ``plan_sequence_path`` allows the user to generate a joint +trajectory for a ``moveit_msgs::msg::MotionSequenceRequest``. +The trajectory is returned and not executed. + +To use the ``MoveGroupSequenceService`` and the ``MoveGroupSequenceAction``, refer to the :codedir:`Pilz Motion Planner sequence example `. +To run this, execute the following commands in separate terminals: + +:: + + ros2 launch moveit2_tutorials pilz_moveit.launch.py + ros2 run moveit2_tutorials pilz_sequence + +For this service and action, the move_group launch file needs to be modified to include these Pilz Motion Planner capabilities. +The new +:codedir:`pilz_moveit.launch.py ` +is used instead: + +:: + + moveit_config = ( + MoveItConfigsBuilder("moveit_resources_panda") + .robot_description(file_path="config/panda.urdf.xacro") + .trajectory_execution(file_path="config/gripper_moveit_controllers.yaml") + .planning_scene_monitor( + publish_robot_description=True, publish_robot_description_semantic=True + ) + .planning_pipelines( + pipelines=["pilz_industrial_motion_planner"] + ) + .to_moveit_configs() + ) + + # Starts Pilz Industrial Motion Planner MoveGroupSequenceAction and MoveGroupSequenceService servers + move_group_capabilities = { + "capabilities": "pilz_industrial_motion_planner/MoveGroupSequenceAction pilz_industrial_motion_planner/MoveGroupSequenceService" + } + +The +:codedir:`pilz_sequence.cpp file ` +creates two target poses that will be reached sequentially. + +:: + + // ----- Motion Sequence Item 1 + // Create a MotionSequenceItem + moveit_msgs::msg::MotionSequenceItem item1; + + // Set pose blend radius + item1.blend_radius = 0.1; + + // MotionSequenceItem configuration + item1.req.group_name = PLANNING_GROUP; + item1.req.planner_id = "LIN"; + item1.req.allowed_planning_time = 5.0; + item1.req.max_velocity_scaling_factor = 0.1; + item1.req.max_acceleration_scaling_factor = 0.1; + + moveit_msgs::msg::Constraints constraints_item1; + moveit_msgs::msg::PositionConstraint pos_constraint_item1; + pos_constraint_item1.header.frame_id = "world"; + pos_constraint_item1.link_name = "panda_hand"; + + // Set a constraint pose + auto target_pose_item1 = [] { + geometry_msgs::msg::PoseStamped msg; + msg.header.frame_id = "world"; + msg.pose.position.x = 0.3; + msg.pose.position.y = -0.2; + msg.pose.position.z = 0.6; + msg.pose.orientation.x = 1.0; + msg.pose.orientation.y = 0.0; + msg.pose.orientation.z = 0.0; + msg.pose.orientation.w = 0.0; + return msg; + } (); + item1.req.goal_constraints.push_back(kinematic_constraints::constructGoalConstraints("panda_link8", target_pose_item1)); + +The service client needs to be initialized: + +:: + + // MoveGroupSequence service client + using GetMotionSequence = moveit_msgs::srv::GetMotionSequence; + auto service_client = node->create_client("/plan_sequence_path"); + + // Verify that the action server is up and running + while (!service_client->wait_for_service(std::chrono::seconds(10))) + { + RCLCPP_WARN(LOGGER, "Waiting for service /plan_sequence_path to be available..."); + } + +Then, the request is created: + +:: + + // Create request + auto service_request = std::make_shared(); + service_request->request.items.push_back(item1); + service_request->request.items.push_back(item2); + +Once the service call is completed, the method ``future.wait_for(timeout_duration)`` blocks until +a specified ``timeout_duration`` has elapsed or the result becomes available, whichever comes +first. The return value identifies the state of the result. This is performed every second +until the future is ready. + + +:: + + // Call the service and process the result + auto service_future = service_client->async_send_request(service_request); + + // Function to draw the trajectory + auto const draw_trajectory_tool_path = + [&moveit_visual_tools, jmg = move_group_interface.getRobotModel()->getJointModelGroup("panda_arm")]( + auto const& trajectories) { + for (const auto& trajectory : trajectories) { + moveit_visual_tools.publishTrajectoryLine(trajectory, jmg); + } + }; + + // Wait for the result + std::future_status service_status; + do + { + switch (service_status = service_future.wait_for(std::chrono::seconds(1)); service_status) + { + case std::future_status::deferred: + RCLCPP_ERROR(LOGGER, "Deferred"); + break; + case std::future_status::timeout: + RCLCPP_INFO(LOGGER, "Waiting for trajectory plan..."); + break; + case std::future_status::ready: + RCLCPP_INFO(LOGGER, "Service ready!"); + break; + } + } while (service_status != std::future_status::ready); + +The future response is read with the ``future.get()`` method. + +:: + + auto service_response = service_future.get(); + if (service_response->response.error_code.val == moveit_msgs::msg::MoveItErrorCodes::SUCCESS) + { + RCLCPP_INFO(LOGGER, "Planning successful"); + + // Access the planned trajectory + auto trajectory = service_response->response.planned_trajectories; + draw_trajectory_tool_path(trajectory); + moveit_visual_tools.trigger(); + } + else + { + RCLCPP_ERROR(LOGGER, "Planning failed with error code: %d", service_response->response.error_code.val); + + rclcpp::shutdown(); + return 0; + } + +In this case, the planned trajectory is drawn. Here is a comparison of a blend radius of 0 and 0.1 for the first and second trajectory, respectively. + +.. figure:: trajectory_comparison.jpeg + :alt: trajectory comparison + Action interface ~~~~~~~~~~~~~~~~ @@ -397,13 +565,83 @@ constraints of an individual ``moveit_msgs::msg::MotionPlanRequest`` are already satisfied but the ``MoveGroupSequenceAction`` capability doesn't implement such a check to allow moving on a circular or comparable path. -See the ``pilz_robot_programming`` package for a `ROS 1 Python script -`_ -that shows how to use the capability. +The action client needs to be initialized: -Service interface -~~~~~~~~~~~~~~~~~ +:: -The service ``plan_sequence_path`` allows the user to generate a joint -trajectory for a ``moveit_msgs::msg::MotionSequenceRequest``. -The trajectory is returned and not executed. + // MoveGroupSequence action client + using MoveGroupSequence = moveit_msgs::action::MoveGroupSequence; + auto action_client = rclcpp_action::create_client(node, "/sequence_move_group"); + + // Verify that the action server is up and running + if (!action_client->wait_for_action_server(std::chrono::seconds(10))) + { + RCLCPP_ERROR(LOGGER, "Error waiting for action server /sequence_move_group"); + return -1; + } + +Then, the request is created: + +:: + + // Create a MotionSequenceRequest + moveit_msgs::msg::MotionSequenceRequest sequence_request; + sequence_request.items.push_back(item1); + sequence_request.items.push_back(item2); + +The goal and planning options are configured. A goal response callback and result callback can be included as well. + +:: + + // Create action goal + auto goal_msg = MoveGroupSequence::Goal(); + goal_msg.request = sequence_request; + + // Planning options + goal_msg.planning_options.planning_scene_diff.is_diff = true; + goal_msg.planning_options.planning_scene_diff.robot_state.is_diff = true; + // goal_msg.planning_options.plan_only = true; // Uncomment to only plan the trajectory + +Finally, send the goal request and wait for the response: + +:: + + // Send the action goal + auto goal_handle_future = action_client->async_send_goal(goal_msg, send_goal_options); + + // Get result + auto action_result_future = action_client->async_get_result(goal_handle_future.get()); + + // Wait for the result + std::future_status action_status; + do + { + switch (action_status = action_result_future.wait_for(std::chrono::seconds(1)); action_status) + { + case std::future_status::deferred: + RCLCPP_ERROR(LOGGER, "Deferred"); + break; + case std::future_status::timeout: + RCLCPP_INFO(LOGGER, "Executing trajectory..."); + break; + case std::future_status::ready: + RCLCPP_INFO(LOGGER, "Action ready!"); + break; + } + } while (action_status != std::future_status::ready); + + if (action_result_future.valid()) + { + auto result = action_result_future.get(); + RCLCPP_INFO(LOGGER, "Action completed. Result: %d", static_cast(result.code)); + } + else + { + RCLCPP_ERROR(LOGGER, "Action couldn't be completed."); + } + +If the motion needs to be stopped mid-execution, the action can be canceled with: + +:: + + auto future_cancel_motion = client->async_cancel_goal(goal_handle_future_new.get()); diff --git a/doc/how_to_guides/pilz_industrial_motion_planner/src/pilz_move_group.cpp b/doc/how_to_guides/pilz_industrial_motion_planner/src/pilz_move_group.cpp index a1290f3f51..6356bfe3fa 100644 --- a/doc/how_to_guides/pilz_industrial_motion_planner/src/pilz_move_group.cpp +++ b/doc/how_to_guides/pilz_industrial_motion_planner/src/pilz_move_group.cpp @@ -9,7 +9,7 @@ * Pilz Example -- MoveGroup Interface * * To run this example, first run this launch file: - * ros2 launch moveit_resources_panda_moveit_config demo.launch.py + * ros2 launch moveit2_tutorials pilz_moveit.launch.py * * For best results, hide the "MotionPlanning" widget in RViz. * diff --git a/doc/how_to_guides/pilz_industrial_motion_planner/src/pilz_sequence.cpp b/doc/how_to_guides/pilz_industrial_motion_planner/src/pilz_sequence.cpp new file mode 100644 index 0000000000..574b929fc5 --- /dev/null +++ b/doc/how_to_guides/pilz_industrial_motion_planner/src/pilz_sequence.cpp @@ -0,0 +1,359 @@ +#include +#include + +#include +#include +#include +#include +#include +#include + +#include +#include +#include +#include +#include + +/** + * Pilz Example -- MoveGroupSequence service and action + * + * To run this example, first run this launch file: + * ros2 launch moveit2_tutorials pilz_moveit.launch.py + * + * Then, run this file: + * ros2 run moveit2_tutorials pilz_sequence + * + */ + +/* Author: Alejo Mancinelli - 02/07/2024 */ + +using moveit_msgs::action::MoveGroupSequence; +using GoalHandleMoveGroupSequence = rclcpp_action::ClientGoalHandle; + +// Create a ROS logger +auto const LOGGER = rclcpp::get_logger("pilz_sequence_node"); + +int main(int argc, char** argv) +{ + // Initialize ROS and create the Node + rclcpp::init(argc, argv); + rclcpp::NodeOptions node_options; + node_options.automatically_declare_parameters_from_overrides(true); + + auto node = rclcpp::Node::make_shared("pilz_sequence_node", node_options); + + // We spin up a SingleThreadedExecutor so MoveItVisualTools interact with ROS + rclcpp::executors::SingleThreadedExecutor executor; + executor.add_node(node); + std::thread([&executor]() { executor.spin(); }).detach(); + + // Planning group + static const std::string PLANNING_GROUP = "panda_arm"; + + // Create the MoveIt MoveGroup Interface + // In this case, this is just necessary for the visual interface + using moveit::planning_interface::MoveGroupInterface; + auto move_group_interface = MoveGroupInterface(node, PLANNING_GROUP); + + // Construct and initialize MoveItVisualTools + auto moveit_visual_tools = moveit_visual_tools::MoveItVisualTools{ node, "panda_link0", "move_group_tutorial", + move_group_interface.getRobotModel() }; + moveit_visual_tools.deleteAllMarkers(); + moveit_visual_tools.loadRemoteControl(); + moveit_visual_tools.prompt("Press 'next' in the RvizVisualToolsGui window to continue the demo"); + + // [ --------------------------------------------------------------- ] + // [ ----------------------- Motion Sequence ----------------------- ] + // [ --------------------------------------------------------------- ] + + // ----- Motion Sequence Item 1 + // Create a MotionSequenceItem + moveit_msgs::msg::MotionSequenceItem item1; + + // Set pose blend radius + item1.blend_radius = 0.1; + + // MotionSequenceItem configuration + item1.req.group_name = PLANNING_GROUP; + item1.req.planner_id = "LIN"; + item1.req.allowed_planning_time = 5.0; + item1.req.max_velocity_scaling_factor = 0.1; + item1.req.max_acceleration_scaling_factor = 0.1; + + moveit_msgs::msg::Constraints constraints_item1; + moveit_msgs::msg::PositionConstraint pos_constraint_item1; + pos_constraint_item1.header.frame_id = "world"; + pos_constraint_item1.link_name = "panda_hand"; + + // Set a constraint pose + auto target_pose_item1 = [] { + geometry_msgs::msg::PoseStamped msg; + msg.header.frame_id = "world"; + msg.pose.position.x = 0.3; + msg.pose.position.y = -0.2; + msg.pose.position.z = 0.6; + msg.pose.orientation.x = 1.0; + msg.pose.orientation.y = 0.0; + msg.pose.orientation.z = 0.0; + msg.pose.orientation.w = 0.0; + return msg; + }(); + item1.req.goal_constraints.push_back( + kinematic_constraints::constructGoalConstraints("panda_link8", target_pose_item1)); + + // ----- Motion Sequence Item 2 + // Create a MotionSequenceItem + moveit_msgs::msg::MotionSequenceItem item2; + + // Set pose blend radius + // For the last pose, it must be 0! + item2.blend_radius = 0.0; + + // MotionSequenceItem configuration + item2.req.group_name = PLANNING_GROUP; + item2.req.planner_id = "LIN"; + item2.req.allowed_planning_time = 5.0; + item2.req.max_velocity_scaling_factor = 0.1; + item2.req.max_acceleration_scaling_factor = 0.1; + + // Set a constraint pose + auto target_pose_item2 = [] { + geometry_msgs::msg::PoseStamped msg; + msg.header.frame_id = "world"; + msg.pose.position.x = 0.3; + msg.pose.position.y = -0.2; + msg.pose.position.z = 0.8; + msg.pose.orientation.x = 1.0; + msg.pose.orientation.y = 0.0; + msg.pose.orientation.z = 0.0; + msg.pose.orientation.w = 0.0; + return msg; + }(); + item2.req.goal_constraints.push_back( + kinematic_constraints::constructGoalConstraints("panda_link8", target_pose_item2)); + + // [ --------------------------------------------------------------- ] + // [ ------------------ MoveGroupSequence Service ------------------ ] + // [ --------------------------------------------------------------- ] + // The trajectory is returned but not executed + + // MoveGroupSequence service client + using GetMotionSequence = moveit_msgs::srv::GetMotionSequence; + auto service_client = node->create_client("/plan_sequence_path"); + + // Verify that the action server is up and running + while (!service_client->wait_for_service(std::chrono::seconds(10))) + { + RCLCPP_WARN(LOGGER, "Waiting for service /plan_sequence_path to be available..."); + } + + // Create request + auto service_request = std::make_shared(); + service_request->request.items.push_back(item1); + service_request->request.items.push_back(item2); + + // Call the service and process the result + auto service_future = service_client->async_send_request(service_request); + + // Function to draw the trajectory + auto const draw_trajectory_tool_path = + [&moveit_visual_tools, + jmg = move_group_interface.getRobotModel()->getJointModelGroup("panda_arm")](auto const& trajectories) { + for (const auto& trajectory : trajectories) + { + moveit_visual_tools.publishTrajectoryLine(trajectory, jmg); + } + }; + + // Wait for the result + std::future_status service_status; + do + { + switch (service_status = service_future.wait_for(std::chrono::seconds(1)); service_status) + { + case std::future_status::deferred: + RCLCPP_ERROR(LOGGER, "Deferred"); + break; + case std::future_status::timeout: + RCLCPP_INFO(LOGGER, "Waiting for trajectory plan..."); + break; + case std::future_status::ready: + RCLCPP_INFO(LOGGER, "Service ready!"); + break; + } + } while (service_status != std::future_status::ready); + + auto service_response = service_future.get(); + if (service_response->response.error_code.val == moveit_msgs::msg::MoveItErrorCodes::SUCCESS) + { + RCLCPP_INFO(LOGGER, "Planning successful"); + + // Access the planned trajectory + auto trajectory = service_response->response.planned_trajectories; + draw_trajectory_tool_path(trajectory); + moveit_visual_tools.trigger(); + } + else + { + RCLCPP_ERROR(LOGGER, "Planning failed with error code: %d", service_response->response.error_code.val); + + rclcpp::shutdown(); + return 0; + } + + moveit_visual_tools.prompt("Press 'next' in the RvizVisualToolsGui window to continue the demo"); + + // [ --------------------------------------------------------------- ] + // [ ------------------ MoveGroupSequence Action ------------------- ] + // [ --------------------------------------------------------------- ] + // Plans and executes the trajectory + + // MoveGroupSequence action client + using MoveGroupSequence = moveit_msgs::action::MoveGroupSequence; + auto action_client = rclcpp_action::create_client(node, "/sequence_move_group"); + + // Verify that the action server is up and running + if (!action_client->wait_for_action_server(std::chrono::seconds(10))) + { + RCLCPP_ERROR(LOGGER, "Error waiting for action server /sequence_move_group"); + return -1; + } + + // Create a MotionSequenceRequest + moveit_msgs::msg::MotionSequenceRequest sequence_request; + sequence_request.items.push_back(item1); + sequence_request.items.push_back(item2); + + // Create action goal + auto goal_msg = MoveGroupSequence::Goal(); + goal_msg.request = sequence_request; + + // Planning options + goal_msg.planning_options.planning_scene_diff.is_diff = true; + goal_msg.planning_options.planning_scene_diff.robot_state.is_diff = true; + // goal_msg.planning_options.plan_only = true; // Uncomment to only plan the trajectory + + // Goal response callback + auto send_goal_options = rclcpp_action::Client::SendGoalOptions(); + send_goal_options.goal_response_callback = [](std::shared_ptr goal_handle) { + try + { + if (!goal_handle) + { + RCLCPP_ERROR(LOGGER, "Goal was rejected by server"); + } + else + { + RCLCPP_INFO(LOGGER, "Goal accepted by server, waiting for result"); + } + } + catch (const std::exception& e) + { + RCLCPP_ERROR(LOGGER, "Exception while waiting for goal response: %s", e.what()); + } + }; + + // Result callback + send_goal_options.result_callback = [](const GoalHandleMoveGroupSequence::WrappedResult& result) { + switch (result.code) + { + case rclcpp_action::ResultCode::SUCCEEDED: + RCLCPP_INFO(LOGGER, "Goal succeeded"); + break; + case rclcpp_action::ResultCode::ABORTED: + RCLCPP_ERROR(LOGGER, "Goal was aborted. Status: %d", result.result->response.error_code.val); + break; + case rclcpp_action::ResultCode::CANCELED: + RCLCPP_ERROR(LOGGER, "Goal was canceled"); + break; + default: + RCLCPP_ERROR(LOGGER, "Unknown result code"); + break; + } + RCLCPP_INFO(LOGGER, "Result received"); + }; + + // Send the action goal + auto goal_handle_future = action_client->async_send_goal(goal_msg, send_goal_options); + + // Get result + auto action_result_future = action_client->async_get_result(goal_handle_future.get()); + + // Wait for the result + std::future_status action_status; + do + { + switch (action_status = action_result_future.wait_for(std::chrono::seconds(1)); action_status) + { + case std::future_status::deferred: + RCLCPP_ERROR(LOGGER, "Deferred"); + break; + case std::future_status::timeout: + RCLCPP_INFO(LOGGER, "Executing trajectory..."); + break; + case std::future_status::ready: + RCLCPP_INFO(LOGGER, "Action ready!"); + break; + } + } while (action_status != std::future_status::ready); + + if (action_result_future.valid()) + { + auto result = action_result_future.get(); + RCLCPP_INFO(LOGGER, "Action completed. Result: %d", static_cast(result.code)); + } + else + { + RCLCPP_ERROR(LOGGER, "Action couldn't be completed."); + } + + moveit_visual_tools.prompt("Press 'next' in the RvizVisualToolsGui window to continue the demo"); + + // [ --------------------------------------------------------------- ] + // [ ------------------------- Stop Motion ------------------------- ] + // [ --------------------------------------------------------------- ] + + // Repeats the motion but after 2 seconds cancels the action + auto cancel_goal_handle_future = action_client->async_send_goal(goal_msg, send_goal_options); + sleep(2); + auto cancel_action_result_future = action_client->async_cancel_goal(cancel_goal_handle_future.get()); + + // Wait for the result + std::future_status action_cancel_status; + do + { + switch (action_cancel_status = cancel_action_result_future.wait_for(std::chrono::seconds(1)); action_cancel_status) + { + case std::future_status::deferred: + RCLCPP_ERROR(LOGGER, "Deferred"); + break; + case std::future_status::timeout: + RCLCPP_INFO(LOGGER, "Waiting for trajectory stop..."); + break; + case std::future_status::ready: + RCLCPP_INFO(LOGGER, "Action cancel!"); + break; + } + } while (action_cancel_status != std::future_status::ready); + + if (cancel_action_result_future.valid()) + { + auto cancel_response = cancel_action_result_future.get(); + + if (!cancel_response->return_code) + { + RCLCPP_INFO(LOGGER, "The action has been canceled by the action server."); + } + else + { + RCLCPP_INFO(LOGGER, "Action cancel error. Code: %d.", cancel_response->return_code); + } + } + else + { + RCLCPP_ERROR(LOGGER, "Action couldn't be canceled."); + } + + rclcpp::shutdown(); + return 0; +} diff --git a/doc/how_to_guides/pilz_industrial_motion_planner/trajectory_comparison.jpeg b/doc/how_to_guides/pilz_industrial_motion_planner/trajectory_comparison.jpeg new file mode 100644 index 0000000000..b0821bc895 Binary files /dev/null and b/doc/how_to_guides/pilz_industrial_motion_planner/trajectory_comparison.jpeg differ