From e5cb99c942807e54dc387a77936c715863873756 Mon Sep 17 00:00:00 2001 From: stevedan Date: Mon, 24 Jun 2024 18:09:55 +0200 Subject: [PATCH 1/5] port backup behavior to new gazebo Signed-off-by: stevedan --- nav2_system_tests/CMakeLists.txt | 2 +- .../src/behaviors/backup/CMakeLists.txt | 3 - .../backup/test_backup_behavior_launch.py | 77 +++++++++++++------ 3 files changed, 53 insertions(+), 29 deletions(-) diff --git a/nav2_system_tests/CMakeLists.txt b/nav2_system_tests/CMakeLists.txt index 13576a6cdd..77df24f230 100644 --- a/nav2_system_tests/CMakeLists.txt +++ b/nav2_system_tests/CMakeLists.txt @@ -123,7 +123,7 @@ if(BUILD_TESTING) # add_subdirectory(src/gps_navigation) # add_subdirectory(src/behaviors/spin) # add_subdirectory(src/behaviors/wait) - # add_subdirectory(src/behaviors/backup) + add_subdirectory(src/behaviors/backup) # add_subdirectory(src/behaviors/drive_on_heading) # add_subdirectory(src/behaviors/assisted_teleop) # add_subdirectory(src/costmap_filters) diff --git a/nav2_system_tests/src/behaviors/backup/CMakeLists.txt b/nav2_system_tests/src/behaviors/backup/CMakeLists.txt index 05afe3c50d..ccbc70458a 100644 --- a/nav2_system_tests/src/behaviors/backup/CMakeLists.txt +++ b/nav2_system_tests/src/behaviors/backup/CMakeLists.txt @@ -15,9 +15,6 @@ ament_add_test(test_backup_recovery WORKING_DIRECTORY "${CMAKE_CURRENT_BINARY_DIR}" TIMEOUT 180 ENV - TEST_MAP=${PROJECT_SOURCE_DIR}/maps/map_circular.yaml TEST_EXECUTABLE=$ - TEST_WORLD=${PROJECT_SOURCE_DIR}/worlds/turtlebot3_ros2_demo.world - GAZEBO_MODEL_PATH=${PROJECT_SOURCE_DIR}/models BT_NAVIGATOR_XML=navigate_to_pose_w_replanning_and_recovery.xml ) diff --git a/nav2_system_tests/src/behaviors/backup/test_backup_behavior_launch.py b/nav2_system_tests/src/behaviors/backup/test_backup_behavior_launch.py index bd4c5432fe..b3b44d0bed 100755 --- a/nav2_system_tests/src/behaviors/backup/test_backup_behavior_launch.py +++ b/nav2_system_tests/src/behaviors/backup/test_backup_behavior_launch.py @@ -14,6 +14,7 @@ # limitations under the License. import os +from pathlib import Path import sys from ament_index_python.packages import get_package_share_directory @@ -21,6 +22,7 @@ from launch import LaunchDescription from launch import LaunchService from launch.actions import ( + AppendEnvironmentVariable, ExecuteProcess, IncludeLaunchDescription, SetEnvironmentVariable, @@ -30,11 +32,22 @@ from launch_testing.legacy import LaunchTestService from nav2_common.launch import RewrittenYaml +from nav2_simple_commander.utils import kill_os_processes def generate_launch_description(): - map_yaml_file = os.getenv('TEST_MAP') - world = os.getenv('TEST_WORLD') + sim_dir = get_package_share_directory('nav2_minimal_tb3_sim') + nav2_bringup_dir = get_package_share_directory('nav2_bringup') + ros_gz_sim_dir = get_package_share_directory('ros_gz_sim') + + world_sdf_xacro = os.path.join(sim_dir, 'worlds', 'tb3_sandbox.sdf.xacro') + robot_sdf = os.path.join(sim_dir, 'urdf', 'gz_waffle.sdf.xacro') + + urdf = os.path.join(sim_dir, 'urdf', 'turtlebot3_waffle.urdf') + with open(urdf, 'r') as infp: + robot_description = infp.read() + + map_yaml_file = os.path.join(nav2_bringup_dir, 'maps', 'tb3_sandbox.yaml') bt_navigator_xml = os.path.join( get_package_share_directory('nav2_bt_navigator'), @@ -42,8 +55,7 @@ def generate_launch_description(): os.getenv('BT_NAVIGATOR_XML'), ) - bringup_dir = get_package_share_directory('nav2_bringup') - params_file = os.path.join(bringup_dir, 'params/nav2_params.yaml') + params_file = os.path.join(nav2_bringup_dir, 'params/nav2_params.yaml') # Replace the `use_astar` setting on the params file configured_params = RewrittenYaml( @@ -55,33 +67,46 @@ def generate_launch_description(): SetEnvironmentVariable('RCUTILS_LOGGING_BUFFERED_STREAM', '1'), SetEnvironmentVariable('RCUTILS_LOGGING_USE_STDOUT', '1'), # Launch gazebo server for simulation - ExecuteProcess( - cmd=[ - 'gzserver', - '-s', - 'libgazebo_ros_init.so', - '--minimal_comms', - world, - ], - output='screen', + AppendEnvironmentVariable( + 'GZ_SIM_RESOURCE_PATH', os.path.join(sim_dir, 'models') ), - # TODO(orduno) Launch the robot state publisher instead - # using a local copy of TB3 urdf file - Node( - package='tf2_ros', - executable='static_transform_publisher', - output='screen', - arguments=['0', '0', '0', '0', '0', '0', 'base_footprint', 'base_link'], + AppendEnvironmentVariable( + 'GZ_SIM_RESOURCE_PATH', + str(Path(os.path.join(sim_dir)).parent.resolve()) + ), + IncludeLaunchDescription( + PythonLaunchDescriptionSource( + os.path.join(ros_gz_sim_dir, 'launch', 'gz_sim.launch.py') + ), + launch_arguments={'gz_args': ['-r -s ', world_sdf_xacro]}.items(), + ), + IncludeLaunchDescription( + PythonLaunchDescriptionSource( + os.path.join(sim_dir, 'launch', 'spawn_tb3.launch.py') + ), + launch_arguments={ + 'use_sim_time': 'True', + 'robot_sdf': robot_sdf, + 'x_pose': '-2.0', + 'y_pose': '-0.5', + 'z_pose': '0.01', + 'roll': '0.0', + 'pitch': '0.0', + 'yaw': '0.0', + }.items(), ), Node( - package='tf2_ros', - executable='static_transform_publisher', + package='robot_state_publisher', + executable='robot_state_publisher', + name='robot_state_publisher', output='screen', - arguments=['0', '0', '0', '0', '0', '0', 'base_link', 'base_scan'], + parameters=[ + {'use_sim_time': True, 'robot_description': robot_description} + ], ), IncludeLaunchDescription( PythonLaunchDescriptionSource( - os.path.join(bringup_dir, 'launch', 'bringup_launch.py') + os.path.join(nav2_bringup_dir, 'launch', 'bringup_launch.py') ), launch_arguments={ 'map': map_yaml_file, @@ -109,7 +134,9 @@ def main(argv=sys.argv[1:]): lts.add_test_action(ld, test1_action) ls = LaunchService(argv=argv) ls.include_launch_description(ld) - return lts.run(ls) + return_code = lts.run(ls) + kill_os_processes('gz sim') + return return_code if __name__ == '__main__': From 2861ce37c0f3128d0b9789510ab218d07fb76a94 Mon Sep 17 00:00:00 2001 From: stevedan Date: Mon, 24 Jun 2024 18:28:25 +0200 Subject: [PATCH 2/5] port drive on heading behavior to new gazebo Signed-off-by: stevedan --- nav2_system_tests/CMakeLists.txt | 2 +- .../behaviors/drive_on_heading/CMakeLists.txt | 3 - .../test_drive_on_heading_behavior_launch.py | 77 ++++++++++++------- 3 files changed, 52 insertions(+), 30 deletions(-) diff --git a/nav2_system_tests/CMakeLists.txt b/nav2_system_tests/CMakeLists.txt index 13576a6cdd..f0a9ff69de 100644 --- a/nav2_system_tests/CMakeLists.txt +++ b/nav2_system_tests/CMakeLists.txt @@ -124,7 +124,7 @@ if(BUILD_TESTING) # add_subdirectory(src/behaviors/spin) # add_subdirectory(src/behaviors/wait) # add_subdirectory(src/behaviors/backup) - # add_subdirectory(src/behaviors/drive_on_heading) + add_subdirectory(src/behaviors/drive_on_heading) # add_subdirectory(src/behaviors/assisted_teleop) # add_subdirectory(src/costmap_filters) add_subdirectory(src/error_codes) diff --git a/nav2_system_tests/src/behaviors/drive_on_heading/CMakeLists.txt b/nav2_system_tests/src/behaviors/drive_on_heading/CMakeLists.txt index fe17417e16..1415c358fe 100644 --- a/nav2_system_tests/src/behaviors/drive_on_heading/CMakeLists.txt +++ b/nav2_system_tests/src/behaviors/drive_on_heading/CMakeLists.txt @@ -15,9 +15,6 @@ ament_add_test(test_drive_on_heading_recovery WORKING_DIRECTORY "${CMAKE_CURRENT_BINARY_DIR}" TIMEOUT 180 ENV - TEST_MAP=${PROJECT_SOURCE_DIR}/maps/map_circular.yaml TEST_EXECUTABLE=$ - TEST_WORLD=${PROJECT_SOURCE_DIR}/worlds/turtlebot3_ros2_demo.world - GAZEBO_MODEL_PATH=${PROJECT_SOURCE_DIR}/models BT_NAVIGATOR_XML=navigate_to_pose_w_replanning_and_recovery.xml ) diff --git a/nav2_system_tests/src/behaviors/drive_on_heading/test_drive_on_heading_behavior_launch.py b/nav2_system_tests/src/behaviors/drive_on_heading/test_drive_on_heading_behavior_launch.py index 8f23ce67b2..2959463712 100755 --- a/nav2_system_tests/src/behaviors/drive_on_heading/test_drive_on_heading_behavior_launch.py +++ b/nav2_system_tests/src/behaviors/drive_on_heading/test_drive_on_heading_behavior_launch.py @@ -14,6 +14,7 @@ # limitations under the License. import os +from pathlib import Path import sys from ament_index_python.packages import get_package_share_directory @@ -21,6 +22,7 @@ from launch import LaunchDescription from launch import LaunchService from launch.actions import ( + AppendEnvironmentVariable, ExecuteProcess, IncludeLaunchDescription, SetEnvironmentVariable, @@ -30,20 +32,29 @@ from launch_testing.legacy import LaunchTestService from nav2_common.launch import RewrittenYaml +from nav2_simple_commander.utils import kill_os_processes def generate_launch_description(): - map_yaml_file = os.getenv('TEST_MAP') - world = os.getenv('TEST_WORLD') + sim_dir = get_package_share_directory('nav2_minimal_tb3_sim') + nav2_bringup_dir = get_package_share_directory('nav2_bringup') + ros_gz_sim_dir = get_package_share_directory('ros_gz_sim') + world_sdf_xacro = os.path.join(sim_dir, 'worlds', 'tb3_sandbox.sdf.xacro') + robot_sdf = os.path.join(sim_dir, 'urdf', 'gz_waffle.sdf.xacro') + + urdf = os.path.join(sim_dir, 'urdf', 'turtlebot3_waffle.urdf') + with open(urdf, 'r') as infp: + robot_description = infp.read() + + map_yaml_file = os.path.join(nav2_bringup_dir, 'maps', 'tb3_sandbox.yaml') bt_navigator_xml = os.path.join( get_package_share_directory('nav2_bt_navigator'), 'behavior_trees', os.getenv('BT_NAVIGATOR_XML'), ) - bringup_dir = get_package_share_directory('nav2_bringup') - params_file = os.path.join(bringup_dir, 'params/nav2_params.yaml') + params_file = os.path.join(nav2_bringup_dir, 'params/nav2_params.yaml') # Replace the `use_astar` setting on the params file configured_params = RewrittenYaml( @@ -54,34 +65,46 @@ def generate_launch_description(): [ SetEnvironmentVariable('RCUTILS_LOGGING_BUFFERED_STREAM', '1'), SetEnvironmentVariable('RCUTILS_LOGGING_USE_STDOUT', '1'), - # Launch gazebo server for simulation - ExecuteProcess( - cmd=[ - 'gzserver', - '-s', - 'libgazebo_ros_init.so', - '--minimal_comms', - world, - ], - output='screen', + AppendEnvironmentVariable( + 'GZ_SIM_RESOURCE_PATH', os.path.join(sim_dir, 'models') ), - # TODO(orduno) Launch the robot state publisher instead - # using a local copy of TB3 urdf file - Node( - package='tf2_ros', - executable='static_transform_publisher', - output='screen', - arguments=['0', '0', '0', '0', '0', '0', 'base_footprint', 'base_link'], + AppendEnvironmentVariable( + 'GZ_SIM_RESOURCE_PATH', + str(Path(os.path.join(sim_dir)).parent.resolve()) + ), + IncludeLaunchDescription( + PythonLaunchDescriptionSource( + os.path.join(ros_gz_sim_dir, 'launch', 'gz_sim.launch.py') + ), + launch_arguments={'gz_args': ['-r -s ', world_sdf_xacro]}.items(), + ), + IncludeLaunchDescription( + PythonLaunchDescriptionSource( + os.path.join(sim_dir, 'launch', 'spawn_tb3.launch.py') + ), + launch_arguments={ + 'use_sim_time': 'True', + 'robot_sdf': robot_sdf, + 'x_pose': '-2.0', + 'y_pose': '-0.5', + 'z_pose': '0.01', + 'roll': '0.0', + 'pitch': '0.0', + 'yaw': '0.0', + }.items(), ), Node( - package='tf2_ros', - executable='static_transform_publisher', + package='robot_state_publisher', + executable='robot_state_publisher', + name='robot_state_publisher', output='screen', - arguments=['0', '0', '0', '0', '0', '0', 'base_link', 'base_scan'], + parameters=[ + {'use_sim_time': True, 'robot_description': robot_description} + ], ), IncludeLaunchDescription( PythonLaunchDescriptionSource( - os.path.join(bringup_dir, 'launch', 'bringup_launch.py') + os.path.join(nav2_bringup_dir, 'launch', 'bringup_launch.py') ), launch_arguments={ 'map': map_yaml_file, @@ -111,7 +134,9 @@ def main(argv=sys.argv[1:]): lts.add_test_action(ld, test1_action) ls = LaunchService(argv=argv) ls.include_launch_description(ld) - return lts.run(ls) + return_code = lts.run(ls) + kill_os_processes('gz sim') + return return_code if __name__ == '__main__': From 2f33d8d0c710e7b339e37d5e40cdd0ea95487e3f Mon Sep 17 00:00:00 2001 From: Steve Macenski Date: Wed, 3 Jul 2024 15:44:24 -0700 Subject: [PATCH 3/5] completely rewritten spin test --- .../src/behaviors/spin/CMakeLists.txt | 28 +- .../behaviors/spin/spin_behavior_tester.cpp | 363 ------------------ .../behaviors/spin/spin_behavior_tester.hpp | 110 ------ .../src/behaviors/spin/spin_tester.py | 321 ++++++++++++++++ ...launch.py => test_spin_behavior.launch.py} | 98 ++--- .../spin/test_spin_behavior_fake_launch.py | 175 --------- .../spin/test_spin_behavior_node.cpp | 143 ------- 7 files changed, 377 insertions(+), 861 deletions(-) delete mode 100644 nav2_system_tests/src/behaviors/spin/spin_behavior_tester.cpp delete mode 100644 nav2_system_tests/src/behaviors/spin/spin_behavior_tester.hpp create mode 100755 nav2_system_tests/src/behaviors/spin/spin_tester.py rename nav2_system_tests/src/behaviors/spin/{test_spin_behavior_launch.py => test_spin_behavior.launch.py} (62%) delete mode 100755 nav2_system_tests/src/behaviors/spin/test_spin_behavior_fake_launch.py delete mode 100644 nav2_system_tests/src/behaviors/spin/test_spin_behavior_node.cpp diff --git a/nav2_system_tests/src/behaviors/spin/CMakeLists.txt b/nav2_system_tests/src/behaviors/spin/CMakeLists.txt index e287b7a534..e9757643fd 100644 --- a/nav2_system_tests/src/behaviors/spin/CMakeLists.txt +++ b/nav2_system_tests/src/behaviors/spin/CMakeLists.txt @@ -1,30 +1,8 @@ -set(test_spin_behavior_exec test_spin_behavior_node) - -ament_add_gtest_executable(${test_spin_behavior_exec} - test_spin_behavior_node.cpp - spin_behavior_tester.cpp -) - -ament_target_dependencies(${test_spin_behavior_exec} - ${dependencies} -) - ament_add_test(test_spin_behavior GENERATE_RESULT_FOR_RETURN_CODE_ZERO - COMMAND "${CMAKE_CURRENT_SOURCE_DIR}/test_spin_behavior_launch.py" - WORKING_DIRECTORY "${CMAKE_CURRENT_BINARY_DIR}" - TIMEOUT 180 - ENV - TEST_EXECUTABLE=$ - BT_NAVIGATOR_XML=navigate_to_pose_w_replanning_and_recovery.xml -) - -ament_add_test(test_spin_behavior_fake - GENERATE_RESULT_FOR_RETURN_CODE_ZERO - COMMAND "${CMAKE_CURRENT_SOURCE_DIR}/test_spin_behavior_fake_launch.py" + COMMAND "${CMAKE_CURRENT_SOURCE_DIR}/test_spin_behavior.launch.py" WORKING_DIRECTORY "${CMAKE_CURRENT_BINARY_DIR}" - TIMEOUT 180 + TIMEOUT 60 ENV - TEST_EXECUTABLE=$ - MAKE_FAKE_COSTMAP=true + TEST_DIR=${CMAKE_CURRENT_SOURCE_DIR} ) diff --git a/nav2_system_tests/src/behaviors/spin/spin_behavior_tester.cpp b/nav2_system_tests/src/behaviors/spin/spin_behavior_tester.cpp deleted file mode 100644 index 1e6d57e0f4..0000000000 --- a/nav2_system_tests/src/behaviors/spin/spin_behavior_tester.cpp +++ /dev/null @@ -1,363 +0,0 @@ -// Copyright (c) 2020 Sarthak Mittal -// Copyright (c) 2018 Intel Corporation -// -// 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. Reserved. - -#include -#include -#include -#include -#include -#include -#include -#include - -#include "spin_behavior_tester.hpp" - -using namespace std::chrono_literals; -using namespace std::chrono; // NOLINT - -namespace nav2_system_tests -{ - -SpinBehaviorTester::SpinBehaviorTester() -: is_active_(false), - initial_pose_received_(false) -{ - node_ = rclcpp::Node::make_shared("spin_behavior_test"); - - - tf_buffer_ = std::make_shared(node_->get_clock()); - tf_listener_ = std::make_shared(*tf_buffer_); - tf_broadcaster_ = std::make_shared(node_); - if (std::getenv("MAKE_FAKE_COSTMAP") != NULL) { - // if this variable is set, make a fake costmap - make_fake_costmap_ = true; - } else { - make_fake_costmap_ = false; - } - - client_ptr_ = rclcpp_action::create_client( - node_->get_node_base_interface(), - node_->get_node_graph_interface(), - node_->get_node_logging_interface(), - node_->get_node_waitables_interface(), - "spin"); - - publisher_ = - node_->create_publisher("initialpose", 10); - fake_costmap_publisher_ = - node_->create_publisher( - "local_costmap/costmap_raw", - rclcpp::QoS(rclcpp::KeepLast(1)).transient_local().reliable()); - fake_footprint_publisher_ = node_->create_publisher( - "local_costmap/published_footprint", rclcpp::SystemDefaultsQoS()); - - subscription_ = node_->create_subscription( - "amcl_pose", rclcpp::QoS(rclcpp::KeepLast(1)).transient_local().reliable(), - std::bind(&SpinBehaviorTester::amclPoseCallback, this, std::placeholders::_1)); - - stamp_ = node_->now(); -} - -SpinBehaviorTester::~SpinBehaviorTester() -{ - if (is_active_) { - deactivate(); - } -} - -void SpinBehaviorTester::activate() -{ - if (is_active_) { - throw std::runtime_error("Trying to activate while already active"); - return; - } - if (!make_fake_costmap_) { - while (!initial_pose_received_) { - RCLCPP_WARN(node_->get_logger(), "Initial pose not received"); - sendInitialPose(); - std::this_thread::sleep_for(100ms); - rclcpp::spin_some(node_); - } - } else { - sendFakeOdom(0.0); - } - - // Wait for lifecycle_manager_navigation to activate behavior_server - std::this_thread::sleep_for(10s); - - if (!client_ptr_) { - RCLCPP_ERROR(node_->get_logger(), "Action client not initialized"); - is_active_ = false; - return; - } - - if (!client_ptr_->wait_for_action_server(10s)) { - RCLCPP_ERROR(node_->get_logger(), "Action server not available after waiting"); - is_active_ = false; - return; - } - - RCLCPP_INFO(this->node_->get_logger(), "Spin action server is ready"); - is_active_ = true; -} - -void SpinBehaviorTester::deactivate() -{ - if (!is_active_) { - throw std::runtime_error("Trying to deactivate while already inactive"); - } - is_active_ = false; -} - -bool SpinBehaviorTester::defaultSpinBehaviorTest( - const float target_yaw, - const double tolerance, - const bool nonblocking_action, - const bool cancel_action) -{ - if (!is_active_) { - RCLCPP_ERROR(node_->get_logger(), "Not activated"); - return false; - } - - // Sleep to let behavior server be ready for serving in multiple runs - std::this_thread::sleep_for(5s); - - if (make_fake_costmap_) { - sendFakeCostmap(target_yaw); - sendFakeOdom(0.0); - } - - auto goal_msg = Spin::Goal(); - goal_msg.target_yaw = target_yaw; - - // Initialize fake costmap - if (make_fake_costmap_) { - sendFakeCostmap(target_yaw); - sendFakeOdom(0.0); - } - - geometry_msgs::msg::PoseStamped initial_pose; - if (!nav2_util::getCurrentPose(initial_pose, *tf_buffer_, "odom")) { - RCLCPP_ERROR(node_->get_logger(), "Current robot pose is not available."); - return false; - } - RCLCPP_INFO(node_->get_logger(), "Found current robot pose"); - RCLCPP_INFO( - node_->get_logger(), - "Init Yaw is %lf", - fabs(tf2::getYaw(initial_pose.pose.orientation))); - RCLCPP_INFO(node_->get_logger(), "Before sending goal"); - - // Initialize fake costmap - if (make_fake_costmap_) { - sendFakeCostmap(target_yaw); - sendFakeOdom(0.0); - } - - rclcpp::sleep_for(std::chrono::milliseconds(100)); - - auto goal_handle_future = client_ptr_->async_send_goal(goal_msg); - - if (rclcpp::spin_until_future_complete(node_, goal_handle_future) != - rclcpp::FutureReturnCode::SUCCESS) - { - RCLCPP_ERROR(node_->get_logger(), "send goal call failed :("); - return false; - } - - rclcpp_action::ClientGoalHandle::SharedPtr goal_handle = goal_handle_future.get(); - if (!goal_handle) { - RCLCPP_ERROR(node_->get_logger(), "Goal was rejected by server"); - return false; - } - - if (!nonblocking_action) { - return true; - } - if (cancel_action) { - sleep(2); - // cancel the goal - auto cancel_response = client_ptr_->async_cancel_goal(goal_handle_future.get()); - rclcpp::spin_until_future_complete(node_, cancel_response); - } - - // Wait for the server to be done with the goal - auto result_future = client_ptr_->async_get_result(goal_handle); - - RCLCPP_INFO(node_->get_logger(), "Waiting for result"); - rclcpp::sleep_for(std::chrono::milliseconds(1000)); - - if (make_fake_costmap_) { // if we are faking the costmap, we will fake success. - sendFakeOdom(0.0); - sendFakeCostmap(target_yaw); - RCLCPP_INFO(node_->get_logger(), "target_yaw %lf", target_yaw); - // Slowly increment command yaw by increment to simulate the robot slowly spinning into place - float step_size = tolerance / 4.0; - for (float command_yaw = 0.0; - abs(command_yaw) < abs(target_yaw); - command_yaw = command_yaw + step_size) - { - sendFakeOdom(command_yaw); - sendFakeCostmap(target_yaw); - rclcpp::sleep_for(std::chrono::milliseconds(3)); - } - sendFakeOdom(target_yaw); - sendFakeCostmap(target_yaw); - RCLCPP_INFO(node_->get_logger(), "After sending goal"); - } - if (rclcpp::spin_until_future_complete(node_, result_future) != - rclcpp::FutureReturnCode::SUCCESS) - { - RCLCPP_ERROR(node_->get_logger(), "get result call failed :("); - return false; - } - - rclcpp_action::ClientGoalHandle::WrappedResult wrapped_result = result_future.get(); - - switch (wrapped_result.code) { - case rclcpp_action::ResultCode::SUCCEEDED: break; - case rclcpp_action::ResultCode::ABORTED: RCLCPP_ERROR( - node_->get_logger(), - "Goal was aborted"); - return false; - case rclcpp_action::ResultCode::CANCELED: RCLCPP_ERROR( - node_->get_logger(), - "Goal was canceled"); - return false; - default: RCLCPP_ERROR(node_->get_logger(), "Unknown result code"); - return false; - } - - RCLCPP_INFO(node_->get_logger(), "result received"); - - geometry_msgs::msg::PoseStamped current_pose; - if (!nav2_util::getCurrentPose(current_pose, *tf_buffer_, "odom")) { - RCLCPP_ERROR(node_->get_logger(), "Current robot pose is not available."); - return false; - } - - double goal_yaw = angles::normalize_angle( - tf2::getYaw(initial_pose.pose.orientation) + target_yaw); - double dyaw = angles::shortest_angular_distance( - goal_yaw, tf2::getYaw(current_pose.pose.orientation)); - - if (fabs(dyaw) > tolerance) { - RCLCPP_ERROR( - node_->get_logger(), - "Init Yaw is %lf (tolerance %lf)", - fabs(tf2::getYaw(initial_pose.pose.orientation)), tolerance); - RCLCPP_ERROR( - node_->get_logger(), - "Current Yaw is %lf (tolerance %lf)", - fabs(tf2::getYaw(current_pose.pose.orientation)), tolerance); - RCLCPP_ERROR( - node_->get_logger(), - "Angular distance from goal is %lf (tolerance %lf)", - fabs(dyaw), tolerance); - return false; - } - - return true; -} - -void SpinBehaviorTester::sendFakeCostmap(float angle) -{ - nav2_msgs::msg::Costmap fake_costmap; - - fake_costmap.header.frame_id = "odom"; - fake_costmap.header.stamp = node_->now(); - fake_costmap.metadata.layer = "master"; - fake_costmap.metadata.resolution = .1; - fake_costmap.metadata.size_x = 100; - fake_costmap.metadata.size_y = 100; - fake_costmap.metadata.origin.position.x = 0; - fake_costmap.metadata.origin.position.y = 0; - fake_costmap.metadata.origin.orientation.w = 1.0; - float costmap_val = 0; - for (int ix = 0; ix < 100; ix++) { - for (int iy = 0; iy < 100; iy++) { - if (fabs(angle) > M_PI_2f32) { - // fake obstacles in the way so we get failure due to potential collision - costmap_val = 253; - } - fake_costmap.data.push_back(costmap_val); - } - } - fake_costmap_publisher_->publish(fake_costmap); -} - -void SpinBehaviorTester::sendInitialPose() -{ - geometry_msgs::msg::PoseWithCovarianceStamped pose; - pose.header.frame_id = "map"; - pose.header.stamp = node_->now(); - pose.pose.pose.position.x = -2.0; - pose.pose.pose.position.y = -0.5; - pose.pose.pose.position.z = 0.0; - pose.pose.pose.orientation.x = 0.0; - pose.pose.pose.orientation.y = 0.0; - pose.pose.pose.orientation.z = 0.0; - pose.pose.pose.orientation.w = 1.0; - for (int i = 0; i < 35; i++) { - pose.pose.covariance[i] = 0.0; - } - pose.pose.covariance[0] = 0.08; - pose.pose.covariance[7] = 0.08; - pose.pose.covariance[35] = 0.05; - - publisher_->publish(pose); - RCLCPP_INFO(node_->get_logger(), "Sent initial pose"); -} - -void SpinBehaviorTester::sendFakeOdom(float angle) -{ - geometry_msgs::msg::TransformStamped transformStamped; - - transformStamped.header.stamp = node_->now(); - transformStamped.header.frame_id = "odom"; - transformStamped.child_frame_id = "base_link"; - transformStamped.transform.translation.x = 0.0; - transformStamped.transform.translation.y = 0.0; - transformStamped.transform.translation.z = 0.0; - tf2::Quaternion q; - q.setRPY(0, 0, angle); - transformStamped.transform.rotation.x = q.x(); - transformStamped.transform.rotation.y = q.y(); - transformStamped.transform.rotation.z = q.z(); - transformStamped.transform.rotation.w = q.w(); - - tf_broadcaster_->sendTransform(transformStamped); - - geometry_msgs::msg::PolygonStamped footprint; - footprint.header.frame_id = "odom"; - footprint.header.stamp = node_->now(); - footprint.polygon.points.resize(4); - footprint.polygon.points[0].x = 0.22; - footprint.polygon.points[0].y = 0.22; - footprint.polygon.points[1].x = 0.22; - footprint.polygon.points[1].y = -0.22; - footprint.polygon.points[2].x = -0.22; - footprint.polygon.points[2].y = -0.22; - footprint.polygon.points[3].x = -0.22; - footprint.polygon.points[3].y = 0.22; - fake_footprint_publisher_->publish(footprint); -} -void SpinBehaviorTester::amclPoseCallback( - const geometry_msgs::msg::PoseWithCovarianceStamped::SharedPtr) -{ - initial_pose_received_ = true; -} -} // namespace nav2_system_tests diff --git a/nav2_system_tests/src/behaviors/spin/spin_behavior_tester.hpp b/nav2_system_tests/src/behaviors/spin/spin_behavior_tester.hpp deleted file mode 100644 index c7fb6f719d..0000000000 --- a/nav2_system_tests/src/behaviors/spin/spin_behavior_tester.hpp +++ /dev/null @@ -1,110 +0,0 @@ -// Copyright (c) 2020 Sarthak Mittal -// Copyright (c) 2018 Intel Corporation -// -// 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. Reserved. - -#ifndef BEHAVIORS__SPIN__SPIN_BEHAVIOR_TESTER_HPP_ -#define BEHAVIORS__SPIN__SPIN_BEHAVIOR_TESTER_HPP_ - -#include -#include -#include -#include -#include - -#include "rclcpp/rclcpp.hpp" -#include "rclcpp_action/rclcpp_action.hpp" -#include "angles/angles.h" -#include "nav2_msgs/action/spin.hpp" -#include "nav2_msgs/msg/costmap.hpp" -#include "nav2_util/robot_utils.hpp" -#include "nav2_util/node_thread.hpp" -#include "geometry_msgs/msg/point32.hpp" -#include "geometry_msgs/msg/polygon_stamped.hpp" -#include "geometry_msgs/msg/pose_stamped.hpp" -#include "geometry_msgs/msg/pose_with_covariance_stamped.hpp" -#include "geometry_msgs/msg/transform_stamped.hpp" -#include "geometry_msgs/msg/quaternion.hpp" - -#include "tf2/utils.h" -#include "tf2_geometry_msgs/tf2_geometry_msgs.hpp" -#include "tf2_ros/buffer.h" -#include "tf2_ros/transform_broadcaster.h" -#include "tf2_ros/transform_listener.h" - -namespace nav2_system_tests -{ - -class SpinBehaviorTester -{ -public: - using Spin = nav2_msgs::action::Spin; - using GoalHandleSpin = rclcpp_action::ClientGoalHandle; - - SpinBehaviorTester(); - ~SpinBehaviorTester(); - - // Runs a single test with given target yaw - bool defaultSpinBehaviorTest( - float target_yaw, - double tolerance = 0.1, - bool nonblocking_action = true, - bool cancel_action = false); - - void activate(); - - void deactivate(); - - bool isActive() const - { - return is_active_; - } - -private: - void sendInitialPose(); - - void sendFakeCostmap(float angle); - void sendFakeOdom(float angle); - - void amclPoseCallback(geometry_msgs::msg::PoseWithCovarianceStamped::SharedPtr); - - bool is_active_; - bool initial_pose_received_; - bool make_fake_costmap_; - rclcpp::Time stamp_; - - std::shared_ptr tf_buffer_; - std::shared_ptr tf_listener_; - std::shared_ptr tf_broadcaster_; - - rclcpp::Node::SharedPtr node_; - - // Publisher to publish initial pose - rclcpp::Publisher::SharedPtr publisher_; - - // Publisher to publish fake costmap raw - rclcpp::Publisher::SharedPtr fake_costmap_publisher_; - - // Publisher to publish fake costmap footprint - rclcpp::Publisher::SharedPtr fake_footprint_publisher_; - - // Subscriber for amcl pose - rclcpp::Subscription::SharedPtr subscription_; - - // Action client to call spin action - rclcpp_action::Client::SharedPtr client_ptr_; -}; - -} // namespace nav2_system_tests - -#endif // BEHAVIORS__SPIN__SPIN_BEHAVIOR_TESTER_HPP_ diff --git a/nav2_system_tests/src/behaviors/spin/spin_tester.py b/nav2_system_tests/src/behaviors/spin/spin_tester.py new file mode 100755 index 0000000000..f1d92ebaa6 --- /dev/null +++ b/nav2_system_tests/src/behaviors/spin/spin_tester.py @@ -0,0 +1,321 @@ +#! /usr/bin/env python3 +# Copyright 2024 Open Navigation LLC +# +# 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 sys +import time + +from action_msgs.msg import GoalStatus +from nav2_msgs.action import Spin +from nav2_msgs.msg import Costmap +from nav2_msgs.srv import ManageLifecycleNodes +from geometry_msgs.msg import Point32, PolygonStamped + +import rclpy + +from rclpy.action import ActionClient +from rclpy.node import Node +from rclpy.duration import Duration +from rclpy.qos import QoSDurabilityPolicy, QoSHistoryPolicy, QoSReliabilityPolicy +from rclpy.qos import QoSProfile + + +class SpinTest(Node): + def __init__(self): + super().__init__(node_name='spin_tester', namespace='') + self.costmap_qos = QoSProfile( + durability=QoSDurabilityPolicy.TRANSIENT_LOCAL, + reliability=QoSReliabilityPolicy.RELIABLE, + history=QoSHistoryPolicy.KEEP_LAST, + depth=1, + ) + self.action_client = ActionClient(self, Spin, 'spin') + self.costmap_pub = self.create_publisher(Costmap, 'local_costmap/costmap_raw', self.costmap_qos) + self.footprint_pub = self.create_publisher(PolygonStamped, 'local_costmap/published_footprint', 10) + self.goal_handle = None + self.action_result = None + + def sendCommand(self, command): + self.info_msg('Sending goal request...') + self.goal_future = self.action_client.send_goal_async(command) + try: + rclpy.spin_until_future_complete(self, self.goal_future) + self.goal_handle = self.goal_future.result() + except Exception as e: # noqa: B902 + self.error_msg(f'Service call failed {e!r}') + + if not self.goal_handle.accepted: + self.error_msg('Goal rejected') + return False + + self.info_msg('Goal accepted') + self.result_future = self.goal_handle.get_result_async() + + self.info_msg("Waiting for 'spin' action to complete") + try: + rclpy.spin_until_future_complete(self, self.result_future) + status = self.result_future.result().status + result = self.result_future.result().result + self.action_result = result + except Exception as e: # noqa: B902 + self.error_msg(f'Service call failed {e!r}') + + if status != GoalStatus.STATUS_SUCCEEDED: + self.info_msg(f'Goal failed with status code: {status}') + return False + if self.action_result.error_code == 0: + self.info_msg('Spin was successful!') + return True + self.info_msg('Spin failed to meet target!') + return False + + def sendAndPreemptWithInvertedCommand(self, command): + # Send initial goal + self.info_msg('Sending goal request...') + self.goal_future = self.action_client.send_goal_async(command) + try: + rclpy.spin_until_future_complete(self, self.goal_future) + self.goal_handle = self.goal_future.result() + except Exception as e: # noqa: B902 + self.error_msg(f'Service call failed {e!r}') + + if not self.goal_handle.accepted: + self.error_msg('Goal rejected') + return False + + self.info_msg('Goal accepted') + self.result_future = self.goal_handle.get_result_async() + + # Now preempt it + time.sleep(0.5) + self.info_msg('Sending preemption request...') + command.target_yaw = -command.target_yaw + self.goal_future = self.action_client.send_goal_async(command) + try: + rclpy.spin_until_future_complete(self, self.goal_future) + self.goal_handle = self.goal_future.result() + except Exception as e: # noqa: B902 + self.error_msg(f'Service call failed {e!r}') + + if not self.goal_handle.accepted: + self.error_msg('Preemption rejected') + return False + + self.info_msg('Preemption accepted') + self.result_future = self.goal_handle.get_result_async() + + # Wait for new goal to complete + self.info_msg("Waiting for 'spin' action Preemption to complete") + try: + rclpy.spin_until_future_complete(self, self.result_future) + status = self.result_future.result().status + result = self.result_future.result().result + self.action_result = result + except Exception as e: # noqa: B902 + self.error_msg(f'Service call failed {e!r}') + + if status != GoalStatus.STATUS_SUCCEEDED: + self.info_msg(f'Goal failed with status code: {status}') + return False + if self.action_result.error_code == 0: + self.info_msg('Spin was successful!') + return True + self.info_msg('Spin failed to meet target!') + return False + + def sendAndCancelCommand(self, command): + self.info_msg('Sending goal request...') + self.goal_future = self.action_client.send_goal_async(command) + try: + rclpy.spin_until_future_complete(self, self.goal_future) + self.goal_handle = self.goal_future.result() + except Exception as e: # noqa: B902 + self.error_msg(f'Service call failed {e!r}') + + if not self.goal_handle.accepted: + self.error_msg('Goal rejected') + return False + + self.info_msg('Goal accepted') + self.result_future = self.goal_handle.get_result_async() + + # Now cancel it + time.sleep(0.5) + cancel_future = self.goal_handle.cancel_goal_async() + rclpy.spin_until_future_complete(self, cancel_future) + rclpy.spin_until_future_complete(self, self.result_future) + status = self.result_future.result().status + if status != GoalStatus.STATUS_CANCELED: + self.info_msg(f'Goal failed with status code: {status}') + return False + else: + self.info_msg('Goal was canceled successfully') + return True + + def sendFreeCostmap(self): + costmap_msg = Costmap() + costmap_msg.header.frame_id = 'odom' + costmap_msg.header.stamp = self.get_clock().now().to_msg() + costmap_msg.metadata.resolution = 0.05 + costmap_msg.metadata.size_x = 100 + costmap_msg.metadata.size_y = 100 + costmap_msg.metadata.origin.position.x = -2.5 + costmap_msg.metadata.origin.position.y = -2.5 + costmap_msg.data = [0] * (costmap_msg.metadata.size_x * costmap_msg.metadata.size_y) + self.costmap_pub.publish(costmap_msg) + + footprint_msg = PolygonStamped() + footprint_msg.header.frame_id = 'odom' + footprint_msg.header.stamp = self.get_clock().now().to_msg() + footprint_msg.polygon.points = [ + Point32(x=0.1, y=0.1, z=0.0), + Point32(x=0.1, y=-0.1, z=0.0), + Point32(x=-0.1, y=-0.1, z=0.0), + Point32(x=-0.1, y=0.1, z=0.0) + ] + self.footprint_pub.publish(footprint_msg) + + def sendOccupiedCostmap(self): + costmap_msg = Costmap() + costmap_msg.header.frame_id = 'odom' + costmap_msg.header.stamp = self.get_clock().now().to_msg() + costmap_msg.metadata.resolution = 0.05 + costmap_msg.metadata.size_x = 100 + costmap_msg.metadata.size_y = 100 + costmap_msg.metadata.origin.position.x = -2.5 + costmap_msg.metadata.origin.position.y = -2.5 + costmap_msg.data = [254] * (costmap_msg.metadata.size_x * costmap_msg.metadata.size_y) + self.costmap_pub.publish(costmap_msg) + + footprint_msg = PolygonStamped() + footprint_msg.header.frame_id = 'odom' + footprint_msg.header.stamp = self.get_clock().now().to_msg() + footprint_msg.polygon.points = [ + Point32(x=0.1, y=0.1, z=0.0), + Point32(x=0.1, y=-0.1, z=0.0), + Point32(x=-0.1, y=-0.1, z=0.0), + Point32(x=-0.1, y=0.1, z=0.0) + ] + self.footprint_pub.publish(footprint_msg) + + def run(self): + while not self.action_client.wait_for_server(timeout_sec=1.0): + self.info_msg("'spin' action server not available, waiting...") + + # Test A: Send without valid costmap + action_request = Spin.Goal() + action_request.target_yaw = 0.349066 # 20 deg + action_request.time_allowance = Duration(seconds=5).to_msg() + cmd1 = self.sendCommand(action_request) + + if cmd1: + self.error_msg('Test A failed: Passed while costmap was not available!') + return not cmd1 + else: + self.info_msg('Test A passed') + + # Test B: Send with valid costmap and spin a couple of times + self.sendFreeCostmap() + time.sleep(1) + cmd1 = self.sendCommand(action_request) + action_request.target_yaw = -3.49066 # -200 deg + cmd2 = self.sendCommand(action_request) + + if not cmd1 or not cmd2: + self.error_msg('Test B failed: Failed to spin with valid costmap!') + return not cmd1 or not cmd2 + + action_request.target_yaw = 0.349066 # 20 deg + cmd_preempt = self.sendAndPreemptWithInvertedCommand(action_request) + if not cmd_preempt: + self.error_msg('Test B failed: Failed to preempt and invert command!') + return not cmd_preempt + + cmd_cancel = self.sendAndCancelCommand(action_request) + if not cmd_cancel: + self.error_msg('Test B failed: Failed to cancel command!') + return not cmd_cancel + else: + self.info_msg('Test B passed') + + # Test C: Send with impossible command in time allowance + action_request.time_allowance = Duration(seconds=0.1).to_msg() + cmd3 = self.sendCommand(action_request) + if cmd3: + self.error_msg('Test C failed: Passed while impoossible timing requested!') + return not cmd3 + else: + self.info_msg('Test C passed') + + # Test D: Send with lethal costmap and spin + action_request.time_allowance = Duration(seconds=5).to_msg() + self.sendOccupiedCostmap() + time.sleep(1) + cmd4 = self.sendCommand(action_request) + if cmd4: + self.error_msg('Test D failed: Passed while costmap was not lethal!') + return not cmd4 + else: + self.info_msg('Test D passed') + return True + + def shutdown(self): + self.info_msg('Shutting down') + + self.action_client.destroy() + self.info_msg('Destroyed follow_gps_waypoints action client') + + transition_service = 'lifecycle_manager_navigation/manage_nodes' + mgr_client = self.create_client(ManageLifecycleNodes, transition_service) + while not mgr_client.wait_for_service(timeout_sec=1.0): + self.info_msg(f'{transition_service} service not available, waiting...') + + req = ManageLifecycleNodes.Request() + req.command = ManageLifecycleNodes.Request().SHUTDOWN + future = mgr_client.call_async(req) + try: + rclpy.spin_until_future_complete(self, future) + future.result() + except Exception as e: # noqa: B902 + self.error_msg(f'{transition_service} service call failed {e!r}') + + self.info_msg(f'{transition_service} finished') + + def info_msg(self, msg: str): + self.get_logger().info(msg) + + def warn_msg(self, msg: str): + self.get_logger().warn(msg) + + def error_msg(self, msg: str): + self.get_logger().error(msg) + + +def main(argv=sys.argv[1:]): + rclpy.init() + time.sleep(10) + test = SpinTest() + result = test.run() + test.shutdown() + + if not result: + test.info_msg('Exiting failed') + exit(1) + else: + test.info_msg('Exiting passed') + exit(0) + + +if __name__ == '__main__': + main() diff --git a/nav2_system_tests/src/behaviors/spin/test_spin_behavior_launch.py b/nav2_system_tests/src/behaviors/spin/test_spin_behavior.launch.py similarity index 62% rename from nav2_system_tests/src/behaviors/spin/test_spin_behavior_launch.py rename to nav2_system_tests/src/behaviors/spin/test_spin_behavior.launch.py index 1a43845ccf..eecdca3736 100755 --- a/nav2_system_tests/src/behaviors/spin/test_spin_behavior_launch.py +++ b/nav2_system_tests/src/behaviors/spin/test_spin_behavior.launch.py @@ -23,51 +23,48 @@ from launch import LaunchService from launch.actions import ( AppendEnvironmentVariable, + DeclareLaunchArgument, ExecuteProcess, IncludeLaunchDescription, - SetEnvironmentVariable, -) -from launch.launch_description_sources import PythonLaunchDescriptionSource + SetEnvironmentVariable) +from launch.substitutions import LaunchConfiguration from launch_ros.actions import Node +from launch.launch_description_sources import PythonLaunchDescriptionSource from launch_testing.legacy import LaunchTestService - from nav2_common.launch import RewrittenYaml -from nav2_simple_commander.utils import kill_os_processes def generate_launch_description(): + bringup_dir = get_package_share_directory('nav2_bringup') 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') + params_file = LaunchConfiguration('params_file') + world_sdf_xacro = os.path.join(sim_dir, 'worlds', 'tb3_sandbox.sdf.xacro') robot_sdf = os.path.join(sim_dir, 'urdf', 'gz_waffle.sdf.xacro') - urdf = os.path.join(sim_dir, 'urdf', 'turtlebot3_waffle.urdf') with open(urdf, 'r') as infp: robot_description = infp.read() - map_yaml_file = os.path.join(nav2_bringup_dir, 'maps', 'tb3_sandbox.yaml') - - bt_navigator_xml = os.path.join( - get_package_share_directory('nav2_bt_navigator'), - 'behavior_trees', - os.getenv('BT_NAVIGATOR_XML'), - ) - - bringup_dir = get_package_share_directory('nav2_bringup') - params_file = os.path.join(bringup_dir, 'params/nav2_params.yaml') - - # Replace the `use_astar` setting on the params file + # Create our own temporary YAML files that include substitutions + param_substitutions = {'use_sim_time': 'True'} configured_params = RewrittenYaml( - source_file=params_file, root_key='', param_rewrites='', convert_types=True + source_file=params_file, + root_key='', + param_rewrites=param_substitutions, + convert_types=True, ) return LaunchDescription( [ SetEnvironmentVariable('RCUTILS_LOGGING_BUFFERED_STREAM', '1'), SetEnvironmentVariable('RCUTILS_LOGGING_USE_STDOUT', '1'), - # Launch gazebo server for simulation + DeclareLaunchArgument( + 'params_file', + default_value=os.path.join(bringup_dir, 'params', 'nav2_params.yaml'), + description='Full path to the ROS2 parameters file to use', + ), + # Simulation for odometry AppendEnvironmentVariable( 'GZ_SIM_RESOURCE_PATH', os.path.join(sim_dir, 'models') ), @@ -75,11 +72,9 @@ def generate_launch_description(): '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(), + ExecuteProcess( + cmd=['gz', 'sim', '-r', '-s', world_sdf_xacro], + output='screen', ), IncludeLaunchDescription( PythonLaunchDescriptionSource( @@ -96,6 +91,15 @@ def generate_launch_description(): 'yaw': '0.0', }.items(), ), + # No need for localization + Node( + package='tf2_ros', + executable='static_transform_publisher', + output='screen', + arguments=['0', '0', '0', '0', '0', '0', 'map', 'odom'], + parameters=[{'use_sim_time': True}], + ), + # Need transforms Node( package='robot_state_publisher', executable='robot_state_publisher', @@ -105,18 +109,24 @@ def generate_launch_description(): {'use_sim_time': True, 'robot_description': robot_description} ], ), - IncludeLaunchDescription( - PythonLaunchDescriptionSource( - os.path.join(bringup_dir, 'launch', 'bringup_launch.py') - ), - launch_arguments={ - 'map': map_yaml_file, - 'use_sim_time': 'True', - 'params_file': configured_params, - 'bt_xml_file': bt_navigator_xml, - 'use_composition': 'False', - 'autostart': 'True', - }.items(), + # Server under test + Node( + package='nav2_behaviors', + executable='behavior_server', + name='behavior_server', + output='screen', + parameters=[configured_params], + ), + Node( + package='nav2_lifecycle_manager', + executable='lifecycle_manager', + name='lifecycle_manager_navigation', + output='screen', + parameters=[ + {'use_sim_time': True}, + {'autostart': True}, + {'node_names': ['behavior_server']}, + ], ), ] ) @@ -125,19 +135,17 @@ def generate_launch_description(): def main(argv=sys.argv[1:]): ld = generate_launch_description() - testExecutable = os.getenv('TEST_EXECUTABLE') - test1_action = ExecuteProcess( - cmd=[testExecutable], name='test_spin_behavior_node', output='screen', + cmd=[os.path.join(os.getenv('TEST_DIR'), 'spin_tester.py'), '--ros-args', '-p', 'use_sim_time:=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) - kill_os_processes('gz sim') - return return_code + return lts.run(ls) if __name__ == '__main__': diff --git a/nav2_system_tests/src/behaviors/spin/test_spin_behavior_fake_launch.py b/nav2_system_tests/src/behaviors/spin/test_spin_behavior_fake_launch.py deleted file mode 100755 index d18b29d5b9..0000000000 --- a/nav2_system_tests/src/behaviors/spin/test_spin_behavior_fake_launch.py +++ /dev/null @@ -1,175 +0,0 @@ -#! /usr/bin/env python3 -# Copyright (c) 2019 Samsung Research America -# -# 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 -import sys - -from ament_index_python.packages import get_package_share_directory - -from launch import LaunchDescription -from launch import LaunchService -from launch.actions import DeclareLaunchArgument, ExecuteProcess, SetEnvironmentVariable -from launch.substitutions import LaunchConfiguration -from launch_ros.actions import Node -from launch_testing.legacy import LaunchTestService -from nav2_common.launch import RewrittenYaml - - -def generate_launch_description(): - - bringup_dir = get_package_share_directory('nav2_bringup') - sim_dir = get_package_share_directory('nav2_minimal_tb3_sim') - - urdf = os.path.join(sim_dir, 'urdf', 'turtlebot3_waffle.urdf') - with open(urdf, 'r') as infp: - robot_description = infp.read() - - namespace = LaunchConfiguration('namespace') - use_sim_time = LaunchConfiguration('use_sim_time') - autostart = LaunchConfiguration('autostart') - params_file = LaunchConfiguration('params_file') - default_nav_through_poses_bt_xml = LaunchConfiguration( - 'default_nav_through_poses_bt_xml' - ) - default_nav_to_pose_bt_xml = LaunchConfiguration('default_nav_to_pose_bt_xml') - map_subscribe_transient_local = LaunchConfiguration('map_subscribe_transient_local') - - # Create our own temporary YAML files that include substitutions - param_substitutions = { - 'use_sim_time': use_sim_time, - 'default_nav_through_poses_bt_xml': default_nav_through_poses_bt_xml, - 'default_nav_to_pose_bt_xml': default_nav_to_pose_bt_xml, - 'autostart': autostart, - 'map_subscribe_transient_local': map_subscribe_transient_local, - } - - configured_params = RewrittenYaml( - source_file=params_file, - root_key=namespace, - param_rewrites=param_substitutions, - convert_types=True, - ) - - lifecycle_nodes = ['behavior_server'] - - # Map fully qualified names to relative ones so the node's namespace can be prepended. - # In case of the transforms (tf), currently, there doesn't seem to be a better alternative - # https://github.com/ros/geometry2/issues/32 - # https://github.com/ros/robot_state_publisher/pull/30 - # TODO(orduno) Substitute with `PushNodeRemapping` - # https://github.com/ros2/launch_ros/issues/56 - remappings = [('/tf', 'tf'), ('/tf_static', 'tf_static')] - - return LaunchDescription( - [ - SetEnvironmentVariable('RCUTILS_LOGGING_BUFFERED_STREAM', '1'), - SetEnvironmentVariable('RCUTILS_LOGGING_USE_STDOUT', '1'), - DeclareLaunchArgument( - 'namespace', default_value='', description='Top-level namespace' - ), - DeclareLaunchArgument( - 'use_sim_time', - default_value='false', - description='Use simulation (Gazebo) clock if true', - ), - DeclareLaunchArgument( - 'autostart', - default_value='true', - description='Automatically startup the nav2 stack', - ), - DeclareLaunchArgument( - 'params_file', - default_value=os.path.join(bringup_dir, 'params', 'nav2_params.yaml'), - description='Full path to the ROS2 parameters file to use', - ), - DeclareLaunchArgument( - 'default_nav_through_poses_bt_xml', - default_value=os.path.join( - get_package_share_directory('nav2_bt_navigator'), - 'behavior_trees', - 'navigate_through_poses_w_replanning_and_recovery.xml', - ), - description='Full path to the behavior tree xml file to use', - ), - DeclareLaunchArgument( - 'default_nav_to_pose_bt_xml', - default_value=os.path.join( - get_package_share_directory('nav2_bt_navigator'), - 'behavior_trees', - 'navigate_to_pose_w_replanning_and_recovery.xml', - ), - description='Full path to the behavior tree xml file to use', - ), - DeclareLaunchArgument( - 'map_subscribe_transient_local', - default_value='false', - description='Whether to set the map subscriber QoS to transient local', - ), - Node( - package='tf2_ros', - executable='static_transform_publisher', - output='screen', - arguments=['0', '0', '0', '0', '0', '0', 'map', 'odom'], - ), - Node( - package='robot_state_publisher', - executable='robot_state_publisher', - name='robot_state_publisher', - output='screen', - parameters=[ - {'use_sim_time': True, 'robot_description': robot_description} - ], - ), - Node( - package='nav2_behaviors', - executable='behavior_server', - name='behavior_server', - output='screen', - parameters=[configured_params], - remappings=remappings, - ), - Node( - package='nav2_lifecycle_manager', - executable='lifecycle_manager', - name='lifecycle_manager_navigation', - output='screen', - parameters=[ - {'use_sim_time': use_sim_time}, - {'autostart': autostart}, - {'node_names': lifecycle_nodes}, - ], - ), - ] - ) - - -def main(argv=sys.argv[1:]): - ld = generate_launch_description() - - testExecutable = os.getenv('TEST_EXECUTABLE') - - test1_action = ExecuteProcess( - cmd=[testExecutable], name='test_spin_behavior_fake_node', output='screen' - ) - - lts = LaunchTestService() - lts.add_test_action(ld, test1_action) - ls = LaunchService(argv=argv) - ls.include_launch_description(ld) - return lts.run(ls) - - -if __name__ == '__main__': - sys.exit(main()) diff --git a/nav2_system_tests/src/behaviors/spin/test_spin_behavior_node.cpp b/nav2_system_tests/src/behaviors/spin/test_spin_behavior_node.cpp deleted file mode 100644 index cf0f97021d..0000000000 --- a/nav2_system_tests/src/behaviors/spin/test_spin_behavior_node.cpp +++ /dev/null @@ -1,143 +0,0 @@ -// Copyright (c) 2020 Samsung Research -// Copyright (c) 2020 Sarthak Mittal -// -// 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. Reserved. - -#include -#include -#include -#include - -#include "rclcpp/rclcpp.hpp" - -#include "spin_behavior_tester.hpp" - -using namespace std::chrono_literals; - -using nav2_system_tests::SpinBehaviorTester; - -std::string testNameGenerator(const testing::TestParamInfo> & param) -{ - std::string name = std::to_string(std::abs(std::get<0>(param.param))) + "_" + std::to_string( - std::get<1>(param.param)); - name.erase(std::remove(name.begin(), name.end(), '.'), name.end()); - return name; -} - -class SpinBehaviorTestFixture - : public ::testing::TestWithParam> -{ -public: - static void SetUpTestCase() - { - spin_recovery_tester = new SpinBehaviorTester(); - if (!spin_recovery_tester->isActive()) { - spin_recovery_tester->activate(); - } - } - - static void TearDownTestCase() - { - delete spin_recovery_tester; - spin_recovery_tester = nullptr; - } - -protected: - static SpinBehaviorTester * spin_recovery_tester; -}; - -SpinBehaviorTester * SpinBehaviorTestFixture::spin_recovery_tester = nullptr; - -// TEST_P(SpinBehaviorTestFixture, testSpinRecovery) -// { -// float target_yaw = std::get<0>(GetParam()); -// float tolerance = std::get<1>(GetParam()); - -// bool success = false; -// int num_tries = 3; -// for (int i = 0; i != num_tries; i++) { -// success = success || spin_recovery_tester->defaultSpinBehaviorTest(target_yaw, tolerance); -// if (success) { -// break; -// } -// } -// if (std::getenv("MAKE_FAKE_COSTMAP") != NULL && abs(target_yaw) > M_PI_2f32) { -// // if this variable is set, make a fake costmap -// // in the fake spin test, we expect a collision for angles > M_PI_2 -// EXPECT_EQ(false, success); -// } else { -// EXPECT_EQ(true, success); -// } -// } - -TEST_F(SpinBehaviorTestFixture, testSpinPreemption) -{ - // Goal - float target_yaw = 3.0 * M_PIf32; - float tolerance = 0.1; - bool nonblocking_action = false; - bool success = false; - - // Send the first goal - success = spin_recovery_tester->defaultSpinBehaviorTest( - target_yaw, tolerance, - nonblocking_action); - EXPECT_EQ(true, success); - // Preempt goal - sleep(2); - success = false; - float prempt_target_yaw = 4.0 * M_PIf32; - float preempt_tolerance = 0.1; - success = spin_recovery_tester->defaultSpinBehaviorTest(prempt_target_yaw, preempt_tolerance); - EXPECT_EQ(false, success); -} - -TEST_F(SpinBehaviorTestFixture, testSpinCancel) -{ - // Goal - float target_yaw = 4.0 * M_PIf32; - float tolerance = 0.1; - bool nonblocking_action = true, cancel_action = true, success = false; - success = spin_recovery_tester->defaultSpinBehaviorTest( - target_yaw, tolerance, - nonblocking_action, cancel_action); - EXPECT_EQ(false, success); -} - -// INSTANTIATE_TEST_SUITE_P( -// SpinRecoveryTests, -// SpinBehaviorTestFixture, -// ::testing::Values( -// std::make_tuple(-M_PIf32 / 6.0, 0.1), -// // std::make_tuple(M_PI_4f32, 0.1), -// // std::make_tuple(-M_PI_2f32, 0.1), -// std::make_tuple(M_PIf32, 0.1), -// std::make_tuple(3.0 * M_PIf32 / 2.0, 0.15), -// std::make_tuple(-2.0 * M_PIf32, 0.1), -// std::make_tuple(4.0 * M_PIf32, 0.15)), -// testNameGenerator); - -int main(int argc, char ** argv) -{ - ::testing::InitGoogleTest(&argc, argv); - - // initialize ROS - rclcpp::init(argc, argv); - - bool all_successful = RUN_ALL_TESTS(); - - // shutdown ROS - rclcpp::shutdown(); - - return all_successful; -} From b3cd7f566afe119f61961c9557257e05fd494f63 Mon Sep 17 00:00:00 2001 From: Steve Macenski Date: Wed, 3 Jul 2024 15:50:30 -0700 Subject: [PATCH 4/5] lint --- nav2_system_tests/src/behaviors/spin/spin_tester.py | 11 +++++++---- .../src/behaviors/spin/test_spin_behavior.launch.py | 5 +++-- 2 files changed, 10 insertions(+), 6 deletions(-) diff --git a/nav2_system_tests/src/behaviors/spin/spin_tester.py b/nav2_system_tests/src/behaviors/spin/spin_tester.py index f1d92ebaa6..9f50ff11a7 100755 --- a/nav2_system_tests/src/behaviors/spin/spin_tester.py +++ b/nav2_system_tests/src/behaviors/spin/spin_tester.py @@ -17,21 +17,22 @@ import time from action_msgs.msg import GoalStatus +from geometry_msgs.msg import Point32, PolygonStamped from nav2_msgs.action import Spin from nav2_msgs.msg import Costmap from nav2_msgs.srv import ManageLifecycleNodes -from geometry_msgs.msg import Point32, PolygonStamped import rclpy from rclpy.action import ActionClient -from rclpy.node import Node from rclpy.duration import Duration +from rclpy.node import Node from rclpy.qos import QoSDurabilityPolicy, QoSHistoryPolicy, QoSReliabilityPolicy from rclpy.qos import QoSProfile class SpinTest(Node): + def __init__(self): super().__init__(node_name='spin_tester', namespace='') self.costmap_qos = QoSProfile( @@ -41,8 +42,10 @@ def __init__(self): depth=1, ) self.action_client = ActionClient(self, Spin, 'spin') - self.costmap_pub = self.create_publisher(Costmap, 'local_costmap/costmap_raw', self.costmap_qos) - self.footprint_pub = self.create_publisher(PolygonStamped, 'local_costmap/published_footprint', 10) + self.costmap_pub = self.create_publisher( + Costmap, 'local_costmap/costmap_raw', self.costmap_qos) + self.footprint_pub = self.create_publisher( + PolygonStamped, 'local_costmap/published_footprint', 10) self.goal_handle = None self.action_result = None diff --git a/nav2_system_tests/src/behaviors/spin/test_spin_behavior.launch.py b/nav2_system_tests/src/behaviors/spin/test_spin_behavior.launch.py index a251463beb..cc9a9efa9b 100755 --- a/nav2_system_tests/src/behaviors/spin/test_spin_behavior.launch.py +++ b/nav2_system_tests/src/behaviors/spin/test_spin_behavior.launch.py @@ -27,9 +27,9 @@ ExecuteProcess, IncludeLaunchDescription, SetEnvironmentVariable) +from launch.launch_description_sources import PythonLaunchDescriptionSource from launch.substitutions import LaunchConfiguration from launch_ros.actions import Node -from launch.launch_description_sources import PythonLaunchDescriptionSource from launch_testing.legacy import LaunchTestService from nav2_common.launch import RewrittenYaml @@ -134,7 +134,8 @@ def main(argv=sys.argv[1:]): ld = generate_launch_description() test1_action = ExecuteProcess( - cmd=[os.path.join(os.getenv('TEST_DIR'), 'spin_tester.py'), '--ros-args', '-p', 'use_sim_time:=True'], + cmd=[os.path.join( + os.getenv('TEST_DIR'), 'spin_tester.py'), '--ros-args', '-p', 'use_sim_time:=True'], name='tester_node', output='screen', ) From 3f06a5c5b413194ed95e0b7504969c2648829b6e Mon Sep 17 00:00:00 2001 From: Steve Macenski Date: Wed, 3 Jul 2024 16:18:08 -0700 Subject: [PATCH 5/5] complete flaky test rewrite --- .../src/behaviors/backup/CMakeLists.txt | 20 +- .../backup/backup_behavior_tester.cpp | 223 ------------ .../backup/backup_behavior_tester.hpp | 90 ----- .../src/behaviors/backup/backup_tester.py | 332 +++++++++++++++++ ...unch.py => test_backup_behavior.launch.py} | 95 ++--- .../backup/test_backup_behavior_node.cpp | 128 ------- .../behaviors/drive_on_heading/CMakeLists.txt | 20 +- .../drive_on_heading_behavior_tester.cpp | 224 ------------ .../drive_on_heading_behavior_tester.hpp | 90 ----- .../drive_on_heading/drive_tester.py | 340 ++++++++++++++++++ ... test_drive_on_heading_behavior.launch.py} | 93 ++--- .../test_drive_on_heading_behavior_node.cpp | 133 ------- .../src/behaviors/spin/spin_tester.py | 2 +- 13 files changed, 784 insertions(+), 1006 deletions(-) delete mode 100644 nav2_system_tests/src/behaviors/backup/backup_behavior_tester.cpp delete mode 100644 nav2_system_tests/src/behaviors/backup/backup_behavior_tester.hpp create mode 100755 nav2_system_tests/src/behaviors/backup/backup_tester.py rename nav2_system_tests/src/behaviors/backup/{test_backup_behavior_launch.py => test_backup_behavior.launch.py} (60%) delete mode 100644 nav2_system_tests/src/behaviors/backup/test_backup_behavior_node.cpp delete mode 100644 nav2_system_tests/src/behaviors/drive_on_heading/drive_on_heading_behavior_tester.cpp delete mode 100644 nav2_system_tests/src/behaviors/drive_on_heading/drive_on_heading_behavior_tester.hpp create mode 100755 nav2_system_tests/src/behaviors/drive_on_heading/drive_tester.py rename nav2_system_tests/src/behaviors/drive_on_heading/{test_drive_on_heading_behavior_launch.py => test_drive_on_heading_behavior.launch.py} (61%) delete mode 100644 nav2_system_tests/src/behaviors/drive_on_heading/test_drive_on_heading_behavior_node.cpp diff --git a/nav2_system_tests/src/behaviors/backup/CMakeLists.txt b/nav2_system_tests/src/behaviors/backup/CMakeLists.txt index ccbc70458a..b5ab0a60b1 100644 --- a/nav2_system_tests/src/behaviors/backup/CMakeLists.txt +++ b/nav2_system_tests/src/behaviors/backup/CMakeLists.txt @@ -1,20 +1,8 @@ -set(test_backup_behavior test_backup_behavior_node) - -ament_add_gtest_executable(${test_backup_behavior} - test_backup_behavior_node.cpp - backup_behavior_tester.cpp -) - -ament_target_dependencies(${test_backup_behavior} - ${dependencies} -) - -ament_add_test(test_backup_recovery +ament_add_test(test_backup_behavior GENERATE_RESULT_FOR_RETURN_CODE_ZERO - COMMAND "${CMAKE_CURRENT_SOURCE_DIR}/test_backup_behavior_launch.py" + COMMAND "${CMAKE_CURRENT_SOURCE_DIR}/test_backup_behavior.launch.py" WORKING_DIRECTORY "${CMAKE_CURRENT_BINARY_DIR}" - TIMEOUT 180 + TIMEOUT 60 ENV - TEST_EXECUTABLE=$ - BT_NAVIGATOR_XML=navigate_to_pose_w_replanning_and_recovery.xml + TEST_DIR=${CMAKE_CURRENT_SOURCE_DIR} ) diff --git a/nav2_system_tests/src/behaviors/backup/backup_behavior_tester.cpp b/nav2_system_tests/src/behaviors/backup/backup_behavior_tester.cpp deleted file mode 100644 index 974c2d60b2..0000000000 --- a/nav2_system_tests/src/behaviors/backup/backup_behavior_tester.cpp +++ /dev/null @@ -1,223 +0,0 @@ -// Copyright (c) 2020 Sarthak Mittal -// Copyright (c) 2018 Intel Corporation -// -// 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. Reserved. - -#include -#include -#include -#include -#include -#include -#include -#include - -#include "backup_behavior_tester.hpp" -#include "nav2_util/geometry_utils.hpp" - -using namespace std::chrono_literals; -using namespace std::chrono; // NOLINT - -namespace nav2_system_tests -{ - -BackupBehaviorTester::BackupBehaviorTester() -: is_active_(false), - initial_pose_received_(false) -{ - rclcpp::NodeOptions options; - options.parameter_overrides({{"use_sim_time", true}}); - node_ = rclcpp::Node::make_shared("backup_behavior_test", options); - - tf_buffer_ = std::make_shared(node_->get_clock()); - tf_listener_ = std::make_shared(*tf_buffer_); - - client_ptr_ = rclcpp_action::create_client( - node_->get_node_base_interface(), - node_->get_node_graph_interface(), - node_->get_node_logging_interface(), - node_->get_node_waitables_interface(), - "backup"); - - publisher_ = - node_->create_publisher("initialpose", 10); - - subscription_ = node_->create_subscription( - "amcl_pose", rclcpp::QoS(rclcpp::KeepLast(1)).transient_local().reliable(), - std::bind(&BackupBehaviorTester::amclPoseCallback, this, std::placeholders::_1)); - - stamp_ = node_->now(); -} - -BackupBehaviorTester::~BackupBehaviorTester() -{ - if (is_active_) { - deactivate(); - } -} - -void BackupBehaviorTester::activate() -{ - if (is_active_) { - throw std::runtime_error("Trying to activate while already active"); - return; - } - - while (!initial_pose_received_) { - RCLCPP_WARN(node_->get_logger(), "Initial pose not received"); - sendInitialPose(); - std::this_thread::sleep_for(100ms); - rclcpp::spin_some(node_); - } - - // Wait for lifecycle_manager_navigation to activate behavior_server - std::this_thread::sleep_for(10s); - - if (!client_ptr_) { - RCLCPP_ERROR(node_->get_logger(), "Action client not initialized"); - is_active_ = false; - return; - } - - if (!client_ptr_->wait_for_action_server(10s)) { - RCLCPP_ERROR(node_->get_logger(), "Action server not available after waiting"); - is_active_ = false; - return; - } - - RCLCPP_INFO(this->node_->get_logger(), "Backup action server is ready"); - is_active_ = true; -} - -void BackupBehaviorTester::deactivate() -{ - if (!is_active_) { - throw std::runtime_error("Trying to deactivate while already inactive"); - } - is_active_ = false; -} - -bool BackupBehaviorTester::defaultBackupBehaviorTest( - const BackUp::Goal goal_msg, - const double tolerance) -{ - if (!is_active_) { - RCLCPP_ERROR(node_->get_logger(), "Not activated"); - return false; - } - - // Sleep to let behavior server be ready for serving in multiple runs - std::this_thread::sleep_for(5s); - - RCLCPP_INFO(this->node_->get_logger(), "Sending goal"); - - geometry_msgs::msg::PoseStamped initial_pose; - if (!nav2_util::getCurrentPose(initial_pose, *tf_buffer_, "odom")) { - RCLCPP_ERROR(node_->get_logger(), "Current robot pose is not available."); - return false; - } - RCLCPP_INFO(node_->get_logger(), "Found current robot pose"); - - auto goal_handle_future = client_ptr_->async_send_goal(goal_msg); - - if (rclcpp::spin_until_future_complete(node_, goal_handle_future) != - rclcpp::FutureReturnCode::SUCCESS) - { - RCLCPP_ERROR(node_->get_logger(), "send goal call failed :("); - return false; - } - - rclcpp_action::ClientGoalHandle::SharedPtr goal_handle = goal_handle_future.get(); - if (!goal_handle) { - RCLCPP_ERROR(node_->get_logger(), "Goal was rejected by server"); - return false; - } - - // Wait for the server to be done with the goal - auto result_future = client_ptr_->async_get_result(goal_handle); - - RCLCPP_INFO(node_->get_logger(), "Waiting for result"); - if (rclcpp::spin_until_future_complete(node_, result_future) != - rclcpp::FutureReturnCode::SUCCESS) - { - RCLCPP_ERROR(node_->get_logger(), "get result call failed :("); - return false; - } - - rclcpp_action::ClientGoalHandle::WrappedResult wrapped_result = result_future.get(); - - switch (wrapped_result.code) { - case rclcpp_action::ResultCode::SUCCEEDED: break; - case rclcpp_action::ResultCode::ABORTED: RCLCPP_ERROR( - node_->get_logger(), - "Goal was aborted"); - return false; - case rclcpp_action::ResultCode::CANCELED: RCLCPP_ERROR( - node_->get_logger(), - "Goal was canceled"); - return false; - default: RCLCPP_ERROR(node_->get_logger(), "Unknown result code"); - return false; - } - - RCLCPP_INFO(node_->get_logger(), "result received"); - - geometry_msgs::msg::PoseStamped current_pose; - if (!nav2_util::getCurrentPose(current_pose, *tf_buffer_, "odom")) { - RCLCPP_ERROR(node_->get_logger(), "Current robot pose is not available."); - return false; - } - - double dist = nav2_util::geometry_utils::euclidean_distance(initial_pose, current_pose); - - if (fabs(dist) > fabs(goal_msg.target.x) + tolerance) { - RCLCPP_ERROR( - node_->get_logger(), - "Distance from goal is %lf (tolerance %lf)", - fabs(dist - goal_msg.target.x), tolerance); - return false; - } - - return true; -} - -void BackupBehaviorTester::sendInitialPose() -{ - geometry_msgs::msg::PoseWithCovarianceStamped pose; - pose.header.frame_id = "map"; - pose.header.stamp = stamp_; - pose.pose.pose.position.x = -2.0; - pose.pose.pose.position.y = -0.5; - pose.pose.pose.position.z = 0.0; - pose.pose.pose.orientation.x = 0.0; - pose.pose.pose.orientation.y = 0.0; - pose.pose.pose.orientation.z = 0.0; - pose.pose.pose.orientation.w = 1.0; - for (int i = 0; i < 35; i++) { - pose.pose.covariance[i] = 0.0; - } - pose.pose.covariance[0] = 0.08; - pose.pose.covariance[7] = 0.08; - pose.pose.covariance[35] = 0.05; - - publisher_->publish(pose); - RCLCPP_INFO(node_->get_logger(), "Sent initial pose"); -} - -void BackupBehaviorTester::amclPoseCallback( - const geometry_msgs::msg::PoseWithCovarianceStamped::SharedPtr) -{ - initial_pose_received_ = true; -} - -} // namespace nav2_system_tests diff --git a/nav2_system_tests/src/behaviors/backup/backup_behavior_tester.hpp b/nav2_system_tests/src/behaviors/backup/backup_behavior_tester.hpp deleted file mode 100644 index bf42c9881a..0000000000 --- a/nav2_system_tests/src/behaviors/backup/backup_behavior_tester.hpp +++ /dev/null @@ -1,90 +0,0 @@ -// Copyright (c) 2020 Samsung Research -// Copyright (c) 2018 Intel Corporation -// -// 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. Reserved. - -#ifndef BEHAVIORS__BACKUP__BACKUP_BEHAVIOR_TESTER_HPP_ -#define BEHAVIORS__BACKUP__BACKUP_BEHAVIOR_TESTER_HPP_ - -#include -#include -#include -#include -#include - -#include "rclcpp/rclcpp.hpp" -#include "rclcpp_action/rclcpp_action.hpp" -#include "angles/angles.h" -#include "nav2_msgs/action/back_up.hpp" -#include "nav2_util/robot_utils.hpp" -#include "nav2_util/node_thread.hpp" -#include "geometry_msgs/msg/pose_stamped.hpp" -#include "geometry_msgs/msg/pose_with_covariance_stamped.hpp" - -#include "tf2/utils.h" -#include "tf2_ros/buffer.h" -#include "tf2_ros/transform_listener.h" - -namespace nav2_system_tests -{ - -class BackupBehaviorTester -{ -public: - using BackUp = nav2_msgs::action::BackUp; - using GoalHandleBackup = rclcpp_action::ClientGoalHandle; - - BackupBehaviorTester(); - ~BackupBehaviorTester(); - - // Runs a single test with given target yaw - bool defaultBackupBehaviorTest( - const BackUp::Goal goal_msg, - const double tolerance); - - void activate(); - - void deactivate(); - - bool isActive() const - { - return is_active_; - } - -private: - void sendInitialPose(); - - void amclPoseCallback(geometry_msgs::msg::PoseWithCovarianceStamped::SharedPtr); - - bool is_active_; - bool initial_pose_received_; - rclcpp::Time stamp_; - - std::shared_ptr tf_buffer_; - std::shared_ptr tf_listener_; - - rclcpp::Node::SharedPtr node_; - - // Publisher to publish initial pose - rclcpp::Publisher::SharedPtr publisher_; - - // Subscriber for amcl pose - rclcpp::Subscription::SharedPtr subscription_; - - // Action client to call Backup action - rclcpp_action::Client::SharedPtr client_ptr_; -}; - -} // namespace nav2_system_tests - -#endif // BEHAVIORS__BACKUP__BACKUP_BEHAVIOR_TESTER_HPP_ diff --git a/nav2_system_tests/src/behaviors/backup/backup_tester.py b/nav2_system_tests/src/behaviors/backup/backup_tester.py new file mode 100755 index 0000000000..ad6f724bf8 --- /dev/null +++ b/nav2_system_tests/src/behaviors/backup/backup_tester.py @@ -0,0 +1,332 @@ +#! /usr/bin/env python3 +# Copyright 2024 Open Navigation LLC +# +# 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 sys +import time + +from action_msgs.msg import GoalStatus +from geometry_msgs.msg import Point32, PolygonStamped +from nav2_msgs.action import BackUp +from nav2_msgs.msg import Costmap +from nav2_msgs.srv import ManageLifecycleNodes + +import rclpy + +from rclpy.action import ActionClient +from rclpy.duration import Duration +from rclpy.node import Node +from rclpy.qos import QoSDurabilityPolicy, QoSHistoryPolicy, QoSReliabilityPolicy +from rclpy.qos import QoSProfile + + +class BackupTest(Node): + + def __init__(self): + super().__init__(node_name='backup_tester', namespace='') + self.costmap_qos = QoSProfile( + durability=QoSDurabilityPolicy.TRANSIENT_LOCAL, + reliability=QoSReliabilityPolicy.RELIABLE, + history=QoSHistoryPolicy.KEEP_LAST, + depth=1, + ) + self.action_client = ActionClient(self, BackUp, 'backup') + self.costmap_pub = self.create_publisher( + Costmap, 'local_costmap/costmap_raw', self.costmap_qos) + self.footprint_pub = self.create_publisher( + PolygonStamped, 'local_costmap/published_footprint', 10) + self.goal_handle = None + self.action_result = None + + def sendCommand(self, command): + self.info_msg('Sending goal request...') + self.goal_future = self.action_client.send_goal_async(command) + try: + rclpy.spin_until_future_complete(self, self.goal_future) + self.goal_handle = self.goal_future.result() + except Exception as e: # noqa: B902 + self.error_msg(f'Service call failed {e!r}') + + if not self.goal_handle.accepted: + self.error_msg('Goal rejected') + return False + + self.info_msg('Goal accepted') + self.result_future = self.goal_handle.get_result_async() + + self.info_msg("Waiting for 'Backup' action to complete") + try: + rclpy.spin_until_future_complete(self, self.result_future) + status = self.result_future.result().status + result = self.result_future.result().result + self.action_result = result + except Exception as e: # noqa: B902 + self.error_msg(f'Service call failed {e!r}') + + if status != GoalStatus.STATUS_SUCCEEDED: + self.info_msg(f'Goal failed with status code: {status}') + return False + if self.action_result.error_code == 0: + self.info_msg('Backup was successful!') + return True + self.info_msg('Backup failed to meet target!') + return False + + def sendAndPreemptWithFasterCommand(self, command): + # Send initial goal + self.info_msg('Sending goal request...') + self.goal_future = self.action_client.send_goal_async(command) + try: + rclpy.spin_until_future_complete(self, self.goal_future) + self.goal_handle = self.goal_future.result() + except Exception as e: # noqa: B902 + self.error_msg(f'Service call failed {e!r}') + + if not self.goal_handle.accepted: + self.error_msg('Goal rejected') + return False + + self.info_msg('Goal accepted') + self.result_future = self.goal_handle.get_result_async() + + # Now preempt it + time.sleep(0.5) + self.info_msg('Sending preemption request...') + command.speed = 0.2 + self.goal_future = self.action_client.send_goal_async(command) + try: + rclpy.spin_until_future_complete(self, self.goal_future) + self.goal_handle = self.goal_future.result() + except Exception as e: # noqa: B902 + self.error_msg(f'Service call failed {e!r}') + + if not self.goal_handle.accepted: + self.error_msg('Preemption rejected') + return False + + self.info_msg('Preemption accepted') + self.result_future = self.goal_handle.get_result_async() + + # Wait for new goal to complete + self.info_msg("Waiting for 'backup' action Preemption to complete") + try: + rclpy.spin_until_future_complete(self, self.result_future) + status = self.result_future.result().status + result = self.result_future.result().result + self.action_result = result + except Exception as e: # noqa: B902 + self.error_msg(f'Service call failed {e!r}') + + if status != GoalStatus.STATUS_SUCCEEDED: + self.info_msg(f'Goal failed with status code: {status}') + return False + if self.action_result.error_code == 0: + self.info_msg('Backup was successful!') + return True + self.info_msg('Backup failed to meet target!') + return False + + def sendAndCancelCommand(self, command): + self.info_msg('Sending goal request...') + self.goal_future = self.action_client.send_goal_async(command) + try: + rclpy.spin_until_future_complete(self, self.goal_future) + self.goal_handle = self.goal_future.result() + except Exception as e: # noqa: B902 + self.error_msg(f'Service call failed {e!r}') + + if not self.goal_handle.accepted: + self.error_msg('Goal rejected') + return False + + self.info_msg('Goal accepted') + self.result_future = self.goal_handle.get_result_async() + + # Now cancel it + time.sleep(0.5) + cancel_future = self.goal_handle.cancel_goal_async() + rclpy.spin_until_future_complete(self, cancel_future) + rclpy.spin_until_future_complete(self, self.result_future) + status = self.result_future.result().status + if status != GoalStatus.STATUS_CANCELED: + self.info_msg(f'Goal failed with status code: {status}') + return False + else: + self.info_msg('Goal was canceled successfully') + return True + + def sendFreeCostmap(self): + costmap_msg = Costmap() + costmap_msg.header.frame_id = 'odom' + costmap_msg.header.stamp = self.get_clock().now().to_msg() + costmap_msg.metadata.resolution = 0.05 + costmap_msg.metadata.size_x = 100 + costmap_msg.metadata.size_y = 100 + costmap_msg.metadata.origin.position.x = -2.5 + costmap_msg.metadata.origin.position.y = -2.5 + costmap_msg.data = [0] * (costmap_msg.metadata.size_x * costmap_msg.metadata.size_y) + self.costmap_pub.publish(costmap_msg) + + footprint_msg = PolygonStamped() + footprint_msg.header.frame_id = 'odom' + footprint_msg.header.stamp = self.get_clock().now().to_msg() + footprint_msg.polygon.points = [ + Point32(x=0.1, y=0.1, z=0.0), + Point32(x=0.1, y=-0.1, z=0.0), + Point32(x=-0.1, y=-0.1, z=0.0), + Point32(x=-0.1, y=0.1, z=0.0) + ] + self.footprint_pub.publish(footprint_msg) + + def sendOccupiedCostmap(self): + costmap_msg = Costmap() + costmap_msg.header.frame_id = 'odom' + costmap_msg.header.stamp = self.get_clock().now().to_msg() + costmap_msg.metadata.resolution = 0.05 + costmap_msg.metadata.size_x = 100 + costmap_msg.metadata.size_y = 100 + costmap_msg.metadata.origin.position.x = -2.5 + costmap_msg.metadata.origin.position.y = -2.5 + costmap_msg.data = [254] * (costmap_msg.metadata.size_x * costmap_msg.metadata.size_y) + self.costmap_pub.publish(costmap_msg) + + footprint_msg = PolygonStamped() + footprint_msg.header.frame_id = 'odom' + footprint_msg.header.stamp = self.get_clock().now().to_msg() + footprint_msg.polygon.points = [ + Point32(x=0.1, y=0.1, z=0.0), + Point32(x=0.1, y=-0.1, z=0.0), + Point32(x=-0.1, y=-0.1, z=0.0), + Point32(x=-0.1, y=0.1, z=0.0) + ] + self.footprint_pub.publish(footprint_msg) + + def run(self): + while not self.action_client.wait_for_server(timeout_sec=1.0): + self.info_msg("'Backup' action server not available, waiting...") + + # Test A: Send without valid costmap + action_request = BackUp.Goal() + action_request.speed = 0.15 + action_request.target.x = 0.15 + action_request.time_allowance = Duration(seconds=5).to_msg() + cmd1 = self.sendCommand(action_request) + + if cmd1: + self.error_msg('Test A failed: Passed while costmap was not available!') + return not cmd1 + else: + self.info_msg('Test A passed') + + # Test B: Send with valid costmap and Backup a couple of times + self.sendFreeCostmap() + time.sleep(1) + cmd1 = self.sendCommand(action_request) + action_request.target.x = 0.1 + cmd2 = self.sendCommand(action_request) + + if not cmd1 or not cmd2: + self.error_msg('Test B failed: Failed to Backup with valid costmap!') + return not cmd1 or not cmd2 + + action_request.target.x = 0.5 + cmd_preempt = self.sendAndPreemptWithFasterCommand(action_request) + if not cmd_preempt: + self.error_msg('Test B failed: Failed to preempt and invert command!') + return not cmd_preempt + + cmd_cancel = self.sendAndCancelCommand(action_request) + if not cmd_cancel: + self.error_msg('Test B failed: Failed to cancel command!') + return not cmd_cancel + else: + self.info_msg('Test B passed') + + # Test C: Send with impossible command in time allowance & target + action_request.time_allowance = Duration(seconds=0.1).to_msg() + cmd3 = self.sendCommand(action_request) + if cmd3: + self.error_msg('Test C failed: Passed while impoossible timing requested!') + return not cmd3 + + action_request.target.y = 0.5 + cmd_invalid_target = self.sendCommand(action_request) + if cmd_invalid_target: + self.error_msg('Test C failed: Passed while impoossible target requested!') + return not cmd_invalid_target + else: + action_request.target.y = 0.0 + self.info_msg('Test C passed') + + # Test D: Send with lethal costmap and Backup + action_request.time_allowance = Duration(seconds=5).to_msg() + self.sendOccupiedCostmap() + time.sleep(1) + cmd4 = self.sendCommand(action_request) + if cmd4: + self.error_msg('Test D failed: Passed while costmap was not lethal!') + return not cmd4 + else: + self.info_msg('Test D passed') + return True + + def shutdown(self): + self.info_msg('Shutting down') + + self.action_client.destroy() + self.info_msg('Destroyed backup action client') + + transition_service = 'lifecycle_manager_navigation/manage_nodes' + mgr_client = self.create_client(ManageLifecycleNodes, transition_service) + while not mgr_client.wait_for_service(timeout_sec=1.0): + self.info_msg(f'{transition_service} service not available, waiting...') + + req = ManageLifecycleNodes.Request() + req.command = ManageLifecycleNodes.Request().SHUTDOWN + future = mgr_client.call_async(req) + try: + rclpy.spin_until_future_complete(self, future) + future.result() + except Exception as e: # noqa: B902 + self.error_msg(f'{transition_service} service call failed {e!r}') + + self.info_msg(f'{transition_service} finished') + + def info_msg(self, msg: str): + self.get_logger().info(msg) + + def warn_msg(self, msg: str): + self.get_logger().warn(msg) + + def error_msg(self, msg: str): + self.get_logger().error(msg) + + +def main(argv=sys.argv[1:]): + rclpy.init() + time.sleep(10) + test = BackupTest() + result = test.run() + test.shutdown() + + if not result: + test.info_msg('Exiting failed') + exit(1) + else: + test.info_msg('Exiting passed') + exit(0) + + +if __name__ == '__main__': + main() diff --git a/nav2_system_tests/src/behaviors/backup/test_backup_behavior_launch.py b/nav2_system_tests/src/behaviors/backup/test_backup_behavior.launch.py similarity index 60% rename from nav2_system_tests/src/behaviors/backup/test_backup_behavior_launch.py rename to nav2_system_tests/src/behaviors/backup/test_backup_behavior.launch.py index b3b44d0bed..d48c6ca0e7 100755 --- a/nav2_system_tests/src/behaviors/backup/test_backup_behavior_launch.py +++ b/nav2_system_tests/src/behaviors/backup/test_backup_behavior.launch.py @@ -1,5 +1,5 @@ #! /usr/bin/env python3 -# Copyright (c) 2012 Samsung Research America +# Copyright (c) 2019 Samsung Research America # # Licensed under the Apache License, Version 2.0 (the "License"); # you may not use this file except in compliance with the License. @@ -23,50 +23,46 @@ from launch import LaunchService from launch.actions import ( AppendEnvironmentVariable, + DeclareLaunchArgument, ExecuteProcess, IncludeLaunchDescription, - SetEnvironmentVariable, -) + SetEnvironmentVariable) from launch.launch_description_sources import PythonLaunchDescriptionSource +from launch.substitutions import LaunchConfiguration from launch_ros.actions import Node from launch_testing.legacy import LaunchTestService - from nav2_common.launch import RewrittenYaml -from nav2_simple_commander.utils import kill_os_processes def generate_launch_description(): + bringup_dir = get_package_share_directory('nav2_bringup') 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') - + params_file = LaunchConfiguration('params_file') world_sdf_xacro = os.path.join(sim_dir, 'worlds', 'tb3_sandbox.sdf.xacro') robot_sdf = os.path.join(sim_dir, 'urdf', 'gz_waffle.sdf.xacro') - urdf = os.path.join(sim_dir, 'urdf', 'turtlebot3_waffle.urdf') with open(urdf, 'r') as infp: robot_description = infp.read() - map_yaml_file = os.path.join(nav2_bringup_dir, 'maps', 'tb3_sandbox.yaml') - - bt_navigator_xml = os.path.join( - get_package_share_directory('nav2_bt_navigator'), - 'behavior_trees', - os.getenv('BT_NAVIGATOR_XML'), - ) - - params_file = os.path.join(nav2_bringup_dir, 'params/nav2_params.yaml') - - # Replace the `use_astar` setting on the params file + # Create our own temporary YAML files that include substitutions + param_substitutions = {'use_sim_time': 'True'} configured_params = RewrittenYaml( - source_file=params_file, root_key='', param_rewrites='', convert_types=True + source_file=params_file, + root_key='', + param_rewrites=param_substitutions, + convert_types=True, ) return LaunchDescription( [ SetEnvironmentVariable('RCUTILS_LOGGING_BUFFERED_STREAM', '1'), SetEnvironmentVariable('RCUTILS_LOGGING_USE_STDOUT', '1'), - # Launch gazebo server for simulation + DeclareLaunchArgument( + 'params_file', + default_value=os.path.join(bringup_dir, 'params', 'nav2_params.yaml'), + description='Full path to the ROS2 parameters file to use', + ), + # Simulation for odometry AppendEnvironmentVariable( 'GZ_SIM_RESOURCE_PATH', os.path.join(sim_dir, 'models') ), @@ -74,11 +70,9 @@ def generate_launch_description(): '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(), + ExecuteProcess( + cmd=['gz', 'sim', '-r', '-s', world_sdf_xacro], + output='screen', ), IncludeLaunchDescription( PythonLaunchDescriptionSource( @@ -95,6 +89,15 @@ def generate_launch_description(): 'yaw': '0.0', }.items(), ), + # No need for localization + Node( + package='tf2_ros', + executable='static_transform_publisher', + output='screen', + arguments=['0', '0', '0', '0', '0', '0', 'map', 'odom'], + parameters=[{'use_sim_time': True}], + ), + # Need transforms Node( package='robot_state_publisher', executable='robot_state_publisher', @@ -104,18 +107,24 @@ def generate_launch_description(): {'use_sim_time': True, 'robot_description': robot_description} ], ), - IncludeLaunchDescription( - PythonLaunchDescriptionSource( - os.path.join(nav2_bringup_dir, 'launch', 'bringup_launch.py') - ), - launch_arguments={ - 'map': map_yaml_file, - 'use_sim_time': 'True', - 'params_file': configured_params, - 'bt_xml_file': bt_navigator_xml, - 'use_composition': 'False', - 'autostart': 'True', - }.items(), + # Server under test + Node( + package='nav2_behaviors', + executable='behavior_server', + name='behavior_server', + output='screen', + parameters=[configured_params], + ), + Node( + package='nav2_lifecycle_manager', + executable='lifecycle_manager', + name='lifecycle_manager_navigation', + output='screen', + parameters=[ + {'use_sim_time': True}, + {'autostart': True}, + {'node_names': ['behavior_server']}, + ], ), ] ) @@ -124,10 +133,11 @@ def generate_launch_description(): def main(argv=sys.argv[1:]): ld = generate_launch_description() - testExecutable = os.getenv('TEST_EXECUTABLE') - test1_action = ExecuteProcess( - cmd=[testExecutable], name='test_backup_behavior_node', output='screen', + cmd=[os.path.join( + os.getenv('TEST_DIR'), 'backup_tester.py'), '--ros-args', '-p', 'use_sim_time:=True'], + name='tester_node', + output='screen', ) lts = LaunchTestService() @@ -135,7 +145,6 @@ def main(argv=sys.argv[1:]): ls = LaunchService(argv=argv) ls.include_launch_description(ld) return_code = lts.run(ls) - kill_os_processes('gz sim') return return_code diff --git a/nav2_system_tests/src/behaviors/backup/test_backup_behavior_node.cpp b/nav2_system_tests/src/behaviors/backup/test_backup_behavior_node.cpp deleted file mode 100644 index e629dca0bb..0000000000 --- a/nav2_system_tests/src/behaviors/backup/test_backup_behavior_node.cpp +++ /dev/null @@ -1,128 +0,0 @@ -// Copyright (c) 2020 Samsung Research -// Copyright (c) 2020 Sarthak Mittal -// Copyright (c) 2022 Joshua Wallace -// -// 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. Reserved. - -#include -#include -#include -#include -#include - -#include "rclcpp/rclcpp.hpp" - -#include "backup_behavior_tester.hpp" -#include "nav2_msgs/action/back_up.hpp" - -using namespace std::chrono_literals; - -using nav2_system_tests::BackupBehaviorTester; - -struct TestParameters -{ - float x; - float y; - float speed; - float tolerance; -}; - - -std::string testNameGenerator(const testing::TestParamInfo &) -{ - static int test_index = 0; - std::string name = "BackUpTest" + std::to_string(test_index); - ++test_index; - return name; -} - -class BackupBehaviorTestFixture - : public ::testing::TestWithParam -{ -public: - static void SetUpTestCase() - { - backup_behavior_tester = new BackupBehaviorTester(); - if (!backup_behavior_tester->isActive()) { - backup_behavior_tester->activate(); - } - } - - static void TearDownTestCase() - { - delete backup_behavior_tester; - backup_behavior_tester = nullptr; - } - -protected: - static BackupBehaviorTester * backup_behavior_tester; -}; - -BackupBehaviorTester * BackupBehaviorTestFixture::backup_behavior_tester = nullptr; - -TEST_P(BackupBehaviorTestFixture, testBackupBehavior) -{ - auto test_params = GetParam(); - auto goal = nav2_msgs::action::BackUp::Goal(); - goal.target.x = test_params.x; - goal.target.y = test_params.y; - goal.speed = test_params.speed; - float tolerance = test_params.tolerance; - - if (!backup_behavior_tester->isActive()) { - backup_behavior_tester->activate(); - } - - bool success = false; - success = backup_behavior_tester->defaultBackupBehaviorTest(goal, tolerance); - - float dist_to_obstacle = 2.0f; - - if ( ((dist_to_obstacle - std::fabs(test_params.x)) < std::fabs(goal.speed)) || - std::fabs(goal.target.y) > 0) - { - EXPECT_FALSE(success); - } else { - EXPECT_TRUE(success); - } -} - -std::vector test_params = {TestParameters{-0.05, 0.0, -0.2, 0.01}, - TestParameters{-0.05, 0.1, -0.2, 0.01}, - TestParameters{-2.0, 0.0, -0.2, 0.1}}; - -INSTANTIATE_TEST_SUITE_P( - BackupBehaviorTests, - BackupBehaviorTestFixture, - ::testing::Values( - test_params[0], - test_params[1], - test_params[2]), - testNameGenerator -); - - -int main(int argc, char ** argv) -{ - ::testing::InitGoogleTest(&argc, argv); - - // initialize ROS - rclcpp::init(argc, argv); - - bool all_successful = RUN_ALL_TESTS(); - - // shutdown ROS - rclcpp::shutdown(); - - return all_successful; -} diff --git a/nav2_system_tests/src/behaviors/drive_on_heading/CMakeLists.txt b/nav2_system_tests/src/behaviors/drive_on_heading/CMakeLists.txt index 1415c358fe..4f42fe3aca 100644 --- a/nav2_system_tests/src/behaviors/drive_on_heading/CMakeLists.txt +++ b/nav2_system_tests/src/behaviors/drive_on_heading/CMakeLists.txt @@ -1,20 +1,8 @@ -set(test_drive_on_heading_behavior test_drive_on_heading_behavior_node) - -ament_add_gtest_executable(${test_drive_on_heading_behavior} - test_drive_on_heading_behavior_node.cpp - drive_on_heading_behavior_tester.cpp -) - -ament_target_dependencies(${test_drive_on_heading_behavior} - ${dependencies} -) - -ament_add_test(test_drive_on_heading_recovery +ament_add_test(test_drive_on_heading_behavior GENERATE_RESULT_FOR_RETURN_CODE_ZERO - COMMAND "${CMAKE_CURRENT_SOURCE_DIR}/test_drive_on_heading_behavior_launch.py" + COMMAND "${CMAKE_CURRENT_SOURCE_DIR}/test_drive_on_heading_behavior.launch.py" WORKING_DIRECTORY "${CMAKE_CURRENT_BINARY_DIR}" - TIMEOUT 180 + TIMEOUT 60 ENV - TEST_EXECUTABLE=$ - BT_NAVIGATOR_XML=navigate_to_pose_w_replanning_and_recovery.xml + TEST_DIR=${CMAKE_CURRENT_SOURCE_DIR} ) diff --git a/nav2_system_tests/src/behaviors/drive_on_heading/drive_on_heading_behavior_tester.cpp b/nav2_system_tests/src/behaviors/drive_on_heading/drive_on_heading_behavior_tester.cpp deleted file mode 100644 index 967c6beb04..0000000000 --- a/nav2_system_tests/src/behaviors/drive_on_heading/drive_on_heading_behavior_tester.cpp +++ /dev/null @@ -1,224 +0,0 @@ -// Copyright (c) 2020 Sarthak Mittal -// Copyright (c) 2018 Intel Corporation -// -// 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. Reserved. - -#include -#include -#include -#include -#include -#include -#include -#include - -#include "drive_on_heading_behavior_tester.hpp" -#include "nav2_util/geometry_utils.hpp" - -using namespace std::chrono_literals; -using namespace std::chrono; // NOLINT - -namespace nav2_system_tests -{ - -DriveOnHeadingBehaviorTester::DriveOnHeadingBehaviorTester() -: is_active_(false), - initial_pose_received_(false) -{ - rclcpp::NodeOptions options; - options.parameter_overrides({{"use_sim_time", true}}); - node_ = rclcpp::Node::make_shared("DriveOnHeading_behavior_test", options); - - tf_buffer_ = std::make_shared(node_->get_clock()); - tf_listener_ = std::make_shared(*tf_buffer_); - - client_ptr_ = rclcpp_action::create_client( - node_->get_node_base_interface(), - node_->get_node_graph_interface(), - node_->get_node_logging_interface(), - node_->get_node_waitables_interface(), - "drive_on_heading"); - - publisher_ = - node_->create_publisher("initialpose", 10); - - subscription_ = node_->create_subscription( - "amcl_pose", rclcpp::QoS(rclcpp::KeepLast(1)).transient_local().reliable(), - std::bind(&DriveOnHeadingBehaviorTester::amclPoseCallback, this, std::placeholders::_1)); - - stamp_ = node_->now(); -} - -DriveOnHeadingBehaviorTester::~DriveOnHeadingBehaviorTester() -{ - if (is_active_) { - deactivate(); - } -} - -void DriveOnHeadingBehaviorTester::activate() -{ - if (is_active_) { - throw std::runtime_error("Trying to activate while already active"); - return; - } - - while (!initial_pose_received_) { - RCLCPP_WARN(node_->get_logger(), "Initial pose not received"); - sendInitialPose(); - std::this_thread::sleep_for(100ms); - rclcpp::spin_some(node_); - } - - // Wait for lifecycle_manager_navigation to activate behavior_server - std::this_thread::sleep_for(10s); - - if (!client_ptr_) { - RCLCPP_ERROR(node_->get_logger(), "Action client not initialized"); - is_active_ = false; - return; - } - - if (!client_ptr_->wait_for_action_server(10s)) { - RCLCPP_ERROR(node_->get_logger(), "Action server not available after waiting"); - is_active_ = false; - return; - } - - RCLCPP_INFO(this->node_->get_logger(), "DriveOnHeading action server is ready"); - is_active_ = true; -} - -void DriveOnHeadingBehaviorTester::deactivate() -{ - if (!is_active_) { - throw std::runtime_error("Trying to deactivate while already inactive"); - } - is_active_ = false; -} - -bool DriveOnHeadingBehaviorTester::defaultDriveOnHeadingBehaviorTest( - const DriveOnHeading::Goal goal_msg, - const double tolerance) -{ - if (!is_active_) { - RCLCPP_ERROR(node_->get_logger(), "Not activated"); - return false; - } - - // Sleep to let behavior server be ready for serving in multiple runs - std::this_thread::sleep_for(5s); - - RCLCPP_INFO(this->node_->get_logger(), "Sending goal"); - - geometry_msgs::msg::PoseStamped initial_pose; - if (!nav2_util::getCurrentPose(initial_pose, *tf_buffer_, "odom")) { - RCLCPP_ERROR(node_->get_logger(), "Current robot pose is not available."); - return false; - } - RCLCPP_INFO(node_->get_logger(), "Found current robot pose"); - - auto goal_handle_future = client_ptr_->async_send_goal(goal_msg); - - if (rclcpp::spin_until_future_complete(node_, goal_handle_future) != - rclcpp::FutureReturnCode::SUCCESS) - { - RCLCPP_ERROR(node_->get_logger(), "send goal call failed :("); - return false; - } - - rclcpp_action::ClientGoalHandle::SharedPtr goal_handle = goal_handle_future.get(); - if (!goal_handle) { - RCLCPP_ERROR(node_->get_logger(), "Goal was rejected by server"); - return false; - } - - // Wait for the server to be done with the goal - auto result_future = client_ptr_->async_get_result(goal_handle); - - RCLCPP_INFO(node_->get_logger(), "Waiting for result"); - if (rclcpp::spin_until_future_complete(node_, result_future) != - rclcpp::FutureReturnCode::SUCCESS) - { - RCLCPP_ERROR(node_->get_logger(), "get result call failed :("); - return false; - } - - rclcpp_action::ClientGoalHandle::WrappedResult wrapped_result = - result_future.get(); - - switch (wrapped_result.code) { - case rclcpp_action::ResultCode::SUCCEEDED: break; - case rclcpp_action::ResultCode::ABORTED: RCLCPP_ERROR( - node_->get_logger(), - "Goal was aborted"); - return false; - case rclcpp_action::ResultCode::CANCELED: RCLCPP_ERROR( - node_->get_logger(), - "Goal was canceled"); - return false; - default: RCLCPP_ERROR(node_->get_logger(), "Unknown result code"); - return false; - } - - RCLCPP_INFO(node_->get_logger(), "result received"); - - geometry_msgs::msg::PoseStamped current_pose; - if (!nav2_util::getCurrentPose(current_pose, *tf_buffer_, "odom")) { - RCLCPP_ERROR(node_->get_logger(), "Current robot pose is not available."); - return false; - } - - double dist = nav2_util::geometry_utils::euclidean_distance(initial_pose, current_pose); - - if (fabs(dist) > fabs(goal_msg.target.x) + tolerance) { - RCLCPP_ERROR( - node_->get_logger(), - "Distance from goal is %lf (tolerance %lf)", - fabs(dist - goal_msg.target.x), tolerance); - return false; - } - - return true; -} - -void DriveOnHeadingBehaviorTester::sendInitialPose() -{ - geometry_msgs::msg::PoseWithCovarianceStamped pose; - pose.header.frame_id = "map"; - pose.header.stamp = stamp_; - pose.pose.pose.position.x = -2.0; - pose.pose.pose.position.y = -0.5; - pose.pose.pose.position.z = 0.0; - pose.pose.pose.orientation.x = 0.0; - pose.pose.pose.orientation.y = 0.0; - pose.pose.pose.orientation.z = 0.0; - pose.pose.pose.orientation.w = 1.0; - for (int i = 0; i < 35; i++) { - pose.pose.covariance[i] = 0.0; - } - pose.pose.covariance[0] = 0.08; - pose.pose.covariance[7] = 0.08; - pose.pose.covariance[35] = 0.05; - - publisher_->publish(pose); - RCLCPP_INFO(node_->get_logger(), "Sent initial pose"); -} - -void DriveOnHeadingBehaviorTester::amclPoseCallback( - const geometry_msgs::msg::PoseWithCovarianceStamped::SharedPtr) -{ - initial_pose_received_ = true; -} - -} // namespace nav2_system_tests diff --git a/nav2_system_tests/src/behaviors/drive_on_heading/drive_on_heading_behavior_tester.hpp b/nav2_system_tests/src/behaviors/drive_on_heading/drive_on_heading_behavior_tester.hpp deleted file mode 100644 index 920164f162..0000000000 --- a/nav2_system_tests/src/behaviors/drive_on_heading/drive_on_heading_behavior_tester.hpp +++ /dev/null @@ -1,90 +0,0 @@ -// Copyright (c) 2020 Samsung Research -// Copyright (c) 2018 Intel Corporation -// -// 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. Reserved. - -#ifndef BEHAVIORS__DRIVE_ON_HEADING__DRIVE_ON_HEADING_BEHAVIOR_TESTER_HPP_ -#define BEHAVIORS__DRIVE_ON_HEADING__DRIVE_ON_HEADING_BEHAVIOR_TESTER_HPP_ - -#include -#include -#include -#include -#include - -#include "rclcpp/rclcpp.hpp" -#include "rclcpp_action/rclcpp_action.hpp" -#include "angles/angles.h" -#include "nav2_msgs/action/drive_on_heading.hpp" -#include "nav2_util/robot_utils.hpp" -#include "nav2_util/node_thread.hpp" -#include "geometry_msgs/msg/pose_stamped.hpp" -#include "geometry_msgs/msg/pose_with_covariance_stamped.hpp" - -#include "tf2/utils.h" -#include "tf2_ros/buffer.h" -#include "tf2_ros/transform_listener.h" - -namespace nav2_system_tests -{ - -class DriveOnHeadingBehaviorTester -{ -public: - using DriveOnHeading = nav2_msgs::action::DriveOnHeading; - using GoalHandleDriveOnHeading = rclcpp_action::ClientGoalHandle; - - DriveOnHeadingBehaviorTester(); - ~DriveOnHeadingBehaviorTester(); - - // Runs a single test with given target yaw - bool defaultDriveOnHeadingBehaviorTest( - const DriveOnHeading::Goal goal_msg, - double tolerance); - - void activate(); - - void deactivate(); - - bool isActive() const - { - return is_active_; - } - -private: - void sendInitialPose(); - - void amclPoseCallback(geometry_msgs::msg::PoseWithCovarianceStamped::SharedPtr); - - bool is_active_; - bool initial_pose_received_; - rclcpp::Time stamp_; - - std::shared_ptr tf_buffer_; - std::shared_ptr tf_listener_; - - rclcpp::Node::SharedPtr node_; - - // Publisher to publish initial pose - rclcpp::Publisher::SharedPtr publisher_; - - // Subscriber for amcl pose - rclcpp::Subscription::SharedPtr subscription_; - - // Action client to call DriveOnHeading action - rclcpp_action::Client::SharedPtr client_ptr_; -}; - -} // namespace nav2_system_tests - -#endif // BEHAVIORS__DRIVE_ON_HEADING__DRIVE_ON_HEADING_BEHAVIOR_TESTER_HPP_ diff --git a/nav2_system_tests/src/behaviors/drive_on_heading/drive_tester.py b/nav2_system_tests/src/behaviors/drive_on_heading/drive_tester.py new file mode 100755 index 0000000000..2f057620ba --- /dev/null +++ b/nav2_system_tests/src/behaviors/drive_on_heading/drive_tester.py @@ -0,0 +1,340 @@ +#! /usr/bin/env python3 +# Copyright 2024 Open Navigation LLC +# +# 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 sys +import time + +from action_msgs.msg import GoalStatus +from geometry_msgs.msg import Point32, PolygonStamped +from nav2_msgs.action import DriveOnHeading +from nav2_msgs.msg import Costmap +from nav2_msgs.srv import ManageLifecycleNodes + +import rclpy + +from rclpy.action import ActionClient +from rclpy.duration import Duration +from rclpy.node import Node +from rclpy.qos import QoSDurabilityPolicy, QoSHistoryPolicy, QoSReliabilityPolicy +from rclpy.qos import QoSProfile + + +class DriveTest(Node): + + def __init__(self): + super().__init__(node_name='drive_tester', namespace='') + self.costmap_qos = QoSProfile( + durability=QoSDurabilityPolicy.TRANSIENT_LOCAL, + reliability=QoSReliabilityPolicy.RELIABLE, + history=QoSHistoryPolicy.KEEP_LAST, + depth=1, + ) + self.action_client = ActionClient(self, DriveOnHeading, 'drive_on_heading') + self.costmap_pub = self.create_publisher( + Costmap, 'local_costmap/costmap_raw', self.costmap_qos) + self.footprint_pub = self.create_publisher( + PolygonStamped, 'local_costmap/published_footprint', 10) + self.goal_handle = None + self.action_result = None + + def sendCommand(self, command): + self.info_msg('Sending goal request...') + self.goal_future = self.action_client.send_goal_async(command) + try: + rclpy.spin_until_future_complete(self, self.goal_future) + self.goal_handle = self.goal_future.result() + except Exception as e: # noqa: B902 + self.error_msg(f'Service call failed {e!r}') + + if not self.goal_handle.accepted: + self.error_msg('Goal rejected') + return False + + self.info_msg('Goal accepted') + self.result_future = self.goal_handle.get_result_async() + + self.info_msg("Waiting for 'DriveOnHeading' action to complete") + try: + rclpy.spin_until_future_complete(self, self.result_future) + status = self.result_future.result().status + result = self.result_future.result().result + self.action_result = result + except Exception as e: # noqa: B902 + self.error_msg(f'Service call failed {e!r}') + + if status != GoalStatus.STATUS_SUCCEEDED: + self.info_msg(f'Goal failed with status code: {status}') + return False + if self.action_result.error_code == 0: + self.info_msg('DriveOnHeading was successful!') + return True + self.info_msg('DriveOnHeading failed to meet target!') + return False + + def sendAndPreemptWithFasterCommand(self, command): + # Send initial goal + self.info_msg('Sending goal request...') + self.goal_future = self.action_client.send_goal_async(command) + try: + rclpy.spin_until_future_complete(self, self.goal_future) + self.goal_handle = self.goal_future.result() + except Exception as e: # noqa: B902 + self.error_msg(f'Service call failed {e!r}') + + if not self.goal_handle.accepted: + self.error_msg('Goal rejected') + return False + + self.info_msg('Goal accepted') + self.result_future = self.goal_handle.get_result_async() + + # Now preempt it + time.sleep(0.5) + self.info_msg('Sending preemption request...') + command.speed = 0.2 + self.goal_future = self.action_client.send_goal_async(command) + try: + rclpy.spin_until_future_complete(self, self.goal_future) + self.goal_handle = self.goal_future.result() + except Exception as e: # noqa: B902 + self.error_msg(f'Service call failed {e!r}') + + if not self.goal_handle.accepted: + self.error_msg('Preemption rejected') + return False + + self.info_msg('Preemption accepted') + self.result_future = self.goal_handle.get_result_async() + + # Wait for new goal to complete + self.info_msg("Waiting for 'DriveOnHeading' action Preemption to complete") + try: + rclpy.spin_until_future_complete(self, self.result_future) + status = self.result_future.result().status + result = self.result_future.result().result + self.action_result = result + except Exception as e: # noqa: B902 + self.error_msg(f'Service call failed {e!r}') + + if status != GoalStatus.STATUS_SUCCEEDED: + self.info_msg(f'Goal failed with status code: {status}') + return False + if self.action_result.error_code == 0: + self.info_msg('DriveOnHeading was successful!') + return True + self.info_msg('DriveOnHeading failed to meet target!') + return False + + def sendAndCancelCommand(self, command): + self.info_msg('Sending goal request...') + self.goal_future = self.action_client.send_goal_async(command) + try: + rclpy.spin_until_future_complete(self, self.goal_future) + self.goal_handle = self.goal_future.result() + except Exception as e: # noqa: B902 + self.error_msg(f'Service call failed {e!r}') + + if not self.goal_handle.accepted: + self.error_msg('Goal rejected') + return False + + self.info_msg('Goal accepted') + self.result_future = self.goal_handle.get_result_async() + + # Now cancel it + time.sleep(0.5) + cancel_future = self.goal_handle.cancel_goal_async() + rclpy.spin_until_future_complete(self, cancel_future) + rclpy.spin_until_future_complete(self, self.result_future) + status = self.result_future.result().status + if status != GoalStatus.STATUS_CANCELED: + self.info_msg(f'Goal failed with status code: {status}') + return False + else: + self.info_msg('Goal was canceled successfully') + return True + + def sendFreeCostmap(self): + costmap_msg = Costmap() + costmap_msg.header.frame_id = 'odom' + costmap_msg.header.stamp = self.get_clock().now().to_msg() + costmap_msg.metadata.resolution = 0.05 + costmap_msg.metadata.size_x = 100 + costmap_msg.metadata.size_y = 100 + costmap_msg.metadata.origin.position.x = -2.5 + costmap_msg.metadata.origin.position.y = -2.5 + costmap_msg.data = [0] * (costmap_msg.metadata.size_x * costmap_msg.metadata.size_y) + self.costmap_pub.publish(costmap_msg) + + footprint_msg = PolygonStamped() + footprint_msg.header.frame_id = 'odom' + footprint_msg.header.stamp = self.get_clock().now().to_msg() + footprint_msg.polygon.points = [ + Point32(x=0.1, y=0.1, z=0.0), + Point32(x=0.1, y=-0.1, z=0.0), + Point32(x=-0.1, y=-0.1, z=0.0), + Point32(x=-0.1, y=0.1, z=0.0) + ] + self.footprint_pub.publish(footprint_msg) + + def sendOccupiedCostmap(self): + costmap_msg = Costmap() + costmap_msg.header.frame_id = 'odom' + costmap_msg.header.stamp = self.get_clock().now().to_msg() + costmap_msg.metadata.resolution = 0.05 + costmap_msg.metadata.size_x = 100 + costmap_msg.metadata.size_y = 100 + costmap_msg.metadata.origin.position.x = -2.5 + costmap_msg.metadata.origin.position.y = -2.5 + costmap_msg.data = [254] * (costmap_msg.metadata.size_x * costmap_msg.metadata.size_y) + self.costmap_pub.publish(costmap_msg) + + footprint_msg = PolygonStamped() + footprint_msg.header.frame_id = 'odom' + footprint_msg.header.stamp = self.get_clock().now().to_msg() + footprint_msg.polygon.points = [ + Point32(x=0.1, y=0.1, z=0.0), + Point32(x=0.1, y=-0.1, z=0.0), + Point32(x=-0.1, y=-0.1, z=0.0), + Point32(x=-0.1, y=0.1, z=0.0) + ] + self.footprint_pub.publish(footprint_msg) + + def run(self): + while not self.action_client.wait_for_server(timeout_sec=1.0): + self.info_msg("'DriveOnHeading' action server not available, waiting...") + + # Test A: Send without valid costmap + action_request = DriveOnHeading.Goal() + action_request.speed = 0.15 + action_request.target.x = 0.15 + action_request.time_allowance = Duration(seconds=5).to_msg() + cmd1 = self.sendCommand(action_request) + + if cmd1: + self.error_msg('Test A failed: Passed while costmap was not available!') + return not cmd1 + else: + self.info_msg('Test A passed') + + # Test B: Send with valid costmap and DriveOnHeading a couple of times + self.sendFreeCostmap() + time.sleep(1) + cmd1 = self.sendCommand(action_request) + action_request.target.x = 0.1 + cmd2 = self.sendCommand(action_request) + + if not cmd1 or not cmd2: + self.error_msg('Test B failed: Failed to DriveOnHeading with valid costmap!') + return not cmd1 or not cmd2 + + action_request.target.x = 0.5 + cmd_preempt = self.sendAndPreemptWithFasterCommand(action_request) + if not cmd_preempt: + self.error_msg('Test B failed: Failed to preempt and invert command!') + return not cmd_preempt + + cmd_cancel = self.sendAndCancelCommand(action_request) + if not cmd_cancel: + self.error_msg('Test B failed: Failed to cancel command!') + return not cmd_cancel + else: + self.info_msg('Test B passed') + + # Test C: Send with impossible command in time allowance & target * signs + action_request.time_allowance = Duration(seconds=0.1).to_msg() + cmd3 = self.sendCommand(action_request) + if cmd3: + self.error_msg('Test C failed: Passed while impoossible timing requested!') + return not cmd3 + + action_request.target.y = 0.5 + cmd_invalid_target = self.sendCommand(action_request) + if cmd_invalid_target: + self.error_msg('Test C failed: Passed while impoossible target requested!') + return not cmd_invalid_target + else: + action_request.target.y = 0.0 + + action_request.target.x = -0.5 + cmd_invalid_sign = self.sendCommand(action_request) + if cmd_invalid_sign: + self.error_msg('Test C failed: Passed while impoossible target sign requested!') + return not cmd_invalid_sign + else: + action_request.target.x = 0.5 + self.info_msg('Test C passed') + + # Test D: Send with lethal costmap and DriveOnHeading + action_request.time_allowance = Duration(seconds=5).to_msg() + self.sendOccupiedCostmap() + time.sleep(1) + cmd4 = self.sendCommand(action_request) + if cmd4: + self.error_msg('Test D failed: Passed while costmap was not lethal!') + return not cmd4 + else: + self.info_msg('Test D passed') + return True + + def shutdown(self): + self.info_msg('Shutting down') + + self.action_client.destroy() + self.info_msg('Destroyed DriveOnHeading action client') + + transition_service = 'lifecycle_manager_navigation/manage_nodes' + mgr_client = self.create_client(ManageLifecycleNodes, transition_service) + while not mgr_client.wait_for_service(timeout_sec=1.0): + self.info_msg(f'{transition_service} service not available, waiting...') + + req = ManageLifecycleNodes.Request() + req.command = ManageLifecycleNodes.Request().SHUTDOWN + future = mgr_client.call_async(req) + try: + rclpy.spin_until_future_complete(self, future) + future.result() + except Exception as e: # noqa: B902 + self.error_msg(f'{transition_service} service call failed {e!r}') + + self.info_msg(f'{transition_service} finished') + + def info_msg(self, msg: str): + self.get_logger().info(msg) + + def warn_msg(self, msg: str): + self.get_logger().warn(msg) + + def error_msg(self, msg: str): + self.get_logger().error(msg) + + +def main(argv=sys.argv[1:]): + rclpy.init() + time.sleep(10) + test = DriveTest() + result = test.run() + test.shutdown() + + if not result: + test.info_msg('Exiting failed') + exit(1) + else: + test.info_msg('Exiting passed') + exit(0) + + +if __name__ == '__main__': + main() diff --git a/nav2_system_tests/src/behaviors/drive_on_heading/test_drive_on_heading_behavior_launch.py b/nav2_system_tests/src/behaviors/drive_on_heading/test_drive_on_heading_behavior.launch.py similarity index 61% rename from nav2_system_tests/src/behaviors/drive_on_heading/test_drive_on_heading_behavior_launch.py rename to nav2_system_tests/src/behaviors/drive_on_heading/test_drive_on_heading_behavior.launch.py index 2959463712..ce6c381cd9 100755 --- a/nav2_system_tests/src/behaviors/drive_on_heading/test_drive_on_heading_behavior_launch.py +++ b/nav2_system_tests/src/behaviors/drive_on_heading/test_drive_on_heading_behavior.launch.py @@ -1,5 +1,5 @@ #! /usr/bin/env python3 -# Copyright (c) 2012 Samsung Research America +# Copyright (c) 2019 Samsung Research America # # Licensed under the Apache License, Version 2.0 (the "License"); # you may not use this file except in compliance with the License. @@ -23,48 +23,46 @@ from launch import LaunchService from launch.actions import ( AppendEnvironmentVariable, + DeclareLaunchArgument, ExecuteProcess, IncludeLaunchDescription, - SetEnvironmentVariable, -) + SetEnvironmentVariable) from launch.launch_description_sources import PythonLaunchDescriptionSource +from launch.substitutions import LaunchConfiguration from launch_ros.actions import Node from launch_testing.legacy import LaunchTestService - from nav2_common.launch import RewrittenYaml -from nav2_simple_commander.utils import kill_os_processes def generate_launch_description(): + bringup_dir = get_package_share_directory('nav2_bringup') 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') - + params_file = LaunchConfiguration('params_file') world_sdf_xacro = os.path.join(sim_dir, 'worlds', 'tb3_sandbox.sdf.xacro') robot_sdf = os.path.join(sim_dir, 'urdf', 'gz_waffle.sdf.xacro') - urdf = os.path.join(sim_dir, 'urdf', 'turtlebot3_waffle.urdf') with open(urdf, 'r') as infp: robot_description = infp.read() - map_yaml_file = os.path.join(nav2_bringup_dir, 'maps', 'tb3_sandbox.yaml') - bt_navigator_xml = os.path.join( - get_package_share_directory('nav2_bt_navigator'), - 'behavior_trees', - os.getenv('BT_NAVIGATOR_XML'), - ) - - params_file = os.path.join(nav2_bringup_dir, 'params/nav2_params.yaml') - - # Replace the `use_astar` setting on the params file + # Create our own temporary YAML files that include substitutions + param_substitutions = {'use_sim_time': 'True'} configured_params = RewrittenYaml( - source_file=params_file, root_key='', param_rewrites='', convert_types=True + source_file=params_file, + root_key='', + param_rewrites=param_substitutions, + convert_types=True, ) return LaunchDescription( [ SetEnvironmentVariable('RCUTILS_LOGGING_BUFFERED_STREAM', '1'), SetEnvironmentVariable('RCUTILS_LOGGING_USE_STDOUT', '1'), + DeclareLaunchArgument( + 'params_file', + default_value=os.path.join(bringup_dir, 'params', 'nav2_params.yaml'), + description='Full path to the ROS2 parameters file to use', + ), + # Simulation for odometry AppendEnvironmentVariable( 'GZ_SIM_RESOURCE_PATH', os.path.join(sim_dir, 'models') ), @@ -72,11 +70,9 @@ def generate_launch_description(): '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(), + ExecuteProcess( + cmd=['gz', 'sim', '-r', '-s', world_sdf_xacro], + output='screen', ), IncludeLaunchDescription( PythonLaunchDescriptionSource( @@ -93,6 +89,15 @@ def generate_launch_description(): 'yaw': '0.0', }.items(), ), + # No need for localization + Node( + package='tf2_ros', + executable='static_transform_publisher', + output='screen', + arguments=['0', '0', '0', '0', '0', '0', 'map', 'odom'], + parameters=[{'use_sim_time': True}], + ), + # Need transforms Node( package='robot_state_publisher', executable='robot_state_publisher', @@ -102,18 +107,24 @@ def generate_launch_description(): {'use_sim_time': True, 'robot_description': robot_description} ], ), - IncludeLaunchDescription( - PythonLaunchDescriptionSource( - os.path.join(nav2_bringup_dir, 'launch', 'bringup_launch.py') - ), - launch_arguments={ - 'map': map_yaml_file, - 'use_sim_time': 'True', - 'params_file': configured_params, - 'bt_xml_file': bt_navigator_xml, - 'use_composition': 'False', - 'autostart': 'True', - }.items(), + # Server under test + Node( + package='nav2_behaviors', + executable='behavior_server', + name='behavior_server', + output='screen', + parameters=[configured_params], + ), + Node( + package='nav2_lifecycle_manager', + executable='lifecycle_manager', + name='lifecycle_manager_navigation', + output='screen', + parameters=[ + {'use_sim_time': True}, + {'autostart': True}, + {'node_names': ['behavior_server']}, + ], ), ] ) @@ -122,11 +133,10 @@ def generate_launch_description(): def main(argv=sys.argv[1:]): ld = generate_launch_description() - testExecutable = os.getenv('TEST_EXECUTABLE') - test1_action = ExecuteProcess( - cmd=[testExecutable], - name='test_drive_on_heading_behavior_node', + cmd=[os.path.join( + os.getenv('TEST_DIR'), 'drive_tester.py'), '--ros-args', '-p', 'use_sim_time:=True'], + name='tester_node', output='screen', ) @@ -135,7 +145,6 @@ def main(argv=sys.argv[1:]): ls = LaunchService(argv=argv) ls.include_launch_description(ld) return_code = lts.run(ls) - kill_os_processes('gz sim') return return_code diff --git a/nav2_system_tests/src/behaviors/drive_on_heading/test_drive_on_heading_behavior_node.cpp b/nav2_system_tests/src/behaviors/drive_on_heading/test_drive_on_heading_behavior_node.cpp deleted file mode 100644 index b0813c1a25..0000000000 --- a/nav2_system_tests/src/behaviors/drive_on_heading/test_drive_on_heading_behavior_node.cpp +++ /dev/null @@ -1,133 +0,0 @@ -// Copyright (c) 2020 Samsung Research -// Copyright (c) 2020 Sarthak Mittal -// -// 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. Reserved. - -#include -#include -#include -#include -#include - -#include "rclcpp/rclcpp.hpp" - -#include "drive_on_heading_behavior_tester.hpp" - -using namespace std::chrono_literals; - -using nav2_system_tests::DriveOnHeadingBehaviorTester; - -struct TestParameters -{ - float x; - float y; - float speed; - float time_allowance; - float tolerance; -}; - -std::string testNameGenerator(const testing::TestParamInfo &) -{ - static int test_index = 0; - std::string name = "DriveOnHeadingTest" + std::to_string(test_index); - ++test_index; - return name; -} - -class DriveOnHeadingBehaviorTestFixture - : public ::testing::TestWithParam -{ -public: - static void SetUpTestCase() - { - drive_on_heading_behavior_tester = new DriveOnHeadingBehaviorTester(); - if (!drive_on_heading_behavior_tester->isActive()) { - drive_on_heading_behavior_tester->activate(); - } - } - - static void TearDownTestCase() - { - delete drive_on_heading_behavior_tester; - drive_on_heading_behavior_tester = nullptr; - } - -protected: - static DriveOnHeadingBehaviorTester * drive_on_heading_behavior_tester; -}; - -DriveOnHeadingBehaviorTester * DriveOnHeadingBehaviorTestFixture::drive_on_heading_behavior_tester = - nullptr; - -TEST_P(DriveOnHeadingBehaviorTestFixture, testBackupBehavior) -{ - auto test_params = GetParam(); - auto goal = nav2_msgs::action::DriveOnHeading::Goal(); - goal.target.x = test_params.x; - goal.target.y = test_params.y; - goal.speed = test_params.speed; - goal.time_allowance.sec = test_params.time_allowance; - float tolerance = test_params.tolerance; - - if (!drive_on_heading_behavior_tester->isActive()) { - drive_on_heading_behavior_tester->activate(); - } - - bool success = false; - success = drive_on_heading_behavior_tester->defaultDriveOnHeadingBehaviorTest( - goal, - tolerance); - - float dist_to_obstacle = 2.0f; - if ( ((dist_to_obstacle - std::fabs(test_params.x)) < std::fabs(goal.speed)) || - std::fabs(goal.target.y) > 0 || - goal.time_allowance.sec < 2.0 || - !((goal.target.x > 0.0) == (goal.speed > 0.0))) - { - EXPECT_FALSE(success); - } else { - EXPECT_TRUE(success); - } -} - -std::vector test_params = {TestParameters{-0.05, 0.0, -0.2, 10.0, 0.01}, - TestParameters{-0.05, 0.1, -0.2, 10.0, 0.01}, - TestParameters{-2.0, 0.0, -0.2, 10.0, 0.1}, - TestParameters{-0.05, 0.0, -0.01, 1.0, 0.01}, - TestParameters{0.05, 0.0, -0.2, 10.0, 0.01}}; - -INSTANTIATE_TEST_SUITE_P( - DriveOnHeadingBehaviorTests, - DriveOnHeadingBehaviorTestFixture, - ::testing::Values( - test_params[0], - test_params[1], - test_params[2], - test_params[3], - test_params[4]), - testNameGenerator); - -int main(int argc, char ** argv) -{ - ::testing::InitGoogleTest(&argc, argv); - - // initialize ROS - rclcpp::init(argc, argv); - - bool all_successful = RUN_ALL_TESTS(); - - // shutdown ROS - rclcpp::shutdown(); - - return all_successful; -} diff --git a/nav2_system_tests/src/behaviors/spin/spin_tester.py b/nav2_system_tests/src/behaviors/spin/spin_tester.py index 9f50ff11a7..b2d6941987 100755 --- a/nav2_system_tests/src/behaviors/spin/spin_tester.py +++ b/nav2_system_tests/src/behaviors/spin/spin_tester.py @@ -277,7 +277,7 @@ def shutdown(self): self.info_msg('Shutting down') self.action_client.destroy() - self.info_msg('Destroyed follow_gps_waypoints action client') + self.info_msg('Destroyed backup action client') transition_service = 'lifecycle_manager_navigation/manage_nodes' mgr_client = self.create_client(ManageLifecycleNodes, transition_service)