preempt/cancel test for time behavior, spin pluguin

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 wait_action,
+  const bool cancel_action)
 {
   if (!is_active_) {
     RCLCPP_ERROR(node_->get_logger(), "Not activated");
@@ -181,6 +183,18 @@ bool SpinBehaviorTester::defaultSpinBehaviorTest(
     return false;
   }
 
+  if (!wait_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 +361,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 wait_action = true,
+    bool cancel_action = false);
 
   void activate();
 

nav2_system_tests/src/behaviors/spin/test_spin_behavior_node.cpp

@@ -80,6 +80,56 @@ TEST_P(SpinBehaviorTestFixture, testSpinRecovery)
   }
 }
 
+
+TEST_F(SpinBehaviorTestFixture, testSpinPreemption)
+{
+  // Goal 
+  float target_yaw = 3.0 * M_PIf32;
+  float tolerance = 0.1;
+  bool wait_action = false; 
+  bool success = false;
+
+  //Send the first goal 
+  success = spin_recovery_tester->defaultSpinBehaviorTest(target_yaw, tolerance,wait_action);
+  if (std::getenv("MAKE_FAKE_COSTMAP") != NULL && abs(target_yaw) > M_PI_2f32) {
+    // if this variable is set, make a fake costmap
+    // in the fake spin test, we expect a collision for angles > M_PI_2
+    EXPECT_EQ(false, success);
+  } else {
+    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);
+  if (std::getenv("MAKE_FAKE_COSTMAP") != NULL && abs(prempt_target_yaw) > M_PI_2f32) {
+    // if this variable is set, make a fake costmap
+    // in the fake spin test, we expect a collision for angles > M_PI_2
+    EXPECT_EQ(false, success);
+  } else {
+    EXPECT_EQ(true, success);
+  }
+}
+
+TEST_F(SpinBehaviorTestFixture, testSpinCancel)
+{
+  // Goal
+  float target_yaw = 4.0 * M_PIf32;
+  float tolerance = 0.1; 
+  bool wait_action = true, cancel_action = true, success = false;
+  success = spin_recovery_tester->defaultSpinBehaviorTest(target_yaw, tolerance, wait_action, cancel_action);
+  if (std::getenv("MAKE_FAKE_COSTMAP") != NULL && abs(target_yaw) > M_PI_2f32) {
+    // if this variable is set, make a fake costmap
+    // in the fake spin test, we expect a collision for angles > M_PI_2
+    EXPECT_EQ(true, success);
+  } else {
+    EXPECT_EQ(false, success);
+  }
+}
+
 INSTANTIATE_TEST_SUITE_P(
   SpinRecoveryTests,
   SpinBehaviorTestFixture,