From 76c5a3ac3ab4493bfc94dbf06400b746c9d0071e Mon Sep 17 00:00:00 2001 From: Sarthak Mittal Date: Sat, 2 May 2020 09:48:47 +0530 Subject: [PATCH] Add test for spin recovery --- nav2_system_tests/CMakeLists.txt | 3 + .../src/recoveries/CMakeLists.txt | 23 ++ nav2_system_tests/src/recoveries/README.md | 4 + .../src/recoveries/spin_recovery_tester.cpp | 211 ++++++++++++++++++ .../src/recoveries/spin_recovery_tester.hpp | 84 +++++++ .../recoveries/test_spin_recovery_launch.py | 102 +++++++++ .../recoveries/test_spin_recovery_node.cpp | 85 +++++++ 7 files changed, 512 insertions(+) create mode 100644 nav2_system_tests/src/recoveries/CMakeLists.txt create mode 100644 nav2_system_tests/src/recoveries/README.md create mode 100644 nav2_system_tests/src/recoveries/spin_recovery_tester.cpp create mode 100644 nav2_system_tests/src/recoveries/spin_recovery_tester.hpp create mode 100755 nav2_system_tests/src/recoveries/test_spin_recovery_launch.py create mode 100644 nav2_system_tests/src/recoveries/test_spin_recovery_node.cpp diff --git a/nav2_system_tests/CMakeLists.txt b/nav2_system_tests/CMakeLists.txt index 1fba96254c9..81841fbf79f 100644 --- a/nav2_system_tests/CMakeLists.txt +++ b/nav2_system_tests/CMakeLists.txt @@ -21,6 +21,7 @@ find_package(rclpy REQUIRED) find_package(nav2_navfn_planner REQUIRED) find_package(nav2_planner REQUIRED) find_package(navigation2) +find_package(angles REQUIRED) nav2_package() @@ -41,6 +42,7 @@ set(dependencies rclpy nav2_planner nav2_navfn_planner + angles ) if(BUILD_TESTING) @@ -55,6 +57,7 @@ if(BUILD_TESTING) add_subdirectory(src/system) add_subdirectory(src/updown) add_subdirectory(src/waypoint_follower) + add_subdirectory(src/recoveries) install(DIRECTORY maps DESTINATION share/${PROJECT_NAME}) endif() diff --git a/nav2_system_tests/src/recoveries/CMakeLists.txt b/nav2_system_tests/src/recoveries/CMakeLists.txt new file mode 100644 index 00000000000..ef01473fcb7 --- /dev/null +++ b/nav2_system_tests/src/recoveries/CMakeLists.txt @@ -0,0 +1,23 @@ +set(test_spin_recovery_exec test_spin_recovery_node) + +ament_add_gtest_executable(${test_spin_recovery_exec} + test_spin_recovery_node.cpp + spin_recovery_tester.cpp +) + +ament_target_dependencies(${test_spin_recovery_exec} + ${dependencies} +) + +ament_add_test(test_spin_recovery + GENERATE_RESULT_FOR_RETURN_CODE_ZERO + COMMAND "${CMAKE_CURRENT_SOURCE_DIR}/test_spin_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/README.md b/nav2_system_tests/src/recoveries/README.md new file mode 100644 index 00000000000..e4b764aaeea --- /dev/null +++ b/nav2_system_tests/src/recoveries/README.md @@ -0,0 +1,4 @@ +# Recoveries Test + +Provides some simple tests for recovery plugins. +It creates an instance of the stack, with the recovery server loading different recovery plugins, and checks for successful recovery behaviors. diff --git a/nav2_system_tests/src/recoveries/spin_recovery_tester.cpp b/nav2_system_tests/src/recoveries/spin_recovery_tester.cpp new file mode 100644 index 00000000000..426294ce94f --- /dev/null +++ b/nav2_system_tests/src/recoveries/spin_recovery_tester.cpp @@ -0,0 +1,211 @@ +// 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 +#include + +#include "spin_recovery_tester.hpp" + +using namespace std::chrono_literals; +using namespace std::chrono; // NOLINT + +namespace nav2_system_tests { + +SpinRecoveryTester::SpinRecoveryTester() + : is_active_(false), + initial_pose_received_(false) { + + node_ = rclcpp::Node::make_shared("spin_recovery_test"); + + tf_buffer_.reset(new tf2_ros::Buffer(node_->get_clock())); + tf_listener_.reset(new tf2_ros::TransformListener(*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(), + "spin"); + + publisher_ = + node_->create_publisher("initialpose", 10); + + subscription_ = node_->create_subscription( + "amcl_pose", rclcpp::QoS(rclcpp::KeepLast(1)).transient_local().reliable(), + std::bind(&SpinRecoveryTester::amclPoseCallback, this, std::placeholders::_1)); +} + +SpinRecoveryTester::~SpinRecoveryTester() { + if (is_active_) + deactivate(); +} + +void SpinRecoveryTester::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(), "Spin action server is ready"); + is_active_ = true; +} + +void SpinRecoveryTester::deactivate() { + if (!is_active_) { + throw std::runtime_error("Trying to deactivate while already inactive"); + } + is_active_ = false; +} + +bool SpinRecoveryTester::defaultSpinRecoveryTest( + const float target_yaw, + const double tolerance) { + + 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 goal_msg = Spin::Goal(); + goal_msg.target_yaw = target_yaw; + + 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 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(), + "Angular distance from goal is %lf (tolerance %lf)", + fabs(dyaw), tolerance); + return false; + } + + return true; +} + +void SpinRecoveryTester::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 SpinRecoveryTester::amclPoseCallback( + const geometry_msgs::msg::PoseWithCovarianceStamped::SharedPtr) { + initial_pose_received_ = true; +} + +} // namespace nav2_system_tests diff --git a/nav2_system_tests/src/recoveries/spin_recovery_tester.hpp b/nav2_system_tests/src/recoveries/spin_recovery_tester.hpp new file mode 100644 index 00000000000..96078a9faa6 --- /dev/null +++ b/nav2_system_tests/src/recoveries/spin_recovery_tester.hpp @@ -0,0 +1,84 @@ +// 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__SPIN_RECOVERY_TESTER_HPP_ +#define RECOVERIES__SPIN_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/spin.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 SpinRecoveryTester { + public: + using Spin = nav2_msgs::action::Spin; + using GoalHandleSpin = rclcpp_action::ClientGoalHandle; + + SpinRecoveryTester(); + ~SpinRecoveryTester(); + + // Runs a single test with given target yaw + bool defaultSpinRecoveryTest(float target_yaw, + double tolerance = 0.1); + + 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 spin action + rclcpp_action::Client::SharedPtr client_ptr_; +}; + +} // namespace nav2_system_tests + +#endif // RECOVERIES__SPIN_RECOVERY_TESTER_HPP_ diff --git a/nav2_system_tests/src/recoveries/test_spin_recovery_launch.py b/nav2_system_tests/src/recoveries/test_spin_recovery_launch.py new file mode 100755 index 00000000000..2b3a8bb7398 --- /dev/null +++ b/nav2_system_tests/src/recoveries/test_spin_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_spin_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/test_spin_recovery_node.cpp b/nav2_system_tests/src/recoveries/test_spin_recovery_node.cpp new file mode 100644 index 00000000000..4c9f889187e --- /dev/null +++ b/nav2_system_tests/src/recoveries/test_spin_recovery_node.cpp @@ -0,0 +1,85 @@ +// 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 "rclcpp/rclcpp.hpp" +#include "nav2_util/lifecycle_utils.hpp" + +#include "spin_recovery_tester.hpp" + +using namespace std::chrono_literals; + +using nav2_system_tests::SpinRecoveryTester; + +class SpinRecoveryTestFixture : public ::testing::TestWithParam > { + public: + static void SetUpTestCase() { + spin_recovery_tester = new SpinRecoveryTester(); + if (!spin_recovery_tester->isActive()) { + spin_recovery_tester->activate(); + } + } + + static void TearDownTestCase() { + delete spin_recovery_tester; + spin_recovery_tester = nullptr; + } + + protected: + static SpinRecoveryTester *spin_recovery_tester; +}; + +SpinRecoveryTester *SpinRecoveryTestFixture::spin_recovery_tester = nullptr; + +TEST_P(SpinRecoveryTestFixture, 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->defaultSpinRecoveryTest(target_yaw, tolerance); + if (success) { + break; + } + } + + EXPECT_EQ(true, success); +} + +INSTANTIATE_TEST_CASE_P(SpinRecoveryTests, + SpinRecoveryTestFixture, + ::testing::Values(std::make_tuple(M_PIf32, 0.1), + std::make_tuple(-2.0 * M_PIf32, 0.1), + std::make_tuple(4.0 * M_PIf32, 0.15), + std::make_tuple(3.0 * M_PIf32 / 2.0, 0.15)), ); + +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; +}