From 38fea14cbaf1ef0c378fa0876d058917b14dda58 Mon Sep 17 00:00:00 2001 From: stevemacenski Date: Tue, 4 Aug 2020 17:55:01 -0700 Subject: [PATCH 1/3] failures tests --- nav2_bt_navigator/src/bt_navigator.cpp | 3 - nav2_system_tests/CMakeLists.txt | 1 + .../src/system_failure/CMakeLists.txt | 12 + .../src/system_failure/README.md | 3 + .../test_system_failure_launch.py | 105 ++++++ .../src/system_failure/tester_node.py | 306 ++++++++++++++++++ 6 files changed, 427 insertions(+), 3 deletions(-) create mode 100644 nav2_system_tests/src/system_failure/CMakeLists.txt create mode 100644 nav2_system_tests/src/system_failure/README.md create mode 100755 nav2_system_tests/src/system_failure/test_system_failure_launch.py create mode 100755 nav2_system_tests/src/system_failure/tester_node.py diff --git a/nav2_bt_navigator/src/bt_navigator.cpp b/nav2_bt_navigator/src/bt_navigator.cpp index 64e076ae621..863d2635b3c 100644 --- a/nav2_bt_navigator/src/bt_navigator.cpp +++ b/nav2_bt_navigator/src/bt_navigator.cpp @@ -312,9 +312,6 @@ BtNavigator::navigateToPose() RCLCPP_INFO(get_logger(), "Navigation canceled"); action_server_->terminate_all(); break; - - default: - throw std::logic_error("Invalid status return from BT"); } } diff --git a/nav2_system_tests/CMakeLists.txt b/nav2_system_tests/CMakeLists.txt index 9eae0646f2f..8b4178c6716 100644 --- a/nav2_system_tests/CMakeLists.txt +++ b/nav2_system_tests/CMakeLists.txt @@ -55,6 +55,7 @@ if(BUILD_TESTING) add_subdirectory(src/planning) add_subdirectory(src/localization) add_subdirectory(src/system) + add_subdirectory(src/system_failure) add_subdirectory(src/updown) add_subdirectory(src/waypoint_follower) add_subdirectory(src/recoveries/spin) diff --git a/nav2_system_tests/src/system_failure/CMakeLists.txt b/nav2_system_tests/src/system_failure/CMakeLists.txt new file mode 100644 index 00000000000..885234adc21 --- /dev/null +++ b/nav2_system_tests/src/system_failure/CMakeLists.txt @@ -0,0 +1,12 @@ +ament_add_test(test_failure_navigator + GENERATE_RESULT_FOR_RETURN_CODE_ZERO + COMMAND "${CMAKE_CURRENT_SOURCE_DIR}/test_system_failure_launch.py" + WORKING_DIRECTORY "${CMAKE_CURRENT_BINARY_DIR}" + TIMEOUT 180 + ENV + TEST_DIR=${CMAKE_CURRENT_SOURCE_DIR} + TEST_MAP=${PROJECT_SOURCE_DIR}/maps/map_circular.yaml + TEST_WORLD=${PROJECT_SOURCE_DIR}/worlds/turtlebot3_ros2_demo.world + GAZEBO_MODEL_PATH=${PROJECT_SOURCE_DIR}/models + BT_NAVIGATOR_XML=navigate_w_replanning_and_recovery.xml +) diff --git a/nav2_system_tests/src/system_failure/README.md b/nav2_system_tests/src/system_failure/README.md new file mode 100644 index 00000000000..4afd4eaac34 --- /dev/null +++ b/nav2_system_tests/src/system_failure/README.md @@ -0,0 +1,3 @@ +# Navigation2 System Tests - Failure + +High level system failures tests diff --git a/nav2_system_tests/src/system_failure/test_system_failure_launch.py b/nav2_system_tests/src/system_failure/test_system_failure_launch.py new file mode 100755 index 00000000000..860e4bddd54 --- /dev/null +++ b/nav2_system_tests/src/system_failure/test_system_failure_launch.py @@ -0,0 +1,105 @@ +#!/usr/bin/env python3 + +# Copyright (c) 2018 Intel Corporation +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +import os +import sys + +from ament_index_python.packages import get_package_share_directory + +from launch import LaunchDescription +from launch import LaunchService +from launch.actions import ExecuteProcess, IncludeLaunchDescription, SetEnvironmentVariable +from launch.launch_description_sources import PythonLaunchDescriptionSource +from launch_ros.actions import Node +from launch_testing.legacy import LaunchTestService + +from nav2_common.launch import RewrittenYaml + + +def generate_launch_description(): + map_yaml_file = os.getenv('TEST_MAP') + world = os.getenv('TEST_WORLD') + + bt_navigator_xml = os.path.join(get_package_share_directory('nav2_bt_navigator'), + 'behavior_trees', + os.getenv('BT_NAVIGATOR_XML')) + + bringup_dir = get_package_share_directory('nav2_bringup') + params_file = os.path.join(bringup_dir, 'params', 'nav2_params.yaml') + + # Replace the `use_astar` setting on the params file + param_substitutions = { + 'planner_server.ros__parameters.GridBased.use_astar': 'False'} + configured_params = RewrittenYaml( + source_file=params_file, + root_key='', + param_rewrites=param_substitutions, + convert_types=True) + + return LaunchDescription([ + SetEnvironmentVariable('RCUTILS_CONSOLE_STDOUT_LINE_BUFFERED', '1'), + + # Launch gazebo server for simulation + ExecuteProcess( + cmd=['gzserver', '-s', 'libgazebo_ros_init.so', + '--minimal_comms', world], + output='screen'), + + # TODO(orduno) Launch the robot state publisher instead + # using a local copy of TB3 urdf file + Node( + package='tf2_ros', + executable='static_transform_publisher', + output='screen', + arguments=['0', '0', '0', '0', '0', '0', 'base_footprint', 'base_link']), + + Node( + package='tf2_ros', + executable='static_transform_publisher', + output='screen', + arguments=['0', '0', '0', '0', '0', '0', 'base_link', 'base_scan']), + + IncludeLaunchDescription( + PythonLaunchDescriptionSource( + os.path.join(bringup_dir, 'launch', 'bringup_launch.py')), + launch_arguments={'namespace': '', + 'use_namespace': 'False', + 'map': map_yaml_file, + 'use_sim_time': 'True', + 'params_file': configured_params, + 'bt_xml_file': bt_navigator_xml, + 'autostart': 'True'}.items()), + ]) + + +def main(argv=sys.argv[1:]): + ld = generate_launch_description() + + test1_action = ExecuteProcess( + cmd=[os.path.join(os.getenv('TEST_DIR'), 'tester_node.py'), + '-r', '-2.0', '-0.5', '100.0', '100.0'], + name='tester_node', + output='screen') + + lts = LaunchTestService() + lts.add_test_action(ld, test1_action) + ls = LaunchService(argv=argv) + ls.include_launch_description(ld) + return lts.run(ls) + + +if __name__ == '__main__': + sys.exit(main()) diff --git a/nav2_system_tests/src/system_failure/tester_node.py b/nav2_system_tests/src/system_failure/tester_node.py new file mode 100755 index 00000000000..dd89df6cebc --- /dev/null +++ b/nav2_system_tests/src/system_failure/tester_node.py @@ -0,0 +1,306 @@ +#! /usr/bin/env python3 +# Copyright 2018 Intel Corporation. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +import argparse +import math +import sys +import time +from typing import Optional + +from action_msgs.msg import GoalStatus +from geometry_msgs.msg import Pose +from geometry_msgs.msg import PoseStamped +from geometry_msgs.msg import PoseWithCovarianceStamped +from lifecycle_msgs.srv import GetState +from nav2_msgs.action import NavigateToPose +from nav2_msgs.srv import ManageLifecycleNodes + +import rclpy + +from rclpy.action import ActionClient +from rclpy.node import Node +from rclpy.qos import QoSDurabilityPolicy, QoSHistoryPolicy, QoSReliabilityPolicy +from rclpy.qos import QoSProfile + + +class NavTester(Node): + + def __init__( + self, + initial_pose: Pose, + goal_pose: Pose, + namespace: str = '' + ): + super().__init__(node_name='nav2_tester', namespace=namespace) + self.initial_pose_pub = self.create_publisher(PoseWithCovarianceStamped, + 'initialpose', 10) + self.goal_pub = self.create_publisher(PoseStamped, 'goal_pose', 10) + + pose_qos = QoSProfile( + durability=QoSDurabilityPolicy.RMW_QOS_POLICY_DURABILITY_TRANSIENT_LOCAL, + reliability=QoSReliabilityPolicy.RMW_QOS_POLICY_RELIABILITY_RELIABLE, + history=QoSHistoryPolicy.RMW_QOS_POLICY_HISTORY_KEEP_LAST, + depth=1) + + self.model_pose_sub = self.create_subscription(PoseWithCovarianceStamped, + 'amcl_pose', self.poseCallback, pose_qos) + self.initial_pose_received = False + self.initial_pose = initial_pose + self.goal_pose = goal_pose + self.action_client = ActionClient(self, NavigateToPose, 'navigate_to_pose') + + def info_msg(self, msg: str): + self.get_logger().info('\033[1;37;44m' + msg + '\033[0m') + + def warn_msg(self, msg: str): + self.get_logger().warn('\033[1;37;43m' + msg + '\033[0m') + + def error_msg(self, msg: str): + self.get_logger().error('\033[1;37;41m' + msg + '\033[0m') + + def setInitialPose(self): + msg = PoseWithCovarianceStamped() + msg.pose.pose = self.initial_pose + msg.header.frame_id = 'map' + self.info_msg('Publishing Initial Pose') + self.initial_pose_pub.publish(msg) + self.currentPose = self.initial_pose + + def getStampedPoseMsg(self, pose: Pose): + msg = PoseStamped() + msg.header.frame_id = 'map' + msg.pose = pose + return msg + + def publishGoalPose(self, goal_pose: Optional[Pose] = None): + self.goal_pose = goal_pose if goal_pose is not None else self.goal_pose + self.goal_pub.publish(self.getStampedPoseMsg(self.goal_pose)) + + def runNavigateAction(self, goal_pose: Optional[Pose] = None): + # Sends a `NavToPose` action request and waits for completion + self.info_msg("Waiting for 'NavigateToPose' action server") + while not self.action_client.wait_for_server(timeout_sec=1.0): + self.info_msg("'NavigateToPose' action server not available, waiting...") + + self.goal_pose = goal_pose if goal_pose is not None else self.goal_pose + goal_msg = NavigateToPose.Goal() + goal_msg.pose = self.getStampedPoseMsg(self.goal_pose) + + self.info_msg('Sending goal request...') + send_goal_future = self.action_client.send_goal_async(goal_msg) + + rclpy.spin_until_future_complete(self, send_goal_future) + goal_handle = send_goal_future.result() + + if not goal_handle.accepted: + self.error_msg('Goal rejected') + return False + + self.info_msg('Goal accepted') + get_result_future = goal_handle.get_result_async() + + self.info_msg("Waiting for 'NavigateToPose' action to complete") + rclpy.spin_until_future_complete(self, get_result_future) + status = get_result_future.result().status + if status != GoalStatus.STATUS_ABORTED: + self.info_msg('Goal failed with status code: {0}'.format(status)) + return False + + self.info_msg('Goal failed, as expected!') + return True + + def poseCallback(self, msg): + self.info_msg('Received amcl_pose') + self.current_pose = msg.pose.pose + self.initial_pose_received = True + + def reachesGoal(self, timeout, distance): + goalReached = False + start_time = time.time() + + while not goalReached: + rclpy.spin_once(self, timeout_sec=1) + if self.distanceFromGoal() < distance: + goalReached = True + self.info_msg('*** GOAL REACHED ***') + return True + elif timeout is not None: + if (time.time() - start_time) > timeout: + self.error_msg('Robot timed out reaching its goal!') + return False + + def distanceFromGoal(self): + d_x = self.current_pose.position.x - self.goal_pose.position.x + d_y = self.current_pose.position.y - self.goal_pose.position.y + distance = math.sqrt(d_x * d_x + d_y * d_y) + self.info_msg('Distance from goal is: ' + str(distance)) + return distance + + def wait_for_node_active(self, node_name: str): + # Waits for the node within the tester namespace to become active + self.info_msg('Waiting for ' + node_name + ' to become active') + node_service = node_name + '/get_state' + state_client = self.create_client(GetState, node_service) + while not state_client.wait_for_service(timeout_sec=1.0): + self.info_msg(node_service + ' service not available, waiting...') + req = GetState.Request() # empty request + state = 'UNKNOWN' + while (state != 'active'): + self.info_msg('Getting ' + node_name + ' state...') + future = state_client.call_async(req) + rclpy.spin_until_future_complete(self, future) + if future.result() is not None: + state = future.result().current_state.label + self.info_msg('Result of get_state: %s' % state) + else: + self.error_msg('Exception while calling service: %r' % future.exception()) + time.sleep(5) + + def shutdown(self): + self.info_msg('Shutting down') + self.action_client.destroy() + + transition_service = 'lifecycle_manager_navigation/manage_nodes' + mgr_client = self.create_client(ManageLifecycleNodes, transition_service) + while not mgr_client.wait_for_service(timeout_sec=1.0): + self.info_msg(transition_service + ' service not available, waiting...') + + req = ManageLifecycleNodes.Request() + req.command = ManageLifecycleNodes.Request().SHUTDOWN + future = mgr_client.call_async(req) + try: + self.info_msg('Shutting down navigation lifecycle manager...') + rclpy.spin_until_future_complete(self, future) + future.result() + self.info_msg('Shutting down navigation lifecycle manager complete.') + except Exception as e: + self.error_msg('Service call failed %r' % (e,)) + transition_service = 'lifecycle_manager_localization/manage_nodes' + mgr_client = self.create_client(ManageLifecycleNodes, transition_service) + while not mgr_client.wait_for_service(timeout_sec=1.0): + self.info_msg(transition_service + ' service not available, waiting...') + + req = ManageLifecycleNodes.Request() + req.command = ManageLifecycleNodes.Request().SHUTDOWN + future = mgr_client.call_async(req) + try: + self.info_msg('Shutting down localization lifecycle manager...') + rclpy.spin_until_future_complete(self, future) + future.result() + self.info_msg('Shutting down localization lifecycle manager complete') + except Exception as e: + self.error_msg('Service call failed %r' % (e,)) + + def wait_for_initial_pose(self): + self.initial_pose_received = False + while not self.initial_pose_received: + self.info_msg('Setting initial pose') + self.setInitialPose() + self.info_msg('Waiting for amcl_pose to be received') + rclpy.spin_once(self, timeout_sec=1) + + +def run_all_tests(robot_tester): + # set transforms to use_sim_time + result = True + if (result): + robot_tester.wait_for_node_active('amcl') + robot_tester.wait_for_initial_pose() + robot_tester.wait_for_node_active('bt_navigator') + result = robot_tester.runNavigateAction() + + # Add more tests here if desired + + if (result): + robot_tester.info_msg('Test PASSED') + else: + robot_tester.error_msg('Test FAILED') + + return result + + +def fwd_pose(x=0.0, y=0.0, z=0.01): + initial_pose = Pose() + initial_pose.position.x = x + initial_pose.position.y = y + initial_pose.position.z = z + initial_pose.orientation.x = 0.0 + initial_pose.orientation.y = 0.0 + initial_pose.orientation.z = 0.0 + initial_pose.orientation.w = 1.0 + return initial_pose + + +def get_testers(args): + testers = [] + + if args.robot: + # Requested tester for one robot + init_x, init_y, final_x, final_y = args.robot[0] + tester = NavTester( + initial_pose=fwd_pose(float(init_x), float(init_y)), + goal_pose=fwd_pose(float(final_x), float(final_y))) + tester.info_msg( + 'Starting tester, robot going from ' + init_x + ', ' + init_y + + ' to ' + final_x + ', ' + final_y + '.') + testers.append(tester) + return testers + + return testers + + +def main(argv=sys.argv[1:]): + # The robot(s) positions from the input arguments + parser = argparse.ArgumentParser(description='System-level navigation tester node') + group = parser.add_mutually_exclusive_group(required=True) + group.add_argument('-r', '--robot', action='append', nargs=4, + metavar=('init_x', 'init_y', 'final_x', 'final_y'), + help='The robot starting and final positions.') + group.add_argument('-rs', '--robots', action='append', nargs=5, + metavar=('name', 'init_x', 'init_y', 'final_x', 'final_y'), + help="The robot's namespace and starting and final positions. " + + 'Repeating the argument for multiple robots is supported.') + + args, unknown = parser.parse_known_args() + + rclpy.init() + + # Create testers for each robot + testers = get_testers(args) + + # wait a few seconds to make sure entire stacks are up + time.sleep(10) + + for tester in testers: + passed = run_all_tests(tester) + if not passed: + break + + for tester in testers: + # stop and shutdown the nav stack to exit cleanly + tester.shutdown() + + testers[0].info_msg('Done Shutting Down.') + + if not passed: + testers[0].info_msg('Exiting failed') + exit(1) + else: + testers[0].info_msg('Exiting passed') + exit(0) + + +if __name__ == '__main__': + main() From ad2bdca1dd9a843db5fe50c9ba963e7fe0d1a79a Mon Sep 17 00:00:00 2001 From: stevemacenski Date: Tue, 4 Aug 2020 17:56:24 -0700 Subject: [PATCH 2/3] adding copyrights --- .../src/system_failure/test_system_failure_launch.py | 1 + nav2_system_tests/src/system_failure/tester_node.py | 1 + 2 files changed, 2 insertions(+) diff --git a/nav2_system_tests/src/system_failure/test_system_failure_launch.py b/nav2_system_tests/src/system_failure/test_system_failure_launch.py index 860e4bddd54..c41e78f3048 100755 --- a/nav2_system_tests/src/system_failure/test_system_failure_launch.py +++ b/nav2_system_tests/src/system_failure/test_system_failure_launch.py @@ -1,6 +1,7 @@ #!/usr/bin/env python3 # Copyright (c) 2018 Intel Corporation +# Copyright (c) 2020 Samsung Research America # # Licensed under the Apache License, Version 2.0 (the "License"); # you may not use this file except in compliance with the License. diff --git a/nav2_system_tests/src/system_failure/tester_node.py b/nav2_system_tests/src/system_failure/tester_node.py index dd89df6cebc..eac29bc6540 100755 --- a/nav2_system_tests/src/system_failure/tester_node.py +++ b/nav2_system_tests/src/system_failure/tester_node.py @@ -1,5 +1,6 @@ #! /usr/bin/env python3 # Copyright 2018 Intel Corporation. +# Copyright 2020 Samsung Research America # # Licensed under the Apache License, Version 2.0 (the "License"); # you may not use this file except in compliance with the License. From 9ba4de3b5b44d0403558b108dc1f441f4819ac44 Mon Sep 17 00:00:00 2001 From: stevemacenski Date: Tue, 4 Aug 2020 18:51:40 -0700 Subject: [PATCH 3/3] cancel test in recoveries --- nav2_controller/src/nav2_controller.cpp | 9 +-- nav2_planner/src/planner_server.cpp | 9 +-- .../src/planning/planner_tester.cpp | 2 + .../wait/test_wait_recovery_node.cpp | 10 ++- .../recoveries/wait/wait_recovery_tester.cpp | 77 +++++++++++++++++++ .../recoveries/wait/wait_recovery_tester.hpp | 2 + 6 files changed, 93 insertions(+), 16 deletions(-) diff --git a/nav2_controller/src/nav2_controller.cpp b/nav2_controller/src/nav2_controller.cpp index 3b6ba3971fb..7d91e0128fd 100644 --- a/nav2_controller/src/nav2_controller.cpp +++ b/nav2_controller/src/nav2_controller.cpp @@ -278,13 +278,8 @@ void ControllerServer::computeControl() rclcpp::Rate loop_rate(controller_frequency_); while (rclcpp::ok()) { - if (action_server_ == nullptr) { - RCLCPP_DEBUG(get_logger(), "Action server unavailable. Stopping."); - return; - } - - if (!action_server_->is_server_active()) { - RCLCPP_DEBUG(get_logger(), "Action server is inactive. Stopping."); + if (action_server_ == nullptr || !action_server_->is_server_active()) { + RCLCPP_DEBUG(get_logger(), "Action server unavailable or inactive. Stopping."); return; } diff --git a/nav2_planner/src/planner_server.cpp b/nav2_planner/src/planner_server.cpp index 9224290fa6b..24f104f6f7e 100644 --- a/nav2_planner/src/planner_server.cpp +++ b/nav2_planner/src/planner_server.cpp @@ -211,13 +211,8 @@ PlannerServer::computePlan() auto result = std::make_shared(); try { - if (action_server_ == nullptr) { - RCLCPP_DEBUG(get_logger(), "Action server unavailable. Stopping."); - return; - } - - if (!action_server_->is_server_active()) { - RCLCPP_DEBUG(get_logger(), "Action server is inactive. Stopping."); + if (action_server_ == nullptr || !action_server_->is_server_active()) { + RCLCPP_DEBUG(get_logger(), "Action server unavailable or inactive. Stopping."); return; } diff --git a/nav2_system_tests/src/planning/planner_tester.cpp b/nav2_system_tests/src/planning/planner_tester.cpp index 4c65a5a7b2a..1367735aa26 100644 --- a/nav2_system_tests/src/planning/planner_tester.cpp +++ b/nav2_system_tests/src/planning/planner_tester.cpp @@ -71,6 +71,8 @@ void PlannerTester::activate() "GridBased.use_astar", rclcpp::ParameterValue(true)); planner_tester_->set_parameter( rclcpp::Parameter(std::string("GridBased.use_astar"), rclcpp::ParameterValue(true))); + planner_tester_->set_parameter( + rclcpp::Parameter(std::string("expected_planner_frequency"), rclcpp::ParameterValue(-1.0))); planner_tester_->onConfigure(state); publishRobotTransform(); map_pub_ = this->create_publisher("map", 1); diff --git a/nav2_system_tests/src/recoveries/wait/test_wait_recovery_node.cpp b/nav2_system_tests/src/recoveries/wait/test_wait_recovery_node.cpp index e14ca5c7dcf..39ee9871d29 100644 --- a/nav2_system_tests/src/recoveries/wait/test_wait_recovery_node.cpp +++ b/nav2_system_tests/src/recoveries/wait/test_wait_recovery_node.cpp @@ -62,11 +62,16 @@ WaitRecoveryTester * WaitRecoveryTestFixture::wait_recovery_tester = nullptr; TEST_P(WaitRecoveryTestFixture, testSWaitRecovery) { float wait_time = std::get<0>(GetParam()); + float cancel = std::get<1>(GetParam()); bool success = false; int num_tries = 3; for (int i = 0; i != num_tries; i++) { - success = success || wait_recovery_tester->recoveryTest(wait_time); + if (cancel == 1.0) { + success = success || wait_recovery_tester->recoveryTestCancel(wait_time); + } else { + success = success || wait_recovery_tester->recoveryTest(wait_time); + } if (success) { break; } @@ -81,7 +86,8 @@ INSTANTIATE_TEST_CASE_P( ::testing::Values( std::make_tuple(1.0, 0.0), std::make_tuple(2.0, 0.0), - std::make_tuple(5.0, 0.0)), + std::make_tuple(5.0, 0.0), + std::make_tuple(10.0, 1.0)), testNameGenerator); int main(int argc, char ** argv) diff --git a/nav2_system_tests/src/recoveries/wait/wait_recovery_tester.cpp b/nav2_system_tests/src/recoveries/wait/wait_recovery_tester.cpp index eb980e9a9d1..7112289b5af 100644 --- a/nav2_system_tests/src/recoveries/wait/wait_recovery_tester.cpp +++ b/nav2_system_tests/src/recoveries/wait/wait_recovery_tester.cpp @@ -172,6 +172,83 @@ bool WaitRecoveryTester::recoveryTest( return true; } +bool WaitRecoveryTester::recoveryTestCancel( + const float wait_time) +{ + if (!is_active_) { + RCLCPP_ERROR(node_->get_logger(), "Not activated"); + return false; + } + + // Sleep to let recovery server be ready for serving in multiple runs + std::this_thread::sleep_for(5s); + + auto start_time = node_->now(); + auto goal_msg = Wait::Goal(); + goal_msg.time = rclcpp::Duration(wait_time, 0.0); + + RCLCPP_INFO(this->node_->get_logger(), "Sending goal"); + + auto goal_handle_future = client_ptr_->async_send_goal(goal_msg); + + if (rclcpp::spin_until_future_complete(node_, goal_handle_future) != + rclcpp::FutureReturnCode::SUCCESS) + { + RCLCPP_ERROR(node_->get_logger(), "send goal call failed :("); + return false; + } + + rclcpp_action::ClientGoalHandle::SharedPtr goal_handle = goal_handle_future.get(); + if (!goal_handle) { + RCLCPP_ERROR(node_->get_logger(), "Goal was rejected by server"); + return false; + } + + // Wait for the server to be done with the goal + auto result_future = client_ptr_->async_cancel_all_goals(); + + RCLCPP_INFO(node_->get_logger(), "Waiting for cancellation"); + if (rclcpp::spin_until_future_complete(node_, result_future) != + rclcpp::FutureReturnCode::SUCCESS) + { + RCLCPP_ERROR(node_->get_logger(), "get cancel result call failed :("); + return false; + } + + auto status = goal_handle_future.get()->get_status(); + + switch (status) { + case rclcpp_action::GoalStatus::STATUS_SUCCEEDED: RCLCPP_ERROR( + node_->get_logger(), + "Goal succeeded"); + return false; + case rclcpp_action::GoalStatus::STATUS_ABORTED: RCLCPP_ERROR( + node_->get_logger(), + "Goal was aborted"); + return false; + case rclcpp_action::GoalStatus::STATUS_CANCELED: RCLCPP_INFO( + node_->get_logger(), + "Goal was canceled"); + return true; + case rclcpp_action::GoalStatus::STATUS_CANCELING: RCLCPP_INFO( + node_->get_logger(), + "Goal is cancelling"); + return true; + case rclcpp_action::GoalStatus::STATUS_EXECUTING: RCLCPP_ERROR( + node_->get_logger(), + "Goal is executing"); + return false; + case rclcpp_action::GoalStatus::STATUS_ACCEPTED: RCLCPP_ERROR( + node_->get_logger(), + "Goal is processing"); + return false; + default: RCLCPP_ERROR(node_->get_logger(), "Unknown result code"); + return false; + } + + return false; +} + void WaitRecoveryTester::sendInitialPose() { geometry_msgs::msg::PoseWithCovarianceStamped pose; diff --git a/nav2_system_tests/src/recoveries/wait/wait_recovery_tester.hpp b/nav2_system_tests/src/recoveries/wait/wait_recovery_tester.hpp index 5daef5025b6..4dfb90c6626 100644 --- a/nav2_system_tests/src/recoveries/wait/wait_recovery_tester.hpp +++ b/nav2_system_tests/src/recoveries/wait/wait_recovery_tester.hpp @@ -52,6 +52,8 @@ class WaitRecoveryTester bool recoveryTest( float time); + bool recoveryTestCancel(float time); + void activate(); void deactivate();