From a6a91aec6f123ace6d964d0c95b3e89fbd9002ba Mon Sep 17 00:00:00 2001 From: stevedan Date: Sun, 16 Jun 2024 07:41:40 +0200 Subject: [PATCH] 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 45a8097b35..faac608f03 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 5fdbc230b7..2f3f45d191 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 39afa461c2..b57f8686e8 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 a98d340f21..07f2d08f64 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 2f0546d156..215bad71c8 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(