From 168600de69033bc026bef8f9b1a48ff84871aada Mon Sep 17 00:00:00 2001 From: Stevedan Ogochukwu Omodolor <61468301+stevedanomodolor@users.noreply.github.com> Date: Tue, 18 Jun 2024 19:55:58 +0200 Subject: [PATCH] Port Localization nav2_system_tests to new gazebo (#4431) * include localization Signed-off-by: stevedan * uncomment error code in cmakelist Signed-off-by: stevedan * formmatting Signed-off-by: stevedan * formmatting Signed-off-by: stevedan * formmatting python Signed-off-by: stevedan * formmatting python Signed-off-by: stevedan * removed unused path Signed-off-by: stevedan --------- Signed-off-by: stevedan --- nav2_system_tests/CMakeLists.txt | 2 +- .../src/localization/CMakeLists.txt | 3 -- .../localization/test_localization_launch.py | 51 +++++++++++++++---- 3 files changed, 43 insertions(+), 13 deletions(-) diff --git a/nav2_system_tests/CMakeLists.txt b/nav2_system_tests/CMakeLists.txt index 45a8097b35..d94298da77 100644 --- a/nav2_system_tests/CMakeLists.txt +++ b/nav2_system_tests/CMakeLists.txt @@ -114,7 +114,7 @@ if(BUILD_TESTING) add_subdirectory(src/behavior_tree) add_subdirectory(src/planning) # Uncomment after https://github.com/ros-navigation/navigation2/pull/3634 - # add_subdirectory(src/localization) + add_subdirectory(src/localization) # add_subdirectory(src/system) # add_subdirectory(src/system_failure) # add_subdirectory(src/updown) diff --git a/nav2_system_tests/src/localization/CMakeLists.txt b/nav2_system_tests/src/localization/CMakeLists.txt index 740d38f53e..7fa35172c3 100644 --- a/nav2_system_tests/src/localization/CMakeLists.txt +++ b/nav2_system_tests/src/localization/CMakeLists.txt @@ -11,8 +11,5 @@ ament_add_test(test_localization COMMAND "${CMAKE_CURRENT_SOURCE_DIR}/test_localization_launch.py" TIMEOUT 180 ENV - TEST_MAP=${PROJECT_SOURCE_DIR}/maps/map_circular.yaml TEST_EXECUTABLE=$ - TEST_WORLD=${PROJECT_SOURCE_DIR}/worlds/turtlebot3_ros2_demo.world - GAZEBO_MODEL_PATH=${PROJECT_SOURCE_DIR}/models ) diff --git a/nav2_system_tests/src/localization/test_localization_launch.py b/nav2_system_tests/src/localization/test_localization_launch.py index d638e02c4e..aedca4fcf7 100755 --- a/nav2_system_tests/src/localization/test_localization_launch.py +++ b/nav2_system_tests/src/localization/test_localization_launch.py @@ -17,23 +17,54 @@ import os import sys +from ament_index_python.packages import get_package_share_directory from launch import LaunchDescription from launch import LaunchService -import launch.actions -from launch.actions import ExecuteProcess +from launch.actions import AppendEnvironmentVariable, ExecuteProcess +from launch.actions import IncludeLaunchDescription +from launch.launch_description_sources import PythonLaunchDescriptionSource import launch_ros.actions from launch_testing.legacy import LaunchTestService def main(argv=sys.argv[1:]): - mapFile = os.getenv('TEST_MAP') testExecutable = os.getenv('TEST_EXECUTABLE') - world = os.getenv('TEST_WORLD') + sim_dir = get_package_share_directory('nav2_minimal_tb3_sim') + nav2_bringup_dir = get_package_share_directory('nav2_bringup') + ros_gz_sim_dir = get_package_share_directory('ros_gz_sim') - launch_gazebo = launch.actions.ExecuteProcess( - cmd=['gzserver', '-s', 'libgazebo_ros_init.so', '--minimal_comms', world], - output='screen', + world_sdf_xacro = os.path.join(sim_dir, 'worlds', 'tb3_sandbox.sdf.xacro') + robot_sdf = os.path.join(sim_dir, 'urdf', 'gz_waffle.sdf') + + map_yaml_file = os.path.join(nav2_bringup_dir, 'maps', 'tb3_sandbox.yaml') + + set_env_vars_resources = AppendEnvironmentVariable( + 'GZ_SIM_RESOURCE_PATH', os.path.join(sim_dir, 'models') + ) + + start_gazebo_server = IncludeLaunchDescription( + PythonLaunchDescriptionSource( + os.path.join(ros_gz_sim_dir, 'launch', 'gz_sim.launch.py') + ), + launch_arguments={'gz_args': ['-r -s ', world_sdf_xacro]}.items(), + ) + + spawn_robot = IncludeLaunchDescription( + PythonLaunchDescriptionSource( + os.path.join(sim_dir, 'launch', 'spawn_tb3.launch.py') + ), + launch_arguments={ + 'use_sim_time': 'True', + 'robot_sdf': robot_sdf, + 'x_pose': '-2.0', + 'y_pose': '-0.5', + 'z_pose': '0.01', + 'roll': '0.0', + 'pitch': '0.0', + 'yaw': '0.0', + }.items(), ) + link_footprint = launch_ros.actions.Node( package='tf2_ros', executable='static_transform_publisher', @@ -51,7 +82,7 @@ def main(argv=sys.argv[1:]): executable='map_server', name='map_server', output='screen', - parameters=[{'yaml_filename': mapFile}], + parameters=[{'yaml_filename': map_yaml_file}], ) run_amcl = launch_ros.actions.Node( package='nav2_amcl', executable='amcl', output='screen' @@ -65,7 +96,9 @@ def main(argv=sys.argv[1:]): ) ld = LaunchDescription( [ - launch_gazebo, + set_env_vars_resources, + start_gazebo_server, + spawn_robot, link_footprint, footprint_scan, run_map_server,