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

… (#1361)
ros-controls · Nov 9, 2024 · 9e5ff27 · 9e5ff27
1 parent f91b264
commit 9e5ff27
Showing 1 changed file with 16 additions and 0 deletions.
diff --git a/joint_trajectory_controller/test/test_trajectory_controller_utils.hpp b/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
 {
@@ -359,6 +360,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 subscribeToStateLegacy()
@@ -778,6 +792,8 @@ class TrajectoryControllerTestParameterized
     state_interface_types_ = std::get<1>(GetParam());
   }
 
+  virtual void TearDown() { TrajectoryControllerTest::DeactivateTrajectoryController(); }
+
   static void TearDownTestCase() { TrajectoryControllerTest::TearDownTestCase(); }
 };