From 3b38374c9507d65e3d7b3d6c51a58b8f3275b0c1 Mon Sep 17 00:00:00 2001 From: Abdullah <88815725+Theonewhomadethings@users.noreply.github.com> Date: Sun, 2 Nov 2025 21:28:40 +0000 Subject: [PATCH] Replace deprecated rclcpp::spin_some() (#541) (cherry picked from commit 18c4b124b8e9fedfcb86360344dcffc4bfbf86da) --- .../test/pid_ros_parameters_tests.cpp | 15 ++++++--- .../test/pid_ros_publisher_tests.cpp | 31 ++++++++++++++----- 2 files changed, 33 insertions(+), 13 deletions(-) diff --git a/control_toolbox/test/pid_ros_parameters_tests.cpp b/control_toolbox/test/pid_ros_parameters_tests.cpp index 8cf4b34c..d80e738a 100644 --- a/control_toolbox/test/pid_ros_parameters_tests.cpp +++ b/control_toolbox/test/pid_ros_parameters_tests.cpp @@ -240,6 +240,9 @@ TEST(PidParametersTest, SetParametersTest) { rclcpp::Node::SharedPtr node = std::make_shared("pid_parameters_test"); + rclcpp::executors::SingleThreadedExecutor executor; + executor.add_node(node); + TestablePidROS pid(node, "", "", false); const double P = 1.0; @@ -295,8 +298,7 @@ TEST(PidParametersTest, SetParametersTest) set_result = node->set_parameter(rclcpp::Parameter("activate_state_publisher", true))); ASSERT_TRUE(set_result.successful); - // process callbacks - rclcpp::spin_some(node->get_node_base_interface()); + executor.spin_some(); // check gains were set using the parameters control_toolbox::Pid::Gains gains = pid.get_gains(); @@ -315,6 +317,9 @@ TEST(PidParametersTest, SetBadParametersTest) { rclcpp::Node::SharedPtr node = std::make_shared("pid_parameters_test"); + rclcpp::executors::SingleThreadedExecutor executor; + executor.add_node(node); + TestablePidROS pid(node, "", "", false); const double P = 1.0; @@ -370,7 +375,7 @@ TEST(PidParametersTest, SetBadParametersTest) ASSERT_TRUE(set_result.successful); // process callbacks - rclcpp::spin_some(node->get_node_base_interface()); + executor.spin_some(); // check gains were NOT set using the parameters but the u_max and u_min // were set to infinity as saturation is false @@ -393,7 +398,7 @@ TEST(PidParametersTest, SetBadParametersTest) ASSERT_TRUE(set_result.successful); // process callbacks - rclcpp::spin_some(node->get_node_base_interface()); + executor.spin_some(); // Setting good gains doesn't help, as the saturation is still false gains = pid.get_gains(); @@ -412,7 +417,7 @@ TEST(PidParametersTest, SetBadParametersTest) ASSERT_TRUE(set_result.successful); // process callbacks - rclcpp::spin_some(node->get_node_base_interface()); + executor.spin_some(); // check gains were NOT set using the parameters control_toolbox::Pid::Gains updated_gains = pid.get_gains(); diff --git a/control_toolbox/test/pid_ros_publisher_tests.cpp b/control_toolbox/test/pid_ros_publisher_tests.cpp index 7ae6d146..bec7e2db 100644 --- a/control_toolbox/test/pid_ros_publisher_tests.cpp +++ b/control_toolbox/test/pid_ros_publisher_tests.cpp @@ -38,6 +38,9 @@ TEST(PidPublisherTest, PublishTest) auto node = std::make_shared("pid_publisher_test"); + rclcpp::executors::SingleThreadedExecutor executor; + executor.add_node(node); + control_toolbox::PidROS pid_ros = control_toolbox::PidROS(node, "", "", true); AntiWindupStrategy antiwindup_strat; @@ -66,7 +69,7 @@ TEST(PidPublisherTest, PublishTest) for (size_t i = 0; i < ATTEMPTS && !callback_called; ++i) { pid_ros.compute_command(-0.5, rclcpp::Duration(1, 0)); - rclcpp::spin_some(node); + executor.spin_some(); std::this_thread::sleep_for(DELAY); } @@ -82,6 +85,9 @@ TEST(PidPublisherTest, PublishTest_start_deactivated) auto node = std::make_shared("pid_publisher_test"); + rclcpp::executors::SingleThreadedExecutor executor; + executor.add_node(node); + control_toolbox::PidROS pid_ros = control_toolbox::PidROS(node, "", "", false); AntiWindupStrategy antiwindup_strat; @@ -110,7 +116,7 @@ TEST(PidPublisherTest, PublishTest_start_deactivated) for (size_t i = 0; i < ATTEMPTS && !callback_called; ++i) { pid_ros.compute_command(-0.5, rclcpp::Duration(1, 0)); - rclcpp::spin_some(node); + executor.spin_some(); std::this_thread::sleep_for(DELAY); } ASSERT_FALSE(callback_called); @@ -126,7 +132,7 @@ TEST(PidPublisherTest, PublishTest_start_deactivated) for (size_t i = 0; i < ATTEMPTS && !callback_called; ++i) { pid_ros.compute_command(-0.5, rclcpp::Duration(1, 0)); - rclcpp::spin_some(node); + executor.spin_some(); std::this_thread::sleep_for(DELAY); } ASSERT_TRUE(callback_called); @@ -135,7 +141,7 @@ TEST(PidPublisherTest, PublishTest_start_deactivated) ASSERT_NO_THROW( set_result = node->set_parameter(rclcpp::Parameter("activate_state_publisher", false))); ASSERT_TRUE(set_result.successful); - rclcpp::spin_some(node); // process callbacks to ensure that no further messages are received + executor.spin_some(); // process callbacks to ensure that no further messages are received callback_called = false; last_state_msg.reset(); @@ -143,7 +149,7 @@ TEST(PidPublisherTest, PublishTest_start_deactivated) for (size_t i = 0; i < ATTEMPTS && !callback_called; ++i) { pid_ros.compute_command(-0.5, rclcpp::Duration(1, 0)); - rclcpp::spin_some(node); + executor.spin_some(); std::this_thread::sleep_for(DELAY); } ASSERT_FALSE(callback_called); @@ -157,6 +163,9 @@ TEST(PidPublisherTest, PublishTest_prefix) auto node = std::make_shared("pid_publisher_test"); + rclcpp::executors::SingleThreadedExecutor executor; + executor.add_node(node); + // test with a prefix for the topic without trailing / (should be auto-added) control_toolbox::PidROS pid_ros = control_toolbox::PidROS(node, "", "global", true); @@ -186,7 +195,7 @@ TEST(PidPublisherTest, PublishTest_prefix) for (size_t i = 0; i < ATTEMPTS && !callback_called; ++i) { pid_ros.compute_command(-0.5, rclcpp::Duration(1, 0)); - rclcpp::spin_some(node); + executor.spin_some(); std::this_thread::sleep_for(DELAY); } @@ -202,6 +211,9 @@ TEST(PidPublisherTest, PublishTest_local_prefix) auto node = std::make_shared("pid_publisher_test"); + rclcpp::executors::SingleThreadedExecutor executor; + executor.add_node(node); + control_toolbox::PidROS pid_ros = control_toolbox::PidROS(node, "", "~/local/", true); AntiWindupStrategy antiwindup_strat; @@ -230,7 +242,7 @@ TEST(PidPublisherTest, PublishTest_local_prefix) for (size_t i = 0; i < ATTEMPTS && !callback_called; ++i) { pid_ros.compute_command(-0.5, rclcpp::Duration(1, 0)); - rclcpp::spin_some(node); + executor.spin_some(); std::this_thread::sleep_for(DELAY); } @@ -302,6 +314,9 @@ TEST(PidPublisherTest, PublishTestLifecycle) auto node = std::make_shared("pid_publisher_test"); + rclcpp::executors::SingleThreadedExecutor executor; + executor.add_node(node->get_node_base_interface()); + control_toolbox::PidROS pid_ros(node, "", "", true); auto state_pub_lifecycle_ = @@ -334,7 +349,7 @@ TEST(PidPublisherTest, PublishTestLifecycle) for (size_t i = 0; i < ATTEMPTS && !callback_called; ++i) { pid_ros.compute_command(-0.5, rclcpp::Duration(1, 0)); - rclcpp::spin_some(node->get_node_base_interface()); + executor.spin_some(); std::this_thread::sleep_for(DELAY); } ASSERT_TRUE(callback_called);