diff --git a/nav2_recoveries/plugins/spin.cpp b/nav2_recoveries/plugins/spin.cpp index 547d56c169..fa1d926c55 100644 --- a/nav2_recoveries/plugins/spin.cpp +++ b/nav2_recoveries/plugins/spin.cpp @@ -36,7 +36,7 @@ namespace nav2_recoveries Spin::Spin() : Recovery() { - initial_yaw_ = 0.0; + prev_yaw_ = 0.0; } Spin::~Spin() @@ -74,9 +74,10 @@ Status Spin::onRun(const std::shared_ptr command) return Status::FAILED; } - initial_yaw_ = tf2::getYaw(current_pose.pose.orientation); + prev_yaw_ = tf2::getYaw(current_pose.pose.orientation); + relative_yaw_ = 0.0; - cmd_yaw_ = -command->target_yaw; + cmd_yaw_ = command->target_yaw; RCLCPP_INFO( node_->get_logger(), "Turning %0.2f for spin recovery.", cmd_yaw_); @@ -92,29 +93,33 @@ Status Spin::onCycleUpdate() } const double current_yaw = tf2::getYaw(current_pose.pose.orientation); - double relative_yaw = abs(current_yaw - initial_yaw_); - if (relative_yaw > M_PI) { - relative_yaw -= 2.0 * M_PI; + + double delta_yaw = current_yaw - prev_yaw_; + if (abs(delta_yaw) > M_PI) { + delta_yaw = copysign(2 * M_PI - abs(delta_yaw), prev_yaw_); } - relative_yaw = abs(relative_yaw); - if (relative_yaw >= abs(cmd_yaw_)) { + relative_yaw_ += delta_yaw; + prev_yaw_ = current_yaw; + + double remaining_yaw = abs(cmd_yaw_) - abs(relative_yaw_); + if (remaining_yaw <= 0) { stopRobot(); return Status::SUCCEEDED; } - double vel = sqrt(2 * rotational_acc_lim_ * relative_yaw); + double vel = sqrt(2 * rotational_acc_lim_ * remaining_yaw); vel = std::min(std::max(vel, min_rotational_vel_), max_rotational_vel_); geometry_msgs::msg::Twist cmd_vel; - cmd_yaw_ < 0 ? cmd_vel.angular.z = -vel : cmd_vel.angular.z = vel; + cmd_vel.angular.z = copysign(vel, cmd_yaw_); geometry_msgs::msg::Pose2D pose2d; pose2d.x = current_pose.pose.position.x; pose2d.y = current_pose.pose.position.y; pose2d.theta = tf2::getYaw(current_pose.pose.orientation); - if (!isCollisionFree(relative_yaw, cmd_vel, pose2d)) { + if (!isCollisionFree(relative_yaw_, cmd_vel, pose2d)) { stopRobot(); RCLCPP_WARN(node_->get_logger(), "Collision Ahead - Exiting Spin"); return Status::SUCCEEDED; diff --git a/nav2_recoveries/plugins/spin.hpp b/nav2_recoveries/plugins/spin.hpp index 358fdec1c9..d7cf0dfdbd 100644 --- a/nav2_recoveries/plugins/spin.hpp +++ b/nav2_recoveries/plugins/spin.hpp @@ -47,7 +47,8 @@ class Spin : public Recovery double max_rotational_vel_; double rotational_acc_lim_; double cmd_yaw_; - double initial_yaw_; + double prev_yaw_; + double relative_yaw_; double simulate_ahead_time_; }; diff --git a/nav2_system_tests/CMakeLists.txt b/nav2_system_tests/CMakeLists.txt index 1fba96254c..81841fbf79 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 0000000000..ef01473fcb --- /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 0000000000..e4b764aaee --- /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 0000000000..521c675955 --- /dev/null +++ b/nav2_system_tests/src/recoveries/spin_recovery_tester.cpp @@ -0,0 +1,224 @@ +// 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_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_ = 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(), + "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::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"); + + 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 0000000000..de0608b022 --- /dev/null +++ b/nav2_system_tests/src/recoveries/spin_recovery_tester.hpp @@ -0,0 +1,89 @@ +// 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__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 0000000000..2b3a8bb739 --- /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 0000000000..6756312f7d --- /dev/null +++ b/nav2_system_tests/src/recoveries/test_spin_recovery_node.cpp @@ -0,0 +1,93 @@ +// 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 "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 / 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)), ); + +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; +}