adding backup tests

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

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()

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=$<TARGET_FILE:${test_backup_recovery_exec}>
+    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
+)

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 <string>
+#include <random>
+#include <tuple>
+#include <memory>
+#include <iostream>
+#include <chrono>
+#include <sstream>
+#include <iomanip>
+
+#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<tf2_ros::Buffer>(node_->get_clock());
+  tf_listener_ = std::make_shared<tf2_ros::TransformListener>(*tf_buffer_);
+
+  client_ptr_ = rclcpp_action::create_client<BackUp>(
+    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<geometry_msgs::msg::PoseWithCovarianceStamped>("initialpose", 10);
+
+  subscription_ = node_->create_subscription<geometry_msgs::msg::PoseWithCovarianceStamped>(
+    "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<BackUp>::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<BackUp>::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

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 <gtest/gtest.h>
+#include <memory>
+#include <string>
+#include <thread>
+#include <algorithm>
+
+#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<BackUp>;
+
+  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<tf2_ros::Buffer> tf_buffer_;
+  std::shared_ptr<tf2_ros::TransformListener> tf_listener_;
+
+  rclcpp::Node::SharedPtr node_;
+
+  // Publisher to publish initial pose
+  rclcpp::Publisher<geometry_msgs::msg::PoseWithCovarianceStamped>::SharedPtr publisher_;
+
+  // Subscriber for amcl pose
+  rclcpp::Subscription<geometry_msgs::msg::PoseWithCovarianceStamped>::SharedPtr subscription_;
+
+  // Action client to call Backup action
+  rclcpp_action::Client<BackUp>::SharedPtr client_ptr_;
+};
+
+}  // namespace nav2_system_tests
+
+#endif  // RECOVERIES__BACKUP__BACKUP_RECOVERY_TESTER_HPP_