[CI] Time out from test_force_torque_sensor_broadcaster (#1586)

Author: Juliaj

force_torque_sensor_broadcaster/test/test_force_torque_sensor_broadcaster.cpp

@@ -24,10 +24,10 @@
 
 #include "geometry_msgs/msg/wrench_stamped.hpp"
 #include "hardware_interface/loaned_state_interface.hpp"
+#include "lifecycle_msgs/msg/state.hpp"
 #include "rclcpp/executor.hpp"
 #include "rclcpp/executors.hpp"
 #include "rclcpp/utilities.hpp"
-
 using hardware_interface::LoanedStateInterface;
 using testing::IsEmpty;
 using testing::SizeIs;
@@ -40,15 +40,43 @@ constexpr auto NODE_ERROR = controller_interface::CallbackReturn::ERROR;
 
 void ForceTorqueSensorBroadcasterTest::SetUpTestCase() {}
 
-void ForceTorqueSensorBroadcasterTest::TearDownTestCase() {}
+void ForceTorqueSensorBroadcasterTest::TearDownTestCase()
+{
+  // Ensure all nodes are properly shutdown
+  rclcpp::shutdown();
+  // Add a small delay to allow proper cleanup
+  std::this_thread::sleep_for(std::chrono::milliseconds(100));
+}
 
 void ForceTorqueSensorBroadcasterTest::SetUp()
 {
   // initialize controller
   fts_broadcaster_ = std::make_unique<FriendForceTorqueSensorBroadcaster>();
 }
 
-void ForceTorqueSensorBroadcasterTest::TearDown() { fts_broadcaster_.reset(nullptr); }
+void ForceTorqueSensorBroadcasterTest::TearDown()
+{
+  // Reset the broadcaster with proper cleanup
+  if (fts_broadcaster_)
+  {
+    if (
+      fts_broadcaster_->get_lifecycle_state().id() !=
+      lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED)
+    {
+      if (
+        fts_broadcaster_->get_lifecycle_state().id() ==
+        lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE)
+      {
+        ASSERT_EQ(fts_broadcaster_->on_deactivate(rclcpp_lifecycle::State()), NODE_SUCCESS);
+      }
+      // Clean up the broadcaster
+      ASSERT_EQ(fts_broadcaster_->on_cleanup(rclcpp_lifecycle::State()), NODE_SUCCESS);
+    }
+    fts_broadcaster_.reset(nullptr);
+  }
+  // Add a small delay between tests
+  std::this_thread::sleep_for(std::chrono::milliseconds(50));
+}
 
 void ForceTorqueSensorBroadcasterTest::SetUpFTSBroadcaster()
 {