diff --git a/joint_trajectory_controller/test/test_trajectory_controller_utils.hpp b/joint_trajectory_controller/test/test_trajectory_controller_utils.hpp index 5891f72f82..b0c9ecb63b 100644 --- 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(); } };