[JTC] Fix the JTC length_error exceptions in the tests (backport #1360) (#1362)

Author: mergify[bot]

joint_trajectory_controller/test/test_trajectory_controller_utils.hpp

@@ -26,6 +26,7 @@
 #include "hardware_interface/types/hardware_interface_type_values.hpp"
 #include "joint_trajectory_controller/joint_trajectory_controller.hpp"
 #include "joint_trajectory_controller/tolerances.hpp"
+#include "lifecycle_msgs/msg/state.hpp"
 
 namespace
 {
@@ -383,6 +384,19 @@ class TrajectoryControllerTest : public ::testing::Test
     return traj_controller_->get_node()->activate();
   }
 
+  void DeactivateTrajectoryController()
+  {
+    if (traj_controller_)
+    {
+      if (traj_controller_->get_state().id() == lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE)
+      {
+        EXPECT_EQ(
+          traj_controller_->get_node()->deactivate().id(),
+          lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE);
+      }
+    }
+  }
+
   static void TearDownTestCase() { rclcpp::shutdown(); }
 
   void subscribeToState(rclcpp::Executor & executor)
@@ -768,6 +782,8 @@ class TrajectoryControllerTestParameterized
     state_interface_types_ = std::get<1>(GetParam());
   }
 
+  virtual void TearDown() { TrajectoryControllerTest::DeactivateTrajectoryController(); }
+
   static void TearDownTestCase() { TrajectoryControllerTest::TearDownTestCase(); }
 };