Adding failure to navigate test (#1912)

* failures tests

* adding copyrights

* cancel test in recoveries

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 @@ public:
   bool recoveryTest(
     float time);
 
+  bool recoveryTestCancel(float time);
+
   void activate();
 
   void deactivate();

nav2_system_tests/src/system_failure/CMakeLists.txt

+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

+# Navigation2 System Tests - Failure
+
+High level system failures tests

nav2_system_tests/src/system_failure/test_system_failure_launch.py

+#!/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())

nav2_system_tests/src/system_failure/tester_node.py

+#! /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.
+# 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()