diff --git a/nav2_system_tests/src/system/CMakeLists.txt b/nav2_system_tests/src/system/CMakeLists.txt index 8c637cf864..df3f6fe861 100644 --- a/nav2_system_tests/src/system/CMakeLists.txt +++ b/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" diff --git a/nav2_system_tests/src/system/nav_to_pose_tester_node.py b/nav2_system_tests/src/system/nav_to_pose_tester_node.py index 176f2e4490..902b547567 100755 --- a/nav2_system_tests/src/system/nav_to_pose_tester_node.py +++ b/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: diff --git a/nav2_system_tests/src/system/test_system_launch.py b/nav2_system_tests/src/system/test_system_launch.py index dd0f97f9dd..84a295a1fb 100755 --- a/nav2_system_tests/src/system/test_system_launch.py +++ b/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') diff --git a/nav2_system_tests/src/system/test_wrong_init_pose_launch.py b/nav2_system_tests/src/system/test_wrong_init_pose_launch.py new file mode 100755 index 0000000000..b45031cec7 --- /dev/null +++ b/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())