diff --git a/nav2_system_tests/CMakeLists.txt b/nav2_system_tests/CMakeLists.txt index 81841fbf79..ce0b6573bd 100644 --- a/nav2_system_tests/CMakeLists.txt +++ b/nav2_system_tests/CMakeLists.txt @@ -57,7 +57,8 @@ if(BUILD_TESTING) add_subdirectory(src/system) add_subdirectory(src/updown) add_subdirectory(src/waypoint_follower) - add_subdirectory(src/recoveries) + add_subdirectory(src/recoveries/spin) + add_subdirectory(src/recoveries/wait) install(DIRECTORY maps DESTINATION share/${PROJECT_NAME}) endif() diff --git a/nav2_system_tests/src/recoveries/CMakeLists.txt b/nav2_system_tests/src/recoveries/spin/CMakeLists.txt similarity index 100% rename from nav2_system_tests/src/recoveries/CMakeLists.txt rename to nav2_system_tests/src/recoveries/spin/CMakeLists.txt diff --git a/nav2_system_tests/src/recoveries/spin_recovery_tester.cpp b/nav2_system_tests/src/recoveries/spin/spin_recovery_tester.cpp similarity index 100% rename from nav2_system_tests/src/recoveries/spin_recovery_tester.cpp rename to nav2_system_tests/src/recoveries/spin/spin_recovery_tester.cpp diff --git a/nav2_system_tests/src/recoveries/spin_recovery_tester.hpp b/nav2_system_tests/src/recoveries/spin/spin_recovery_tester.hpp similarity index 93% rename from nav2_system_tests/src/recoveries/spin_recovery_tester.hpp rename to nav2_system_tests/src/recoveries/spin/spin_recovery_tester.hpp index de0608b022..29622efbe7 100644 --- a/nav2_system_tests/src/recoveries/spin_recovery_tester.hpp +++ b/nav2_system_tests/src/recoveries/spin/spin_recovery_tester.hpp @@ -13,8 +13,8 @@ // See the License for the specific language governing permissions and // limitations under the License. Reserved. -#ifndef RECOVERIES__SPIN_RECOVERY_TESTER_HPP_ -#define RECOVERIES__SPIN_RECOVERY_TESTER_HPP_ +#ifndef RECOVERIES__SPIN__SPIN_RECOVERY_TESTER_HPP_ +#define RECOVERIES__SPIN__SPIN_RECOVERY_TESTER_HPP_ #include #include @@ -86,4 +86,4 @@ class SpinRecoveryTester } // namespace nav2_system_tests -#endif // RECOVERIES__SPIN_RECOVERY_TESTER_HPP_ +#endif // RECOVERIES__SPIN__SPIN_RECOVERY_TESTER_HPP_ diff --git a/nav2_system_tests/src/recoveries/test_spin_recovery_launch.py b/nav2_system_tests/src/recoveries/spin/test_spin_recovery_launch.py similarity index 100% rename from nav2_system_tests/src/recoveries/test_spin_recovery_launch.py rename to nav2_system_tests/src/recoveries/spin/test_spin_recovery_launch.py diff --git a/nav2_system_tests/src/recoveries/test_spin_recovery_node.cpp b/nav2_system_tests/src/recoveries/spin/test_spin_recovery_node.cpp similarity index 98% rename from nav2_system_tests/src/recoveries/test_spin_recovery_node.cpp rename to nav2_system_tests/src/recoveries/spin/test_spin_recovery_node.cpp index 6756312f7d..c7a66482cb 100644 --- a/nav2_system_tests/src/recoveries/test_spin_recovery_node.cpp +++ b/nav2_system_tests/src/recoveries/spin/test_spin_recovery_node.cpp @@ -1,3 +1,4 @@ +// Copyright (c) 2020 Samsung Research // Copyright (c) 2020 Sarthak Mittal // // Licensed under the Apache License, Version 2.0 (the "License"); diff --git a/nav2_system_tests/src/recoveries/wait/CMakeLists.txt b/nav2_system_tests/src/recoveries/wait/CMakeLists.txt new file mode 100644 index 0000000000..b91374df43 --- /dev/null +++ b/nav2_system_tests/src/recoveries/wait/CMakeLists.txt @@ -0,0 +1,23 @@ +set(test_wait_recovery_exec test_wait_recovery_node) + +ament_add_gtest_executable(${test_wait_recovery_exec} + test_wait_recovery_node.cpp + wait_recovery_tester.cpp +) + +ament_target_dependencies(${test_wait_recovery_exec} + ${dependencies} +) + +ament_add_test(test_wait_recovery + GENERATE_RESULT_FOR_RETURN_CODE_ZERO + COMMAND "${CMAKE_CURRENT_SOURCE_DIR}/test_wait_recovery_launch.py" + WORKING_DIRECTORY "${CMAKE_CURRENT_BINARY_DIR}" + TIMEOUT 100 + 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_w_replanning_and_recovery.xml +) diff --git a/nav2_system_tests/src/recoveries/wait/test_wait_recovery_launch.py b/nav2_system_tests/src/recoveries/wait/test_wait_recovery_launch.py new file mode 100755 index 0000000000..8c81cc20b4 --- /dev/null +++ b/nav2_system_tests/src/recoveries/wait/test_wait_recovery_launch.py @@ -0,0 +1,102 @@ +#! /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 ExecuteProcess, IncludeLaunchDescription, SetEnvironmentVariable +from launch.launch_description_sources import PythonLaunchDescriptionSource +from launch_ros.actions import Node +from launch_testing.legacy import LaunchTestService + +from nav2_common.launch import RewrittenYaml + + +def generate_launch_description(): + map_yaml_file = os.getenv('TEST_MAP') + world = os.getenv('TEST_WORLD') + + 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 + configured_params = RewrittenYaml( + source_file=params_file, + root_key='', + param_rewrites='', + convert_types=True) + + return LaunchDescription([ + SetEnvironmentVariable('RCUTILS_CONSOLE_STDOUT_LINE_BUFFERED', '1'), + + # Launch gazebo server for simulation + ExecuteProcess( + cmd=['gzserver', '-s', 'libgazebo_ros_init.so', + '--minimal_comms', world], + output='screen'), + + # TODO(orduno) Launch the robot state publisher instead + # using a local copy of TB3 urdf file + Node( + package='tf2_ros', + node_executable='static_transform_publisher', + output='screen', + arguments=['0', '0', '0', '0', '0', '0', 'base_footprint', 'base_link']), + + Node( + package='tf2_ros', + node_executable='static_transform_publisher', + output='screen', + arguments=['0', '0', '0', '0', '0', '0', 'base_link', 'base_scan']), + + IncludeLaunchDescription( + PythonLaunchDescriptionSource( + os.path.join(bringup_dir, 'launch', 'bringup_launch.py')), + launch_arguments={ + 'map': map_yaml_file, + 'use_sim_time': 'True', + 'params_file': configured_params, + 'bt_xml_file': bt_navigator_xml, + 'autostart': 'True'}.items()), + ]) + + +def main(argv=sys.argv[1:]): + ld = generate_launch_description() + + testExecutable = os.getenv('TEST_EXECUTABLE') + + test1_action = ExecuteProcess( + cmd=[testExecutable], + name='test_wait_recovery_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/recoveries/wait/test_wait_recovery_node.cpp b/nav2_system_tests/src/recoveries/wait/test_wait_recovery_node.cpp new file mode 100644 index 0000000000..4ed4db839a --- /dev/null +++ b/nav2_system_tests/src/recoveries/wait/test_wait_recovery_node.cpp @@ -0,0 +1,89 @@ +// 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 "rclcpp/rclcpp.hpp" + +#include "wait_recovery_tester.hpp" + +using namespace std::chrono_literals; + +using nav2_system_tests::WaitRecoveryTester; + +class WaitRecoveryTestFixture + : public ::testing::TestWithParam> +{ +public: + static void SetUpTestCase() + { + wait_recovery_tester = new WaitRecoveryTester(); + if (!wait_recovery_tester->isActive()) { + wait_recovery_tester->activate(); + } + } + + static void TearDownTestCase() + { + delete wait_recovery_tester; + wait_recovery_tester = nullptr; + } + +protected: + static WaitRecoveryTester * wait_recovery_tester; +}; + +WaitRecoveryTester * WaitRecoveryTestFixture::wait_recovery_tester = nullptr; + +TEST_P(WaitRecoveryTestFixture, testSWaitRecovery) +{ + float wait_time = std::get<0>(GetParam()); + + bool success = false; + int num_tries = 3; + for (int i = 0; i != num_tries; i++) { + success = success || wait_recovery_tester->recoveryTest(wait_time); + if (success) { + break; + } + } + + EXPECT_EQ(true, success); +} + +INSTANTIATE_TEST_CASE_P( + WaitRecoveryTests, + WaitRecoveryTestFixture, + ::testing::Values( + std::make_tuple(1.0, 0.0), + std::make_tuple(2.0, 0.0), + std::make_tuple(5.0, 0.0)), ); + +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/recoveries/wait/wait_recovery_tester.cpp b/nav2_system_tests/src/recoveries/wait/wait_recovery_tester.cpp new file mode 100644 index 0000000000..b6bdd62148 --- /dev/null +++ b/nav2_system_tests/src/recoveries/wait/wait_recovery_tester.cpp @@ -0,0 +1,204 @@ +// Copyright (c) 2020 Samsung Research +// 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 "wait_recovery_tester.hpp" + +using namespace std::chrono_literals; +using namespace std::chrono; // NOLINT + +namespace nav2_system_tests +{ + +WaitRecoveryTester::WaitRecoveryTester() +: is_active_(false), + initial_pose_received_(false) +{ + node_ = rclcpp::Node::make_shared("wait_recovery_test"); + + 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(), + "wait"); + + publisher_ = + node_->create_publisher("initialpose", 10); + + subscription_ = node_->create_subscription( + "amcl_pose", rclcpp::QoS(rclcpp::KeepLast(1)).transient_local().reliable(), + std::bind(&WaitRecoveryTester::amclPoseCallback, this, std::placeholders::_1)); +} + +WaitRecoveryTester::~WaitRecoveryTester() +{ + if (is_active_) { + deactivate(); + } +} + +void WaitRecoveryTester::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 recoveries_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(), "Wait action server is ready"); + is_active_ = true; +} + +void WaitRecoveryTester::deactivate() +{ + if (!is_active_) { + throw std::runtime_error("Trying to deactivate while already inactive"); + } + is_active_ = false; +} + +bool WaitRecoveryTester::recoveryTest( + const float wait_time) +{ + if (!is_active_) { + RCLCPP_ERROR(node_->get_logger(), "Not activated"); + return false; + } + + // Sleep to let recovery server be ready for serving in multiple runs + std::this_thread::sleep_for(5s); + + auto start_time = node_->now(); + auto goal_msg = Wait::Goal(); + goal_msg.time = rclcpp::Duration(wait_time, 0.0); + + RCLCPP_INFO(this->node_->get_logger(), "Sending goal"); + + auto goal_handle_future = client_ptr_->async_send_goal(goal_msg); + + if (rclcpp::spin_until_future_complete(node_, goal_handle_future) != + rclcpp::executor::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::executor::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"); + + + if ((node_->now() - start_time).seconds() < static_cast(wait_time)) { + return false; + } + + return true; +} + +void WaitRecoveryTester::sendInitialPose() +{ + geometry_msgs::msg::PoseWithCovarianceStamped pose; + pose.header.frame_id = "map"; + pose.header.stamp = rclcpp::Time(); + 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 WaitRecoveryTester::amclPoseCallback( + const geometry_msgs::msg::PoseWithCovarianceStamped::SharedPtr) +{ + initial_pose_received_ = true; +} + +} // namespace nav2_system_tests diff --git a/nav2_system_tests/src/recoveries/wait/wait_recovery_tester.hpp b/nav2_system_tests/src/recoveries/wait/wait_recovery_tester.hpp new file mode 100644 index 0000000000..5daef5025b --- /dev/null +++ b/nav2_system_tests/src/recoveries/wait/wait_recovery_tester.hpp @@ -0,0 +1,89 @@ +// Copyright (c) 2020 Samsung Research +// 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 RECOVERIES__WAIT__WAIT_RECOVERY_TESTER_HPP_ +#define RECOVERIES__WAIT__WAIT_RECOVERY_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/wait.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 WaitRecoveryTester +{ +public: + using Wait = nav2_msgs::action::Wait; + using GoalHandleWait = rclcpp_action::ClientGoalHandle; + + WaitRecoveryTester(); + ~WaitRecoveryTester(); + + // Runs a single test with given target yaw + bool recoveryTest( + float time); + + 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_; + + 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 wait action + rclcpp_action::Client::SharedPtr client_ptr_; +}; + +} // namespace nav2_system_tests + +#endif // RECOVERIES__WAIT__WAIT_RECOVERY_TESTER_HPP_