port spin behavior to new gazebo (#4470)

Signed-off-by: stevedan <[email protected]>

nav2_system_tests/CMakeLists.txt

@@ -121,7 +121,7 @@ if(BUILD_TESTING)
   add_subdirectory(src/waypoint_follower)
   # Uncomment after https://github.com/ros-navigation/navigation2/pull/3634
   # add_subdirectory(src/gps_navigation)
-  # add_subdirectory(src/behaviors/spin)
+  add_subdirectory(src/behaviors/spin)
   # add_subdirectory(src/behaviors/wait)
   # add_subdirectory(src/behaviors/backup)
   # add_subdirectory(src/behaviors/drive_on_heading)

nav2_system_tests/src/behaviors/spin/CMakeLists.txt

@@ -15,10 +15,7 @@ ament_add_test(test_spin_behavior
   WORKING_DIRECTORY "${CMAKE_CURRENT_BINARY_DIR}"
   TIMEOUT 180
   ENV
-    TEST_MAP=${PROJECT_SOURCE_DIR}/maps/map_circular.yaml
     TEST_EXECUTABLE=$<TARGET_FILE:${test_spin_behavior_exec}>
-    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
 )
 
@@ -28,7 +25,6 @@ ament_add_test(test_spin_behavior_fake
   WORKING_DIRECTORY "${CMAKE_CURRENT_BINARY_DIR}"
   TIMEOUT 180
   ENV
-    TEST_MAP=${PROJECT_SOURCE_DIR}/maps/map_circular.yaml
     TEST_EXECUTABLE=$<TARGET_FILE:${test_spin_behavior_exec}>
     MAKE_FAKE_COSTMAP=true
 )

nav2_system_tests/src/behaviors/spin/test_spin_behavior_fake_launch.py

@@ -30,6 +30,11 @@
 def generate_launch_description():
 
     bringup_dir = get_package_share_directory('nav2_bringup')
+    sim_dir = get_package_share_directory('nav2_minimal_tb3_sim')
+
+    urdf = os.path.join(sim_dir, 'urdf', 'turtlebot3_waffle.urdf')
+    with open(urdf, 'r') as infp:
+        robot_description = infp.read()
 
     namespace = LaunchConfiguration('namespace')
     use_sim_time = LaunchConfiguration('use_sim_time')
@@ -112,25 +117,20 @@ def generate_launch_description():
                 default_value='false',
                 description='Whether to set the map subscriber QoS to transient local',
             ),
-            # 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', 'map', 'odom'],
             ),
             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',
+                package='robot_state_publisher',
+                executable='robot_state_publisher',
+                name='robot_state_publisher',
                 output='screen',
-                arguments=['0', '0', '0', '0', '0', '0', 'base_link', 'base_scan'],
+                parameters=[
+                    {'use_sim_time': True, 'robot_description': robot_description}
+                ],
             ),
             Node(
                 package='nav2_behaviors',

nav2_system_tests/src/behaviors/spin/test_spin_behavior_launch.py

@@ -14,13 +14,15 @@
 # limitations under the License.
 
 import os
+from pathlib import Path
 import sys
 
 from ament_index_python.packages import get_package_share_directory
 
 from launch import LaunchDescription
 from launch import LaunchService
 from launch.actions import (
+    AppendEnvironmentVariable,
     ExecuteProcess,
     IncludeLaunchDescription,
     SetEnvironmentVariable,
@@ -30,11 +32,22 @@
 from launch_testing.legacy import LaunchTestService
 
 from nav2_common.launch import RewrittenYaml
+from nav2_simple_commander.utils import kill_os_processes
 
 
 def generate_launch_description():
-    map_yaml_file = os.getenv('TEST_MAP')
-    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')
+
+    world_sdf_xacro = os.path.join(sim_dir, 'worlds', 'tb3_sandbox.sdf.xacro')
+    robot_sdf = os.path.join(sim_dir, 'urdf', 'gz_waffle.sdf.xacro')
+
+    urdf = os.path.join(sim_dir, 'urdf', 'turtlebot3_waffle.urdf')
+    with open(urdf, 'r') as infp:
+        robot_description = infp.read()
+
+    map_yaml_file = os.path.join(nav2_bringup_dir, 'maps', 'tb3_sandbox.yaml')
 
     bt_navigator_xml = os.path.join(
         get_package_share_directory('nav2_bt_navigator'),
@@ -55,31 +68,42 @@ def generate_launch_description():
             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',
-                    '-s',
-                    'libgazebo_ros_factory.so',
-                    '--minimal_comms',
-                    world,
-                ],
-                output='screen',
+            AppendEnvironmentVariable(
+                'GZ_SIM_RESOURCE_PATH', os.path.join(sim_dir, 'models')
             ),
-            # 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'],
+            AppendEnvironmentVariable(
+                'GZ_SIM_RESOURCE_PATH',
+                str(Path(os.path.join(sim_dir)).parent.resolve())
+            ),
+            IncludeLaunchDescription(
+                PythonLaunchDescriptionSource(
+                    os.path.join(ros_gz_sim_dir, 'launch', 'gz_sim.launch.py')
+                ),
+                launch_arguments={'gz_args': ['-r -s ', world_sdf_xacro]}.items(),
+            ),
+            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(),
             ),
             Node(
-                package='tf2_ros',
-                executable='static_transform_publisher',
+                package='robot_state_publisher',
+                executable='robot_state_publisher',
+                name='robot_state_publisher',
                 output='screen',
-                arguments=['0', '0', '0', '0', '0', '0', 'base_link', 'base_scan'],
+                parameters=[
+                    {'use_sim_time': True, 'robot_description': robot_description}
+                ],
             ),
             IncludeLaunchDescription(
                 PythonLaunchDescriptionSource(
@@ -111,7 +135,9 @@ def main(argv=sys.argv[1:]):
     lts.add_test_action(ld, test1_action)
     ls = LaunchService(argv=argv)
     ls.include_launch_description(ld)
-    return lts.run(ls)
+    return_code = lts.run(ls)
+    kill_os_processes('gz sim')
+    return return_code
 
 
 if __name__ == '__main__':