Improve coverage for NavFn package

nav2_system_tests/src/system/CMakeLists.txt

@@ -15,6 +15,23 @@ ament_add_test(test_bt_navigator
     PLANNER=nav2_navfn_planner/NavfnPlanner
 )
 
+ament_add_test(test_bt_navigator_with_wrong_init_pose
+  GENERATE_RESULT_FOR_RETURN_CODE_ZERO
+  COMMAND "${CMAKE_CURRENT_SOURCE_DIR}/test_wrong_init_pose_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_to_pose_w_replanning_and_recovery.xml
+    TESTER=nav_to_pose_tester_node.py
+    ASTAR=True
+    CONTROLLER=nav2_regulated_pure_pursuit_controller::RegulatedPurePursuitController
+    PLANNER=nav2_navfn_planner/NavfnPlanner
+)
+
 ament_add_test(test_bt_navigator_with_dijkstra
   GENERATE_RESULT_FOR_RETURN_CODE_ZERO
   COMMAND "${CMAKE_CURRENT_SOURCE_DIR}/test_system_launch.py"

nav2_system_tests/src/system/nav_to_pose_tester_node.py

@@ -380,9 +380,19 @@ def get_testers(args):
     return testers
 
 
+def check_args(expect_failure: str):
+    # Check if --expect_failure is True or False
+    if expect_failure != 'True' and expect_failure != 'False':
+        print('\033[1;37;41m' + ' -e flag must be set to True or False only. ' + '\033[0m')
+        exit(1)
+    else:
+        return eval(expect_failure)
+
+
 def main(argv=sys.argv[1:]):
     # The robot(s) positions from the input arguments
     parser = argparse.ArgumentParser(description='System-level navigation tester node')
+    parser.add_argument('-e', '--expect_failure')
     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'),
@@ -394,6 +404,8 @@ def main(argv=sys.argv[1:]):
 
     args, unknown = parser.parse_known_args()
 
+    expect_failure = check_args(args.expect_failure)
+
     rclpy.init()
 
     # Create testers for each robot
@@ -404,7 +416,7 @@ def main(argv=sys.argv[1:]):
 
     for tester in testers:
         passed = run_all_tests(tester)
-        if not passed:
+        if passed != expect_failure:
             break
 
     for tester in testers:
@@ -413,7 +425,7 @@ def main(argv=sys.argv[1:]):
 
     testers[0].info_msg('Done Shutting Down.')
 
-    if not passed:
+    if passed != expect_failure:
         testers[0].info_msg('Exiting failed')
         exit(1)
     else:

nav2_system_tests/src/system/test_system_launch.py

@@ -108,7 +108,8 @@ def main(argv=sys.argv[1:]):
 
     test1_action = ExecuteProcess(
         cmd=[os.path.join(os.getenv('TEST_DIR'), os.getenv('TESTER')),
-             '-r', '-2.0', '-0.5', '0.0', '2.0'],
+             '-r', '-2.0', '-0.5', '0.0', '2.0',
+             '-e', 'True'],
         name='tester_node',
         output='screen')
 

nav2_system_tests/src/system/test_wrong_init_pose_launch.py

@@ -0,0 +1,124 @@
+#!/usr/bin/env python3
+
+# Copyright (c) 2018 Intel Corporation
+# Copyright (c) 2020 Florian Gramss
+#
+# 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_context import LaunchContext
+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 default parameter values for testing special features
+    # without having multiple params_files inside the nav2 stack
+    context = LaunchContext()
+    param_substitutions = {}
+
+    if (os.getenv('ASTAR') == 'True'):
+        param_substitutions.update({'use_astar': 'True'})
+
+    if (os.getenv('GROOT_MONITORING') == 'True'):
+        param_substitutions.update({'enable_groot_monitoring': 'True'})
+
+    param_substitutions.update(
+        {'planner_server.ros__parameters.GridBased.plugin': os.getenv('PLANNER')})
+    param_substitutions.update(
+        {'controller_server.ros__parameters.FollowPath.plugin': os.getenv('CONTROLLER')})
+
+    configured_params = RewrittenYaml(
+        source_file=params_file,
+        root_key='',
+        param_rewrites=param_substitutions,
+        convert_types=True)
+
+    new_yaml = configured_params.perform(context)
+
+    return LaunchDescription([
+        SetEnvironmentVariable('RCUTILS_LOGGING_BUFFERED_STREAM', '1'),
+        SetEnvironmentVariable('RCUTILS_LOGGING_USE_STDOUT', '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': new_yaml,
+                              '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'), os.getenv('TESTER')),
+             '-r', '-200000.0', '-200000.0', '0.0', '2.0',
+             '-e', 'False'],
+        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())