From b79a29c94d240edcd916133696cb64e40023e87b Mon Sep 17 00:00:00 2001 From: Sarthak Mittal Date: Fri, 1 May 2020 04:42:57 +0530 Subject: [PATCH 1/5] Fix infinite rotation in Spin recovery when angle > PI --- nav2_recoveries/plugins/spin.cpp | 27 ++++++++++++++++----------- nav2_recoveries/plugins/spin.hpp | 3 ++- 2 files changed, 18 insertions(+), 12 deletions(-) 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_; }; From c19fad445b7cdbdaf26f54696a09feecaf03a17f Mon Sep 17 00:00:00 2001 From: Sarthak Mittal Date: Sat, 2 May 2020 09:48:47 +0530 Subject: [PATCH 2/5] 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 | 86 +++++++ 7 files changed, 513 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 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..627c10edde --- /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::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..96078a9faa --- /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 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..c1742e0694 --- /dev/null +++ b/nav2_system_tests/src/recoveries/test_spin_recovery_node.cpp @@ -0,0 +1,86 @@ +// 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(-M_PI_2f32, 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; +} From 3aedf58579e23373cf6fca333abb4b1573cc8c1e Mon Sep 17 00:00:00 2001 From: Sarthak Mittal Date: Sat, 2 May 2020 12:28:42 +0530 Subject: [PATCH 3/5] Fix formatting --- .../src/recoveries/spin_recovery_tester.cpp | 80 +++++++++++-------- .../src/recoveries/spin_recovery_tester.hpp | 18 +++-- .../recoveries/test_spin_recovery_node.cpp | 45 ++++++----- 3 files changed, 82 insertions(+), 61 deletions(-) diff --git a/nav2_system_tests/src/recoveries/spin_recovery_tester.cpp b/nav2_system_tests/src/recoveries/spin_recovery_tester.cpp index 627c10edde..3893f6fed8 100644 --- a/nav2_system_tests/src/recoveries/spin_recovery_tester.cpp +++ b/nav2_system_tests/src/recoveries/spin_recovery_tester.cpp @@ -15,8 +15,6 @@ #include #include #include -#include -#include #include #include #include @@ -28,38 +26,42 @@ using namespace std::chrono_literals; using namespace std::chrono; // NOLINT -namespace nav2_system_tests { +namespace nav2_system_tests +{ SpinRecoveryTester::SpinRecoveryTester() - : is_active_(false), - initial_pose_received_(false) { - +: 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"); + 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); + 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)); + "amcl_pose", rclcpp::QoS(rclcpp::KeepLast(1)).transient_local().reliable(), + std::bind(&SpinRecoveryTester::amclPoseCallback, this, std::placeholders::_1)); } -SpinRecoveryTester::~SpinRecoveryTester() { - if (is_active_) +SpinRecoveryTester::~SpinRecoveryTester() +{ + if (is_active_) { deactivate(); + } } -void SpinRecoveryTester::activate() { +void SpinRecoveryTester::activate() +{ if (is_active_) { throw std::runtime_error("Trying to activate while already active"); return; @@ -91,7 +93,8 @@ void SpinRecoveryTester::activate() { is_active_ = true; } -void SpinRecoveryTester::deactivate() { +void SpinRecoveryTester::deactivate() +{ if (!is_active_) { throw std::runtime_error("Trying to deactivate while already inactive"); } @@ -99,9 +102,9 @@ void SpinRecoveryTester::deactivate() { } bool SpinRecoveryTester::defaultSpinRecoveryTest( - const float target_yaw, - const double tolerance) { - + const float target_yaw, + const double tolerance) +{ if (!is_active_) { RCLCPP_ERROR(node_->get_logger(), "Not activated"); return false; @@ -125,7 +128,8 @@ bool SpinRecoveryTester::defaultSpinRecoveryTest( 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::executor::FutureReturnCode::SUCCESS) + { RCLCPP_ERROR(node_->get_logger(), "send goal call failed :("); return false; } @@ -141,7 +145,8 @@ bool SpinRecoveryTester::defaultSpinRecoveryTest( RCLCPP_INFO(node_->get_logger(), "Waiting for result"); if (rclcpp::spin_until_future_complete(node_, result_future) != - rclcpp::executor::FutureReturnCode::SUCCESS) { + rclcpp::executor::FutureReturnCode::SUCCESS) + { RCLCPP_ERROR(node_->get_logger(), "get result call failed :("); return false; } @@ -149,12 +154,16 @@ bool SpinRecoveryTester::defaultSpinRecoveryTest( 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"); + 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"); + case rclcpp_action::ResultCode::CANCELED: RCLCPP_ERROR( + node_->get_logger(), + "Goal was canceled"); return false; - default:RCLCPP_ERROR(node_->get_logger(), "Unknown result code"); + default: RCLCPP_ERROR(node_->get_logger(), "Unknown result code"); return false; } @@ -167,21 +176,23 @@ bool SpinRecoveryTester::defaultSpinRecoveryTest( } double goal_yaw = angles::normalize_angle( - tf2::getYaw(initial_pose.pose.orientation) + target_yaw); + tf2::getYaw(initial_pose.pose.orientation) + target_yaw); double dyaw = angles::shortest_angular_distance( - goal_yaw, tf2::getYaw(current_pose.pose.orientation)); + 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); + RCLCPP_ERROR( + node_->get_logger(), + "Angular distance from goal is %lf (tolerance %lf)", + fabs(dyaw), tolerance); return false; } return true; } -void SpinRecoveryTester::sendInitialPose() { +void SpinRecoveryTester::sendInitialPose() +{ geometry_msgs::msg::PoseWithCovarianceStamped pose; pose.header.frame_id = "map"; pose.header.stamp = rclcpp::Time(); @@ -204,7 +215,8 @@ void SpinRecoveryTester::sendInitialPose() { } void SpinRecoveryTester::amclPoseCallback( - const geometry_msgs::msg::PoseWithCovarianceStamped::SharedPtr) { + const geometry_msgs::msg::PoseWithCovarianceStamped::SharedPtr) +{ initial_pose_received_ = true; } diff --git a/nav2_system_tests/src/recoveries/spin_recovery_tester.hpp b/nav2_system_tests/src/recoveries/spin_recovery_tester.hpp index 96078a9faa..91a6c7eb81 100644 --- a/nav2_system_tests/src/recoveries/spin_recovery_tester.hpp +++ b/nav2_system_tests/src/recoveries/spin_recovery_tester.hpp @@ -34,10 +34,12 @@ #include "tf2_ros/buffer.h" #include "tf2_ros/transform_listener.h" -namespace nav2_system_tests { +namespace nav2_system_tests +{ -class SpinRecoveryTester { - public: +class SpinRecoveryTester +{ +public: using Spin = nav2_msgs::action::Spin; using GoalHandleSpin = rclcpp_action::ClientGoalHandle; @@ -45,18 +47,20 @@ class SpinRecoveryTester { ~SpinRecoveryTester(); // Runs a single test with given target yaw - bool defaultSpinRecoveryTest(float target_yaw, - double tolerance = 0.1); + bool defaultSpinRecoveryTest( + float target_yaw, + double tolerance = 0.1); void activate(); void deactivate(); - bool isActive() const { + bool isActive() const + { return is_active_; } - private: +private: void sendInitialPose(); void amclPoseCallback(geometry_msgs::msg::PoseWithCovarianceStamped::SharedPtr); diff --git a/nav2_system_tests/src/recoveries/test_spin_recovery_node.cpp b/nav2_system_tests/src/recoveries/test_spin_recovery_node.cpp index c1742e0694..adf3fd1a31 100644 --- a/nav2_system_tests/src/recoveries/test_spin_recovery_node.cpp +++ b/nav2_system_tests/src/recoveries/test_spin_recovery_node.cpp @@ -13,13 +13,10 @@ // 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" @@ -27,27 +24,32 @@ using namespace std::chrono_literals; using nav2_system_tests::SpinRecoveryTester; -class SpinRecoveryTestFixture : public ::testing::TestWithParam > { - public: - static void SetUpTestCase() { +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() { + static void TearDownTestCase() + { delete spin_recovery_tester; spin_recovery_tester = nullptr; } - protected: - static SpinRecoveryTester *spin_recovery_tester; +protected: + static SpinRecoveryTester * spin_recovery_tester; }; -SpinRecoveryTester *SpinRecoveryTestFixture::spin_recovery_tester = nullptr; +SpinRecoveryTester * SpinRecoveryTestFixture::spin_recovery_tester = nullptr; -TEST_P(SpinRecoveryTestFixture, testSpinRecovery) { +TEST_P(SpinRecoveryTestFixture, testSpinRecovery) +{ float target_yaw = std::get<0>(GetParam()); float tolerance = std::get<1>(GetParam()); @@ -63,15 +65,18 @@ TEST_P(SpinRecoveryTestFixture, testSpinRecovery) { EXPECT_EQ(true, success); } -INSTANTIATE_TEST_CASE_P(SpinRecoveryTests, - SpinRecoveryTestFixture, - ::testing::Values(std::make_tuple(M_PIf32, 0.1), - std::make_tuple(-M_PI_2f32, 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) { +INSTANTIATE_TEST_CASE_P( + SpinRecoveryTests, + SpinRecoveryTestFixture, + ::testing::Values( + std::make_tuple(M_PIf32, 0.1), + std::make_tuple(-M_PI_2f32, 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 From b68b2c7164a8cf4617d9495efae4f2be089a2556 Mon Sep 17 00:00:00 2001 From: Sarthak Mittal Date: Tue, 5 May 2020 05:24:47 +0530 Subject: [PATCH 4/5] Add general optimizations --- nav2_system_tests/src/recoveries/spin_recovery_tester.cpp | 6 +++--- nav2_system_tests/src/recoveries/spin_recovery_tester.hpp | 2 +- .../src/recoveries/test_spin_recovery_node.cpp | 6 ++++-- 3 files changed, 8 insertions(+), 6 deletions(-) diff --git a/nav2_system_tests/src/recoveries/spin_recovery_tester.cpp b/nav2_system_tests/src/recoveries/spin_recovery_tester.cpp index 3893f6fed8..403ed2e751 100644 --- a/nav2_system_tests/src/recoveries/spin_recovery_tester.cpp +++ b/nav2_system_tests/src/recoveries/spin_recovery_tester.cpp @@ -1,4 +1,4 @@ -// Copyright (c) 2018 Intel Corporation +// 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. @@ -35,8 +35,8 @@ SpinRecoveryTester::SpinRecoveryTester() { 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_)); + 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(), diff --git a/nav2_system_tests/src/recoveries/spin_recovery_tester.hpp b/nav2_system_tests/src/recoveries/spin_recovery_tester.hpp index 91a6c7eb81..ab2fb5b2cd 100644 --- a/nav2_system_tests/src/recoveries/spin_recovery_tester.hpp +++ b/nav2_system_tests/src/recoveries/spin_recovery_tester.hpp @@ -1,4 +1,4 @@ -// Copyright (c) 2018 Intel Corporation +// 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. diff --git a/nav2_system_tests/src/recoveries/test_spin_recovery_node.cpp b/nav2_system_tests/src/recoveries/test_spin_recovery_node.cpp index adf3fd1a31..6cdc4bc260 100644 --- a/nav2_system_tests/src/recoveries/test_spin_recovery_node.cpp +++ b/nav2_system_tests/src/recoveries/test_spin_recovery_node.cpp @@ -1,4 +1,4 @@ -// Copyright (c) 2018 Intel Corporation +// 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. @@ -69,8 +69,10 @@ INSTANTIATE_TEST_CASE_P( SpinRecoveryTests, SpinRecoveryTestFixture, ::testing::Values( - std::make_tuple(M_PIf32, 0.1), + 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(-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)), ); From 4a880ac67ff85687a4c9870c43c0727c640d8ff6 Mon Sep 17 00:00:00 2001 From: Sarthak Mittal Date: Tue, 5 May 2020 07:33:32 +0530 Subject: [PATCH 5/5] Fix copyright --- nav2_system_tests/src/recoveries/spin_recovery_tester.cpp | 1 + nav2_system_tests/src/recoveries/spin_recovery_tester.hpp | 1 + nav2_system_tests/src/recoveries/test_spin_recovery_node.cpp | 4 ++-- 3 files changed, 4 insertions(+), 2 deletions(-) diff --git a/nav2_system_tests/src/recoveries/spin_recovery_tester.cpp b/nav2_system_tests/src/recoveries/spin_recovery_tester.cpp index 403ed2e751..521c675955 100644 --- a/nav2_system_tests/src/recoveries/spin_recovery_tester.cpp +++ b/nav2_system_tests/src/recoveries/spin_recovery_tester.cpp @@ -1,4 +1,5 @@ // 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. diff --git a/nav2_system_tests/src/recoveries/spin_recovery_tester.hpp b/nav2_system_tests/src/recoveries/spin_recovery_tester.hpp index ab2fb5b2cd..de0608b022 100644 --- a/nav2_system_tests/src/recoveries/spin_recovery_tester.hpp +++ b/nav2_system_tests/src/recoveries/spin_recovery_tester.hpp @@ -1,4 +1,5 @@ // 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. diff --git a/nav2_system_tests/src/recoveries/test_spin_recovery_node.cpp b/nav2_system_tests/src/recoveries/test_spin_recovery_node.cpp index 6cdc4bc260..6756312f7d 100644 --- a/nav2_system_tests/src/recoveries/test_spin_recovery_node.cpp +++ b/nav2_system_tests/src/recoveries/test_spin_recovery_node.cpp @@ -73,9 +73,9 @@ INSTANTIATE_TEST_CASE_P( 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), - std::make_tuple(3.0 * M_PIf32 / 2.0, 0.15)), ); + std::make_tuple(4.0 * M_PIf32, 0.15)), ); int main(int argc, char ** argv) {