diff --git a/ackermann_steering_controller/test/test_ackermann_steering_controller.hpp b/ackermann_steering_controller/test/test_ackermann_steering_controller.hpp index 2e7d688471..12a7fcb278 100644 --- a/ackermann_steering_controller/test/test_ackermann_steering_controller.hpp +++ b/ackermann_steering_controller/test/test_ackermann_steering_controller.hpp @@ -126,9 +126,20 @@ class AckermannSteeringControllerFixture : public ::testing::Test "/test_ackermann_steering_controller/reference", rclcpp::SystemDefaultsQoS()); } - static void TearDownTestCase() {} + void TearDown() + { + try + { + controller_->get_node()->shutdown(); + } + catch (...) + { + // ignore case where node is not initialized + } + controller_.reset(nullptr); // this calls the dtor, but does not call shutdown transition + } - void TearDown() { controller_.reset(nullptr); } + static void TearDownTestCase() {} protected: void SetUpController(const std::string controller_name = "test_ackermann_steering_controller") diff --git a/admittance_controller/test/test_admittance_controller.hpp b/admittance_controller/test/test_admittance_controller.hpp index 7ee56b8c11..3d70ea48c9 100644 --- a/admittance_controller/test/test_admittance_controller.hpp +++ b/admittance_controller/test/test_admittance_controller.hpp @@ -129,13 +129,21 @@ class AdmittanceControllerTest : public ::testing::Test test_broadcaster_node_ = std::make_shared("test_broadcaster_node"); } - static void TearDownTestCase() + static void TearDownTestCase() {} + + void TearDown() { - // rclcpp::shutdown(); + try + { + controller_->get_node()->shutdown(); + } + catch (...) + { + // ignore case where node is not initialized + } + controller_.reset(nullptr); // this calls the dtor, but does not call shutdown transition } - void TearDown() { controller_.reset(nullptr); } - protected: controller_interface::return_type SetUpController( const std::string & controller_name, const std::vector & parameter_overrides) diff --git a/bicycle_steering_controller/test/test_bicycle_steering_controller.hpp b/bicycle_steering_controller/test/test_bicycle_steering_controller.hpp index 25fec7fdf1..d4bc4f499c 100644 --- a/bicycle_steering_controller/test/test_bicycle_steering_controller.hpp +++ b/bicycle_steering_controller/test/test_bicycle_steering_controller.hpp @@ -124,9 +124,20 @@ class BicycleSteeringControllerFixture : public ::testing::Test "/test_bicycle_steering_controller/reference", rclcpp::SystemDefaultsQoS()); } - static void TearDownTestCase() {} + void TearDown() + { + try + { + controller_->get_node()->shutdown(); + } + catch (...) + { + // ignore case where node is not initialized + } + controller_.reset(nullptr); // this calls the dtor, but does not call shutdown transition + } - void TearDown() { controller_.reset(nullptr); } + static void TearDownTestCase() {} protected: void SetUpController(const std::string controller_name = "test_bicycle_steering_controller") diff --git a/diff_drive_controller/test/test_diff_drive_controller.cpp b/diff_drive_controller/test/test_diff_drive_controller.cpp index f2fc671920..069446f506 100644 --- a/diff_drive_controller/test/test_diff_drive_controller.cpp +++ b/diff_drive_controller/test/test_diff_drive_controller.cpp @@ -85,7 +85,9 @@ class TestableDiffDriveController : public diff_drive_controller::DiffDriveContr class TestDiffDriveController : public ::testing::Test { protected: - void SetUp() override + static void SetUpTestCase() {} + + void SetUp() { // use the name of the test as the controller name (i.e, the node name) to be able to set // parameters from yaml for each test @@ -97,7 +99,20 @@ class TestDiffDriveController : public ::testing::Test controller_name + "/cmd_vel", rclcpp::SystemDefaultsQoS()); } - static void TearDownTestCase() { rclcpp::shutdown(); } + void TearDown() + { + try + { + controller_->get_node()->shutdown(); + } + catch (...) + { + // ignore case where node is not initialized + } + controller_.reset(nullptr); // this calls the dtor, but does not call shutdown transition + } + + static void TearDownTestCase() {} /// Publish velocity msgs /** diff --git a/effort_controllers/test/test_joint_group_effort_controller.cpp b/effort_controllers/test/test_joint_group_effort_controller.cpp index c19cbff9cf..0e6e614476 100644 --- a/effort_controllers/test/test_joint_group_effort_controller.cpp +++ b/effort_controllers/test/test_joint_group_effort_controller.cpp @@ -34,7 +34,18 @@ void JointGroupEffortControllerTest::SetUp() controller_ = std::make_unique(); } -void JointGroupEffortControllerTest::TearDown() { controller_.reset(nullptr); } +void JointGroupEffortControllerTest::TearDown() +{ + try + { + controller_->get_node()->shutdown(); + } + catch (...) + { + // ignore case where node is not initialized + } + controller_.reset(nullptr); // this calls the dtor, but does not call shutdown transition +} void JointGroupEffortControllerTest::SetUpController() { diff --git a/force_torque_sensor_broadcaster/test/test_force_torque_sensor_broadcaster.cpp b/force_torque_sensor_broadcaster/test/test_force_torque_sensor_broadcaster.cpp index 26f959e88f..7fb2cf940e 100644 --- a/force_torque_sensor_broadcaster/test/test_force_torque_sensor_broadcaster.cpp +++ b/force_torque_sensor_broadcaster/test/test_force_torque_sensor_broadcaster.cpp @@ -48,7 +48,18 @@ void ForceTorqueSensorBroadcasterTest::SetUp() fts_broadcaster_ = std::make_unique(); } -void ForceTorqueSensorBroadcasterTest::TearDown() { fts_broadcaster_.reset(nullptr); } +void ForceTorqueSensorBroadcasterTest::TearDown() +{ + try + { + fts_broadcaster_->get_node()->shutdown(); + } + catch (...) + { + // ignore case where node is not initialized + } + fts_broadcaster_.reset(nullptr); // this calls the dtor, but does not call shutdown transition +} void ForceTorqueSensorBroadcasterTest::SetUpFTSBroadcaster() { diff --git a/forward_command_controller/test/test_forward_command_controller.cpp b/forward_command_controller/test/test_forward_command_controller.cpp index fcfa65ee1c..c04b843c2c 100644 --- a/forward_command_controller/test/test_forward_command_controller.cpp +++ b/forward_command_controller/test/test_forward_command_controller.cpp @@ -46,7 +46,18 @@ void ForwardCommandControllerTest::SetUp() controller_ = std::make_unique(); } -void ForwardCommandControllerTest::TearDown() { controller_.reset(nullptr); } +void ForwardCommandControllerTest::TearDown() +{ + try + { + controller_->get_node()->shutdown(); + } + catch (...) + { + // ignore case where node is not initialized + } + controller_.reset(nullptr); // this calls the dtor, but does not call shutdown transition +} void ForwardCommandControllerTest::SetUpController() { diff --git a/forward_command_controller/test/test_multi_interface_forward_command_controller.cpp b/forward_command_controller/test/test_multi_interface_forward_command_controller.cpp index c52f4f3ab6..ff218083b1 100644 --- a/forward_command_controller/test/test_multi_interface_forward_command_controller.cpp +++ b/forward_command_controller/test/test_multi_interface_forward_command_controller.cpp @@ -48,7 +48,18 @@ void MultiInterfaceForwardCommandControllerTest::SetUp() controller_ = std::make_unique(); } -void MultiInterfaceForwardCommandControllerTest::TearDown() { controller_.reset(nullptr); } +void MultiInterfaceForwardCommandControllerTest::TearDown() +{ + try + { + controller_->get_node()->shutdown(); + } + catch (...) + { + // ignore case where node is not initialized + } + controller_.reset(nullptr); // this calls the dtor, but does not call shutdown transition +} void MultiInterfaceForwardCommandControllerTest::SetUpController(bool set_params_and_activate) { diff --git a/gripper_controllers/test/test_gripper_controllers.cpp b/gripper_controllers/test/test_gripper_controllers.cpp index 97fef24eaf..a263f1e681 100644 --- a/gripper_controllers/test/test_gripper_controllers.cpp +++ b/gripper_controllers/test/test_gripper_controllers.cpp @@ -52,7 +52,15 @@ void GripperControllerTest::SetUp() template void GripperControllerTest::TearDown() { - controller_.reset(nullptr); + try + { + controller_->get_node()->shutdown(); + } + catch (...) + { + // ignore case where node is not initialized + } + controller_.reset(nullptr); // this calls the dtor, but does not call shutdown transition } template diff --git a/imu_sensor_broadcaster/test/test_imu_sensor_broadcaster.cpp b/imu_sensor_broadcaster/test/test_imu_sensor_broadcaster.cpp index 67cf31f8a5..9debe67836 100644 --- a/imu_sensor_broadcaster/test/test_imu_sensor_broadcaster.cpp +++ b/imu_sensor_broadcaster/test/test_imu_sensor_broadcaster.cpp @@ -48,7 +48,18 @@ void IMUSensorBroadcasterTest::SetUp() imu_broadcaster_ = std::make_unique(); } -void IMUSensorBroadcasterTest::TearDown() { imu_broadcaster_.reset(nullptr); } +void IMUSensorBroadcasterTest::TearDown() +{ + try + { + imu_broadcaster_->get_node()->shutdown(); + } + catch (...) + { + // ignore case where node is not initialized + } + imu_broadcaster_.reset(nullptr); // this calls the dtor, but does not call shutdown transition +} void IMUSensorBroadcasterTest::SetUpIMUBroadcaster() { diff --git a/joint_state_broadcaster/test/test_joint_state_broadcaster.cpp b/joint_state_broadcaster/test/test_joint_state_broadcaster.cpp index 3ec7ff4ac2..952a2c766e 100644 --- a/joint_state_broadcaster/test/test_joint_state_broadcaster.cpp +++ b/joint_state_broadcaster/test/test_joint_state_broadcaster.cpp @@ -57,7 +57,18 @@ void JointStateBroadcasterTest::SetUp() state_broadcaster_ = std::make_unique(); } -void JointStateBroadcasterTest::TearDown() { state_broadcaster_.reset(nullptr); } +void JointStateBroadcasterTest::TearDown() +{ + try + { + state_broadcaster_->get_node()->shutdown(); + } + catch (...) + { + // ignore case where node is not initialized + } + state_broadcaster_.reset(nullptr); // this calls the dtor, but does not call shutdown transition +} void JointStateBroadcasterTest::SetUpStateBroadcaster( const std::vector & joint_names, const std::vector & interfaces) diff --git a/joint_trajectory_controller/test/test_trajectory_controller_utils.hpp b/joint_trajectory_controller/test/test_trajectory_controller_utils.hpp index b0e66394d1..513afbda1a 100644 --- a/joint_trajectory_controller/test/test_trajectory_controller_utils.hpp +++ b/joint_trajectory_controller/test/test_trajectory_controller_utils.hpp @@ -404,6 +404,14 @@ class TrajectoryControllerTest : public ::testing::Test traj_controller_->get_node()->deactivate().id(), lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE); } + try + { + traj_controller_->get_node()->shutdown(); + } + catch (...) + { + // ignore case where node is not initialized + } } } diff --git a/mecanum_drive_controller/test/test_mecanum_drive_controller.hpp b/mecanum_drive_controller/test/test_mecanum_drive_controller.hpp index 5f7cf36dc7..bdd578aa27 100644 --- a/mecanum_drive_controller/test/test_mecanum_drive_controller.hpp +++ b/mecanum_drive_controller/test/test_mecanum_drive_controller.hpp @@ -155,9 +155,20 @@ class MecanumDriveControllerFixture : public ::testing::Test "/test_mecanum_drive_controller/tf_odometry", rclcpp::SystemDefaultsQoS()); } - static void TearDownTestCase() {} + void TearDown() + { + try + { + controller_->get_node()->shutdown(); + } + catch (...) + { + // ignore case where node is not initialized + } + controller_.reset(nullptr); // this calls the dtor, but does not call shutdown transition + } - void TearDown() { controller_.reset(nullptr); } + static void TearDownTestCase() {} protected: void SetUpController(const std::string controller_name = "test_mecanum_drive_controller") diff --git a/parallel_gripper_controller/test/test_parallel_gripper_controller.cpp b/parallel_gripper_controller/test/test_parallel_gripper_controller.cpp index 417d87d40f..d12c3eb495 100644 --- a/parallel_gripper_controller/test/test_parallel_gripper_controller.cpp +++ b/parallel_gripper_controller/test/test_parallel_gripper_controller.cpp @@ -44,7 +44,18 @@ void GripperControllerTest::SetUp() controller_ = std::make_unique(); } -void GripperControllerTest::TearDown() { controller_.reset(nullptr); } +void GripperControllerTest::TearDown() +{ + try + { + controller_->get_node()->shutdown(); + } + catch (...) + { + // ignore case where node is not initialized + } + controller_.reset(nullptr); // this calls the dtor, but does not call shutdown transition +} void GripperControllerTest::SetUpController( const std::string & controller_name = "test_gripper_action_position_controller", diff --git a/pid_controller/test/test_pid_controller.hpp b/pid_controller/test/test_pid_controller.hpp index 4471f35a12..a23846a1b3 100644 --- a/pid_controller/test/test_pid_controller.hpp +++ b/pid_controller/test/test_pid_controller.hpp @@ -128,9 +128,20 @@ class PidControllerFixture : public ::testing::Test "/test_pid_controller/set_feedforward_control"); } - static void TearDownTestCase() { rclcpp::shutdown(); } + void TearDown() + { + try + { + controller_->get_node()->shutdown(); + } + catch (...) + { + // ignore case where node is not initialized + } + controller_.reset(nullptr); // this calls the dtor, but does not call shutdown transition + } - void TearDown() { controller_.reset(nullptr); } + static void TearDownTestCase() {} protected: void SetUpController(const std::string controller_name = "test_pid_controller") diff --git a/pose_broadcaster/test/test_pose_broadcaster.cpp b/pose_broadcaster/test/test_pose_broadcaster.cpp index 0ed2e84619..a24ba655dd 100644 --- a/pose_broadcaster/test/test_pose_broadcaster.cpp +++ b/pose_broadcaster/test/test_pose_broadcaster.cpp @@ -20,7 +20,18 @@ using hardware_interface::LoanedStateInterface; void PoseBroadcasterTest::SetUp() { pose_broadcaster_ = std::make_unique(); } -void PoseBroadcasterTest::TearDown() { pose_broadcaster_.reset(nullptr); } +void PoseBroadcasterTest::TearDown() +{ + try + { + pose_broadcaster_->get_node()->shutdown(); + } + catch (...) + { + // ignore case where node is not initialized + } + pose_broadcaster_.reset(nullptr); // this calls the dtor, but does not call shutdown transition +} void PoseBroadcasterTest::SetUpPoseBroadcaster() { diff --git a/position_controllers/test/test_joint_group_position_controller.cpp b/position_controllers/test/test_joint_group_position_controller.cpp index 96a5cead17..e84080b20f 100644 --- a/position_controllers/test/test_joint_group_position_controller.cpp +++ b/position_controllers/test/test_joint_group_position_controller.cpp @@ -34,7 +34,18 @@ void JointGroupPositionControllerTest::SetUp() controller_ = std::make_unique(); } -void JointGroupPositionControllerTest::TearDown() { controller_.reset(nullptr); } +void JointGroupPositionControllerTest::TearDown() +{ + try + { + controller_->get_node()->shutdown(); + } + catch (...) + { + // ignore case where node is not initialized + } + controller_.reset(nullptr); // this calls the dtor, but does not call shutdown transition +} void JointGroupPositionControllerTest::SetUpController() { diff --git a/range_sensor_broadcaster/test/test_range_sensor_broadcaster.cpp b/range_sensor_broadcaster/test/test_range_sensor_broadcaster.cpp index 052c0384d3..ea125c2cd7 100644 --- a/range_sensor_broadcaster/test/test_range_sensor_broadcaster.cpp +++ b/range_sensor_broadcaster/test/test_range_sensor_broadcaster.cpp @@ -33,7 +33,18 @@ void RangeSensorBroadcasterTest::SetUp() range_broadcaster_ = std::make_unique(); } -void RangeSensorBroadcasterTest::TearDown() { range_broadcaster_.reset(nullptr); } +void RangeSensorBroadcasterTest::TearDown() +{ + try + { + range_broadcaster_->get_node()->shutdown(); + } + catch (...) + { + // ignore case where node is not initialized + } + range_broadcaster_.reset(nullptr); // this calls the dtor, but does not call shutdown transition +} controller_interface::return_type RangeSensorBroadcasterTest::init_broadcaster( std::string broadcaster_name) diff --git a/steering_controllers_library/test/test_steering_controllers_library.hpp b/steering_controllers_library/test/test_steering_controllers_library.hpp index 20d136ab32..3e2b43c3b4 100644 --- a/steering_controllers_library/test/test_steering_controllers_library.hpp +++ b/steering_controllers_library/test/test_steering_controllers_library.hpp @@ -147,9 +147,20 @@ class SteeringControllersLibraryFixture : public ::testing::Test "/test_steering_controllers_library/reference", rclcpp::SystemDefaultsQoS()); } - static void TearDownTestCase() {} + void TearDown() + { + try + { + controller_->get_node()->shutdown(); + } + catch (...) + { + // ignore case where node is not initialized + } + controller_.reset(nullptr); // this calls the dtor, but does not call shutdown transition + } - void TearDown() { controller_.reset(nullptr); } + static void TearDownTestCase() {} protected: void SetUpController(const std::string controller_name = "test_steering_controllers_library") diff --git a/tricycle_controller/test/test_tricycle_controller.cpp b/tricycle_controller/test/test_tricycle_controller.cpp index 5e868ebeea..17c66c9d3d 100644 --- a/tricycle_controller/test/test_tricycle_controller.cpp +++ b/tricycle_controller/test/test_tricycle_controller.cpp @@ -82,7 +82,7 @@ class TestTricycleController : public ::testing::Test protected: static void SetUpTestCase() { rclcpp::init(0, nullptr); } - void SetUp() override + void SetUp() { controller_ = std::make_unique(); pub_node = std::make_shared("velocity_publisher"); @@ -90,6 +90,19 @@ class TestTricycleController : public ::testing::Test controller_name + "/cmd_vel", rclcpp::SystemDefaultsQoS()); } + void TearDown() + { + try + { + controller_->get_node()->shutdown(); + } + catch (...) + { + // ignore case where node is not initialized + } + controller_.reset(nullptr); // this calls the dtor, but does not call shutdown transition + } + static void TearDownTestCase() { rclcpp::shutdown(); } /// Publish velocity msgs diff --git a/tricycle_steering_controller/test/test_tricycle_steering_controller.hpp b/tricycle_steering_controller/test/test_tricycle_steering_controller.hpp index cfecf96cc8..137da927df 100644 --- a/tricycle_steering_controller/test/test_tricycle_steering_controller.hpp +++ b/tricycle_steering_controller/test/test_tricycle_steering_controller.hpp @@ -125,9 +125,20 @@ class TricycleSteeringControllerFixture : public ::testing::Test "/test_tricycle_steering_controller/reference", rclcpp::SystemDefaultsQoS()); } - static void TearDownTestCase() {} + void TearDown() + { + try + { + controller_->get_node()->shutdown(); + } + catch (...) + { + // ignore case where node is not initialized + } + controller_.reset(nullptr); // this calls the dtor, but does not call shutdown transition + } - void TearDown() { controller_.reset(nullptr); } + static void TearDownTestCase() {} protected: void SetUpController(const std::string controller_name = "test_tricycle_steering_controller") diff --git a/velocity_controllers/test/test_joint_group_velocity_controller.cpp b/velocity_controllers/test/test_joint_group_velocity_controller.cpp index d781b6ca5c..ea7aeeaf2e 100644 --- a/velocity_controllers/test/test_joint_group_velocity_controller.cpp +++ b/velocity_controllers/test/test_joint_group_velocity_controller.cpp @@ -34,7 +34,18 @@ void JointGroupVelocityControllerTest::SetUp() controller_ = std::make_unique(); } -void JointGroupVelocityControllerTest::TearDown() { controller_.reset(nullptr); } +void JointGroupVelocityControllerTest::TearDown() +{ + try + { + controller_->get_node()->shutdown(); + } + catch (...) + { + // ignore case where node is not initialized + } + controller_.reset(nullptr); // this calls the dtor, but does not call shutdown transition +} void JointGroupVelocityControllerTest::SetUpController() {