From 806bc688e8ceee4c56e992c709a49d969ee06232 Mon Sep 17 00:00:00 2001
From: Stevedan Ogochukwu Omodolor
 <61468301+stevedanomodolor@users.noreply.github.com>
Date: Tue, 29 Nov 2022 20:31:48 +0100
Subject: [PATCH] preempt/cancel test for time behavior, spin pluguin (#3301)

* include preempt/cancel test for time behavior, spin pluguin

* linting

* fix bug in code
---
 .../behaviors/spin/spin_behavior_tester.cpp   | 15 ++++++--
 .../behaviors/spin/spin_behavior_tester.hpp   |  4 ++-
 .../spin/test_spin_behavior_node.cpp          | 34 +++++++++++++++++++
 3 files changed, 50 insertions(+), 3 deletions(-)

diff --git a/nav2_system_tests/src/behaviors/spin/spin_behavior_tester.cpp b/nav2_system_tests/src/behaviors/spin/spin_behavior_tester.cpp
index 557f5b79595..27b1f5e800b 100644
--- a/nav2_system_tests/src/behaviors/spin/spin_behavior_tester.cpp
+++ b/nav2_system_tests/src/behaviors/spin/spin_behavior_tester.cpp
@@ -123,7 +123,9 @@ void SpinBehaviorTester::deactivate()
 
 bool SpinBehaviorTester::defaultSpinBehaviorTest(
   const float target_yaw,
-  const double tolerance)
+  const double tolerance,
+  const bool nonblocking_action,
+  const bool cancel_action)
 {
   if (!is_active_) {
     RCLCPP_ERROR(node_->get_logger(), "Not activated");
@@ -181,6 +183,16 @@ bool SpinBehaviorTester::defaultSpinBehaviorTest(
     return false;
   }
 
+  if (!nonblocking_action) {
+    return true;
+  }
+  if (cancel_action) {
+    sleep(2);
+    // cancel the goal
+    auto cancel_response = client_ptr_->async_cancel_goal(goal_handle_future.get());
+    rclcpp::spin_until_future_complete(node_, cancel_response);
+  }
+
   // Wait for the server to be done with the goal
   auto result_future = client_ptr_->async_get_result(goal_handle);
 
@@ -347,5 +359,4 @@ void SpinBehaviorTester::amclPoseCallback(
 {
   initial_pose_received_ = true;
 }
-
 }  // namespace nav2_system_tests
diff --git a/nav2_system_tests/src/behaviors/spin/spin_behavior_tester.hpp b/nav2_system_tests/src/behaviors/spin/spin_behavior_tester.hpp
index 745cfe42124..c7fb6f719d3 100644
--- a/nav2_system_tests/src/behaviors/spin/spin_behavior_tester.hpp
+++ b/nav2_system_tests/src/behaviors/spin/spin_behavior_tester.hpp
@@ -57,7 +57,9 @@ class SpinBehaviorTester
   // Runs a single test with given target yaw
   bool defaultSpinBehaviorTest(
     float target_yaw,
-    double tolerance = 0.1);
+    double tolerance = 0.1,
+    bool nonblocking_action = true,
+    bool cancel_action = false);
 
   void activate();
 
diff --git a/nav2_system_tests/src/behaviors/spin/test_spin_behavior_node.cpp b/nav2_system_tests/src/behaviors/spin/test_spin_behavior_node.cpp
index 150a6599fab..a1efed8cd96 100644
--- a/nav2_system_tests/src/behaviors/spin/test_spin_behavior_node.cpp
+++ b/nav2_system_tests/src/behaviors/spin/test_spin_behavior_node.cpp
@@ -80,6 +80,40 @@ TEST_P(SpinBehaviorTestFixture, testSpinRecovery)
   }
 }
 
+TEST_F(SpinBehaviorTestFixture, testSpinPreemption)
+{
+  // Goal
+  float target_yaw = 3.0 * M_PIf32;
+  float tolerance = 0.1;
+  bool nonblocking_action = false;
+  bool success = false;
+
+  // Send the first goal
+  success = spin_recovery_tester->defaultSpinBehaviorTest(
+    target_yaw, tolerance,
+    nonblocking_action);
+  EXPECT_EQ(true, success);
+  // Preempt goal
+  sleep(2);
+  success = false;
+  float prempt_target_yaw = 4.0 * M_PIf32;
+  float preempt_tolerance = 0.1;
+  success = spin_recovery_tester->defaultSpinBehaviorTest(prempt_target_yaw, preempt_tolerance);
+  EXPECT_EQ(false, success);
+}
+
+TEST_F(SpinBehaviorTestFixture, testSpinCancel)
+{
+  // Goal
+  float target_yaw = 4.0 * M_PIf32;
+  float tolerance = 0.1;
+  bool nonblocking_action = true, cancel_action = true, success = false;
+  success = spin_recovery_tester->defaultSpinBehaviorTest(
+    target_yaw, tolerance,
+    nonblocking_action, cancel_action);
+  EXPECT_EQ(false, success);
+}
+
 INSTANTIATE_TEST_SUITE_P(
   SpinRecoveryTests,
   SpinBehaviorTestFixture,