Adding failure to navigate test

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");
   }
 }
 

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

nav2_planner/src/planner_server.cpp

@@ -211,13 +211,8 @@ PlannerServer::computePlan()
   auto result = std::make_shared<nav2_msgs::action::ComputePathToPose::Result>();
 
   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;
     }
 

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)

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<nav_msgs::msg::OccupancyGrid>("map", 1);

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)

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<Wait>::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;

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

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

nav2_system_tests/src/system_failure/README.md

@@ -0,0 +1,3 @@
+# Navigation2 System Tests - Failure
+
+High level system failures tests

nav2_system_tests/src/system_failure/test_system_failure_launch.py

@@ -0,0 +1,106 @@
+#!/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.
+# 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())