From dece3e01c2f3041a48acd7c7b2e520a92be2fdd1 Mon Sep 17 00:00:00 2001 From: Stevedan Ogochukwu Omodolor <61468301+stevedanomodolor@users.noreply.github.com> Date: Mon, 24 Jun 2024 19:43:31 +0200 Subject: [PATCH] port spin behavior to new gazebo (#4470) Signed-off-by: stevedan --- nav2_system_tests/CMakeLists.txt | 2 +- .../src/behaviors/spin/CMakeLists.txt | 4 - .../spin/test_spin_behavior_fake_launch.py | 22 +++--- .../spin/test_spin_behavior_launch.py | 74 +++++++++++++------ 4 files changed, 62 insertions(+), 40 deletions(-) diff --git a/nav2_system_tests/CMakeLists.txt b/nav2_system_tests/CMakeLists.txt index e49ea9e624..560417f8ca 100644 --- a/nav2_system_tests/CMakeLists.txt +++ b/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) diff --git a/nav2_system_tests/src/behaviors/spin/CMakeLists.txt b/nav2_system_tests/src/behaviors/spin/CMakeLists.txt index 84ee7246e5..e287b7a534 100644 --- a/nav2_system_tests/src/behaviors/spin/CMakeLists.txt +++ b/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=$ - 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=$ MAKE_FAKE_COSTMAP=true ) diff --git a/nav2_system_tests/src/behaviors/spin/test_spin_behavior_fake_launch.py b/nav2_system_tests/src/behaviors/spin/test_spin_behavior_fake_launch.py index 22b8327cd2..d18b29d5b9 100755 --- a/nav2_system_tests/src/behaviors/spin/test_spin_behavior_fake_launch.py +++ b/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,8 +117,6 @@ 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', @@ -121,16 +124,13 @@ def generate_launch_description(): 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', diff --git a/nav2_system_tests/src/behaviors/spin/test_spin_behavior_launch.py b/nav2_system_tests/src/behaviors/spin/test_spin_behavior_launch.py index 8edbcf8fe8..1a43845ccf 100755 --- a/nav2_system_tests/src/behaviors/spin/test_spin_behavior_launch.py +++ b/nav2_system_tests/src/behaviors/spin/test_spin_behavior_launch.py @@ -14,6 +14,7 @@ # limitations under the License. import os +from pathlib import Path import sys from ament_index_python.packages import get_package_share_directory @@ -21,6 +22,7 @@ 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__':