Port Localization nav2_system_tests to new gazebo (ros-navigation#4431)

* include localization Signed-off-by: stevedan <[email protected]> * uncomment error code in cmakelist Signed-off-by: stevedan <[email protected]> * formmatting Signed-off-by: stevedan <[email protected]> * formmatting Signed-off-by: stevedan <[email protected]> * formmatting python Signed-off-by: stevedan <[email protected]> * formmatting python Signed-off-by: stevedan <[email protected]> * removed unused path Signed-off-by: stevedan <[email protected]> --------- Signed-off-by: stevedan <[email protected]>
Manos-G · Aug 1, 2024 · 168600d · 168600d
1 parent f03ca10
commit 168600d
Show file tree

Hide file tree

Showing 3 changed files with 43 additions and 13 deletions.
diff --git 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
@@ -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=$<TARGET_FILE:test_localization_node>
-    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
@@ -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,