From 8b84f4ec78c24dafc6fc98e7b3ce1eaf13634d25 Mon Sep 17 00:00:00 2001 From: Stevedan Ogochukwu Omodolor <61468301+stevedanomodolor@users.noreply.github.com> Date: Thu, 4 Jul 2024 00:17:48 +0200 Subject: [PATCH] port wait behavior to new gazebo (#4471) Signed-off-by: stevedan <stevedan.o.omodolor@gmail.com> Signed-off-by: Steve Macenski <stevenmacenski@gmail.com> Co-authored-by: Steve Macenski <stevenmacenski@gmail.com> --- nav2_system_tests/CMakeLists.txt | 3 +- .../src/behaviors/wait/CMakeLists.txt | 3 - .../wait/test_wait_behavior_launch.py | 73 +++++++++++++------ 3 files changed, 51 insertions(+), 28 deletions(-) diff --git a/nav2_system_tests/CMakeLists.txt b/nav2_system_tests/CMakeLists.txt index 1b212e6eca..9e67c0a48b 100644 --- a/nav2_system_tests/CMakeLists.txt +++ b/nav2_system_tests/CMakeLists.txt @@ -117,10 +117,9 @@ if(BUILD_TESTING) 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/gps_navigation) + add_subdirectory(src/behaviors/wait) add_subdirectory(src/behaviors/spin) - # add_subdirectory(src/behaviors/wait) # add_subdirectory(src/behaviors/backup) # add_subdirectory(src/behaviors/drive_on_heading) add_subdirectory(src/behaviors/assisted_teleop) diff --git a/nav2_system_tests/src/behaviors/wait/CMakeLists.txt b/nav2_system_tests/src/behaviors/wait/CMakeLists.txt index d6fb45401d..36bf50dec8 100644 --- a/nav2_system_tests/src/behaviors/wait/CMakeLists.txt +++ b/nav2_system_tests/src/behaviors/wait/CMakeLists.txt @@ -15,9 +15,6 @@ ament_add_test(test_wait_behavior WORKING_DIRECTORY "${CMAKE_CURRENT_BINARY_DIR}" TIMEOUT 180 ENV - TEST_MAP=${PROJECT_SOURCE_DIR}/maps/map_circular.yaml TEST_EXECUTABLE=$<TARGET_FILE:${test_wait_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_behavior.xml ) diff --git a/nav2_system_tests/src/behaviors/wait/test_wait_behavior_launch.py b/nav2_system_tests/src/behaviors/wait/test_wait_behavior_launch.py index 80f1529820..b13024b2d8 100755 --- a/nav2_system_tests/src/behaviors/wait/test_wait_behavior_launch.py +++ b/nav2_system_tests/src/behaviors/wait/test_wait_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'), @@ -54,30 +67,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', - '--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( @@ -109,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__':