diff --git a/nav2_bringup/bringup/params/nav2_params.yaml b/nav2_bringup/bringup/params/nav2_params.yaml index 50738827bea..fd1ba0c6152 100644 --- a/nav2_bringup/bringup/params/nav2_params.yaml +++ b/nav2_bringup/bringup/params/nav2_params.yaml @@ -250,7 +250,7 @@ recoveries_server: costmap_topic: local_costmap/costmap_raw footprint_topic: local_costmap/published_footprint cycle_frequency: 10.0 - plugin_names: ["spin", "back_up", "wait"] + plugin_names: ["spin", "backup", "wait"] plugin_types: ["nav2_recoveries/Spin", "nav2_recoveries/BackUp", "nav2_recoveries/Wait"] global_frame: odom robot_base_frame: base_link diff --git a/nav2_system_tests/CMakeLists.txt b/nav2_system_tests/CMakeLists.txt index 3e8b52caf46..9eae0646f2f 100644 --- a/nav2_system_tests/CMakeLists.txt +++ b/nav2_system_tests/CMakeLists.txt @@ -59,6 +59,7 @@ if(BUILD_TESTING) add_subdirectory(src/waypoint_follower) add_subdirectory(src/recoveries/spin) add_subdirectory(src/recoveries/wait) + add_subdirectory(src/recoveries/backup) install(DIRECTORY maps DESTINATION share/${PROJECT_NAME}) endif() diff --git a/nav2_system_tests/src/recoveries/backup/CMakeLists.txt b/nav2_system_tests/src/recoveries/backup/CMakeLists.txt new file mode 100644 index 00000000000..d21387dec46 --- /dev/null +++ b/nav2_system_tests/src/recoveries/backup/CMakeLists.txt @@ -0,0 +1,23 @@ +set(test_backup_recovery_exec test_backup_recovery_node) + +ament_add_gtest_executable(${test_backup_recovery_exec} + test_backup_recovery_node.cpp + backup_recovery_tester.cpp +) + +ament_target_dependencies(${test_backup_recovery_exec} + ${dependencies} +) + +ament_add_test(test_backup_recovery + GENERATE_RESULT_FOR_RETURN_CODE_ZERO + COMMAND "${CMAKE_CURRENT_SOURCE_DIR}/test_backup_recovery_launch.py" + WORKING_DIRECTORY "${CMAKE_CURRENT_BINARY_DIR}" + TIMEOUT 180 + ENV + TEST_MAP=${PROJECT_SOURCE_DIR}/maps/map_circular.yaml + TEST_EXECUTABLE=$ + TEST_WORLD=${PROJECT_SOURCE_DIR}/worlds/turtlebot3_ros2_demo.world + GAZEBO_MODEL_PATH=${PROJECT_SOURCE_DIR}/models + BT_NAVIGATOR_XML=navigate_w_replanning_and_recovery.xml +) diff --git a/nav2_system_tests/src/recoveries/backup/backup_recovery_tester.cpp b/nav2_system_tests/src/recoveries/backup/backup_recovery_tester.cpp new file mode 100644 index 00000000000..6d87fb1bd28 --- /dev/null +++ b/nav2_system_tests/src/recoveries/backup/backup_recovery_tester.cpp @@ -0,0 +1,223 @@ +// Copyright (c) 2020 Sarthak Mittal +// Copyright (c) 2018 Intel Corporation +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. Reserved. + +#include +#include +#include +#include +#include +#include +#include +#include + +#include "backup_recovery_tester.hpp" +#include "nav2_util/geometry_utils.hpp" + +using namespace std::chrono_literals; +using namespace std::chrono; // NOLINT + +namespace nav2_system_tests +{ + +BackupRecoveryTester::BackupRecoveryTester() +: is_active_(false), + initial_pose_received_(false) +{ + node_ = rclcpp::Node::make_shared("backup_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(), + "backup"); + + publisher_ = + node_->create_publisher("initialpose", 10); + + subscription_ = node_->create_subscription( + "amcl_pose", rclcpp::QoS(rclcpp::KeepLast(1)).transient_local().reliable(), + std::bind(&BackupRecoveryTester::amclPoseCallback, this, std::placeholders::_1)); +} + +BackupRecoveryTester::~BackupRecoveryTester() +{ + if (is_active_) { + deactivate(); + } +} + +void BackupRecoveryTester::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(), "Backup action server is ready"); + is_active_ = true; +} + +void BackupRecoveryTester::deactivate() +{ + if (!is_active_) { + throw std::runtime_error("Trying to deactivate while already inactive"); + } + is_active_ = false; +} + +bool BackupRecoveryTester::defaultBackupRecoveryTest( + const float target_dist, + 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 = BackUp::Goal(); + goal_msg.target.x = target_dist; + goal_msg.speed = 0.2; + + 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 dist = nav2_util::geometry_utils::euclidean_distance(initial_pose, current_pose); + + if (fabs(dist) > fabs(target_dist) + tolerance) { + RCLCPP_ERROR( + node_->get_logger(), + "Distance from goal is %lf (tolerance %lf)", + fabs(dist - target_dist), tolerance); + return false; + } + + return true; +} + +void BackupRecoveryTester::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 BackupRecoveryTester::amclPoseCallback( + const geometry_msgs::msg::PoseWithCovarianceStamped::SharedPtr) +{ + initial_pose_received_ = true; +} + +} // namespace nav2_system_tests diff --git a/nav2_system_tests/src/recoveries/backup/backup_recovery_tester.hpp b/nav2_system_tests/src/recoveries/backup/backup_recovery_tester.hpp new file mode 100644 index 00000000000..4c658860a09 --- /dev/null +++ b/nav2_system_tests/src/recoveries/backup/backup_recovery_tester.hpp @@ -0,0 +1,89 @@ +// Copyright (c) 2020 Samsung Research +// Copyright (c) 2018 Intel Corporation +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. Reserved. + +#ifndef RECOVERIES__BACKUP__BACKUP_RECOVERY_TESTER_HPP_ +#define RECOVERIES__BACKUP__BACKUP_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/back_up.hpp" +#include "nav2_util/robot_utils.hpp" +#include "nav2_util/node_thread.hpp" +#include "geometry_msgs/msg/pose_stamped.hpp" +#include "geometry_msgs/msg/pose_with_covariance_stamped.hpp" + +#include "tf2/utils.h" +#include "tf2_ros/buffer.h" +#include "tf2_ros/transform_listener.h" + +namespace nav2_system_tests +{ + +class BackupRecoveryTester +{ +public: + using BackUp = nav2_msgs::action::BackUp; + using GoalHandleBackup = rclcpp_action::ClientGoalHandle; + + BackupRecoveryTester(); + ~BackupRecoveryTester(); + + // Runs a single test with given target yaw + bool defaultBackupRecoveryTest( + float target_dist, + 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 Backup action + rclcpp_action::Client::SharedPtr client_ptr_; +}; + +} // namespace nav2_system_tests + +#endif // RECOVERIES__BACKUP__BACKUP_RECOVERY_TESTER_HPP_ diff --git a/nav2_system_tests/src/recoveries/backup/test_backup_recovery_launch.py b/nav2_system_tests/src/recoveries/backup/test_backup_recovery_launch.py new file mode 100755 index 00000000000..7f23662f2e4 --- /dev/null +++ b/nav2_system_tests/src/recoveries/backup/test_backup_recovery_launch.py @@ -0,0 +1,102 @@ +#! /usr/bin/env python3 +# Copyright (c) 2012 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_backup_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/backup/test_backup_recovery_node.cpp b/nav2_system_tests/src/recoveries/backup/test_backup_recovery_node.cpp new file mode 100644 index 00000000000..82c6effbd9e --- /dev/null +++ b/nav2_system_tests/src/recoveries/backup/test_backup_recovery_node.cpp @@ -0,0 +1,110 @@ +// Copyright (c) 2020 Samsung Research +// Copyright (c) 2020 Sarthak Mittal +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. Reserved. + +#include +#include +#include +#include +#include + +#include "rclcpp/rclcpp.hpp" + +#include "backup_recovery_tester.hpp" + +using namespace std::chrono_literals; + +using nav2_system_tests::BackupRecoveryTester; + +std::string testNameGenerator(const testing::TestParamInfo> & param) +{ + std::string name = std::to_string(std::abs(std::get<0>(param.param))) + "_" + std::to_string( + std::get<1>(param.param)); + name.erase(std::remove(name.begin(), name.end(), '.'), name.end()); + return name; +} + +class BackupRecoveryTestFixture + : public ::testing::TestWithParam> +{ +public: + static void SetUpTestCase() + { + backup_recovery_tester = new BackupRecoveryTester(); + if (!backup_recovery_tester->isActive()) { + backup_recovery_tester->activate(); + } + } + + static void TearDownTestCase() + { + delete backup_recovery_tester; + backup_recovery_tester = nullptr; + } + +protected: + static BackupRecoveryTester * backup_recovery_tester; +}; + +BackupRecoveryTester * BackupRecoveryTestFixture::backup_recovery_tester = nullptr; + +TEST_P(BackupRecoveryTestFixture, testBackupRecovery) +{ + float target_dist = std::get<0>(GetParam()); + float tolerance = std::get<1>(GetParam()); + + if (!backup_recovery_tester->isActive()) { + backup_recovery_tester->activate(); + } + + bool success = false; + success = backup_recovery_tester->defaultBackupRecoveryTest(target_dist, tolerance); + + // if intentionally backing into an obstacle, should fail. + // TODO(stevemacenski): uncomment this once note below completed. + // if (target_dist < -1.0) { + // success = !success; + // } + + EXPECT_EQ(true, success); +} + +// TODO(stevemacenski): See issue #1779, while the 3rd test collides, +// it returns success due to technical debt in the BT. This test will +// remain as a reminder to update this to a `false` case once the +// recovery server returns true values. + +INSTANTIATE_TEST_CASE_P( + BackupRecoveryTests, + BackupRecoveryTestFixture, + ::testing::Values( + std::make_tuple(-0.1, 0.1), + std::make_tuple(-0.2, 0.1), + std::make_tuple(-2.0, 0.1)), + testNameGenerator); + +int main(int argc, char ** argv) +{ + ::testing::InitGoogleTest(&argc, argv); + + // initialize ROS + rclcpp::init(argc, argv); + + bool all_successful = RUN_ALL_TESTS(); + + // shutdown ROS + rclcpp::shutdown(); + + return all_successful; +} diff --git a/nav2_util/include/nav2_util/geometry_utils.hpp b/nav2_util/include/nav2_util/geometry_utils.hpp index 90fa6503ae7..52bd5ebb417 100644 --- a/nav2_util/include/nav2_util/geometry_utils.hpp +++ b/nav2_util/include/nav2_util/geometry_utils.hpp @@ -18,6 +18,7 @@ #include #include "geometry_msgs/msg/pose.hpp" +#include "geometry_msgs/msg/pose_stamped.hpp" #include "geometry_msgs/msg/point.hpp" #include "geometry_msgs/msg/quaternion.hpp" #include "tf2_geometry_msgs/tf2_geometry_msgs.h" @@ -54,6 +55,13 @@ inline double euclidean_distance( return std::sqrt(dx * dx + dy * dy + dz * dz); } +inline double euclidean_distance( + const geometry_msgs::msg::PoseStamped & pos1, + const geometry_msgs::msg::PoseStamped & pos2) +{ + return euclidean_distance(pos1.pose, pos2.pose); +} + } // namespace geometry_utils } // namespace nav2_util