From a6a91aec6f123ace6d964d0c95b3e89fbd9002ba Mon Sep 17 00:00:00 2001 From: stevedan Date: Sun, 16 Jun 2024 07:41:40 +0200 Subject: [PATCH 01/19] wip, ported only test_bt_navigator Signed-off-by: stevedan --- nav2_system_tests/CMakeLists.txt | 3 +- nav2_system_tests/package.xml | 2 + nav2_system_tests/src/system/CMakeLists.txt | 160 +++++++++--------- .../src/system/nav2_system_params.yaml | 45 +++++ .../src/system/test_system_launch.py | 67 ++++++-- 5 files changed, 181 insertions(+), 96 deletions(-) diff --git a/nav2_system_tests/CMakeLists.txt b/nav2_system_tests/CMakeLists.txt index 45a8097b355..faac608f03a 100644 --- a/nav2_system_tests/CMakeLists.txt +++ b/nav2_system_tests/CMakeLists.txt @@ -28,6 +28,7 @@ find_package(navigation2) find_package(angles REQUIRED) find_package(behaviortree_cpp REQUIRED) find_package(pluginlib REQUIRED) +find_package(nav2_minimal_tb3_sim REQUIRED) nav2_package() @@ -115,7 +116,7 @@ if(BUILD_TESTING) add_subdirectory(src/planning) # Uncomment after https://github.com/ros-navigation/navigation2/pull/3634 # add_subdirectory(src/localization) - # add_subdirectory(src/system) + add_subdirectory(src/system) # add_subdirectory(src/system_failure) # add_subdirectory(src/updown) # add_subdirectory(src/waypoint_follower) diff --git a/nav2_system_tests/package.xml b/nav2_system_tests/package.xml index 5fdbc230b73..2f3f45d1911 100644 --- a/nav2_system_tests/package.xml +++ b/nav2_system_tests/package.xml @@ -30,6 +30,7 @@ launch_ros launch_testing nav2_planner + nav2_minimal_tb3_sim launch_ros launch_testing @@ -48,6 +49,7 @@ nav2_amcl std_msgs tf2_geometry_msgs + nav2_minimal_tb3_sim navigation2 lcov diff --git a/nav2_system_tests/src/system/CMakeLists.txt b/nav2_system_tests/src/system/CMakeLists.txt index 39afa461c2f..b57f8686e83 100644 --- a/nav2_system_tests/src/system/CMakeLists.txt +++ b/nav2_system_tests/src/system/CMakeLists.txt @@ -15,90 +15,90 @@ ament_add_test(test_bt_navigator PLANNER=nav2_navfn_planner::NavfnPlanner ) -ament_add_test(test_bt_navigator_with_wrong_init_pose - GENERATE_RESULT_FOR_RETURN_CODE_ZERO - COMMAND "${CMAKE_CURRENT_SOURCE_DIR}/test_wrong_init_pose_launch.py" - WORKING_DIRECTORY "${CMAKE_CURRENT_BINARY_DIR}" - 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 - TESTER=nav_to_pose_tester_node.py - ASTAR=True - CONTROLLER=nav2_regulated_pure_pursuit_controller::RegulatedPurePursuitController - PLANNER=nav2_navfn_planner::NavfnPlanner -) +# ament_add_test(test_bt_navigator_with_wrong_init_pose +# GENERATE_RESULT_FOR_RETURN_CODE_ZERO +# COMMAND "${CMAKE_CURRENT_SOURCE_DIR}/test_wrong_init_pose_launch.py" +# WORKING_DIRECTORY "${CMAKE_CURRENT_BINARY_DIR}" +# 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 +# TESTER=nav_to_pose_tester_node.py +# ASTAR=True +# CONTROLLER=nav2_regulated_pure_pursuit_controller::RegulatedPurePursuitController +# PLANNER=nav2_navfn_planner::NavfnPlanner +# ) -ament_add_test(test_bt_navigator_with_dijkstra - GENERATE_RESULT_FOR_RETURN_CODE_ZERO - COMMAND "${CMAKE_CURRENT_SOURCE_DIR}/test_system_launch.py" - WORKING_DIRECTORY "${CMAKE_CURRENT_BINARY_DIR}" - 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 - TESTER=nav_to_pose_tester_node.py - ASTAR=False - CONTROLLER=dwb_core::DWBLocalPlanner - PLANNER=nav2_navfn_planner::NavfnPlanner -) +# ament_add_test(test_bt_navigator_with_dijkstra +# GENERATE_RESULT_FOR_RETURN_CODE_ZERO +# COMMAND "${CMAKE_CURRENT_SOURCE_DIR}/test_system_launch.py" +# WORKING_DIRECTORY "${CMAKE_CURRENT_BINARY_DIR}" +# 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 +# TESTER=nav_to_pose_tester_node.py +# ASTAR=False +# CONTROLLER=dwb_core::DWBLocalPlanner +# PLANNER=nav2_navfn_planner::NavfnPlanner +# ) -ament_add_test(test_bt_navigator_2 - GENERATE_RESULT_FOR_RETURN_CODE_ZERO - COMMAND "${CMAKE_CURRENT_SOURCE_DIR}/test_system_launch.py" - WORKING_DIRECTORY "${CMAKE_CURRENT_BINARY_DIR}" - 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 - TESTER=nav_to_pose_tester_node.py - ASTAR=False - CONTROLLER=dwb_core::DWBLocalPlanner - PLANNER=nav2_navfn_planner::NavfnPlanner -) +# ament_add_test(test_bt_navigator_2 +# GENERATE_RESULT_FOR_RETURN_CODE_ZERO +# COMMAND "${CMAKE_CURRENT_SOURCE_DIR}/test_system_launch.py" +# WORKING_DIRECTORY "${CMAKE_CURRENT_BINARY_DIR}" +# 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 +# TESTER=nav_to_pose_tester_node.py +# ASTAR=False +# CONTROLLER=dwb_core::DWBLocalPlanner +# PLANNER=nav2_navfn_planner::NavfnPlanner +# ) -ament_add_test(test_dynamic_obstacle - GENERATE_RESULT_FOR_RETURN_CODE_ZERO - COMMAND "${CMAKE_CURRENT_SOURCE_DIR}/test_system_launch.py" - WORKING_DIRECTORY "${CMAKE_CURRENT_BINARY_DIR}" - 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_obstacle.world - GAZEBO_MODEL_PATH=${PROJECT_SOURCE_DIR}/models - BT_NAVIGATOR_XML=navigate_to_pose_w_replanning_and_recovery.xml - TESTER=nav_to_pose_tester_node.py - ASTAR=False - CONTROLLER=dwb_core::DWBLocalPlanner - PLANNER=nav2_navfn_planner::NavfnPlanner -) +# ament_add_test(test_dynamic_obstacle +# GENERATE_RESULT_FOR_RETURN_CODE_ZERO +# COMMAND "${CMAKE_CURRENT_SOURCE_DIR}/test_system_launch.py" +# WORKING_DIRECTORY "${CMAKE_CURRENT_BINARY_DIR}" +# 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_obstacle.world +# GAZEBO_MODEL_PATH=${PROJECT_SOURCE_DIR}/models +# BT_NAVIGATOR_XML=navigate_to_pose_w_replanning_and_recovery.xml +# TESTER=nav_to_pose_tester_node.py +# ASTAR=False +# CONTROLLER=dwb_core::DWBLocalPlanner +# PLANNER=nav2_navfn_planner::NavfnPlanner +# ) -ament_add_test(test_nav_through_poses - GENERATE_RESULT_FOR_RETURN_CODE_ZERO - COMMAND "${CMAKE_CURRENT_SOURCE_DIR}/test_system_launch.py" - WORKING_DIRECTORY "${CMAKE_CURRENT_BINARY_DIR}" - 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_obstacle.world - GAZEBO_MODEL_PATH=${PROJECT_SOURCE_DIR}/models - BT_NAVIGATOR_XML=navigate_through_poses_w_replanning_and_recovery.xml - TESTER=nav_through_poses_tester_node.py - ASTAR=False - CONTROLLER=dwb_core::DWBLocalPlanner - PLANNER=nav2_navfn_planner::NavfnPlanner -) +# ament_add_test(test_nav_through_poses +# GENERATE_RESULT_FOR_RETURN_CODE_ZERO +# COMMAND "${CMAKE_CURRENT_SOURCE_DIR}/test_system_launch.py" +# WORKING_DIRECTORY "${CMAKE_CURRENT_BINARY_DIR}" +# 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_obstacle.world +# GAZEBO_MODEL_PATH=${PROJECT_SOURCE_DIR}/models +# BT_NAVIGATOR_XML=navigate_through_poses_w_replanning_and_recovery.xml +# TESTER=nav_through_poses_tester_node.py +# ASTAR=False +# CONTROLLER=dwb_core::DWBLocalPlanner +# PLANNER=nav2_navfn_planner::NavfnPlanner +# ) # ament_add_test(test_multi_robot # GENERATE_RESULT_FOR_RETURN_CODE_ZERO diff --git a/nav2_system_tests/src/system/nav2_system_params.yaml b/nav2_system_tests/src/system/nav2_system_params.yaml index a98d340f212..07f2d08f641 100644 --- a/nav2_system_tests/src/system/nav2_system_params.yaml +++ b/nav2_system_tests/src/system/nav2_system_params.yaml @@ -317,3 +317,48 @@ collision_monitor: min_height: 0.15 max_height: 2.0 enabled: True + +docking_server: + ros__parameters: + controller_frequency: 50.0 + initial_perception_timeout: 5.0 + wait_charge_timeout: 5.0 + dock_approach_timeout: 30.0 + undock_linear_tolerance: 0.05 + undock_angular_tolerance: 0.1 + max_retries: 3 + base_frame: "base_link" + fixed_frame: "odom" + dock_backwards: false + dock_prestaging_tolerance: 0.5 + + # Types of docks + dock_plugins: ['simple_charging_dock'] + simple_charging_dock: + plugin: 'opennav_docking::SimpleChargingDock' + docking_threshold: 0.05 + staging_x_offset: -0.7 + use_external_detection_pose: true + use_battery_status: false # true + use_stall_detection: false # true + + external_detection_timeout: 1.0 + external_detection_translation_x: -0.18 + external_detection_translation_y: 0.0 + external_detection_rotation_roll: -1.57 + external_detection_rotation_pitch: -1.57 + external_detection_rotation_yaw: 0.0 + filter_coef: 0.1 + + # Dock instances + docks: ['home_dock'] # Input your docks here + home_dock: + type: 'simple_charging_dock' + frame: map + pose: [0.0, 0.0, 0.0] + + controller: + k_phi: 3.0 + k_delta: 2.0 + v_linear_min: 0.15 + v_linear_max: 0.15 \ No newline at end of file diff --git a/nav2_system_tests/src/system/test_system_launch.py b/nav2_system_tests/src/system/test_system_launch.py index 2f0546d1568..215bad71c84 100755 --- a/nav2_system_tests/src/system/test_system_launch.py +++ b/nav2_system_tests/src/system/test_system_launch.py @@ -17,12 +17,13 @@ import os import sys - +from pathlib import Path 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, @@ -36,8 +37,16 @@ def generate_launch_description(): - map_yaml_file = os.getenv('TEST_MAP') - world = os.getenv('TEST_WORLD') + # 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') + + 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'), @@ -77,19 +86,47 @@ 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', + # SetEnvironmentVariable('RCUTILS_LOGGING_BUFFERED_STREAM', '1'), + # SetEnvironmentVariable('RCUTILS_LOGGING_USE_STDOUT', '1'), + 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(), ), + # # 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( From 73049cec214064e6f2d88c5f751c399d5f49d612 Mon Sep 17 00:00:00 2001 From: stevedan Date: Sun, 16 Jun 2024 22:01:37 +0200 Subject: [PATCH 02/19] include test_bt_navigator_with dijlstra and test_bt_navigator_2 Signed-off-by: stevedan --- nav2_system_tests/src/system/CMakeLists.txt | 58 +++++++++---------- .../src/system/test_system_launch.py | 11 ---- 2 files changed, 26 insertions(+), 43 deletions(-) diff --git a/nav2_system_tests/src/system/CMakeLists.txt b/nav2_system_tests/src/system/CMakeLists.txt index b57f8686e83..36626ebe982 100644 --- a/nav2_system_tests/src/system/CMakeLists.txt +++ b/nav2_system_tests/src/system/CMakeLists.txt @@ -32,39 +32,33 @@ ament_add_test(test_bt_navigator # PLANNER=nav2_navfn_planner::NavfnPlanner # ) -# ament_add_test(test_bt_navigator_with_dijkstra -# GENERATE_RESULT_FOR_RETURN_CODE_ZERO -# COMMAND "${CMAKE_CURRENT_SOURCE_DIR}/test_system_launch.py" -# WORKING_DIRECTORY "${CMAKE_CURRENT_BINARY_DIR}" -# 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 -# TESTER=nav_to_pose_tester_node.py -# ASTAR=False -# CONTROLLER=dwb_core::DWBLocalPlanner -# PLANNER=nav2_navfn_planner::NavfnPlanner -# ) +ament_add_test(test_bt_navigator_with_dijkstra + GENERATE_RESULT_FOR_RETURN_CODE_ZERO + COMMAND "${CMAKE_CURRENT_SOURCE_DIR}/test_system_launch.py" + WORKING_DIRECTORY "${CMAKE_CURRENT_BINARY_DIR}" + TIMEOUT 180 + ENV + TEST_DIR=${CMAKE_CURRENT_SOURCE_DIR} + BT_NAVIGATOR_XML=navigate_to_pose_w_replanning_and_recovery.xml + TESTER=nav_to_pose_tester_node.py + ASTAR=False + CONTROLLER=dwb_core::DWBLocalPlanner + PLANNER=nav2_navfn_planner::NavfnPlanner +) -# ament_add_test(test_bt_navigator_2 -# GENERATE_RESULT_FOR_RETURN_CODE_ZERO -# COMMAND "${CMAKE_CURRENT_SOURCE_DIR}/test_system_launch.py" -# WORKING_DIRECTORY "${CMAKE_CURRENT_BINARY_DIR}" -# 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 -# TESTER=nav_to_pose_tester_node.py -# ASTAR=False -# CONTROLLER=dwb_core::DWBLocalPlanner -# PLANNER=nav2_navfn_planner::NavfnPlanner -# ) +ament_add_test(test_bt_navigator_2 + GENERATE_RESULT_FOR_RETURN_CODE_ZERO + COMMAND "${CMAKE_CURRENT_SOURCE_DIR}/test_system_launch.py" + WORKING_DIRECTORY "${CMAKE_CURRENT_BINARY_DIR}" + TIMEOUT 180 + ENV + TEST_DIR=${CMAKE_CURRENT_SOURCE_DIR} + BT_NAVIGATOR_XML=navigate_to_pose_w_replanning_and_recovery.xml + TESTER=nav_to_pose_tester_node.py + ASTAR=False + CONTROLLER=dwb_core::DWBLocalPlanner + PLANNER=nav2_navfn_planner::NavfnPlanner +) # ament_add_test(test_dynamic_obstacle # GENERATE_RESULT_FOR_RETURN_CODE_ZERO diff --git a/nav2_system_tests/src/system/test_system_launch.py b/nav2_system_tests/src/system/test_system_launch.py index 215bad71c84..2d917ec9c62 100755 --- a/nav2_system_tests/src/system/test_system_launch.py +++ b/nav2_system_tests/src/system/test_system_launch.py @@ -116,17 +116,6 @@ def generate_launch_description(): 'yaw': '0.0', }.items(), ), - # # 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( From 751aa616c2f3de85908a06193dbc4f68317f1b2e Mon Sep 17 00:00:00 2001 From: stevedan Date: Sun, 16 Jun 2024 22:06:46 +0200 Subject: [PATCH 03/19] uncomment some lines Signed-off-by: stevedan --- nav2_system_tests/src/system/test_system_launch.py | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/nav2_system_tests/src/system/test_system_launch.py b/nav2_system_tests/src/system/test_system_launch.py index 2d917ec9c62..e0791cdf991 100755 --- a/nav2_system_tests/src/system/test_system_launch.py +++ b/nav2_system_tests/src/system/test_system_launch.py @@ -86,8 +86,8 @@ def generate_launch_description(): return LaunchDescription( [ - # SetEnvironmentVariable('RCUTILS_LOGGING_BUFFERED_STREAM', '1'), - # SetEnvironmentVariable('RCUTILS_LOGGING_USE_STDOUT', '1'), + SetEnvironmentVariable('RCUTILS_LOGGING_BUFFERED_STREAM', '1'), + SetEnvironmentVariable('RCUTILS_LOGGING_USE_STDOUT', '1'), AppendEnvironmentVariable( 'GZ_SIM_RESOURCE_PATH', os.path.join(sim_dir, 'models') ), From 2b8833c52a7d8bce8f75e9a6f9390ef8cb5b9740 Mon Sep 17 00:00:00 2001 From: stevedan Date: Wed, 19 Jun 2024 07:58:41 +0200 Subject: [PATCH 04/19] More tests Signed-off-by: stevedan --- nav2_system_tests/src/system/CMakeLists.txt | 89 ++++--- .../src/system/nav_to_pose_tester_node.py | 15 +- .../src/system/test_system_launch.py | 43 +++- .../test_system_with_obstacle_launch.py | 227 ++++++++++++++++++ .../src/system/test_wrong_init_pose_launch.py | 95 ++++++-- 5 files changed, 400 insertions(+), 69 deletions(-) create mode 100755 nav2_system_tests/src/system/test_system_with_obstacle_launch.py diff --git a/nav2_system_tests/src/system/CMakeLists.txt b/nav2_system_tests/src/system/CMakeLists.txt index 36626ebe982..fe724ea04f0 100644 --- a/nav2_system_tests/src/system/CMakeLists.txt +++ b/nav2_system_tests/src/system/CMakeLists.txt @@ -15,22 +15,19 @@ ament_add_test(test_bt_navigator PLANNER=nav2_navfn_planner::NavfnPlanner ) -# ament_add_test(test_bt_navigator_with_wrong_init_pose -# GENERATE_RESULT_FOR_RETURN_CODE_ZERO -# COMMAND "${CMAKE_CURRENT_SOURCE_DIR}/test_wrong_init_pose_launch.py" -# WORKING_DIRECTORY "${CMAKE_CURRENT_BINARY_DIR}" -# 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 -# TESTER=nav_to_pose_tester_node.py -# ASTAR=True -# CONTROLLER=nav2_regulated_pure_pursuit_controller::RegulatedPurePursuitController -# PLANNER=nav2_navfn_planner::NavfnPlanner -# ) +ament_add_test(test_bt_navigator_with_wrong_init_pose + GENERATE_RESULT_FOR_RETURN_CODE_ZERO + COMMAND "${CMAKE_CURRENT_SOURCE_DIR}/test_wrong_init_pose_launch.py" + WORKING_DIRECTORY "${CMAKE_CURRENT_BINARY_DIR}" + TIMEOUT 180 + ENV + TEST_DIR=${CMAKE_CURRENT_SOURCE_DIR} + BT_NAVIGATOR_XML=navigate_to_pose_w_replanning_and_recovery.xml + TESTER=nav_to_pose_tester_node.py + ASTAR=True + CONTROLLER=nav2_regulated_pure_pursuit_controller::RegulatedPurePursuitController + PLANNER=nav2_navfn_planner::NavfnPlanner +) ament_add_test(test_bt_navigator_with_dijkstra GENERATE_RESULT_FOR_RETURN_CODE_ZERO @@ -60,39 +57,35 @@ ament_add_test(test_bt_navigator_2 PLANNER=nav2_navfn_planner::NavfnPlanner ) -# ament_add_test(test_dynamic_obstacle -# GENERATE_RESULT_FOR_RETURN_CODE_ZERO -# COMMAND "${CMAKE_CURRENT_SOURCE_DIR}/test_system_launch.py" -# WORKING_DIRECTORY "${CMAKE_CURRENT_BINARY_DIR}" -# 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_obstacle.world -# GAZEBO_MODEL_PATH=${PROJECT_SOURCE_DIR}/models -# BT_NAVIGATOR_XML=navigate_to_pose_w_replanning_and_recovery.xml -# TESTER=nav_to_pose_tester_node.py -# ASTAR=False -# CONTROLLER=dwb_core::DWBLocalPlanner -# PLANNER=nav2_navfn_planner::NavfnPlanner -# ) +ament_add_test(test_dynamic_obstacle + GENERATE_RESULT_FOR_RETURN_CODE_ZERO + COMMAND "${CMAKE_CURRENT_SOURCE_DIR}/test_system_with_obstacle_launch.py" + WORKING_DIRECTORY "${CMAKE_CURRENT_BINARY_DIR}" + TIMEOUT 180 + ENV + TEST_DIR=${CMAKE_CURRENT_SOURCE_DIR} + ADD_OBSTACLE=True + BT_NAVIGATOR_XML=navigate_to_pose_w_replanning_and_recovery.xml + TESTER=nav_to_pose_tester_node.py + ASTAR=False + CONTROLLER=dwb_core::DWBLocalPlanner + PLANNER=nav2_navfn_planner::NavfnPlanner +) -# ament_add_test(test_nav_through_poses -# GENERATE_RESULT_FOR_RETURN_CODE_ZERO -# COMMAND "${CMAKE_CURRENT_SOURCE_DIR}/test_system_launch.py" -# WORKING_DIRECTORY "${CMAKE_CURRENT_BINARY_DIR}" -# 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_obstacle.world -# GAZEBO_MODEL_PATH=${PROJECT_SOURCE_DIR}/models -# BT_NAVIGATOR_XML=navigate_through_poses_w_replanning_and_recovery.xml -# TESTER=nav_through_poses_tester_node.py -# ASTAR=False -# CONTROLLER=dwb_core::DWBLocalPlanner -# PLANNER=nav2_navfn_planner::NavfnPlanner -# ) +ament_add_test(test_nav_through_poses + GENERATE_RESULT_FOR_RETURN_CODE_ZERO + COMMAND "${CMAKE_CURRENT_SOURCE_DIR}/test_system_with_obstacle_launch.py" + WORKING_DIRECTORY "${CMAKE_CURRENT_BINARY_DIR}" + TIMEOUT 180 + ENV + TEST_DIR=${CMAKE_CURRENT_SOURCE_DIR} + ADD_OBSTACLE=True + BT_NAVIGATOR_XML=navigate_through_poses_w_replanning_and_recovery.xml + TESTER=nav_through_poses_tester_node.py + ASTAR=False + CONTROLLER=dwb_core::DWBLocalPlanner + PLANNER=nav2_navfn_planner::NavfnPlanner +) # ament_add_test(test_multi_robot # GENERATE_RESULT_FOR_RETURN_CODE_ZERO diff --git a/nav2_system_tests/src/system/nav_to_pose_tester_node.py b/nav2_system_tests/src/system/nav_to_pose_tester_node.py index 8fd1bc60342..a96ea4da96f 100755 --- a/nav2_system_tests/src/system/nav_to_pose_tester_node.py +++ b/nav2_system_tests/src/system/nav_to_pose_tester_node.py @@ -59,6 +59,7 @@ def __init__(self, initial_pose: Pose, goal_pose: Pose, namespace: str = ''): self.initial_pose_received = False self.initial_pose = initial_pose self.goal_pose = goal_pose + self.set_initial_pose_timeout = 100 self.action_client = ActionClient(self, NavigateToPose, 'navigate_to_pose') def info_msg(self, msg: str): @@ -212,11 +213,22 @@ def shutdown(self): def wait_for_initial_pose(self): self.initial_pose_received = False + # If the initial pose is not received within 100 seconds, return False + # this is because when setting a wrong initial pose, amcl_pose is not received + # and the test will hang indefinitely + start_time = time.time() + duration = 0 while not self.initial_pose_received: self.info_msg('Setting initial pose') self.setInitialPose() self.info_msg('Waiting for amcl_pose to be received') + duration = time.time() - start_time + print("waited ", duration, " for initial pose to be set") + if duration > self.set_initial_pose_timeout: + self.error_msg('Timeout waiting for initial pose to be set') + return False rclpy.spin_once(self, timeout_sec=1) + return True def test_RobotMovesToGoal(robot_tester): @@ -231,7 +243,8 @@ def run_all_tests(robot_tester): result = True if result: robot_tester.wait_for_node_active('amcl') - robot_tester.wait_for_initial_pose() + result = robot_tester.wait_for_initial_pose() + if result: robot_tester.wait_for_node_active('bt_navigator') result = robot_tester.runNavigateAction() diff --git a/nav2_system_tests/src/system/test_system_launch.py b/nav2_system_tests/src/system/test_system_launch.py index e0791cdf991..e3e04e6eaca 100755 --- a/nav2_system_tests/src/system/test_system_launch.py +++ b/nav2_system_tests/src/system/test_system_launch.py @@ -16,8 +16,10 @@ # limitations under the License. import os -import sys from pathlib import Path +import signal +import subprocess +import sys from ament_index_python.packages import get_package_share_directory from launch import LaunchDescription @@ -37,8 +39,6 @@ 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') @@ -148,6 +148,26 @@ def generate_launch_description(): ] ) +def find_gz_sim_processes(): + ps_output = subprocess.check_output(['ps', 'aux'], text=True) + ps_lines = ps_output.split('\n') + gz_sim_processes = [] + for line in ps_lines: + if 'gz sim' in line: + columns = line.split() + pid = columns[1] + command = ' '.join(columns[10:]) + if command.startswith('gz sim'): + gz_sim_processes.append((pid, command)) + + return gz_sim_processes + +def kill_process(pid): + try: + os.kill(int(pid), signal.SIGKILL) + print(f"Successfully killed process with PID: {pid}") + except Exception as e: + print(f"Failed to kill process with PID: {pid}. Error: {e}") def main(argv=sys.argv[1:]): ld = generate_launch_description() @@ -171,7 +191,22 @@ 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) + # (TODO ) This is a workaround to kill the gz server after the test + # We noticed that the gz server is not killed after the test + # and it is still running in the background. This affects + # the next test run. This is a temporary fix until we find + # a better way of killing the gz server after the test. + processes = find_gz_sim_processes() + if processes: + print("Found the following processes starting with 'gz_sim':") + for pid, command in processes: + print(f"PID: {pid}, Command: {command}") + kill_process(pid) + else: + print("No processes found starting with 'gz_sim'") + + return return_code if __name__ == '__main__': diff --git a/nav2_system_tests/src/system/test_system_with_obstacle_launch.py b/nav2_system_tests/src/system/test_system_with_obstacle_launch.py new file mode 100755 index 00000000000..901226d74a0 --- /dev/null +++ b/nav2_system_tests/src/system/test_system_with_obstacle_launch.py @@ -0,0 +1,227 @@ +#!/usr/bin/env python3 + +# Copyright (c) 2018 Intel Corporation +# Copyright (c) 2020 Florian Gramss +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +import os +from pathlib import Path +import signal +import subprocess +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, +) +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 + + +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') + + 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') + + # Use local param file + launch_dir = os.path.dirname(os.path.realpath(__file__)) + params_file = os.path.join(launch_dir, 'nav2_system_params.yaml') + + # Replace the default parameter values for testing special features + # without having multiple params_files inside the nav2 stack + context = LaunchContext() + param_substitutions = {} + + if os.getenv('ASTAR') == 'True': + param_substitutions.update({'use_astar': 'True'}) + + param_substitutions.update( + {'planner_server.ros__parameters.GridBased.plugin': os.getenv('PLANNER')} + ) + param_substitutions.update( + {'controller_server.ros__parameters.FollowPath.plugin': os.getenv('CONTROLLER')} + ) + + configured_params = RewrittenYaml( + source_file=params_file, + root_key='', + param_rewrites=param_substitutions, + convert_types=True, + ) + + new_yaml = configured_params.perform(context) + + cardbox_sdf = os.path.join(sim_dir, 'urdf', 'cardboard_box.sdf') + + return LaunchDescription( + [ + SetEnvironmentVariable('RCUTILS_LOGGING_BUFFERED_STREAM', '1'), + SetEnvironmentVariable('RCUTILS_LOGGING_USE_STDOUT', '1'), + 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='ros_gz_sim', + executable='create', + output='screen', + arguments=[ + '-entity', 'cardboard_box', + '-file', cardbox_sdf, + '-x', '-1.0', '-y', '0.6', '-z', '0.15', + '-R', '0.0', '-P', '0.0', '-Y', '0.0',] + ), + # 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'], + ), + Node( + package='tf2_ros', + executable='static_transform_publisher', + output='screen', + arguments=['0', '0', '0', '0', '0', '0', 'base_link', 'base_scan'], + ), + IncludeLaunchDescription( + PythonLaunchDescriptionSource( + os.path.join(bringup_dir, 'launch', 'bringup_launch.py') + ), + launch_arguments={ + 'namespace': '', + 'use_namespace': 'False', + 'map': map_yaml_file, + 'use_sim_time': 'True', + 'params_file': new_yaml, + 'bt_xml_file': bt_navigator_xml, + 'use_composition': 'False', + 'autostart': 'True', + }.items(), + ), + ] + ) + +def find_gz_sim_processes(): + ps_output = subprocess.check_output(['ps', 'aux'], text=True) + ps_lines = ps_output.split('\n') + gz_sim_processes = [] + for line in ps_lines: + if 'gz sim' in line: + columns = line.split() + pid = columns[1] + command = ' '.join(columns[10:]) + if command.startswith('gz sim'): + gz_sim_processes.append((pid, command)) + + return gz_sim_processes + +def kill_process(pid): + try: + os.kill(int(pid), signal.SIGKILL) + print(f"Successfully killed process with PID: {pid}") + except Exception as e: + print(f"Failed to kill process with PID: {pid}. Error: {e}") + +def main(argv=sys.argv[1:]): + ld = generate_launch_description() + + test1_action = ExecuteProcess( + cmd=[ + os.path.join(os.getenv('TEST_DIR'), os.getenv('TESTER')), + '-r', + '-2.0', + '-0.5', + '0.0', + '2.0', + '-e', + 'True', + ], + name='tester_node', + output='screen', + ) + + lts = LaunchTestService() + lts.add_test_action(ld, test1_action) + ls = LaunchService(argv=argv) + ls.include_launch_description(ld) + return_code = lts.run(ls) + # (TODO ) This is a workaround to kill the gz server after the test + # We noticed that the gz server is not killed after the test + # and it is still running in the background. This affects + # the next test run. This is a temporary fix until we find + # a better way of killing the gz server after the test. + processes = find_gz_sim_processes() + if processes: + print("Found the following processes starting with 'gz_sim':") + for pid, command in processes: + print(f"PID: {pid}, Command: {command}") + kill_process(pid) + else: + print("No processes found starting with 'gz_sim'") + + return return_code + + +if __name__ == '__main__': + sys.exit(main()) diff --git a/nav2_system_tests/src/system/test_wrong_init_pose_launch.py b/nav2_system_tests/src/system/test_wrong_init_pose_launch.py index f42d276fcce..d021ac107de 100755 --- a/nav2_system_tests/src/system/test_wrong_init_pose_launch.py +++ b/nav2_system_tests/src/system/test_wrong_init_pose_launch.py @@ -16,13 +16,15 @@ # limitations under the License. import os +from pathlib import Path +import signal +import subprocess 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, @@ -36,8 +38,14 @@ 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') + + 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'), @@ -46,7 +54,10 @@ def generate_launch_description(): ) bringup_dir = get_package_share_directory('nav2_bringup') - params_file = os.path.join(bringup_dir, 'params', 'nav2_params.yaml') + + # Use local param file + launch_dir = os.path.dirname(os.path.realpath(__file__)) + params_file = os.path.join(launch_dir, 'nav2_system_params.yaml') # Replace the default parameter values for testing special features # without having multiple params_files inside the nav2 stack @@ -76,16 +87,33 @@ 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') + ), + 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(), ), # TODO(orduno) Launch the robot state publisher instead # using a local copy of TB3 urdf file @@ -119,6 +147,26 @@ def generate_launch_description(): ] ) +def find_gz_sim_processes(): + ps_output = subprocess.check_output(['ps', 'aux'], text=True) + ps_lines = ps_output.split('\n') + gz_sim_processes = [] + for line in ps_lines: + if 'gz sim' in line: + columns = line.split() + pid = columns[1] + command = ' '.join(columns[10:]) + if command.startswith('gz sim'): + gz_sim_processes.append((pid, command)) + + return gz_sim_processes + +def kill_process(pid): + try: + os.kill(int(pid), signal.SIGKILL) + print(f"Successfully killed process with PID: {pid}") + except Exception as e: + print(f"Failed to kill process with PID: {pid}. Error: {e}") def main(argv=sys.argv[1:]): ld = generate_launch_description() @@ -142,7 +190,22 @@ 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) + # (TODO ) This is a workaround to kill the gz server after the test + # We noticed that the gz server is not killed after the test + # and it is still running in the background. This affects + # the next test run. This is a temporary fix until we find + # a better way of killing the gz server after the test. + processes = find_gz_sim_processes() + if processes: + print("Found the following processes starting with 'gz_sim':") + for pid, command in processes: + print(f"PID: {pid}, Command: {command}") + kill_process(pid) + else: + print("No processes found starting with 'gz_sim'") + + return return_code if __name__ == '__main__': From 7d02b33bd9b43b49943d78ef5fcee51c37a31962 Mon Sep 17 00:00:00 2001 From: stevedan Date: Wed, 19 Jun 2024 08:09:32 +0200 Subject: [PATCH 05/19] Include end of line Signed-off-by: stevedan --- nav2_system_tests/src/system/nav2_system_params.yaml | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/nav2_system_tests/src/system/nav2_system_params.yaml b/nav2_system_tests/src/system/nav2_system_params.yaml index 07f2d08f641..5ee29e6536e 100644 --- a/nav2_system_tests/src/system/nav2_system_params.yaml +++ b/nav2_system_tests/src/system/nav2_system_params.yaml @@ -361,4 +361,5 @@ docking_server: k_phi: 3.0 k_delta: 2.0 v_linear_min: 0.15 - v_linear_max: 0.15 \ No newline at end of file + v_linear_max: 0.15 + \ No newline at end of file From 4028524ae52595c4125db60316e69289e219613d Mon Sep 17 00:00:00 2001 From: stevedan Date: Wed, 19 Jun 2024 21:05:41 +0200 Subject: [PATCH 06/19] move gz_sim cleanup process to utils nav2_simple_commander Signed-off-by: stevedan --- .../nav2_simple_commander/utils.py | 27 +++++++++++++++++++ 1 file changed, 27 insertions(+) create mode 100644 nav2_simple_commander/nav2_simple_commander/utils.py diff --git a/nav2_simple_commander/nav2_simple_commander/utils.py b/nav2_simple_commander/nav2_simple_commander/utils.py new file mode 100644 index 00000000000..deafdb068d0 --- /dev/null +++ b/nav2_simple_commander/nav2_simple_commander/utils.py @@ -0,0 +1,27 @@ +import os +import signal +import subprocess + + +def find_gz_sim_processes(): + """ Find all the processes that are running gz sim.""" + ps_output = subprocess.check_output(['ps', 'aux'], text=True) + ps_lines = ps_output.split('\n') + gz_sim_processes = [] + for line in ps_lines: + if 'gz sim' in line: + columns = line.split() + pid = columns[1] + command = ' '.join(columns[10:]) + if command.startswith('gz sim'): + gz_sim_processes.append((pid, command)) + + return gz_sim_processes + +def kill_process(pid): + """Kill a process with a given PID.""" + try: + os.kill(int(pid), signal.SIGKILL) + print(f'Successfully killed process with PID: {pid}') + except Exception as e: + print(f'Failed to kill process with PID: {pid}. Error: {e}') \ No newline at end of file From a0cfaf7e3943a0d36e01109d0570d3beb714b758 Mon Sep 17 00:00:00 2001 From: stevedan Date: Wed, 19 Jun 2024 21:06:54 +0200 Subject: [PATCH 07/19] fix linter Signed-off-by: stevedan --- nav2_system_tests/src/system/CMakeLists.txt | 3 -- .../src/system/nav_to_pose_tester_node.py | 6 ++-- .../src/system/test_system_launch.py | 28 +++--------------- .../test_system_with_obstacle_launch.py | 28 +++--------------- .../src/system/test_wrong_init_pose_launch.py | 29 +++---------------- 5 files changed, 15 insertions(+), 79 deletions(-) diff --git a/nav2_system_tests/src/system/CMakeLists.txt b/nav2_system_tests/src/system/CMakeLists.txt index fe724ea04f0..34ebf0e30e5 100644 --- a/nav2_system_tests/src/system/CMakeLists.txt +++ b/nav2_system_tests/src/system/CMakeLists.txt @@ -5,9 +5,6 @@ ament_add_test(test_bt_navigator 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 TESTER=nav_to_pose_tester_node.py ASTAR=True diff --git a/nav2_system_tests/src/system/nav_to_pose_tester_node.py b/nav2_system_tests/src/system/nav_to_pose_tester_node.py index a96ea4da96f..b8d844bf1c7 100755 --- a/nav2_system_tests/src/system/nav_to_pose_tester_node.py +++ b/nav2_system_tests/src/system/nav_to_pose_tester_node.py @@ -59,7 +59,7 @@ def __init__(self, initial_pose: Pose, goal_pose: Pose, namespace: str = ''): self.initial_pose_received = False self.initial_pose = initial_pose self.goal_pose = goal_pose - self.set_initial_pose_timeout = 100 + self.set_initial_pose_timeout = 80 self.action_client = ActionClient(self, NavigateToPose, 'navigate_to_pose') def info_msg(self, msg: str): @@ -217,13 +217,13 @@ def wait_for_initial_pose(self): # this is because when setting a wrong initial pose, amcl_pose is not received # and the test will hang indefinitely start_time = time.time() - duration = 0 + duration = 0 while not self.initial_pose_received: self.info_msg('Setting initial pose') self.setInitialPose() self.info_msg('Waiting for amcl_pose to be received') duration = time.time() - start_time - print("waited ", duration, " for initial pose to be set") + self.info_msg(f'waited {duration} for initial pose to be set') if duration > self.set_initial_pose_timeout: self.error_msg('Timeout waiting for initial pose to be set') return False diff --git a/nav2_system_tests/src/system/test_system_launch.py b/nav2_system_tests/src/system/test_system_launch.py index e3e04e6eaca..656ca6d58f4 100755 --- a/nav2_system_tests/src/system/test_system_launch.py +++ b/nav2_system_tests/src/system/test_system_launch.py @@ -17,9 +17,8 @@ import os from pathlib import Path -import signal -import subprocess import sys + from ament_index_python.packages import get_package_share_directory from launch import LaunchDescription @@ -36,6 +35,7 @@ from launch_testing.legacy import LaunchTestService from nav2_common.launch import RewrittenYaml +from nav2_simple_commander.utils import find_gz_sim_processes, kill_process def generate_launch_description(): @@ -92,7 +92,7 @@ def generate_launch_description(): 'GZ_SIM_RESOURCE_PATH', os.path.join(sim_dir, 'models') ), AppendEnvironmentVariable( - 'GZ_SIM_RESOURCE_PATH', + 'GZ_SIM_RESOURCE_PATH', str(Path(os.path.join(sim_dir)).parent.resolve()) ), IncludeLaunchDescription( @@ -148,26 +148,6 @@ def generate_launch_description(): ] ) -def find_gz_sim_processes(): - ps_output = subprocess.check_output(['ps', 'aux'], text=True) - ps_lines = ps_output.split('\n') - gz_sim_processes = [] - for line in ps_lines: - if 'gz sim' in line: - columns = line.split() - pid = columns[1] - command = ' '.join(columns[10:]) - if command.startswith('gz sim'): - gz_sim_processes.append((pid, command)) - - return gz_sim_processes - -def kill_process(pid): - try: - os.kill(int(pid), signal.SIGKILL) - print(f"Successfully killed process with PID: {pid}") - except Exception as e: - print(f"Failed to kill process with PID: {pid}. Error: {e}") def main(argv=sys.argv[1:]): ld = generate_launch_description() @@ -191,7 +171,7 @@ def main(argv=sys.argv[1:]): lts.add_test_action(ld, test1_action) ls = LaunchService(argv=argv) ls.include_launch_description(ld) - return_code = lts.run(ls) + return_code = lts.run(ls) # (TODO ) This is a workaround to kill the gz server after the test # We noticed that the gz server is not killed after the test # and it is still running in the background. This affects diff --git a/nav2_system_tests/src/system/test_system_with_obstacle_launch.py b/nav2_system_tests/src/system/test_system_with_obstacle_launch.py index 901226d74a0..709e2d679da 100755 --- a/nav2_system_tests/src/system/test_system_with_obstacle_launch.py +++ b/nav2_system_tests/src/system/test_system_with_obstacle_launch.py @@ -17,9 +17,8 @@ import os from pathlib import Path -import signal -import subprocess import sys + from ament_index_python.packages import get_package_share_directory from launch import LaunchDescription @@ -36,6 +35,7 @@ from launch_testing.legacy import LaunchTestService from nav2_common.launch import RewrittenYaml +from nav2_simple_commander.utils import find_gz_sim_processes, kill_process def generate_launch_description(): @@ -96,7 +96,7 @@ def generate_launch_description(): 'GZ_SIM_RESOURCE_PATH', os.path.join(sim_dir, 'models') ), AppendEnvironmentVariable( - 'GZ_SIM_RESOURCE_PATH', + 'GZ_SIM_RESOURCE_PATH', str(Path(os.path.join(sim_dir)).parent.resolve()) ), IncludeLaunchDescription( @@ -162,26 +162,6 @@ def generate_launch_description(): ] ) -def find_gz_sim_processes(): - ps_output = subprocess.check_output(['ps', 'aux'], text=True) - ps_lines = ps_output.split('\n') - gz_sim_processes = [] - for line in ps_lines: - if 'gz sim' in line: - columns = line.split() - pid = columns[1] - command = ' '.join(columns[10:]) - if command.startswith('gz sim'): - gz_sim_processes.append((pid, command)) - - return gz_sim_processes - -def kill_process(pid): - try: - os.kill(int(pid), signal.SIGKILL) - print(f"Successfully killed process with PID: {pid}") - except Exception as e: - print(f"Failed to kill process with PID: {pid}. Error: {e}") def main(argv=sys.argv[1:]): ld = generate_launch_description() @@ -205,7 +185,7 @@ def main(argv=sys.argv[1:]): lts.add_test_action(ld, test1_action) ls = LaunchService(argv=argv) ls.include_launch_description(ld) - return_code = lts.run(ls) + return_code = lts.run(ls) # (TODO ) This is a workaround to kill the gz server after the test # We noticed that the gz server is not killed after the test # and it is still running in the background. This affects diff --git a/nav2_system_tests/src/system/test_wrong_init_pose_launch.py b/nav2_system_tests/src/system/test_wrong_init_pose_launch.py index d021ac107de..9f66212e732 100755 --- a/nav2_system_tests/src/system/test_wrong_init_pose_launch.py +++ b/nav2_system_tests/src/system/test_wrong_init_pose_launch.py @@ -17,9 +17,8 @@ import os from pathlib import Path -import signal -import subprocess import sys + from ament_index_python.packages import get_package_share_directory from launch import LaunchDescription from launch import LaunchService @@ -35,7 +34,7 @@ from launch_testing.legacy import LaunchTestService from nav2_common.launch import RewrittenYaml - +from nav2_simple_commander.utils import find_gz_sim_processes, kill_process def generate_launch_description(): sim_dir = get_package_share_directory('nav2_minimal_tb3_sim') @@ -91,7 +90,7 @@ def generate_launch_description(): 'GZ_SIM_RESOURCE_PATH', os.path.join(sim_dir, 'models') ), AppendEnvironmentVariable( - 'GZ_SIM_RESOURCE_PATH', + 'GZ_SIM_RESOURCE_PATH', str(Path(os.path.join(sim_dir)).parent.resolve()) ), IncludeLaunchDescription( @@ -147,26 +146,6 @@ def generate_launch_description(): ] ) -def find_gz_sim_processes(): - ps_output = subprocess.check_output(['ps', 'aux'], text=True) - ps_lines = ps_output.split('\n') - gz_sim_processes = [] - for line in ps_lines: - if 'gz sim' in line: - columns = line.split() - pid = columns[1] - command = ' '.join(columns[10:]) - if command.startswith('gz sim'): - gz_sim_processes.append((pid, command)) - - return gz_sim_processes - -def kill_process(pid): - try: - os.kill(int(pid), signal.SIGKILL) - print(f"Successfully killed process with PID: {pid}") - except Exception as e: - print(f"Failed to kill process with PID: {pid}. Error: {e}") def main(argv=sys.argv[1:]): ld = generate_launch_description() @@ -190,7 +169,7 @@ def main(argv=sys.argv[1:]): lts.add_test_action(ld, test1_action) ls = LaunchService(argv=argv) ls.include_launch_description(ld) - return_code = lts.run(ls) + return_code = lts.run(ls) # (TODO ) This is a workaround to kill the gz server after the test # We noticed that the gz server is not killed after the test # and it is still running in the background. This affects From 037edb867b2e8b5c73e122816753780725910340 Mon Sep 17 00:00:00 2001 From: stevedan Date: Wed, 19 Jun 2024 21:41:21 +0200 Subject: [PATCH 08/19] cleanup Signed-off-by: stevedan --- .../nav2_simple_commander/utils.py | 28 ++++++++++++++----- .../src/system/nav_to_pose_tester_node.py | 2 +- .../src/system/test_system_launch.py | 12 ++------ .../test_system_with_obstacle_launch.py | 14 ++-------- .../src/system/test_wrong_init_pose_launch.py | 13 ++------- 5 files changed, 30 insertions(+), 39 deletions(-) diff --git a/nav2_simple_commander/nav2_simple_commander/utils.py b/nav2_simple_commander/nav2_simple_commander/utils.py index deafdb068d0..6ad8151dd53 100644 --- a/nav2_simple_commander/nav2_simple_commander/utils.py +++ b/nav2_simple_commander/nav2_simple_commander/utils.py @@ -1,27 +1,41 @@ +""" +General utility functions +""" + import os import signal import subprocess -def find_gz_sim_processes(): - """ Find all the processes that are running gz sim.""" +def find_os_processes(name): + """Find all the processes that are running gz sim.""" ps_output = subprocess.check_output(['ps', 'aux'], text=True) ps_lines = ps_output.split('\n') gz_sim_processes = [] for line in ps_lines: - if 'gz sim' in line: + if name in line: columns = line.split() pid = columns[1] command = ' '.join(columns[10:]) - if command.startswith('gz sim'): - gz_sim_processes.append((pid, command)) - + if command.startswith(name): + gz_sim_processes.append((pid, command)) return gz_sim_processes + def kill_process(pid): """Kill a process with a given PID.""" try: os.kill(int(pid), signal.SIGKILL) print(f'Successfully killed process with PID: {pid}') except Exception as e: - print(f'Failed to kill process with PID: {pid}. Error: {e}') \ No newline at end of file + print(f'Failed to kill process with PID: {pid}. Error: {e}') + + +def kill_os_processes(name): + """Kill all processes that are running with name.""" + processes = find_os_processes(name) + if processes: + for pid, _ in processes: + kill_process(pid) + else: + print(f'No processes found starting with {name}') diff --git a/nav2_system_tests/src/system/nav_to_pose_tester_node.py b/nav2_system_tests/src/system/nav_to_pose_tester_node.py index b8d844bf1c7..e82dc745c30 100755 --- a/nav2_system_tests/src/system/nav_to_pose_tester_node.py +++ b/nav2_system_tests/src/system/nav_to_pose_tester_node.py @@ -217,7 +217,7 @@ def wait_for_initial_pose(self): # this is because when setting a wrong initial pose, amcl_pose is not received # and the test will hang indefinitely start_time = time.time() - duration = 0 + duration = 0 while not self.initial_pose_received: self.info_msg('Setting initial pose') self.setInitialPose() diff --git a/nav2_system_tests/src/system/test_system_launch.py b/nav2_system_tests/src/system/test_system_launch.py index 656ca6d58f4..b5ae80c8371 100755 --- a/nav2_system_tests/src/system/test_system_launch.py +++ b/nav2_system_tests/src/system/test_system_launch.py @@ -35,7 +35,7 @@ from launch_testing.legacy import LaunchTestService from nav2_common.launch import RewrittenYaml -from nav2_simple_commander.utils import find_gz_sim_processes, kill_process +from nav2_simple_commander.utils import kill_os_processes def generate_launch_description(): @@ -177,15 +177,7 @@ def main(argv=sys.argv[1:]): # and it is still running in the background. This affects # the next test run. This is a temporary fix until we find # a better way of killing the gz server after the test. - processes = find_gz_sim_processes() - if processes: - print("Found the following processes starting with 'gz_sim':") - for pid, command in processes: - print(f"PID: {pid}, Command: {command}") - kill_process(pid) - else: - print("No processes found starting with 'gz_sim'") - + kill_os_processes('gz sim') return return_code diff --git a/nav2_system_tests/src/system/test_system_with_obstacle_launch.py b/nav2_system_tests/src/system/test_system_with_obstacle_launch.py index 709e2d679da..86e516c2f80 100755 --- a/nav2_system_tests/src/system/test_system_with_obstacle_launch.py +++ b/nav2_system_tests/src/system/test_system_with_obstacle_launch.py @@ -35,7 +35,7 @@ from launch_testing.legacy import LaunchTestService from nav2_common.launch import RewrittenYaml -from nav2_simple_commander.utils import find_gz_sim_processes, kill_process +from nav2_simple_commander.utils import kill_os_processes def generate_launch_description(): @@ -120,7 +120,7 @@ def generate_launch_description(): 'yaw': '0.0', }.items(), ), - Node ( + Node( package='ros_gz_sim', executable='create', output='screen', @@ -191,15 +191,7 @@ def main(argv=sys.argv[1:]): # and it is still running in the background. This affects # the next test run. This is a temporary fix until we find # a better way of killing the gz server after the test. - processes = find_gz_sim_processes() - if processes: - print("Found the following processes starting with 'gz_sim':") - for pid, command in processes: - print(f"PID: {pid}, Command: {command}") - kill_process(pid) - else: - print("No processes found starting with 'gz_sim'") - + kill_os_processes('gz sim') return return_code diff --git a/nav2_system_tests/src/system/test_wrong_init_pose_launch.py b/nav2_system_tests/src/system/test_wrong_init_pose_launch.py index 9f66212e732..7ac9a5c897a 100755 --- a/nav2_system_tests/src/system/test_wrong_init_pose_launch.py +++ b/nav2_system_tests/src/system/test_wrong_init_pose_launch.py @@ -34,7 +34,8 @@ from launch_testing.legacy import LaunchTestService from nav2_common.launch import RewrittenYaml -from nav2_simple_commander.utils import find_gz_sim_processes, kill_process +from nav2_simple_commander.utils import kill_os_processes + def generate_launch_description(): sim_dir = get_package_share_directory('nav2_minimal_tb3_sim') @@ -175,15 +176,7 @@ def main(argv=sys.argv[1:]): # and it is still running in the background. This affects # the next test run. This is a temporary fix until we find # a better way of killing the gz server after the test. - processes = find_gz_sim_processes() - if processes: - print("Found the following processes starting with 'gz_sim':") - for pid, command in processes: - print(f"PID: {pid}, Command: {command}") - kill_process(pid) - else: - print("No processes found starting with 'gz_sim'") - + kill_os_processes('gz sim') return return_code From b80934c2e729f0a08293a84eab04b67d5697ced1 Mon Sep 17 00:00:00 2001 From: stevedan Date: Wed, 19 Jun 2024 21:51:12 +0200 Subject: [PATCH 09/19] removed unused path Signed-off-by: stevedan --- nav2_system_tests/urdf/common_properties.urdf | 45 -- nav2_system_tests/urdf/turtlebot3_burger.urdf | 155 ----- nav2_system_tests/urdf/turtlebot3_waffle.urdf | 279 --------- .../worlds/turtlebot3_ros2_demo.world | 497 ---------------- .../worlds/turtlebot3_ros2_demo_gps.world | 555 ----------------- .../turtlebot3_ros2_demo_obstacle.world | 558 ------------------ nav2_system_tests/worlds/world_only.model | 54 -- 7 files changed, 2143 deletions(-) delete mode 100644 nav2_system_tests/urdf/common_properties.urdf delete mode 100644 nav2_system_tests/urdf/turtlebot3_burger.urdf delete mode 100644 nav2_system_tests/urdf/turtlebot3_waffle.urdf delete mode 100644 nav2_system_tests/worlds/turtlebot3_ros2_demo.world delete mode 100644 nav2_system_tests/worlds/turtlebot3_ros2_demo_gps.world delete mode 100644 nav2_system_tests/worlds/turtlebot3_ros2_demo_obstacle.world delete mode 100644 nav2_system_tests/worlds/world_only.model diff --git a/nav2_system_tests/urdf/common_properties.urdf b/nav2_system_tests/urdf/common_properties.urdf deleted file mode 100644 index 9d457ea1d6b..00000000000 --- a/nav2_system_tests/urdf/common_properties.urdf +++ /dev/null @@ -1,45 +0,0 @@ - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - diff --git a/nav2_system_tests/urdf/turtlebot3_burger.urdf b/nav2_system_tests/urdf/turtlebot3_burger.urdf deleted file mode 100644 index 90f34dbbb8f..00000000000 --- a/nav2_system_tests/urdf/turtlebot3_burger.urdf +++ /dev/null @@ -1,155 +0,0 @@ - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - diff --git a/nav2_system_tests/urdf/turtlebot3_waffle.urdf b/nav2_system_tests/urdf/turtlebot3_waffle.urdf deleted file mode 100644 index 9a5bca11411..00000000000 --- a/nav2_system_tests/urdf/turtlebot3_waffle.urdf +++ /dev/null @@ -1,279 +0,0 @@ - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - diff --git a/nav2_system_tests/worlds/turtlebot3_ros2_demo.world b/nav2_system_tests/worlds/turtlebot3_ros2_demo.world deleted file mode 100644 index 9a9e1bdbe84..00000000000 --- a/nav2_system_tests/worlds/turtlebot3_ros2_demo.world +++ /dev/null @@ -1,497 +0,0 @@ - - - - - - model://ground_plane - - - - model://sun - - - - false - - - - - 0.319654 -0.235002 9.29441 0 1.5138 0.009599 - orbit - perspective - - - - - 1000.0 - 0.001 - 1 - - - quick - 150 - 0 - 1.400000 - 1 - - - 0.00001 - 0.2 - 2000.000000 - 0.01000 - - - - - - 1 - - model://turtlebot3_world - - - - - -2.0 -0.5 0.01 0.0 0.0 0.0 - - - - - - - -0.064 0 0.048 0 0 0 - - 0.001 - 0.000 - 0.000 - 0.001 - 0.000 - 0.001 - - 1.0 - - - - -0.064 0 0.048 0 0 0 - - - 0.265 0.265 0.089 - - - - - - -0.064 0 0 0 0 0 - - - model://turtlebot3_waffle/meshes/waffle_base.dae - 0.001 0.001 0.001 - - - - - - - - true - 200 - - - - - 0.0 - 2e-4 - - - - - 0.0 - 2e-4 - - - - - 0.0 - 2e-4 - - - - - - - 0.0 - 1.7e-2 - - - - - 0.0 - 1.7e-2 - - - - - 0.0 - 1.7e-2 - - - - - - false - - - ~/out:=imu - - - - - - - - -0.052 0 0.111 0 0 0 - - 0.001 - 0.000 - 0.000 - 0.001 - 0.000 - 0.001 - - 0.125 - - - - -0.064 0 0.121 0 0 0 - - - 0.0508 - 0.055 - - - - - - -0.032 0 0.171 0 0 0 - - - model://turtlebot3_waffle/meshes/lds.dae - 0.001 0.001 0.001 - - - - - - true - false - -0.064 0 0.121 0 0 0 - 5 - - - - 360 - 1.000000 - 0.000000 - 6.280000 - - - - 0.120000 - 3.5 - 0.015000 - - - gaussian - 0.0 - 0.01 - - - - - - ~/out:=scan - - sensor_msgs/LaserScan - - - - - - - - 0.0 0.144 0.023 -1.57 0 0 - - 0.001 - 0.000 - 0.000 - 0.001 - 0.000 - 0.001 - - 0.1 - - - - 0.0 0.144 0.023 -1.57 0 0 - - - 0.033 - 0.018 - - - - - - - 100000.0 - 100000.0 - 0 0 0 - 0.0 - 0.0 - - - - - 0 - 0.2 - 1e+5 - 1 - 0.01 - 0.001 - - - - - - - 0.0 0.144 0.023 0 0 0 - - - model://turtlebot3_waffle/meshes/left_tire.dae - 0.001 0.001 0.001 - - - - - - - - - 0.0 -0.144 0.023 -1.57 0 0 - - 0.001 - 0.000 - 0.000 - 0.001 - 0.000 - 0.001 - - 0.1 - - - - 0.0 -0.144 0.023 -1.57 0 0 - - - 0.033 - 0.018 - - - - - - - 100000.0 - 100000.0 - 0 0 0 - 0.0 - 0.0 - - - - - 0 - 0.2 - 1e+5 - 1 - 0.01 - 0.001 - - - - - - - 0.0 -0.144 0.023 0 0 0 - - - model://turtlebot3_waffle/meshes/right_tire.dae - 0.001 0.001 0.001 - - - - - - - -0.177 -0.064 -0.004 0 0 0 - - 0.001 - - 0.00001 - 0.000 - 0.000 - 0.00001 - 0.000 - 0.00001 - - - - - - 0.005000 - - - - - - 0 - 0.2 - 1e+5 - 1 - 0.01 - 0.001 - - - - - - - - -0.177 0.064 -0.004 0 0 0 - - 0.001 - - 0.00001 - 0.000 - 0.000 - 0.00001 - 0.000 - 0.00001 - - - - - - 0.005000 - - - - - - 0 - 0.2 - 1e+5 - 1 - 0.01 - 0.001 - - - - - - - - base_footprint - base_link - 0.0 0.0 0.010 0 0 0 - - - - base_link - wheel_left_link - 0.0 0.144 0.023 -1.57 0 0 - - 0 0 1 - - - - - base_link - wheel_right_link - 0.0 -0.144 0.023 -1.57 0 0 - - 0 0 1 - - - - - base_link - caster_back_right_link - - - - base_link - caster_back_left_link - - - - base_link - imu_link - -0.032 0 0.068 0 0 0 - - 0 0 1 - - - - - base_link - base_scan - -0.064 0 0.121 0 0 0 - - 0 0 1 - - - - - - - - /tf:=tf - - - - wheel_left_joint - wheel_right_joint - - - 0.287 - 0.066 - - - 20 - 1.0 - - - true - true - false - - odom - base_footprint - - - - - - - ~/out:=joint_states - - 250 - wheel_left_joint - wheel_right_joint - - - - - - - diff --git a/nav2_system_tests/worlds/turtlebot3_ros2_demo_gps.world b/nav2_system_tests/worlds/turtlebot3_ros2_demo_gps.world deleted file mode 100644 index 94b72499a21..00000000000 --- a/nav2_system_tests/worlds/turtlebot3_ros2_demo_gps.world +++ /dev/null @@ -1,555 +0,0 @@ - - - - - - - EARTH_WGS84 - ENU - 55.944831 - -3.186998 - 0.0 - 180.0 - - - - model://ground_plane - - - - model://sun - - - - false - - - - - 0.319654 -0.235002 9.29441 0 1.5138 0.009599 - orbit - perspective - - - - - 1000.0 - 0.001 - 1 - - - quick - 150 - 0 - 1.400000 - 1 - - - 0.00001 - 0.2 - 2000.000000 - 0.01000 - - - - - - -2.0 -0.5 0.01 0.0 0.0 0.0 - - - - - - - -0.064 0 0.048 0 0 0 - - 0.001 - 0.000 - 0.000 - 0.001 - 0.000 - 0.001 - - 1.0 - - - - -0.064 0 0.048 0 0 0 - - - 0.265 0.265 0.089 - - - - - - -0.064 0 0 0 0 0 - - - model://turtlebot3_waffle/meshes/waffle_base.dae - 0.001 0.001 0.001 - - - - - - - - true - 200 - - - - - 0.0 - 2e-4 - - - - - 0.0 - 2e-4 - - - - - 0.0 - 2e-4 - - - - - - - 0.0 - 1.7e-2 - - - - - 0.0 - 1.7e-2 - - - - - 0.0 - 1.7e-2 - - - - - - false - - - ~/out:=/imu/data - - - - - - - - - -0.052 0 0.111 0 0 0 - - 0.001 - 0.001 - 0.000 - 0.000 - 0.001 - 0.000 - 0.001 - - 0.125 - - - true - 1 - 0 0 0 0 0 0 - - - - - 0.0 - 0.1 - - - - - 0.0 - 0.0 - - - - - - - ~/out:=/gps/fix - - - - - - - - -0.052 0 0.111 0 0 0 - - 0.001 - 0.000 - 0.000 - 0.001 - 0.000 - 0.001 - - 0.125 - - - - -0.064 0 0.121 0 0 0 - - - 0.0508 - 0.055 - - - - - - -0.032 0 0.171 0 0 0 - - - model://turtlebot3_waffle/meshes/lds.dae - 0.001 0.001 0.001 - - - - - - true - false - -0.064 0 0.121 0 0 0 - 5 - - - - 360 - 1.000000 - 0.000000 - 6.280000 - - - - 0.120000 - 3.5 - 0.015000 - - - gaussian - 0.0 - 0.01 - - - - - - ~/out:=scan - - sensor_msgs/LaserScan - - - - - - - - 0.0 0.144 0.023 -1.57 0 0 - - 0.001 - 0.000 - 0.000 - 0.001 - 0.000 - 0.001 - - 0.1 - - - - 0.0 0.144 0.023 -1.57 0 0 - - - 0.033 - 0.018 - - - - - - - 100000.0 - 100000.0 - 0 0 0 - 0.0 - 0.0 - - - - - 0 - 0.2 - 1e+5 - 1 - 0.01 - 0.001 - - - - - - - 0.0 0.144 0.023 0 0 0 - - - model://turtlebot3_waffle/meshes/left_tire.dae - 0.001 0.001 0.001 - - - - - - - - - 0.0 -0.144 0.023 -1.57 0 0 - - 0.001 - 0.000 - 0.000 - 0.001 - 0.000 - 0.001 - - 0.1 - - - - 0.0 -0.144 0.023 -1.57 0 0 - - - 0.033 - 0.018 - - - - - - - 100000.0 - 100000.0 - 0 0 0 - 0.0 - 0.0 - - - - - 0 - 0.2 - 1e+5 - 1 - 0.01 - 0.001 - - - - - - - 0.0 -0.144 0.023 0 0 0 - - - model://turtlebot3_waffle/meshes/right_tire.dae - 0.001 0.001 0.001 - - - - - - - -0.177 -0.064 -0.004 0 0 0 - - 0.001 - - 0.00001 - 0.000 - 0.000 - 0.00001 - 0.000 - 0.00001 - - - - - - 0.005000 - - - - - - 0 - 0.2 - 1e+5 - 1 - 0.01 - 0.001 - - - - - - - - -0.177 0.064 -0.004 0 0 0 - - 0.001 - - 0.00001 - 0.000 - 0.000 - 0.00001 - 0.000 - 0.00001 - - - - - - 0.005000 - - - - - - 0 - 0.2 - 1e+5 - 1 - 0.01 - 0.001 - - - - - - - - base_footprint - base_link - 0.0 0.0 0.010 0 0 0 - - - - base_link - wheel_left_link - 0.0 0.144 0.023 -1.57 0 0 - - 0 0 1 - - - - - base_link - wheel_right_link - 0.0 -0.144 0.023 -1.57 0 0 - - 0 0 1 - - - - - base_link - caster_back_right_link - - - - base_link - caster_back_left_link - - - - base_link - imu_link - -0.032 0 0.068 0 0 0 - - 0 0 1 - - - - - base_link - gps_link - -0.05 0 0.05 0 0 0 - - 0 0 1 - - - - - base_link - base_scan - -0.064 0 0.121 0 0 0 - - 0 0 1 - - - - - - - - /tf:=tf - - - - wheel_left_joint - wheel_right_joint - - - 0.287 - 0.066 - - - 20 - 1.0 - - - true - false - false - - odom - base_link - - - - - - - ~/out:=joint_states - - 250 - wheel_left_joint - wheel_right_joint - - - - - - - diff --git a/nav2_system_tests/worlds/turtlebot3_ros2_demo_obstacle.world b/nav2_system_tests/worlds/turtlebot3_ros2_demo_obstacle.world deleted file mode 100644 index b8c0d0d128d..00000000000 --- a/nav2_system_tests/worlds/turtlebot3_ros2_demo_obstacle.world +++ /dev/null @@ -1,558 +0,0 @@ - - - - - - model://ground_plane - - - - model://sun - - - - false - - - - - 0.319654 -0.235002 9.29441 0 1.5138 0.009599 - orbit - perspective - - - - - 1000.0 - 0.001 - 1 - - - quick - 150 - 0 - 1.400000 - 1 - - - 0.00001 - 0.2 - 2000.000000 - 0.01000 - - - - - - 1 - - model://turtlebot3_world - - - - - -2.0 -0.5 0.01 0.0 0.0 0.0 - - - - - - - -0.064 0 0.048 0 0 0 - - 0.001 - 0.000 - 0.000 - 0.001 - 0.000 - 0.001 - - 1.0 - - - - -0.064 0 0.048 0 0 0 - - - 0.265 0.265 0.089 - - - - - - -0.064 0 0 0 0 0 - - - model://turtlebot3_waffle/meshes/waffle_base.dae - 0.001 0.001 0.001 - - - - - - - - true - 200 - - - - - 0.0 - 2e-4 - - - - - 0.0 - 2e-4 - - - - - 0.0 - 2e-4 - - - - - - - 0.0 - 1.7e-2 - - - - - 0.0 - 1.7e-2 - - - - - 0.0 - 1.7e-2 - - - - - - false - - - ~/out:=imu - - - - - - - - -0.052 0 0.111 0 0 0 - - 0.001 - 0.000 - 0.000 - 0.001 - 0.000 - 0.001 - - 0.125 - - - - -0.020 0 0.161 0 0 0 - - - 0.0508 - 0.055 - - - - - - -0.064 0 0.121 0 0 0 - - - model://turtlebot3_waffle/meshes/lds.dae - 0.001 0.001 0.001 - - - - - - true - false - -0.064 0 0.121 0 0 0 - 5 - - - - 360 - 1.000000 - 0.000000 - 6.280000 - - - - 0.120000 - 3.5 - 0.015000 - - - gaussian - 0.0 - 0.01 - - - - - - ~/out:=scan - - sensor_msgs/LaserScan - - - - - - - - 0.0 0.144 0.023 -1.57 0 0 - - 0.001 - 0.000 - 0.000 - 0.001 - 0.000 - 0.001 - - 0.1 - - - - 0.0 0.144 0.023 -1.57 0 0 - - - 0.033 - 0.018 - - - - - - - 100000.0 - 100000.0 - 0 0 0 - 0.0 - 0.0 - - - - - 0 - 0.2 - 1e+5 - 1 - 0.01 - 0.001 - - - - - - - 0.0 0.144 0.023 0 0 0 - - - model://turtlebot3_waffle/meshes/left_tire.dae - 0.001 0.001 0.001 - - - - - - - - - 0.0 -0.144 0.023 -1.57 0 0 - - 0.001 - 0.000 - 0.000 - 0.001 - 0.000 - 0.001 - - 0.1 - - - - 0.0 -0.144 0.023 -1.57 0 0 - - - 0.033 - 0.018 - - - - - - - 100000.0 - 100000.0 - 0 0 0 - 0.0 - 0.0 - - - - - 0 - 0.2 - 1e+5 - 1 - 0.01 - 0.001 - - - - - - - 0.0 -0.144 0.023 0 0 0 - - - model://turtlebot3_waffle/meshes/right_tire.dae - 0.001 0.001 0.001 - - - - - - - -0.177 -0.064 -0.004 0 0 0 - - 0.001 - - 0.00001 - 0.000 - 0.000 - 0.00001 - 0.000 - 0.00001 - - - - - - 0.005000 - - - - - - 0 - 0.2 - 1e+5 - 1 - 0.01 - 0.001 - - - - - - - - -0.177 0.064 -0.004 0 0 0 - - 0.001 - - 0.00001 - 0.000 - 0.000 - 0.00001 - 0.000 - 0.00001 - - - - - - 0.005000 - - - - - - 0 - 0.2 - 1e+5 - 1 - 0.01 - 0.001 - - - - - - - - base_footprint - base_link - 0.0 0.0 0.010 0 0 0 - - - - base_link - wheel_left_link - 0.0 0.144 0.023 -1.57 0 0 - - 0 0 1 - - - - - base_link - wheel_right_link - 0.0 -0.144 0.023 -1.57 0 0 - - 0 0 1 - - - - - base_link - caster_back_right_link - - - - base_link - caster_back_left_link - - - - base_link - imu_link - -0.032 0 0.068 0 0 0 - - 0 0 1 - - - - - base_link - base_scan - -0.064 0 0.121 0 0 0 - - 0 0 1 - - - - - - - - /tf:=tf - - - - wheel_left_joint - wheel_right_joint - - - 0.287 - 0.066 - - - 20 - 1.0 - - - true - true - false - - odom - base_footprint - - - - - - - ~/out:=joint_states - - 250 - wheel_left_joint - wheel_right_joint - - - - - - -1.0 0.6 0.15 0 -0 0 - - - - 20 - - 0.0416667 - 0 - 0 - 0.0566667 - 0 - 0.0683333 - - 0 0 0 0 -0 0 - - - - - - 0.5 0.4 0.3 - - - - - - 1 - 1 - - - - - - - - 1e+07 - 1 - 0.001 - 0.1 - - - - - 10 - - - - 0 0 -0.15 0 -0 0 - - - model://cardboard_box/meshes/cardboard_box.dae - 1.25932 1.00745 0.755591 - - - - - 0 - 0 - 0 - - - - - diff --git a/nav2_system_tests/worlds/world_only.model b/nav2_system_tests/worlds/world_only.model deleted file mode 100644 index 4c45d4e2f94..00000000000 --- a/nav2_system_tests/worlds/world_only.model +++ /dev/null @@ -1,54 +0,0 @@ - - - - - - model://ground_plane - - - - model://sun - - - - false - - - - - 0.319654 -0.235002 9.29441 0 1.5138 0.009599 - orbit - perspective - - - - - 1000.0 - 0.001 - 1 - - - quick - 150 - 0 - 1.400000 - 1 - - - 0.00001 - 0.2 - 2000.000000 - 0.01000 - - - - - - 1 - - model://turtlebot3_world - - - - - From cd2e5c3bc983c18b3a318960c05a6e552f6f88b6 Mon Sep 17 00:00:00 2001 From: stevedan Date: Wed, 19 Jun 2024 22:13:20 +0200 Subject: [PATCH 10/19] cleanup Signed-off-by: stevedan --- nav2_simple_commander/nav2_simple_commander/utils.py | 6 ++---- nav2_system_tests/CMakeLists.txt | 2 +- nav2_system_tests/models/cardboard_box.sdf | 9 +++++++++ nav2_system_tests/src/system/CMakeLists.txt | 1 + .../src/system/test_system_with_obstacle_launch.py | 5 ++--- 5 files changed, 15 insertions(+), 8 deletions(-) create mode 100644 nav2_system_tests/models/cardboard_box.sdf diff --git a/nav2_simple_commander/nav2_simple_commander/utils.py b/nav2_simple_commander/nav2_simple_commander/utils.py index 6ad8151dd53..e244e3ab0ee 100644 --- a/nav2_simple_commander/nav2_simple_commander/utils.py +++ b/nav2_simple_commander/nav2_simple_commander/utils.py @@ -1,6 +1,4 @@ -""" -General utility functions -""" +"""General utility functions""" import os import signal @@ -18,7 +16,7 @@ def find_os_processes(name): pid = columns[1] command = ' '.join(columns[10:]) if command.startswith(name): - gz_sim_processes.append((pid, command)) + gz_sim_processes.append((pid, command)) return gz_sim_processes diff --git a/nav2_system_tests/CMakeLists.txt b/nav2_system_tests/CMakeLists.txt index 1d8204f1c87..baa1256d52b 100644 --- a/nav2_system_tests/CMakeLists.txt +++ b/nav2_system_tests/CMakeLists.txt @@ -128,7 +128,7 @@ if(BUILD_TESTING) # add_subdirectory(src/behaviors/assisted_teleop) # add_subdirectory(src/costmap_filters) add_subdirectory(src/error_codes) - install(DIRECTORY maps DESTINATION share/${PROJECT_NAME}) + install(DIRECTORY maps models DESTINATION share/${PROJECT_NAME}) endif() diff --git a/nav2_system_tests/models/cardboard_box.sdf b/nav2_system_tests/models/cardboard_box.sdf new file mode 100644 index 00000000000..97a40603c21 --- /dev/null +++ b/nav2_system_tests/models/cardboard_box.sdf @@ -0,0 +1,9 @@ + + + + + https://fuel.gazebosim.org/1.0/OpenRobotics/models/Cardboard box + + + + \ No newline at end of file diff --git a/nav2_system_tests/src/system/CMakeLists.txt b/nav2_system_tests/src/system/CMakeLists.txt index 34ebf0e30e5..2a8f91855ef 100644 --- a/nav2_system_tests/src/system/CMakeLists.txt +++ b/nav2_system_tests/src/system/CMakeLists.txt @@ -76,6 +76,7 @@ ament_add_test(test_nav_through_poses TIMEOUT 180 ENV TEST_DIR=${CMAKE_CURRENT_SOURCE_DIR} + GZ_SIM_RESOURCE_PATH=${PROJECT_SOURCE_DIR}/models ADD_OBSTACLE=True BT_NAVIGATOR_XML=navigate_through_poses_w_replanning_and_recovery.xml TESTER=nav_through_poses_tester_node.py diff --git a/nav2_system_tests/src/system/test_system_with_obstacle_launch.py b/nav2_system_tests/src/system/test_system_with_obstacle_launch.py index 86e516c2f80..4a72333d556 100755 --- a/nav2_system_tests/src/system/test_system_with_obstacle_launch.py +++ b/nav2_system_tests/src/system/test_system_with_obstacle_launch.py @@ -39,11 +39,10 @@ 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') + nav2_system_tests_dir = get_package_share_directory('nav2_system_tests') world_sdf_xacro = os.path.join(sim_dir, 'worlds', 'tb3_sandbox.sdf.xacro') robot_sdf = os.path.join(sim_dir, 'urdf', 'gz_waffle.sdf') @@ -86,7 +85,7 @@ def generate_launch_description(): new_yaml = configured_params.perform(context) - cardbox_sdf = os.path.join(sim_dir, 'urdf', 'cardboard_box.sdf') + cardbox_sdf = os.path.join(nav2_system_tests_dir, 'models', 'cardboard_box.sdf') return LaunchDescription( [ From efa1938867e6610a173683f0a08c249a5ed8de55 Mon Sep 17 00:00:00 2001 From: stevedan Date: Wed, 19 Jun 2024 22:23:06 +0200 Subject: [PATCH 11/19] more cleanup Signed-off-by: stevedan --- nav2_simple_commander/nav2_simple_commander/utils.py | 2 +- nav2_system_tests/src/system/CMakeLists.txt | 1 - 2 files changed, 1 insertion(+), 2 deletions(-) diff --git a/nav2_simple_commander/nav2_simple_commander/utils.py b/nav2_simple_commander/nav2_simple_commander/utils.py index e244e3ab0ee..5d553701f86 100644 --- a/nav2_simple_commander/nav2_simple_commander/utils.py +++ b/nav2_simple_commander/nav2_simple_commander/utils.py @@ -1,4 +1,4 @@ -"""General utility functions""" +"""General utility functions.""" import os import signal diff --git a/nav2_system_tests/src/system/CMakeLists.txt b/nav2_system_tests/src/system/CMakeLists.txt index 2a8f91855ef..34ebf0e30e5 100644 --- a/nav2_system_tests/src/system/CMakeLists.txt +++ b/nav2_system_tests/src/system/CMakeLists.txt @@ -76,7 +76,6 @@ ament_add_test(test_nav_through_poses TIMEOUT 180 ENV TEST_DIR=${CMAKE_CURRENT_SOURCE_DIR} - GZ_SIM_RESOURCE_PATH=${PROJECT_SOURCE_DIR}/models ADD_OBSTACLE=True BT_NAVIGATOR_XML=navigate_through_poses_w_replanning_and_recovery.xml TESTER=nav_through_poses_tester_node.py From 475df16204699e3c5554f14f4247710dbc061768 Mon Sep 17 00:00:00 2001 From: stevedan Date: Wed, 19 Jun 2024 22:33:54 +0200 Subject: [PATCH 12/19] reduce set initial pose time Signed-off-by: stevedan --- nav2_system_tests/src/system/nav_to_pose_tester_node.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/nav2_system_tests/src/system/nav_to_pose_tester_node.py b/nav2_system_tests/src/system/nav_to_pose_tester_node.py index e82dc745c30..450d3c18bcc 100755 --- a/nav2_system_tests/src/system/nav_to_pose_tester_node.py +++ b/nav2_system_tests/src/system/nav_to_pose_tester_node.py @@ -59,7 +59,7 @@ def __init__(self, initial_pose: Pose, goal_pose: Pose, namespace: str = ''): self.initial_pose_received = False self.initial_pose = initial_pose self.goal_pose = goal_pose - self.set_initial_pose_timeout = 80 + self.set_initial_pose_timeout = 15 self.action_client = ActionClient(self, NavigateToPose, 'navigate_to_pose') def info_msg(self, msg: str): From 2d29e0bb56b55a940ce24f5d81a198383fdd3017 Mon Sep 17 00:00:00 2001 From: stevedan Date: Thu, 20 Jun 2024 07:14:46 +0200 Subject: [PATCH 13/19] remove repeated variable Signed-off-by: stevedan --- nav2_system_tests/src/system/test_system_launch.py | 4 +--- .../src/system/test_system_with_obstacle_launch.py | 4 +--- nav2_system_tests/src/system/test_wrong_init_pose_launch.py | 4 +--- 3 files changed, 3 insertions(+), 9 deletions(-) diff --git a/nav2_system_tests/src/system/test_system_launch.py b/nav2_system_tests/src/system/test_system_launch.py index b5ae80c8371..1acf812da88 100755 --- a/nav2_system_tests/src/system/test_system_launch.py +++ b/nav2_system_tests/src/system/test_system_launch.py @@ -54,8 +54,6 @@ def generate_launch_description(): os.getenv('BT_NAVIGATOR_XML'), ) - bringup_dir = get_package_share_directory('nav2_bringup') - # Use local param file launch_dir = os.path.dirname(os.path.realpath(__file__)) params_file = os.path.join(launch_dir, 'nav2_system_params.yaml') @@ -132,7 +130,7 @@ def generate_launch_description(): ), IncludeLaunchDescription( PythonLaunchDescriptionSource( - os.path.join(bringup_dir, 'launch', 'bringup_launch.py') + os.path.join(nav2_bringup_dir, 'launch', 'bringup_launch.py') ), launch_arguments={ 'namespace': '', diff --git a/nav2_system_tests/src/system/test_system_with_obstacle_launch.py b/nav2_system_tests/src/system/test_system_with_obstacle_launch.py index 4a72333d556..31f43f74d07 100755 --- a/nav2_system_tests/src/system/test_system_with_obstacle_launch.py +++ b/nav2_system_tests/src/system/test_system_with_obstacle_launch.py @@ -55,8 +55,6 @@ def generate_launch_description(): os.getenv('BT_NAVIGATOR_XML'), ) - bringup_dir = get_package_share_directory('nav2_bringup') - # Use local param file launch_dir = os.path.dirname(os.path.realpath(__file__)) params_file = os.path.join(launch_dir, 'nav2_system_params.yaml') @@ -145,7 +143,7 @@ def generate_launch_description(): ), IncludeLaunchDescription( PythonLaunchDescriptionSource( - os.path.join(bringup_dir, 'launch', 'bringup_launch.py') + os.path.join(nav2_bringup_dir, 'launch', 'bringup_launch.py') ), launch_arguments={ 'namespace': '', diff --git a/nav2_system_tests/src/system/test_wrong_init_pose_launch.py b/nav2_system_tests/src/system/test_wrong_init_pose_launch.py index 7ac9a5c897a..7e113983d84 100755 --- a/nav2_system_tests/src/system/test_wrong_init_pose_launch.py +++ b/nav2_system_tests/src/system/test_wrong_init_pose_launch.py @@ -53,8 +53,6 @@ def generate_launch_description(): os.getenv('BT_NAVIGATOR_XML'), ) - bringup_dir = get_package_share_directory('nav2_bringup') - # Use local param file launch_dir = os.path.dirname(os.path.realpath(__file__)) params_file = os.path.join(launch_dir, 'nav2_system_params.yaml') @@ -131,7 +129,7 @@ def generate_launch_description(): ), IncludeLaunchDescription( PythonLaunchDescriptionSource( - os.path.join(bringup_dir, 'launch', 'bringup_launch.py') + os.path.join(nav2_bringup_dir, 'launch', 'bringup_launch.py') ), launch_arguments={ 'namespace': '', From c52a08f6ed4cbeb7e5635b87e82ce3aa5f3a6d24 Mon Sep 17 00:00:00 2001 From: stevedan Date: Thu, 20 Jun 2024 18:24:28 +0200 Subject: [PATCH 14/19] Remove log Signed-off-by: stevedan --- nav2_system_tests/src/system/nav_to_pose_tester_node.py | 1 - 1 file changed, 1 deletion(-) diff --git a/nav2_system_tests/src/system/nav_to_pose_tester_node.py b/nav2_system_tests/src/system/nav_to_pose_tester_node.py index 450d3c18bcc..0eca0680395 100755 --- a/nav2_system_tests/src/system/nav_to_pose_tester_node.py +++ b/nav2_system_tests/src/system/nav_to_pose_tester_node.py @@ -223,7 +223,6 @@ def wait_for_initial_pose(self): self.setInitialPose() self.info_msg('Waiting for amcl_pose to be received') duration = time.time() - start_time - self.info_msg(f'waited {duration} for initial pose to be set') if duration > self.set_initial_pose_timeout: self.error_msg('Timeout waiting for initial pose to be set') return False From 21f63d84f7a6464585144cf34ee50c44a6f24fd6 Mon Sep 17 00:00:00 2001 From: stevedan Date: Thu, 20 Jun 2024 18:33:30 +0200 Subject: [PATCH 15/19] Remove todo Signed-off-by: stevedan --- .../src/localization/test_localization_launch.py | 6 +++++- nav2_system_tests/src/system/test_system_launch.py | 7 ------- .../src/system/test_system_with_obstacle_launch.py | 7 ------- .../src/system/test_wrong_init_pose_launch.py | 7 ------- 4 files changed, 5 insertions(+), 22 deletions(-) diff --git a/nav2_system_tests/src/localization/test_localization_launch.py b/nav2_system_tests/src/localization/test_localization_launch.py index aedca4fcf79..099d4b1129a 100755 --- a/nav2_system_tests/src/localization/test_localization_launch.py +++ b/nav2_system_tests/src/localization/test_localization_launch.py @@ -26,6 +26,8 @@ import launch_ros.actions from launch_testing.legacy import LaunchTestService +from nav2_simple_commander.utils import kill_os_processes + def main(argv=sys.argv[1:]): testExecutable = os.getenv('TEST_EXECUTABLE') @@ -115,7 +117,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__': diff --git a/nav2_system_tests/src/system/test_system_launch.py b/nav2_system_tests/src/system/test_system_launch.py index 1acf812da88..b1b62f0fb22 100755 --- a/nav2_system_tests/src/system/test_system_launch.py +++ b/nav2_system_tests/src/system/test_system_launch.py @@ -114,8 +114,6 @@ def generate_launch_description(): 'yaw': '0.0', }.items(), ), - # TODO(orduno) Launch the robot state publisher instead - # using a local copy of TB3 urdf file Node( package='tf2_ros', executable='static_transform_publisher', @@ -170,11 +168,6 @@ def main(argv=sys.argv[1:]): ls = LaunchService(argv=argv) ls.include_launch_description(ld) return_code = lts.run(ls) - # (TODO ) This is a workaround to kill the gz server after the test - # We noticed that the gz server is not killed after the test - # and it is still running in the background. This affects - # the next test run. This is a temporary fix until we find - # a better way of killing the gz server after the test. kill_os_processes('gz sim') return return_code diff --git a/nav2_system_tests/src/system/test_system_with_obstacle_launch.py b/nav2_system_tests/src/system/test_system_with_obstacle_launch.py index 31f43f74d07..196239220bd 100755 --- a/nav2_system_tests/src/system/test_system_with_obstacle_launch.py +++ b/nav2_system_tests/src/system/test_system_with_obstacle_launch.py @@ -127,8 +127,6 @@ def generate_launch_description(): '-x', '-1.0', '-y', '0.6', '-z', '0.15', '-R', '0.0', '-P', '0.0', '-Y', '0.0',] ), - # TODO(orduno) Launch the robot state publisher instead - # using a local copy of TB3 urdf file Node( package='tf2_ros', executable='static_transform_publisher', @@ -183,11 +181,6 @@ def main(argv=sys.argv[1:]): ls = LaunchService(argv=argv) ls.include_launch_description(ld) return_code = lts.run(ls) - # (TODO ) This is a workaround to kill the gz server after the test - # We noticed that the gz server is not killed after the test - # and it is still running in the background. This affects - # the next test run. This is a temporary fix until we find - # a better way of killing the gz server after the test. kill_os_processes('gz sim') return return_code diff --git a/nav2_system_tests/src/system/test_wrong_init_pose_launch.py b/nav2_system_tests/src/system/test_wrong_init_pose_launch.py index 7e113983d84..112b6e1551a 100755 --- a/nav2_system_tests/src/system/test_wrong_init_pose_launch.py +++ b/nav2_system_tests/src/system/test_wrong_init_pose_launch.py @@ -113,8 +113,6 @@ def generate_launch_description(): 'yaw': '0.0', }.items(), ), - # TODO(orduno) Launch the robot state publisher instead - # using a local copy of TB3 urdf file Node( package='tf2_ros', executable='static_transform_publisher', @@ -169,11 +167,6 @@ def main(argv=sys.argv[1:]): ls = LaunchService(argv=argv) ls.include_launch_description(ld) return_code = lts.run(ls) - # (TODO ) This is a workaround to kill the gz server after the test - # We noticed that the gz server is not killed after the test - # and it is still running in the background. This affects - # the next test run. This is a temporary fix until we find - # a better way of killing the gz server after the test. kill_os_processes('gz sim') return return_code From 27ea9129608583b636d847354c4b2e4ac03ea0c6 Mon Sep 17 00:00:00 2001 From: stevedan Date: Thu, 20 Jun 2024 19:51:13 +0200 Subject: [PATCH 16/19] use robot publisher Signed-off-by: stevedan --- .../src/system/test_system_launch.py | 19 ++++++++++--------- .../test_system_with_obstacle_launch.py | 19 ++++++++++--------- .../src/system/test_wrong_init_pose_launch.py | 19 ++++++++++--------- 3 files changed, 30 insertions(+), 27 deletions(-) diff --git a/nav2_system_tests/src/system/test_system_launch.py b/nav2_system_tests/src/system/test_system_launch.py index b1b62f0fb22..a644072ebbb 100755 --- a/nav2_system_tests/src/system/test_system_launch.py +++ b/nav2_system_tests/src/system/test_system_launch.py @@ -46,6 +46,10 @@ def generate_launch_description(): world_sdf_xacro = os.path.join(sim_dir, 'worlds', 'tb3_sandbox.sdf.xacro') robot_sdf = os.path.join(sim_dir, 'urdf', 'gz_waffle.sdf') + 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( @@ -115,16 +119,13 @@ def generate_launch_description(): }.items(), ), 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} + ], ), IncludeLaunchDescription( PythonLaunchDescriptionSource( diff --git a/nav2_system_tests/src/system/test_system_with_obstacle_launch.py b/nav2_system_tests/src/system/test_system_with_obstacle_launch.py index 196239220bd..fb4468afef2 100755 --- a/nav2_system_tests/src/system/test_system_with_obstacle_launch.py +++ b/nav2_system_tests/src/system/test_system_with_obstacle_launch.py @@ -47,6 +47,10 @@ def generate_launch_description(): world_sdf_xacro = os.path.join(sim_dir, 'worlds', 'tb3_sandbox.sdf.xacro') robot_sdf = os.path.join(sim_dir, 'urdf', 'gz_waffle.sdf') + 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( @@ -128,16 +132,13 @@ def generate_launch_description(): '-R', '0.0', '-P', '0.0', '-Y', '0.0',] ), 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} + ], ), IncludeLaunchDescription( PythonLaunchDescriptionSource( diff --git a/nav2_system_tests/src/system/test_wrong_init_pose_launch.py b/nav2_system_tests/src/system/test_wrong_init_pose_launch.py index 112b6e1551a..b0e23226920 100755 --- a/nav2_system_tests/src/system/test_wrong_init_pose_launch.py +++ b/nav2_system_tests/src/system/test_wrong_init_pose_launch.py @@ -45,6 +45,10 @@ def generate_launch_description(): world_sdf_xacro = os.path.join(sim_dir, 'worlds', 'tb3_sandbox.sdf.xacro') robot_sdf = os.path.join(sim_dir, 'urdf', 'gz_waffle.sdf') + 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( @@ -114,16 +118,13 @@ def generate_launch_description(): }.items(), ), 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} + ], ), IncludeLaunchDescription( PythonLaunchDescriptionSource( From 993eac19b54401ff08fc0fb3e5153a89d6e2bf24 Mon Sep 17 00:00:00 2001 From: stevedan Date: Thu, 20 Jun 2024 20:09:07 +0200 Subject: [PATCH 17/19] use robot publisher Signed-off-by: stevedan --- .../localization/test_localization_launch.py | 31 +++++++++++-------- 1 file changed, 18 insertions(+), 13 deletions(-) diff --git a/nav2_system_tests/src/localization/test_localization_launch.py b/nav2_system_tests/src/localization/test_localization_launch.py index 099d4b1129a..e56ca779863 100755 --- a/nav2_system_tests/src/localization/test_localization_launch.py +++ b/nav2_system_tests/src/localization/test_localization_launch.py @@ -15,6 +15,7 @@ # limitations under the License. import os +from pathlib import Path import sys from ament_index_python.packages import get_package_share_directory @@ -38,11 +39,19 @@ def main(argv=sys.argv[1:]): world_sdf_xacro = os.path.join(sim_dir, 'worlds', 'tb3_sandbox.sdf.xacro') robot_sdf = os.path.join(sim_dir, 'urdf', 'gz_waffle.sdf') + 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') set_env_vars_resources = AppendEnvironmentVariable( 'GZ_SIM_RESOURCE_PATH', os.path.join(sim_dir, 'models') ) + set_env_vars_resources2 = AppendEnvironmentVariable( + 'GZ_SIM_RESOURCE_PATH', + str(Path(os.path.join(sim_dir)).parent.resolve()) + ) start_gazebo_server = IncludeLaunchDescription( PythonLaunchDescriptionSource( @@ -66,18 +75,14 @@ def main(argv=sys.argv[1:]): 'yaw': '0.0', }.items(), ) - - link_footprint = launch_ros.actions.Node( - package='tf2_ros', - executable='static_transform_publisher', - output='screen', - arguments=['0', '0', '0', '0', '0', '0', 'base_footprint', 'base_link'], - ) - footprint_scan = launch_ros.actions.Node( - package='tf2_ros', - executable='static_transform_publisher', + run_robot_state_publisher = launch_ros.actions.Node( + 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} + ], ) run_map_server = launch_ros.actions.Node( package='nav2_map_server', @@ -99,10 +104,10 @@ def main(argv=sys.argv[1:]): ld = LaunchDescription( [ set_env_vars_resources, + set_env_vars_resources2, start_gazebo_server, spawn_robot, - link_footprint, - footprint_scan, + run_robot_state_publisher, run_map_server, run_amcl, run_lifecycle_manager, From c1de841eb7e31c941c5e765eb89bde972284e36c Mon Sep 17 00:00:00 2001 From: stevedan Date: Thu, 20 Jun 2024 21:54:31 +0200 Subject: [PATCH 18/19] include copyright Signed-off-by: stevedan --- .../nav2_simple_commander/utils.py | 16 ++++++++++++++++ 1 file changed, 16 insertions(+) diff --git a/nav2_simple_commander/nav2_simple_commander/utils.py b/nav2_simple_commander/nav2_simple_commander/utils.py index 5d553701f86..7818ecdae71 100644 --- a/nav2_simple_commander/nav2_simple_commander/utils.py +++ b/nav2_simple_commander/nav2_simple_commander/utils.py @@ -1,3 +1,19 @@ +#! /usr/bin/env python3 +# Copyright 2021 Samsung Research America +# Copyright 2022 Stevedan Ogochukwu Omodolor +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + """General utility functions.""" import os From d69fd361ec9ce487901c32aa3d0b18393bb6126a Mon Sep 17 00:00:00 2001 From: stevedan Date: Thu, 20 Jun 2024 21:57:50 +0200 Subject: [PATCH 19/19] correct year Signed-off-by: stevedan --- nav2_simple_commander/nav2_simple_commander/utils.py | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/nav2_simple_commander/nav2_simple_commander/utils.py b/nav2_simple_commander/nav2_simple_commander/utils.py index 7818ecdae71..d3639010202 100644 --- a/nav2_simple_commander/nav2_simple_commander/utils.py +++ b/nav2_simple_commander/nav2_simple_commander/utils.py @@ -1,6 +1,6 @@ #! /usr/bin/env python3 -# Copyright 2021 Samsung Research America -# Copyright 2022 Stevedan Ogochukwu Omodolor +# Copyright 2024 Open Navigation LLC +# Copyright 2024 Stevedan Ogochukwu Omodolor Omodia # # Licensed under the Apache License, Version 2.0 (the "License"); # you may not use this file except in compliance with the License.