preempt/cancel test for time behavior, spin pluguin (ros-navigation#3301)

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

* linting

* fix bug in code

Author: stevedanomodolor

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

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

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,