From 57fd2f5043bd2b8b03b4ad17f3282e654b8ff3a4 Mon Sep 17 00:00:00 2001 From: Stevedan Ogochukwu Omodolor <61468301+stevedanomodolor@users.noreply.github.com> Date: Mon, 24 Jun 2024 19:41:30 +0200 Subject: [PATCH] port to new gazebo (#4469) Signed-off-by: stevedan --- nav2_system_tests/CMakeLists.txt | 2 +- .../src/waypoint_follower/CMakeLists.txt | 3 - .../src/waypoint_follower/test_case_py.launch | 87 +++++++++++++------ 3 files changed, 62 insertions(+), 30 deletions(-) diff --git a/nav2_system_tests/CMakeLists.txt b/nav2_system_tests/CMakeLists.txt index 13576a6cdd7..a8dd47ac0d0 100644 --- a/nav2_system_tests/CMakeLists.txt +++ b/nav2_system_tests/CMakeLists.txt @@ -118,8 +118,8 @@ if(BUILD_TESTING) add_subdirectory(src/system) add_subdirectory(src/system_failure) add_subdirectory(src/updown) + add_subdirectory(src/waypoint_follower) # Uncomment after https://github.com/ros-navigation/navigation2/pull/3634 - # add_subdirectory(src/waypoint_follower) # add_subdirectory(src/gps_navigation) # add_subdirectory(src/behaviors/spin) # add_subdirectory(src/behaviors/wait) diff --git a/nav2_system_tests/src/waypoint_follower/CMakeLists.txt b/nav2_system_tests/src/waypoint_follower/CMakeLists.txt index 6ba85bc7bfb..a50ab4e13ff 100644 --- a/nav2_system_tests/src/waypoint_follower/CMakeLists.txt +++ b/nav2_system_tests/src/waypoint_follower/CMakeLists.txt @@ -5,8 +5,5 @@ ament_add_test(test_waypoint_follower 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 ) diff --git a/nav2_system_tests/src/waypoint_follower/test_case_py.launch b/nav2_system_tests/src/waypoint_follower/test_case_py.launch index 51ccbb451e8..8c7bed7a468 100755 --- a/nav2_system_tests/src/waypoint_follower/test_case_py.launch +++ b/nav2_system_tests/src/waypoint_follower/test_case_py.launch @@ -14,31 +14,48 @@ # 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 ExecuteProcess, IncludeLaunchDescription, SetEnvironmentVariable +from launch.actions import ( + AppendEnvironmentVariable, + 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 +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'), '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') + params_file = os.path.join(nav2_bringup_dir, 'params/nav2_params.yaml') # Replace the `use_astar` setting on the params file param_substitutions = { @@ -55,30 +72,46 @@ def generate_launch_description(): 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']), - + AppendEnvironmentVariable( + 'GZ_SIM_RESOURCE_PATH', os.path.join(sim_dir, 'models') + ), + 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( - os.path.join(bringup_dir, 'launch', 'bringup_launch.py')), + os.path.join(nav2_bringup_dir, 'launch', 'bringup_launch.py')), launch_arguments={ 'map': map_yaml_file, 'use_sim_time': 'True', @@ -101,7 +134,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__':