From 2b89b4d62124a5015b434f37c30dbe1d59623bc6 Mon Sep 17 00:00:00 2001 From: jsantoso Date: Fri, 25 Apr 2025 13:19:49 -0500 Subject: [PATCH 1/5] update tests to use smart pointers for gps broadcaster and gpio controller --- .../test/test_gpio_command_controller.cpp | 145 ++++++++++-------- .../test/test_gps_sensor_broadcaster.cpp | 53 ++++--- 2 files changed, 114 insertions(+), 84 deletions(-) diff --git a/gpio_controllers/test/test_gpio_command_controller.cpp b/gpio_controllers/test/test_gpio_command_controller.cpp index ec0e0f1228..de085894f2 100644 --- a/gpio_controllers/test/test_gpio_command_controller.cpp +++ b/gpio_controllers/test/test_gpio_command_controller.cpp @@ -95,7 +95,13 @@ class GpioCommandControllerTestSuite : public ::testing::Test rclcpp::init(0, nullptr); node = std::make_unique("node"); } + ~GpioCommandControllerTestSuite() { rclcpp::shutdown(); } + + void SetUp() { controller_ = std::make_unique(); } + + void TearDown() { controller_.reset(nullptr); } + void setup_command_and_state_interfaces() { std::vector command_interfaces; @@ -108,15 +114,15 @@ class GpioCommandControllerTestSuite : public ::testing::Test state_interfaces.emplace_back(gpio_1_2_dig_state); state_interfaces.emplace_back(gpio_2_ana_state); - controller.assign_interfaces(std::move(command_interfaces), std::move(state_interfaces)); + controller_->assign_interfaces(std::move(command_interfaces), std::move(state_interfaces)); } void move_to_activate_state(controller_interface::return_type result_of_initialization) { ASSERT_EQ(result_of_initialization, controller_interface::return_type::OK); - ASSERT_EQ(controller.on_configure(rclcpp_lifecycle::State()), CallbackReturn::SUCCESS); + ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), CallbackReturn::SUCCESS); setup_command_and_state_interfaces(); - ASSERT_EQ(controller.on_activate(rclcpp_lifecycle::State()), CallbackReturn::SUCCESS); + ASSERT_EQ(controller_->on_activate(rclcpp_lifecycle::State()), CallbackReturn::SUCCESS); } void stop_test_when_message_cannot_be_published(int max_sub_check_loop_count) @@ -138,7 +144,7 @@ class GpioCommandControllerTestSuite : public ::testing::Test void update_controller_loop() { ASSERT_EQ( - controller.update(rclcpp::Time(0), rclcpp::Duration::from_seconds(0.01)), + controller_->update(rclcpp::Time(0), rclcpp::Duration::from_seconds(0.01)), controller_interface::return_type::OK); } @@ -165,10 +171,10 @@ class GpioCommandControllerTestSuite : public ::testing::Test void wait_one_miliseconds_for_timeout() { rclcpp::executors::SingleThreadedExecutor executor; - executor.add_node(controller.get_node()->get_node_base_interface()); + executor.add_node(controller_->get_node()->get_node_base_interface()); const auto timeout = std::chrono::milliseconds{1}; - const auto until = controller.get_node()->get_clock()->now() + timeout; - while (controller.get_node()->get_clock()->now() < until) + const auto until = controller_->get_node()->get_clock()->now() + timeout; + while (controller_->get_node()->get_clock()->now() < until) { executor.spin_some(); std::this_thread::sleep_for(std::chrono::microseconds(10)); @@ -182,7 +188,7 @@ class GpioCommandControllerTestSuite : public ::testing::Test wait_set.add_subscription(subscription); while (max_sub_check_loop_count--) { - controller.update(rclcpp::Time(0), rclcpp::Duration::from_seconds(0.01)); + controller_->update(rclcpp::Time(0), rclcpp::Duration::from_seconds(0.01)); if (wait_set.wait(std::chrono::milliseconds(2)).kind() == rclcpp::WaitResultKind::Ready) { break; @@ -191,7 +197,7 @@ class GpioCommandControllerTestSuite : public ::testing::Test return max_sub_check_loop_count; } - FriendGpioCommandController controller; + std::unique_ptr controller_; const std::vector gpio_names{"gpio1", "gpio2"}; std::vector gpio_commands{1.0, 0.0, 3.1}; @@ -209,9 +215,9 @@ class GpioCommandControllerTestSuite : public ::testing::Test TEST_F(GpioCommandControllerTestSuite, WhenNoParametersAreSetInitShouldFail) { - const auto result = controller.init( + const auto result = controller_->init( "test_gpio_command_controller", ros2_control_test_assets::minimal_robot_urdf, 0, "", - controller.define_custom_node_options()); + controller_->define_custom_node_options()); ASSERT_EQ(result, controller_interface::return_type::ERROR); } @@ -219,7 +225,7 @@ TEST_F(GpioCommandControllerTestSuite, WhenGpiosParameterIsEmptyInitShouldFail) { const auto node_options = create_node_options_with_overriden_parameters({{"gpios", std::vector{}}}); - const auto result = controller.init("test_gpio_command_controller", "", 0, "", node_options); + const auto result = controller_->init("test_gpio_command_controller", "", 0, "", node_options); ASSERT_EQ(result, controller_interface::return_type::ERROR); } @@ -230,7 +236,7 @@ TEST_F(GpioCommandControllerTestSuite, WhenInterfacesParameterForGpioIsEmptyInit {{"gpios", std::vector{"gpio1"}}, {"command_interfaces.gpio1.interfaces", std::vector{}}, {"state_interfaces.gpio1.interfaces", std::vector{}}}); - const auto result = controller.init("test_gpio_command_controller", "", 0, "", node_options); + const auto result = controller_->init("test_gpio_command_controller", "", 0, "", node_options); ASSERT_EQ(result, controller_interface::return_type::OK); } @@ -239,7 +245,7 @@ TEST_F(GpioCommandControllerTestSuite, WhenInterfacesParameterForGpioIsNotSetIni { const auto node_options = create_node_options_with_overriden_parameters({{"gpios", std::vector{"gpio1"}}}); - const auto result = controller.init("test_gpio_command_controller", "", 0, "", node_options); + const auto result = controller_->init("test_gpio_command_controller", "", 0, "", node_options); ASSERT_EQ(result, controller_interface::return_type::OK); } @@ -255,7 +261,7 @@ TEST_F( {"state_interfaces.gpio1.interfaces", std::vector{"dig.1", "dig.2"}}, {"state_interfaces.gpio2.interfaces", std::vector{"ana.1"}}}); - const auto result = controller.init("test_gpio_command_controller", "", 0, "", node_options); + const auto result = controller_->init("test_gpio_command_controller", "", 0, "", node_options); ASSERT_EQ(result, controller_interface::return_type::OK); } @@ -267,11 +273,11 @@ TEST_F( {{"gpios", std::vector{"gpio1"}}, {"command_interfaces.gpio1.interfaces", std::vector{}}, {"state_interfaces.gpio1.interfaces", std::vector{}}}); - const auto result = controller.init( + const auto result = controller_->init( "test_gpio_command_controller", ros2_control_test_assets::minimal_robot_urdf, 0, "", node_options); ASSERT_EQ(result, controller_interface::return_type::OK); - ASSERT_EQ(controller.on_configure(rclcpp_lifecycle::State()), CallbackReturn::ERROR); + ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), CallbackReturn::ERROR); } TEST_F( @@ -282,11 +288,11 @@ TEST_F( {{"gpios", std::vector{"gpio1"}}, {"command_interfaces.gpio1.interfaces", std::vector{}}, {"state_interfaces.gpio1.interfaces", std::vector{}}}); - const auto result = controller.init( + const auto result = controller_->init( "test_gpio_command_controller", minimal_robot_urdf_with_gpio, 0, "", node_options); ASSERT_EQ(result, controller_interface::return_type::OK); - ASSERT_EQ(controller.on_configure(rclcpp_lifecycle::State()), CallbackReturn::SUCCESS); + ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), CallbackReturn::SUCCESS); } TEST_F( @@ -297,10 +303,10 @@ TEST_F( {{"gpios", std::vector{"gpio1"}}, {"command_interfaces.gpio1.interfaces", std::vector{}}, {"state_interfaces.gpio1.interfaces", std::vector{}}}); - const auto result = controller.init("test_gpio_command_controller", "", 0, "", node_options); + const auto result = controller_->init("test_gpio_command_controller", "", 0, "", node_options); ASSERT_EQ(result, controller_interface::return_type::OK); - ASSERT_EQ(controller.on_configure(rclcpp_lifecycle::State()), CallbackReturn::ERROR); + ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), CallbackReturn::ERROR); } TEST_F(GpioCommandControllerTestSuite, ConfigureAndActivateParamsSuccess) @@ -311,12 +317,12 @@ TEST_F(GpioCommandControllerTestSuite, ConfigureAndActivateParamsSuccess) {"command_interfaces.gpio2.interfaces", std::vector{"ana.1"}}, {"state_interfaces.gpio1.interfaces", std::vector{"dig.1", "dig.2"}}, {"state_interfaces.gpio2.interfaces", std::vector{"ana.1"}}}); - const auto result = controller.init("test_gpio_command_controller", "", 0, "", node_options); + const auto result = controller_->init("test_gpio_command_controller", "", 0, "", node_options); ASSERT_EQ(result, controller_interface::return_type::OK); - ASSERT_EQ(controller.on_configure(rclcpp_lifecycle::State()), CallbackReturn::SUCCESS); + ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), CallbackReturn::SUCCESS); setup_command_and_state_interfaces(); - ASSERT_EQ(controller.on_activate(rclcpp_lifecycle::State()), CallbackReturn::SUCCESS); + ASSERT_EQ(controller_->on_activate(rclcpp_lifecycle::State()), CallbackReturn::SUCCESS); } TEST_F( @@ -329,10 +335,10 @@ TEST_F( {"command_interfaces.gpio2.interfaces", std::vector{"ana.1"}}, {"state_interfaces.gpio1.interfaces", std::vector{"dig.1", "dig.2"}}, {"state_interfaces.gpio2.interfaces", std::vector{"ana.1"}}}); - const auto result = controller.init("test_gpio_command_controller", "", 0, "", node_options); + const auto result = controller_->init("test_gpio_command_controller", "", 0, "", node_options); ASSERT_EQ(result, controller_interface::return_type::OK); - ASSERT_EQ(controller.on_configure(rclcpp_lifecycle::State()), CallbackReturn::SUCCESS); + ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), CallbackReturn::SUCCESS); std::vector command_interfaces; command_interfaces.emplace_back(gpio_1_1_dig_cmd); @@ -343,8 +349,8 @@ TEST_F( state_interfaces.emplace_back(gpio_1_2_dig_state); state_interfaces.emplace_back(gpio_2_ana_state); - controller.assign_interfaces(std::move(command_interfaces), std::move(state_interfaces)); - ASSERT_EQ(controller.on_activate(rclcpp_lifecycle::State()), CallbackReturn::ERROR); + controller_->assign_interfaces(std::move(command_interfaces), std::move(state_interfaces)); + ASSERT_EQ(controller_->on_activate(rclcpp_lifecycle::State()), CallbackReturn::ERROR); } TEST_F( @@ -357,10 +363,10 @@ TEST_F( {"command_interfaces.gpio2.interfaces", std::vector{"ana.1"}}, {"state_interfaces.gpio1.interfaces", std::vector{"dig.1", "dig.2"}}, {"state_interfaces.gpio2.interfaces", std::vector{"ana.1"}}}); - const auto result = controller.init("test_gpio_command_controller", "", 0, "", node_options); + const auto result = controller_->init("test_gpio_command_controller", "", 0, "", node_options); ASSERT_EQ(result, controller_interface::return_type::OK); - ASSERT_EQ(controller.on_configure(rclcpp_lifecycle::State()), CallbackReturn::SUCCESS); + ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), CallbackReturn::SUCCESS); std::vector command_interfaces; command_interfaces.emplace_back(gpio_1_1_dig_cmd); @@ -371,9 +377,9 @@ TEST_F( state_interfaces.emplace_back(gpio_1_1_dig_state); state_interfaces.emplace_back(gpio_1_2_dig_state); - controller.assign_interfaces(std::move(command_interfaces), std::move(state_interfaces)); + controller_->assign_interfaces(std::move(command_interfaces), std::move(state_interfaces)); - ASSERT_EQ(controller.on_activate(rclcpp_lifecycle::State()), CallbackReturn::ERROR); + ASSERT_EQ(controller_->on_activate(rclcpp_lifecycle::State()), CallbackReturn::ERROR); } TEST_F( @@ -386,10 +392,10 @@ TEST_F( {"command_interfaces.gpio2.interfaces", std::vector{"ana.1"}}, {"state_interfaces.gpio1.interfaces", std::vector{"dig.1"}}, {"state_interfaces.gpio2.interfaces", std::vector{"ana.1"}}}); - const auto result = controller.init("test_gpio_command_controller", "", 0, "", node_options); + const auto result = controller_->init("test_gpio_command_controller", "", 0, "", node_options); ASSERT_EQ(result, controller_interface::return_type::OK); - ASSERT_EQ(controller.on_configure(rclcpp_lifecycle::State()), CallbackReturn::SUCCESS); + ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), CallbackReturn::SUCCESS); std::vector command_interfaces; command_interfaces.emplace_back(gpio_1_1_dig_cmd); @@ -400,9 +406,9 @@ TEST_F( state_interfaces.emplace_back(gpio_1_1_dig_state); state_interfaces.emplace_back(gpio_2_ana_state); - controller.assign_interfaces(std::move(command_interfaces), std::move(state_interfaces)); + controller_->assign_interfaces(std::move(command_interfaces), std::move(state_interfaces)); - ASSERT_EQ(controller.on_activate(rclcpp_lifecycle::State()), CallbackReturn::SUCCESS); + ASSERT_EQ(controller_->on_activate(rclcpp_lifecycle::State()), CallbackReturn::SUCCESS); } TEST_F( @@ -416,7 +422,8 @@ TEST_F( {"state_interfaces.gpio1.interfaces", std::vector{"dig.1", "dig.2"}}, {"state_interfaces.gpio2.interfaces", std::vector{"ana.1"}}}); - move_to_activate_state(controller.init("test_gpio_command_controller", "", 0, "", node_options)); + move_to_activate_state( + controller_->init("test_gpio_command_controller", "", 0, "", node_options)); assert_default_command_and_state_values(); update_controller_loop(); assert_default_command_and_state_values(); @@ -432,14 +439,15 @@ TEST_F( {"command_interfaces.gpio2.interfaces", std::vector{"ana.1"}}, {"state_interfaces.gpio1.interfaces", std::vector{"dig.1", "dig.2"}}, {"state_interfaces.gpio2.interfaces", std::vector{"ana.1"}}}); - move_to_activate_state(controller.init("test_gpio_command_controller", "", 0, "", node_options)); + move_to_activate_state( + controller_->init("test_gpio_command_controller", "", 0, "", node_options)); const auto command = createGpioCommand( {"gpio1", "gpio2"}, {createInterfaceValue({"dig.1", "dig.2"}, {0.0, 1.0, 1.0}), createInterfaceValue({"ana.1"}, {30.0})}); - controller.rt_command_ptr_.writeFromNonRT(std::make_shared(command)); + controller_->rt_command_ptr_.writeFromNonRT(std::make_shared(command)); ASSERT_EQ( - controller.update(rclcpp::Time(0), rclcpp::Duration::from_seconds(0.01)), + controller_->update(rclcpp::Time(0), rclcpp::Duration::from_seconds(0.01)), controller_interface::return_type::ERROR); } @@ -453,14 +461,15 @@ TEST_F( {"command_interfaces.gpio2.interfaces", std::vector{"ana.1"}}, {"state_interfaces.gpio1.interfaces", std::vector{"dig.1", "dig.2"}}, {"state_interfaces.gpio2.interfaces", std::vector{"ana.1"}}}); - move_to_activate_state(controller.init("test_gpio_command_controller", "", 0, "", node_options)); + move_to_activate_state( + controller_->init("test_gpio_command_controller", "", 0, "", node_options)); const auto command = createGpioCommand( {"gpio1", "gpio2"}, {createInterfaceValue({"dig.1", "dig.2"}, {0.0}), createInterfaceValue({"ana.1"}, {30.0})}); - controller.rt_command_ptr_.writeFromNonRT(std::make_shared(command)); + controller_->rt_command_ptr_.writeFromNonRT(std::make_shared(command)); ASSERT_EQ( - controller.update(rclcpp::Time(0), rclcpp::Duration::from_seconds(0.01)), + controller_->update(rclcpp::Time(0), rclcpp::Duration::from_seconds(0.01)), controller_interface::return_type::ERROR); } @@ -474,12 +483,13 @@ TEST_F( {"command_interfaces.gpio2.interfaces", std::vector{"ana.1"}}, {"state_interfaces.gpio1.interfaces", std::vector{"dig.1", "dig.2"}}, {"state_interfaces.gpio2.interfaces", std::vector{"ana.1"}}}); - move_to_activate_state(controller.init("test_gpio_command_controller", "", 0, "", node_options)); + move_to_activate_state( + controller_->init("test_gpio_command_controller", "", 0, "", node_options)); const auto command = createGpioCommand( {"gpio1", "gpio2"}, {createInterfaceValue({"dig.1", "dig.2"}, {0.0, 1.0}), createInterfaceValue({"ana.1"}, {30.0})}); - controller.rt_command_ptr_.writeFromNonRT(std::make_shared(command)); + controller_->rt_command_ptr_.writeFromNonRT(std::make_shared(command)); update_controller_loop(); ASSERT_EQ(gpio_1_1_dig_cmd.get_value(), 0.0); @@ -497,12 +507,13 @@ TEST_F( {"command_interfaces.gpio2.interfaces", std::vector{"ana.1"}}, {"state_interfaces.gpio1.interfaces", std::vector{"dig.1", "dig.2"}}, {"state_interfaces.gpio2.interfaces", std::vector{"ana.1"}}}); - move_to_activate_state(controller.init("test_gpio_command_controller", "", 0, "", node_options)); + move_to_activate_state( + controller_->init("test_gpio_command_controller", "", 0, "", node_options)); const auto command = createGpioCommand( {"gpio2", "gpio1"}, {createInterfaceValue({"ana.1"}, {30.0}), createInterfaceValue({"dig.2", "dig.1"}, {1.0, 0.0})}); - controller.rt_command_ptr_.writeFromNonRT(std::make_shared(command)); + controller_->rt_command_ptr_.writeFromNonRT(std::make_shared(command)); update_controller_loop(); ASSERT_EQ(gpio_1_1_dig_cmd.get_value(), 0.0); @@ -520,12 +531,13 @@ TEST_F( {"command_interfaces.gpio2.interfaces", std::vector{"ana.1"}}, {"state_interfaces.gpio1.interfaces", std::vector{"dig.1", "dig.2"}}, {"state_interfaces.gpio2.interfaces", std::vector{"ana.1"}}}); - move_to_activate_state(controller.init("test_gpio_command_controller", "", 0, "", node_options)); + move_to_activate_state( + controller_->init("test_gpio_command_controller", "", 0, "", node_options)); const auto command = createGpioCommand({"gpio1"}, {createInterfaceValue({"dig.1", "dig.2"}, {0.0, 1.0})}); - controller.rt_command_ptr_.writeFromNonRT(std::make_shared(command)); + controller_->rt_command_ptr_.writeFromNonRT(std::make_shared(command)); update_controller_loop(); ASSERT_EQ(gpio_1_1_dig_cmd.get_value(), 0.0); @@ -543,13 +555,14 @@ TEST_F( {"command_interfaces.gpio2.interfaces", std::vector{"ana.1"}}, {"state_interfaces.gpio1.interfaces", std::vector{"dig.1", "dig.2"}}, {"state_interfaces.gpio2.interfaces", std::vector{"ana.1"}}}); - move_to_activate_state(controller.init("test_gpio_command_controller", "", 0, "", node_options)); + move_to_activate_state( + controller_->init("test_gpio_command_controller", "", 0, "", node_options)); const auto command = createGpioCommand( {"gpio1", "gpio3"}, {createInterfaceValue({"dig.3", "dig.4"}, {20.0, 25.0}), createInterfaceValue({"ana.1"}, {21.0})}); - controller.rt_command_ptr_.writeFromNonRT(std::make_shared(command)); + controller_->rt_command_ptr_.writeFromNonRT(std::make_shared(command)); update_controller_loop(); ASSERT_EQ(gpio_1_1_dig_cmd.get_value(), gpio_commands.at(0)); @@ -567,10 +580,11 @@ TEST_F( {"command_interfaces.gpio2.interfaces", std::vector{"ana.1"}}, {"state_interfaces.gpio1.interfaces", std::vector{"dig.1", "dig.2"}}, {"state_interfaces.gpio2.interfaces", std::vector{"ana.1"}}}); - move_to_activate_state(controller.init("test_gpio_command_controller", "", 0, "", node_options)); + move_to_activate_state( + controller_->init("test_gpio_command_controller", "", 0, "", node_options)); auto command_pub = node->create_publisher( - std::string(controller.get_node()->get_name()) + "/commands", rclcpp::SystemDefaultsQoS()); + std::string(controller_->get_node()->get_name()) + "/commands", rclcpp::SystemDefaultsQoS()); const auto command = createGpioCommand( {"gpio1", "gpio2"}, {createInterfaceValue({"dig.1", "dig.2"}, {0.0, 1.0}), createInterfaceValue({"ana.1"}, {30.0})}); @@ -591,10 +605,11 @@ TEST_F(GpioCommandControllerTestSuite, ControllerShouldPublishGpioStatesWithCurr {"command_interfaces.gpio2.interfaces", std::vector{"ana.1"}}, {"state_interfaces.gpio1.interfaces", std::vector{"dig.1", "dig.2"}}, {"state_interfaces.gpio2.interfaces", std::vector{"ana.1"}}}); - move_to_activate_state(controller.init("test_gpio_command_controller", "", 0, "", node_options)); + move_to_activate_state( + controller_->init("test_gpio_command_controller", "", 0, "", node_options)); auto subscription = node->create_subscription( - std::string(controller.get_node()->get_name()) + "/gpio_states", 10, + std::string(controller_->get_node()->get_name()) + "/gpio_states", 10, [&](const StateType::SharedPtr) {}); stop_test_when_message_cannot_be_published(wait_for_subscription(subscription)); @@ -630,15 +645,15 @@ TEST_F( state_interfaces.emplace_back(gpio_1_1_dig_state); state_interfaces.emplace_back(gpio_2_ana_state); - const auto result_of_initialization = controller.init( + const auto result_of_initialization = controller_->init( "test_gpio_command_controller", minimal_robot_urdf_with_gpio, 0, "", node_options); ASSERT_EQ(result_of_initialization, controller_interface::return_type::OK); - ASSERT_EQ(controller.on_configure(rclcpp_lifecycle::State()), CallbackReturn::SUCCESS); - controller.assign_interfaces(std::move(command_interfaces), std::move(state_interfaces)); - ASSERT_EQ(controller.on_activate(rclcpp_lifecycle::State()), CallbackReturn::SUCCESS); + ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), CallbackReturn::SUCCESS); + controller_->assign_interfaces(std::move(command_interfaces), std::move(state_interfaces)); + ASSERT_EQ(controller_->on_activate(rclcpp_lifecycle::State()), CallbackReturn::SUCCESS); auto subscription = node->create_subscription( - std::string(controller.get_node()->get_name()) + "/gpio_states", 10, + std::string(controller_->get_node()->get_name()) + "/gpio_states", 10, [&](const StateType::SharedPtr) {}); stop_test_when_message_cannot_be_published(wait_for_subscription(subscription)); @@ -667,14 +682,14 @@ TEST_F( state_interfaces.emplace_back(gpio_2_ana_state); const auto result_of_initialization = - controller.init("test_gpio_command_controller", "", 0, "", node_options); + controller_->init("test_gpio_command_controller", "", 0, "", node_options); ASSERT_EQ(result_of_initialization, controller_interface::return_type::OK); - ASSERT_EQ(controller.on_configure(rclcpp_lifecycle::State()), CallbackReturn::SUCCESS); - controller.assign_interfaces({}, std::move(state_interfaces)); - ASSERT_EQ(controller.on_activate(rclcpp_lifecycle::State()), CallbackReturn::SUCCESS); + ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), CallbackReturn::SUCCESS); + controller_->assign_interfaces({}, std::move(state_interfaces)); + ASSERT_EQ(controller_->on_activate(rclcpp_lifecycle::State()), CallbackReturn::SUCCESS); auto subscription = node->create_subscription( - std::string(controller.get_node()->get_name()) + "/gpio_states", 10, + std::string(controller_->get_node()->get_name()) + "/gpio_states", 10, [&](const StateType::SharedPtr) {}); stop_test_when_message_cannot_be_published(wait_for_subscription(subscription)); diff --git a/gps_sensor_broadcaster/test/test_gps_sensor_broadcaster.cpp b/gps_sensor_broadcaster/test/test_gps_sensor_broadcaster.cpp index d0e6f885d9..ea5b2d890e 100644 --- a/gps_sensor_broadcaster/test/test_gps_sensor_broadcaster.cpp +++ b/gps_sensor_broadcaster/test/test_gps_sensor_broadcaster.cpp @@ -60,7 +60,18 @@ class GPSSensorBroadcasterTest : public ::testing::Test public: GPSSensorBroadcasterTest() { rclcpp::init(0, nullptr); } - ~GPSSensorBroadcasterTest() { rclcpp::shutdown(); } + ~GPSSensorBroadcasterTest() + { + gps_broadcaster_.reset(nullptr); + rclcpp::shutdown(); + } + + void SetUp() + { + gps_broadcaster_ = std::make_unique(); + } + + void TearDown() { gps_broadcaster_.reset(nullptr); } template < semantic_components::GPSSensorOption sensor_option = @@ -80,7 +91,7 @@ class GPSSensorBroadcasterTest : public ::testing::Test state_ifs.emplace_back(altitude_covariance_); } - gps_broadcaster_.assign_interfaces({}, std::move(state_ifs)); + gps_broadcaster_->assign_interfaces({}, std::move(state_ifs)); } sensor_msgs::msg::NavSatFix subscribe_and_get_message() @@ -89,7 +100,7 @@ class GPSSensorBroadcasterTest : public ::testing::Test auto subscription = test_subscription_node.create_subscription( "/test_gps_sensor_broadcaster/gps/fix", 10, [](const sensor_msgs::msg::NavSatFix::SharedPtr) {}); - gps_broadcaster_.update(rclcpp::Time{}, rclcpp::Duration::from_seconds(0)); + gps_broadcaster_->update(rclcpp::Time{}, rclcpp::Duration::from_seconds(0)); wait_for(subscription); rclcpp::MessageInfo msg_info; @@ -115,21 +126,21 @@ class GPSSensorBroadcasterTest : public ::testing::Test hardware_interface::StateInterface altitude_covariance_{ sensor_name_, "altitude_covariance", &sensor_values_[7]}; - gps_sensor_broadcaster::GPSSensorBroadcaster gps_broadcaster_; + std::unique_ptr gps_broadcaster_; }; TEST_F(GPSSensorBroadcasterTest, whenNoParamsAreSetThenInitShouldFail) { - const auto result = gps_broadcaster_.init( + const auto result = gps_broadcaster_->init( "test_gps_sensor_broadcaster", ros2_control_test_assets::minimal_robot_urdf, 0, "", - gps_broadcaster_.define_custom_node_options()); + gps_broadcaster_->define_custom_node_options()); ASSERT_EQ(result, controller_interface::return_type::ERROR); } TEST_F(GPSSensorBroadcasterTest, whenOnlySensorNameIsSetThenInitShouldFail) { const auto node_options = create_node_options_with_overriden_parameters({sensor_name_param_}); - const auto result = gps_broadcaster_.init( + const auto result = gps_broadcaster_->init( "test_gps_sensor_broadcaster", ros2_control_test_assets::minimal_robot_urdf, 0, "", node_options); ASSERT_EQ(result, controller_interface::return_type::ERROR); @@ -141,13 +152,14 @@ TEST_F( { const auto node_options = create_node_options_with_overriden_parameters({sensor_name_param_, frame_id_}); - const auto result = gps_broadcaster_.init( + const auto result = gps_broadcaster_->init( "test_gps_sensor_broadcaster", ros2_control_test_assets::minimal_robot_urdf, 0, "", node_options); ASSERT_EQ(result, controller_interface::return_type::OK); ASSERT_EQ( - gps_broadcaster_.on_configure(rclcpp_lifecycle::State()), callback_return_type::SUCCESS); - ASSERT_EQ(gps_broadcaster_.on_activate(rclcpp_lifecycle::State()), callback_return_type::SUCCESS); + gps_broadcaster_->on_configure(rclcpp_lifecycle::State()), callback_return_type::SUCCESS); + ASSERT_EQ( + gps_broadcaster_->on_activate(rclcpp_lifecycle::State()), callback_return_type::SUCCESS); } TEST_F( @@ -155,14 +167,15 @@ TEST_F( { const auto node_options = create_node_options_with_overriden_parameters({sensor_name_param_, frame_id_}); - const auto result = gps_broadcaster_.init( + const auto result = gps_broadcaster_->init( "test_gps_sensor_broadcaster", ros2_control_test_assets::minimal_robot_urdf, 0, "", node_options); ASSERT_EQ(result, controller_interface::return_type::OK); ASSERT_EQ( - gps_broadcaster_.on_configure(rclcpp_lifecycle::State()), callback_return_type::SUCCESS); + gps_broadcaster_->on_configure(rclcpp_lifecycle::State()), callback_return_type::SUCCESS); setup_gps_broadcaster(); - ASSERT_EQ(gps_broadcaster_.on_activate(rclcpp_lifecycle::State()), callback_return_type::SUCCESS); + ASSERT_EQ( + gps_broadcaster_->on_activate(rclcpp_lifecycle::State()), callback_return_type::SUCCESS); const auto gps_msg = subscribe_and_get_message(); EXPECT_EQ(gps_msg.header.frame_id, frame_id_.get_value()); @@ -186,14 +199,15 @@ TEST_F( frame_id_, {"static_position_covariance", std::vector{static_covariance.begin(), static_covariance.end()}}}); - const auto result = gps_broadcaster_.init( + const auto result = gps_broadcaster_->init( "test_gps_sensor_broadcaster", ros2_control_test_assets::minimal_robot_urdf, 0, "", node_options); ASSERT_EQ(result, controller_interface::return_type::OK); ASSERT_EQ( - gps_broadcaster_.on_configure(rclcpp_lifecycle::State()), callback_return_type::SUCCESS); + gps_broadcaster_->on_configure(rclcpp_lifecycle::State()), callback_return_type::SUCCESS); setup_gps_broadcaster(); - ASSERT_EQ(gps_broadcaster_.on_activate(rclcpp_lifecycle::State()), callback_return_type::SUCCESS); + ASSERT_EQ( + gps_broadcaster_->on_activate(rclcpp_lifecycle::State()), callback_return_type::SUCCESS); const auto gps_msg = subscribe_and_get_message(); EXPECT_EQ(gps_msg.header.frame_id, frame_id_.get_value()); @@ -213,14 +227,15 @@ TEST_F( { const auto node_options = create_node_options_with_overriden_parameters( {sensor_name_param_, frame_id_, {"read_covariance_from_interface", true}}); - const auto result = gps_broadcaster_.init( + const auto result = gps_broadcaster_->init( "test_gps_sensor_broadcaster", ros2_control_test_assets::minimal_robot_urdf, 0, "", node_options); ASSERT_EQ(result, controller_interface::return_type::OK); ASSERT_EQ( - gps_broadcaster_.on_configure(rclcpp_lifecycle::State()), callback_return_type::SUCCESS); + gps_broadcaster_->on_configure(rclcpp_lifecycle::State()), callback_return_type::SUCCESS); setup_gps_broadcaster(); - ASSERT_EQ(gps_broadcaster_.on_activate(rclcpp_lifecycle::State()), callback_return_type::SUCCESS); + ASSERT_EQ( + gps_broadcaster_->on_activate(rclcpp_lifecycle::State()), callback_return_type::SUCCESS); const auto gps_msg = subscribe_and_get_message(); EXPECT_EQ(gps_msg.header.frame_id, frame_id_.get_value()); From af368fca7d8ab34856f0d951ccb4f02c2319a595 Mon Sep 17 00:00:00 2001 From: jsantoso Date: Fri, 25 Apr 2025 13:37:19 -0500 Subject: [PATCH 2/5] remove extra line --- gps_sensor_broadcaster/test/test_gps_sensor_broadcaster.cpp | 6 +----- 1 file changed, 1 insertion(+), 5 deletions(-) diff --git a/gps_sensor_broadcaster/test/test_gps_sensor_broadcaster.cpp b/gps_sensor_broadcaster/test/test_gps_sensor_broadcaster.cpp index ea5b2d890e..e4ac047052 100644 --- a/gps_sensor_broadcaster/test/test_gps_sensor_broadcaster.cpp +++ b/gps_sensor_broadcaster/test/test_gps_sensor_broadcaster.cpp @@ -60,11 +60,7 @@ class GPSSensorBroadcasterTest : public ::testing::Test public: GPSSensorBroadcasterTest() { rclcpp::init(0, nullptr); } - ~GPSSensorBroadcasterTest() - { - gps_broadcaster_.reset(nullptr); - rclcpp::shutdown(); - } + ~GPSSensorBroadcasterTest() { rclcpp::shutdown(); } void SetUp() { From 0f01d2c9d620c8b1d48692f4ce296b21d682d0c9 Mon Sep 17 00:00:00 2001 From: jsantoso Date: Wed, 14 May 2025 09:53:31 -0500 Subject: [PATCH 3/5] update tests to use configure/activate instead of on_configure/on_activate --- .../test/test_diff_drive_controller.cpp | 162 ++++++++++-------- .../test/test_gpio_command_controller.cpp | 89 ++++++---- .../test/test_gps_sensor_broadcaster.cpp | 59 +++++-- 3 files changed, 190 insertions(+), 120 deletions(-) diff --git a/diff_drive_controller/test/test_diff_drive_controller.cpp b/diff_drive_controller/test/test_diff_drive_controller.cpp index 1b01e5b067..f7342049aa 100644 --- a/diff_drive_controller/test/test_diff_drive_controller.cpp +++ b/diff_drive_controller/test/test_diff_drive_controller.cpp @@ -198,6 +198,27 @@ class TestDiffDriveController : public ::testing::Test return controller_->init(controller_name, urdf_, 0, ns, node_options); } + void expect_configure_succeeded( + std::unique_ptr & controller, bool succeeded) + { + auto state = controller->configure(); + + if (succeeded) + ASSERT_EQ(State::PRIMARY_STATE_INACTIVE, state.id()); + else + ASSERT_EQ(State::PRIMARY_STATE_UNCONFIGURED, state.id()); + } + + void expect_activate_succeeded( + std::unique_ptr & controller, bool succeeded) + { + auto state = controller->get_node()->activate(); + if (succeeded) + ASSERT_EQ(State::PRIMARY_STATE_ACTIVE, state.id()); + else + ASSERT_EQ(State::PRIMARY_STATE_UNCONFIGURED, state.id()); + } + std::string controller_name; std::unique_ptr controller_; @@ -243,7 +264,7 @@ TEST_F(TestDiffDriveController, configure_fails_with_mismatching_wheel_side_size InitController(left_wheel_names, {right_wheel_names[0], "extra_wheel"}), controller_interface::return_type::OK); - ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), CallbackReturn::ERROR); + expect_configure_succeeded(controller_, false); } TEST_F( @@ -252,7 +273,7 @@ TEST_F( { ASSERT_EQ(InitController(), controller_interface::return_type::OK); - ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), CallbackReturn::SUCCESS); + expect_configure_succeeded(controller_, true); auto state_if_conf = controller_->state_interface_configuration(); ASSERT_THAT(state_if_conf.names, SizeIs(left_wheel_names.size() + right_wheel_names.size())); @@ -262,7 +283,7 @@ TEST_F( EXPECT_EQ(cmd_if_conf.type, controller_interface::interface_configuration_type::INDIVIDUAL); } -TEST_F(TestDiffDriveController, configure_succeeds_tf_test_prefix_false_no_namespace) +TEST_F(TestDiffDriveController, expect_configure_succeededs_tf_test_prefix_false_no_namespace) { std::string odom_id = "odom"; std::string base_link_id = "base_link"; @@ -277,7 +298,7 @@ TEST_F(TestDiffDriveController, configure_succeeds_tf_test_prefix_false_no_names rclcpp::Parameter("base_frame_id", rclcpp::ParameterValue(base_link_id))}), controller_interface::return_type::OK); - ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), CallbackReturn::SUCCESS); + expect_configure_succeeded(controller_, true); auto odometry_message = controller_->get_rt_odom_publisher()->msg_; std::string test_odom_frame_id = odometry_message.header.frame_id; @@ -287,7 +308,7 @@ TEST_F(TestDiffDriveController, configure_succeeds_tf_test_prefix_false_no_names ASSERT_EQ(test_base_frame_id, base_link_id); } -TEST_F(TestDiffDriveController, configure_succeeds_tf_test_prefix_true_no_namespace) +TEST_F(TestDiffDriveController, expect_configure_succeededs_tf_test_prefix_true_no_namespace) { std::string odom_id = "odom"; std::string base_link_id = "base_link"; @@ -302,7 +323,7 @@ TEST_F(TestDiffDriveController, configure_succeeds_tf_test_prefix_true_no_namesp rclcpp::Parameter("base_frame_id", rclcpp::ParameterValue(base_link_id))}), controller_interface::return_type::OK); - ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), CallbackReturn::SUCCESS); + expect_configure_succeeded(controller_, true); auto odometry_message = controller_->get_rt_odom_publisher()->msg_; std::string test_odom_frame_id = odometry_message.header.frame_id; @@ -314,7 +335,7 @@ TEST_F(TestDiffDriveController, configure_succeeds_tf_test_prefix_true_no_namesp ASSERT_EQ(test_base_frame_id, frame_prefix + "/" + base_link_id); } -TEST_F(TestDiffDriveController, configure_succeeds_tf_blank_prefix_true_no_namespace) +TEST_F(TestDiffDriveController, expect_configure_succeededs_tf_blank_prefix_true_no_namespace) { std::string odom_id = "odom"; std::string base_link_id = "base_link"; @@ -329,7 +350,7 @@ TEST_F(TestDiffDriveController, configure_succeeds_tf_blank_prefix_true_no_names rclcpp::Parameter("base_frame_id", rclcpp::ParameterValue(base_link_id))}), controller_interface::return_type::OK); - ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), CallbackReturn::SUCCESS); + expect_configure_succeeded(controller_, true); auto odometry_message = controller_->get_rt_odom_publisher()->msg_; std::string test_odom_frame_id = odometry_message.header.frame_id; @@ -340,7 +361,7 @@ TEST_F(TestDiffDriveController, configure_succeeds_tf_blank_prefix_true_no_names ASSERT_EQ(test_base_frame_id, base_link_id); } -TEST_F(TestDiffDriveController, configure_succeeds_tf_test_prefix_false_set_namespace) +TEST_F(TestDiffDriveController, expect_configure_succeededs_tf_test_prefix_false_set_namespace) { std::string test_namespace = "/test_namespace"; @@ -358,7 +379,7 @@ TEST_F(TestDiffDriveController, configure_succeeds_tf_test_prefix_false_set_name test_namespace), controller_interface::return_type::OK); - ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), CallbackReturn::SUCCESS); + expect_configure_succeeded(controller_, true); auto odometry_message = controller_->get_rt_odom_publisher()->msg_; std::string test_odom_frame_id = odometry_message.header.frame_id; @@ -368,7 +389,7 @@ TEST_F(TestDiffDriveController, configure_succeeds_tf_test_prefix_false_set_name ASSERT_EQ(test_base_frame_id, base_link_id); } -TEST_F(TestDiffDriveController, configure_succeeds_tf_test_prefix_true_set_namespace) +TEST_F(TestDiffDriveController, expect_configure_succeededs_tf_test_prefix_true_set_namespace) { std::string test_namespace = "/test_namespace"; @@ -386,7 +407,7 @@ TEST_F(TestDiffDriveController, configure_succeeds_tf_test_prefix_true_set_names test_namespace), controller_interface::return_type::OK); - ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), CallbackReturn::SUCCESS); + expect_configure_succeeded(controller_, true); auto odometry_message = controller_->get_rt_odom_publisher()->msg_; std::string test_odom_frame_id = odometry_message.header.frame_id; @@ -398,7 +419,7 @@ TEST_F(TestDiffDriveController, configure_succeeds_tf_test_prefix_true_set_names ASSERT_EQ(test_base_frame_id, frame_prefix + "/" + base_link_id); } -TEST_F(TestDiffDriveController, configure_succeeds_tf_blank_prefix_true_set_namespace) +TEST_F(TestDiffDriveController, expect_configure_succeededs_tf_blank_prefix_true_set_namespace) { std::string test_namespace = "/test_namespace"; std::string odom_id = "odom"; @@ -415,7 +436,7 @@ TEST_F(TestDiffDriveController, configure_succeeds_tf_blank_prefix_true_set_name test_namespace), controller_interface::return_type::OK); - ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), CallbackReturn::SUCCESS); + expect_configure_succeeded(controller_, true); auto odometry_message = controller_->get_rt_odom_publisher()->msg_; std::string test_odom_frame_id = odometry_message.header.frame_id; @@ -431,21 +452,24 @@ TEST_F(TestDiffDriveController, activate_fails_without_resources_assigned) { ASSERT_EQ(InitController(), controller_interface::return_type::OK); - ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), CallbackReturn::SUCCESS); - ASSERT_EQ(controller_->on_activate(rclcpp_lifecycle::State()), CallbackReturn::ERROR); + expect_configure_succeeded(controller_, true); + + expect_activate_succeeded(controller_, false); } -TEST_F(TestDiffDriveController, activate_succeeds_with_pos_resources_assigned) +TEST_F(TestDiffDriveController, expect_activate_succeededs_with_pos_resources_assigned) { ASSERT_EQ(InitController(), controller_interface::return_type::OK); // We implicitly test that by default position feedback is required - ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), CallbackReturn::SUCCESS); + expect_configure_succeeded(controller_, true); + assignResourcesPosFeedback(); - ASSERT_EQ(controller_->on_activate(rclcpp_lifecycle::State()), CallbackReturn::SUCCESS); + + expect_activate_succeeded(controller_, true); } -TEST_F(TestDiffDriveController, activate_succeeds_with_vel_resources_assigned) +TEST_F(TestDiffDriveController, expect_activate_succeededs_with_vel_resources_assigned) { ASSERT_EQ( InitController( @@ -453,9 +477,11 @@ TEST_F(TestDiffDriveController, activate_succeeds_with_vel_resources_assigned) {rclcpp::Parameter("position_feedback", rclcpp::ParameterValue(false))}), controller_interface::return_type::OK); - ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), CallbackReturn::SUCCESS); + expect_configure_succeeded(controller_, true); + assignResourcesVelFeedback(); - ASSERT_EQ(controller_->on_activate(rclcpp_lifecycle::State()), CallbackReturn::SUCCESS); + + expect_activate_succeeded(controller_, true); } TEST_F(TestDiffDriveController, test_speed_limiter) @@ -487,12 +513,12 @@ TEST_F(TestDiffDriveController, test_speed_limiter) rclcpp::executors::SingleThreadedExecutor executor; executor.add_node(controller_->get_node()->get_node_base_interface()); - auto state = controller_->configure(); - ASSERT_EQ(State::PRIMARY_STATE_INACTIVE, state.id()); + + expect_configure_succeeded(controller_, true); + assignResourcesPosFeedback(); - state = controller_->get_node()->activate(); - ASSERT_EQ(State::PRIMARY_STATE_ACTIVE, state.id()); + expect_activate_succeeded(controller_, true); waitForSetup(); @@ -672,9 +698,11 @@ TEST_F(TestDiffDriveController, activate_fails_with_wrong_resources_assigned_1) {rclcpp::Parameter("position_feedback", rclcpp::ParameterValue(false))}), controller_interface::return_type::OK); - ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), CallbackReturn::SUCCESS); + expect_configure_succeeded(controller_, true); + assignResourcesPosFeedback(); - ASSERT_EQ(controller_->on_activate(rclcpp_lifecycle::State()), CallbackReturn::ERROR); + + expect_activate_succeeded(controller_, false); } TEST_F(TestDiffDriveController, activate_fails_with_wrong_resources_assigned_2) @@ -685,9 +713,11 @@ TEST_F(TestDiffDriveController, activate_fails_with_wrong_resources_assigned_2) {rclcpp::Parameter("position_feedback", rclcpp::ParameterValue(true))}), controller_interface::return_type::OK); - ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), CallbackReturn::SUCCESS); + expect_configure_succeeded(controller_, true); + assignResourcesVelFeedback(); - ASSERT_EQ(controller_->on_activate(rclcpp_lifecycle::State()), CallbackReturn::ERROR); + + expect_activate_succeeded(controller_, false); } TEST_F(TestDiffDriveController, cleanup) @@ -700,12 +730,12 @@ TEST_F(TestDiffDriveController, cleanup) rclcpp::executors::SingleThreadedExecutor executor; executor.add_node(controller_->get_node()->get_node_base_interface()); - auto state = controller_->configure(); - ASSERT_EQ(State::PRIMARY_STATE_INACTIVE, state.id()); + + expect_configure_succeeded(controller_, true); + assignResourcesPosFeedback(); - state = controller_->get_node()->activate(); - ASSERT_EQ(State::PRIMARY_STATE_ACTIVE, state.id()); + expect_activate_succeeded(controller_, true); waitForSetup(); @@ -723,7 +753,7 @@ TEST_F(TestDiffDriveController, cleanup) EXPECT_LT(0.0, left_wheel_vel_cmd_.get_optional().value()); EXPECT_LT(0.0, right_wheel_vel_cmd_.get_optional().value()); - state = controller_->get_node()->deactivate(); + auto state = controller_->get_node()->deactivate(); ASSERT_EQ(State::PRIMARY_STATE_INACTIVE, state.id()); // should be stopped @@ -753,15 +783,14 @@ TEST_F(TestDiffDriveController, correct_initialization_using_parameters) rclcpp::executors::SingleThreadedExecutor executor; executor.add_node(controller_->get_node()->get_node_base_interface()); - auto state = controller_->configure(); + expect_configure_succeeded(controller_, true); + assignResourcesPosFeedback(); - ASSERT_EQ(State::PRIMARY_STATE_INACTIVE, state.id()); EXPECT_EQ(0.01, left_wheel_vel_cmd_.get_optional().value()); EXPECT_EQ(0.02, right_wheel_vel_cmd_.get_optional().value()); - state = controller_->get_node()->activate(); - ASSERT_EQ(State::PRIMARY_STATE_ACTIVE, state.id()); + expect_activate_succeeded(controller_, true); // send msg const double linear = 1.0; @@ -779,7 +808,7 @@ TEST_F(TestDiffDriveController, correct_initialization_using_parameters) // deactivated // wait so controller process the second point when deactivated std::this_thread::sleep_for(std::chrono::milliseconds(500)); - state = controller_->get_node()->deactivate(); + auto state = controller_->get_node()->deactivate(); ASSERT_EQ(state.id(), State::PRIMARY_STATE_INACTIVE); ASSERT_EQ( controller_->update(rclcpp::Time(0, 0, RCL_ROS_TIME), rclcpp::Duration::from_seconds(0.01)), @@ -795,8 +824,8 @@ TEST_F(TestDiffDriveController, correct_initialization_using_parameters) EXPECT_EQ(0.0, left_wheel_vel_cmd_.get_optional().value()); EXPECT_EQ(0.0, right_wheel_vel_cmd_.get_optional().value()); - state = controller_->configure(); - ASSERT_EQ(State::PRIMARY_STATE_INACTIVE, state.id()); + expect_configure_succeeded(controller_, true); + executor.cancel(); } @@ -821,12 +850,11 @@ TEST_F(TestDiffDriveController, chainable_controller_unchained_mode) ASSERT_TRUE(controller_->set_chained_mode(false)); ASSERT_FALSE(controller_->is_in_chained_mode()); - auto state = controller_->configure(); - ASSERT_EQ(State::PRIMARY_STATE_INACTIVE, state.id()); + expect_configure_succeeded(controller_, true); + assignResourcesPosFeedback(); - state = controller_->get_node()->activate(); - ASSERT_EQ(State::PRIMARY_STATE_ACTIVE, state.id()); + expect_activate_succeeded(controller_, true); waitForSetup(); @@ -873,7 +901,7 @@ TEST_F(TestDiffDriveController, chainable_controller_unchained_mode) // Now check that the command interfaces are set to 0.0 on deactivation // (despite calls to update()) std::this_thread::sleep_for(std::chrono::milliseconds(300)); - state = controller_->get_node()->deactivate(); + auto state = controller_->get_node()->deactivate(); ASSERT_EQ(state.id(), State::PRIMARY_STATE_INACTIVE); ASSERT_EQ( controller_->update(rclcpp::Time(0, 0, RCL_ROS_TIME), rclcpp::Duration::from_seconds(0.01)), @@ -892,8 +920,8 @@ TEST_F(TestDiffDriveController, chainable_controller_unchained_mode) EXPECT_EQ(0.0, right_wheel_vel_cmd_.get_optional().value()) << "Wheels should be halted on cleanup()"; - state = controller_->configure(); - ASSERT_EQ(State::PRIMARY_STATE_INACTIVE, state.id()); + expect_configure_succeeded(controller_, true); + executor.cancel(); } @@ -917,12 +945,11 @@ TEST_F(TestDiffDriveController, chainable_controller_chained_mode) ASSERT_TRUE(controller_->set_chained_mode(true)); ASSERT_TRUE(controller_->is_in_chained_mode()); - auto state = controller_->configure(); - ASSERT_EQ(State::PRIMARY_STATE_INACTIVE, state.id()); + expect_configure_succeeded(controller_, true); + assignResourcesPosFeedback(); - state = controller_->get_node()->activate(); - ASSERT_EQ(State::PRIMARY_STATE_ACTIVE, state.id()); + expect_activate_succeeded(controller_, true); waitForSetup(); @@ -952,7 +979,7 @@ TEST_F(TestDiffDriveController, chainable_controller_chained_mode) // Now check that the command interfaces are set to 0.0 on deactivation // (despite calls to update()) std::this_thread::sleep_for(std::chrono::milliseconds(300)); - state = controller_->get_node()->deactivate(); + auto state = controller_->get_node()->deactivate(); ASSERT_EQ(state.id(), State::PRIMARY_STATE_INACTIVE); ASSERT_EQ( controller_->update(rclcpp::Time(0, 0, RCL_ROS_TIME), rclcpp::Duration::from_seconds(0.01)), @@ -971,8 +998,8 @@ TEST_F(TestDiffDriveController, chainable_controller_chained_mode) EXPECT_EQ(0.0, right_wheel_vel_cmd_.get_optional().value()) << "Wheels should be halted on cleanup()"; - state = controller_->configure(); - ASSERT_EQ(State::PRIMARY_STATE_INACTIVE, state.id()); + expect_configure_succeeded(controller_, true); + executor.cancel(); } @@ -981,7 +1008,7 @@ TEST_F(TestDiffDriveController, reference_interfaces_are_properly_exported) ASSERT_EQ( InitController(left_wheel_names, right_wheel_names), controller_interface::return_type::OK); - ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), CallbackReturn::SUCCESS); + expect_configure_succeeded(controller_, true); auto reference_interfaces = controller_->export_reference_interfaces(); ASSERT_EQ(reference_interfaces.size(), 2) @@ -1021,12 +1048,11 @@ TEST_F(TestDiffDriveController, deactivate_then_activate) ASSERT_TRUE(controller_->set_chained_mode(false)); - auto state = controller_->configure(); - ASSERT_EQ(State::PRIMARY_STATE_INACTIVE, state.id()); + expect_configure_succeeded(controller_, true); + assignResourcesPosFeedback(); - state = controller_->get_node()->activate(); - ASSERT_EQ(State::PRIMARY_STATE_ACTIVE, state.id()); + expect_activate_succeeded(controller_, true); waitForSetup(); @@ -1057,7 +1083,7 @@ TEST_F(TestDiffDriveController, deactivate_then_activate) // Now check that the command interfaces are set to 0.0 on deactivation // (despite calls to update()) std::this_thread::sleep_for(std::chrono::milliseconds(300)); - state = controller_->get_node()->deactivate(); + auto state = controller_->get_node()->deactivate(); ASSERT_EQ(state.id(), State::PRIMARY_STATE_INACTIVE); ASSERT_EQ( controller_->update(rclcpp::Time(0, 0, RCL_ROS_TIME), rclcpp::Duration::from_seconds(0.01)), @@ -1069,8 +1095,7 @@ TEST_F(TestDiffDriveController, deactivate_then_activate) << "Wheels should be halted on deactivate()"; // Activate again - state = controller_->get_node()->activate(); - ASSERT_EQ(State::PRIMARY_STATE_ACTIVE, state.id()); + expect_activate_succeeded(controller_, true); waitForSetup(); @@ -1119,12 +1144,11 @@ TEST_F(TestDiffDriveController, command_with_zero_timestamp_is_accepted_with_war ASSERT_TRUE(controller_->set_chained_mode(false)); - auto state = controller_->configure(); - ASSERT_EQ(State::PRIMARY_STATE_INACTIVE, state.id()); + expect_configure_succeeded(controller_, true); + assignResourcesPosFeedback(); - state = controller_->get_node()->activate(); - ASSERT_EQ(State::PRIMARY_STATE_ACTIVE, state.id()); + expect_activate_succeeded(controller_, true); waitForSetup(); @@ -1142,7 +1166,7 @@ TEST_F(TestDiffDriveController, command_with_zero_timestamp_is_accepted_with_war // Deactivate and cleanup std::this_thread::sleep_for(std::chrono::milliseconds(300)); - state = controller_->get_node()->deactivate(); + auto state = controller_->get_node()->deactivate(); ASSERT_EQ(state.id(), State::PRIMARY_STATE_INACTIVE); state = controller_->get_node()->cleanup(); ASSERT_EQ(state.id(), State::PRIMARY_STATE_UNCONFIGURED); diff --git a/gpio_controllers/test/test_gpio_command_controller.cpp b/gpio_controllers/test/test_gpio_command_controller.cpp index de085894f2..4b4763d000 100644 --- a/gpio_controllers/test/test_gpio_command_controller.cpp +++ b/gpio_controllers/test/test_gpio_command_controller.cpp @@ -53,6 +53,7 @@ using CmdType = control_msgs::msg::DynamicInterfaceGroupValues; using StateType = control_msgs::msg::DynamicInterfaceGroupValues; using hardware_interface::CommandInterface; using hardware_interface::StateInterface; +using lifecycle_msgs::msg::State; namespace { @@ -120,9 +121,12 @@ class GpioCommandControllerTestSuite : public ::testing::Test void move_to_activate_state(controller_interface::return_type result_of_initialization) { ASSERT_EQ(result_of_initialization, controller_interface::return_type::OK); - ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), CallbackReturn::SUCCESS); + + expect_configure_succeeded(controller_, true); + setup_command_and_state_interfaces(); - ASSERT_EQ(controller_->on_activate(rclcpp_lifecycle::State()), CallbackReturn::SUCCESS); + + expect_activate_succeeded(controller_, true); } void stop_test_when_message_cannot_be_published(int max_sub_check_loop_count) @@ -197,6 +201,27 @@ class GpioCommandControllerTestSuite : public ::testing::Test return max_sub_check_loop_count; } + void expect_configure_succeeded( + std::unique_ptr & controller, bool succeeded) + { + auto state = controller->configure(); + + if (succeeded) + ASSERT_EQ(State::PRIMARY_STATE_INACTIVE, state.id()); + else + ASSERT_EQ(State::PRIMARY_STATE_UNCONFIGURED, state.id()); + } + + void expect_activate_succeeded( + std::unique_ptr & controller, bool succeeded) + { + auto state = controller->get_node()->activate(); + if (succeeded) + ASSERT_EQ(State::PRIMARY_STATE_ACTIVE, state.id()); + else + ASSERT_EQ(State::PRIMARY_STATE_UNCONFIGURED, state.id()); + } + std::unique_ptr controller_; const std::vector gpio_names{"gpio1", "gpio2"}; @@ -277,7 +302,8 @@ TEST_F( "test_gpio_command_controller", ros2_control_test_assets::minimal_robot_urdf, 0, "", node_options); ASSERT_EQ(result, controller_interface::return_type::OK); - ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), CallbackReturn::ERROR); + + expect_configure_succeeded(controller_, false); } TEST_F( @@ -292,7 +318,8 @@ TEST_F( "test_gpio_command_controller", minimal_robot_urdf_with_gpio, 0, "", node_options); ASSERT_EQ(result, controller_interface::return_type::OK); - ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), CallbackReturn::SUCCESS); + + expect_configure_succeeded(controller_, true); } TEST_F( @@ -306,7 +333,8 @@ TEST_F( const auto result = controller_->init("test_gpio_command_controller", "", 0, "", node_options); ASSERT_EQ(result, controller_interface::return_type::OK); - ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), CallbackReturn::ERROR); + + expect_configure_succeeded(controller_, false); } TEST_F(GpioCommandControllerTestSuite, ConfigureAndActivateParamsSuccess) @@ -317,12 +345,9 @@ TEST_F(GpioCommandControllerTestSuite, ConfigureAndActivateParamsSuccess) {"command_interfaces.gpio2.interfaces", std::vector{"ana.1"}}, {"state_interfaces.gpio1.interfaces", std::vector{"dig.1", "dig.2"}}, {"state_interfaces.gpio2.interfaces", std::vector{"ana.1"}}}); - const auto result = controller_->init("test_gpio_command_controller", "", 0, "", node_options); - ASSERT_EQ(result, controller_interface::return_type::OK); - ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), CallbackReturn::SUCCESS); - setup_command_and_state_interfaces(); - ASSERT_EQ(controller_->on_activate(rclcpp_lifecycle::State()), CallbackReturn::SUCCESS); + move_to_activate_state( + controller_->init("test_gpio_command_controller", "", 0, "", node_options)); } TEST_F( @@ -336,9 +361,9 @@ TEST_F( {"state_interfaces.gpio1.interfaces", std::vector{"dig.1", "dig.2"}}, {"state_interfaces.gpio2.interfaces", std::vector{"ana.1"}}}); const auto result = controller_->init("test_gpio_command_controller", "", 0, "", node_options); - ASSERT_EQ(result, controller_interface::return_type::OK); - ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), CallbackReturn::SUCCESS); + + expect_configure_succeeded(controller_, true); std::vector command_interfaces; command_interfaces.emplace_back(gpio_1_1_dig_cmd); @@ -350,7 +375,8 @@ TEST_F( state_interfaces.emplace_back(gpio_2_ana_state); controller_->assign_interfaces(std::move(command_interfaces), std::move(state_interfaces)); - ASSERT_EQ(controller_->on_activate(rclcpp_lifecycle::State()), CallbackReturn::ERROR); + + expect_activate_succeeded(controller_, false); } TEST_F( @@ -366,7 +392,8 @@ TEST_F( const auto result = controller_->init("test_gpio_command_controller", "", 0, "", node_options); ASSERT_EQ(result, controller_interface::return_type::OK); - ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), CallbackReturn::SUCCESS); + + expect_configure_succeeded(controller_, true); std::vector command_interfaces; command_interfaces.emplace_back(gpio_1_1_dig_cmd); @@ -379,7 +406,7 @@ TEST_F( controller_->assign_interfaces(std::move(command_interfaces), std::move(state_interfaces)); - ASSERT_EQ(controller_->on_activate(rclcpp_lifecycle::State()), CallbackReturn::ERROR); + expect_activate_succeeded(controller_, false); } TEST_F( @@ -392,23 +419,9 @@ TEST_F( {"command_interfaces.gpio2.interfaces", std::vector{"ana.1"}}, {"state_interfaces.gpio1.interfaces", std::vector{"dig.1"}}, {"state_interfaces.gpio2.interfaces", std::vector{"ana.1"}}}); - const auto result = controller_->init("test_gpio_command_controller", "", 0, "", node_options); - ASSERT_EQ(result, controller_interface::return_type::OK); - ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), CallbackReturn::SUCCESS); - - std::vector command_interfaces; - command_interfaces.emplace_back(gpio_1_1_dig_cmd); - command_interfaces.emplace_back(gpio_1_2_dig_cmd); - command_interfaces.emplace_back(gpio_2_ana_cmd); - - std::vector state_interfaces; - state_interfaces.emplace_back(gpio_1_1_dig_state); - state_interfaces.emplace_back(gpio_2_ana_state); - - controller_->assign_interfaces(std::move(command_interfaces), std::move(state_interfaces)); - - ASSERT_EQ(controller_->on_activate(rclcpp_lifecycle::State()), CallbackReturn::SUCCESS); + move_to_activate_state( + controller_->init("test_gpio_command_controller", "", 0, "", node_options)); } TEST_F( @@ -648,9 +661,12 @@ TEST_F( const auto result_of_initialization = controller_->init( "test_gpio_command_controller", minimal_robot_urdf_with_gpio, 0, "", node_options); ASSERT_EQ(result_of_initialization, controller_interface::return_type::OK); - ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), CallbackReturn::SUCCESS); + + expect_configure_succeeded(controller_, true); + controller_->assign_interfaces(std::move(command_interfaces), std::move(state_interfaces)); - ASSERT_EQ(controller_->on_activate(rclcpp_lifecycle::State()), CallbackReturn::SUCCESS); + + expect_activate_succeeded(controller_, true); auto subscription = node->create_subscription( std::string(controller_->get_node()->get_name()) + "/gpio_states", 10, @@ -684,9 +700,12 @@ TEST_F( const auto result_of_initialization = controller_->init("test_gpio_command_controller", "", 0, "", node_options); ASSERT_EQ(result_of_initialization, controller_interface::return_type::OK); - ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), CallbackReturn::SUCCESS); + + expect_configure_succeeded(controller_, true); + controller_->assign_interfaces({}, std::move(state_interfaces)); - ASSERT_EQ(controller_->on_activate(rclcpp_lifecycle::State()), CallbackReturn::SUCCESS); + + expect_activate_succeeded(controller_, true); auto subscription = node->create_subscription( std::string(controller_->get_node()->get_name()) + "/gpio_states", 10, diff --git a/gps_sensor_broadcaster/test/test_gps_sensor_broadcaster.cpp b/gps_sensor_broadcaster/test/test_gps_sensor_broadcaster.cpp index e4ac047052..97dc821032 100644 --- a/gps_sensor_broadcaster/test/test_gps_sensor_broadcaster.cpp +++ b/gps_sensor_broadcaster/test/test_gps_sensor_broadcaster.cpp @@ -27,13 +27,16 @@ #include "hardware_interface/loaned_state_interface.hpp" #include "hardware_interface/types/hardware_interface_return_values.hpp" #include "hardware_interface/types/hardware_interface_type_values.hpp" +#include "lifecycle_msgs/msg/state.hpp" #include "rclcpp_lifecycle/node_interfaces/lifecycle_node_interface.hpp" #include "ros2_control_test_assets/descriptions.hpp" #include "sensor_msgs/msg/nav_sat_fix.hpp" using hardware_interface::LoanedStateInterface; +using lifecycle_msgs::msg::State; using callback_return_type = rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn; + namespace { constexpr uint16_t GPS_SERVICE = 1; @@ -105,6 +108,27 @@ class GPSSensorBroadcasterTest : public ::testing::Test return gps_msg; } + void expect_configure_succeeded( + std::unique_ptr & broadcaster, bool succeeded) + { + auto state = broadcaster->configure(); + + if (succeeded) + ASSERT_EQ(State::PRIMARY_STATE_INACTIVE, state.id()); + else + ASSERT_EQ(State::PRIMARY_STATE_UNCONFIGURED, state.id()); + } + + void expect_activate_succeeded( + std::unique_ptr & broadcaster, bool succeeded) + { + auto state = broadcaster->get_node()->activate(); + if (succeeded) + ASSERT_EQ(State::PRIMARY_STATE_ACTIVE, state.id()); + else + ASSERT_EQ(State::PRIMARY_STATE_UNCONFIGURED, state.id()); + } + protected: const rclcpp::Parameter sensor_name_param_ = rclcpp::Parameter("sensor_name", "gps_sensor"); const std::string sensor_name_ = sensor_name_param_.get_value(); @@ -152,10 +176,10 @@ TEST_F( "test_gps_sensor_broadcaster", ros2_control_test_assets::minimal_robot_urdf, 0, "", node_options); ASSERT_EQ(result, controller_interface::return_type::OK); - ASSERT_EQ( - gps_broadcaster_->on_configure(rclcpp_lifecycle::State()), callback_return_type::SUCCESS); - ASSERT_EQ( - gps_broadcaster_->on_activate(rclcpp_lifecycle::State()), callback_return_type::SUCCESS); + + expect_configure_succeeded(gps_broadcaster_, true); + + expect_activate_succeeded(gps_broadcaster_, true); } TEST_F( @@ -167,11 +191,12 @@ TEST_F( "test_gps_sensor_broadcaster", ros2_control_test_assets::minimal_robot_urdf, 0, "", node_options); ASSERT_EQ(result, controller_interface::return_type::OK); - ASSERT_EQ( - gps_broadcaster_->on_configure(rclcpp_lifecycle::State()), callback_return_type::SUCCESS); + + expect_configure_succeeded(gps_broadcaster_, true); + setup_gps_broadcaster(); - ASSERT_EQ( - gps_broadcaster_->on_activate(rclcpp_lifecycle::State()), callback_return_type::SUCCESS); + + expect_activate_succeeded(gps_broadcaster_, true); const auto gps_msg = subscribe_and_get_message(); EXPECT_EQ(gps_msg.header.frame_id, frame_id_.get_value()); @@ -199,11 +224,12 @@ TEST_F( "test_gps_sensor_broadcaster", ros2_control_test_assets::minimal_robot_urdf, 0, "", node_options); ASSERT_EQ(result, controller_interface::return_type::OK); - ASSERT_EQ( - gps_broadcaster_->on_configure(rclcpp_lifecycle::State()), callback_return_type::SUCCESS); + + expect_configure_succeeded(gps_broadcaster_, true); + setup_gps_broadcaster(); - ASSERT_EQ( - gps_broadcaster_->on_activate(rclcpp_lifecycle::State()), callback_return_type::SUCCESS); + + expect_activate_succeeded(gps_broadcaster_, true); const auto gps_msg = subscribe_and_get_message(); EXPECT_EQ(gps_msg.header.frame_id, frame_id_.get_value()); @@ -227,11 +253,12 @@ TEST_F( "test_gps_sensor_broadcaster", ros2_control_test_assets::minimal_robot_urdf, 0, "", node_options); ASSERT_EQ(result, controller_interface::return_type::OK); - ASSERT_EQ( - gps_broadcaster_->on_configure(rclcpp_lifecycle::State()), callback_return_type::SUCCESS); + + expect_configure_succeeded(gps_broadcaster_, true); + setup_gps_broadcaster(); - ASSERT_EQ( - gps_broadcaster_->on_activate(rclcpp_lifecycle::State()), callback_return_type::SUCCESS); + + expect_activate_succeeded(gps_broadcaster_, true); const auto gps_msg = subscribe_and_get_message(); EXPECT_EQ(gps_msg.header.frame_id, frame_id_.get_value()); From 49cf7bfb023ae905d7b1cd126408eb544dd6fe2e Mon Sep 17 00:00:00 2001 From: jsantoso Date: Wed, 14 May 2025 10:06:31 -0500 Subject: [PATCH 4/5] revert unexpected changes in test case names --- .../test/test_diff_drive_controller.cpp | 16 ++++++++-------- 1 file changed, 8 insertions(+), 8 deletions(-) diff --git a/diff_drive_controller/test/test_diff_drive_controller.cpp b/diff_drive_controller/test/test_diff_drive_controller.cpp index f7342049aa..483e054dfa 100644 --- a/diff_drive_controller/test/test_diff_drive_controller.cpp +++ b/diff_drive_controller/test/test_diff_drive_controller.cpp @@ -283,7 +283,7 @@ TEST_F( EXPECT_EQ(cmd_if_conf.type, controller_interface::interface_configuration_type::INDIVIDUAL); } -TEST_F(TestDiffDriveController, expect_configure_succeededs_tf_test_prefix_false_no_namespace) +TEST_F(TestDiffDriveController, configure_succeededs_tf_test_prefix_false_no_namespace) { std::string odom_id = "odom"; std::string base_link_id = "base_link"; @@ -308,7 +308,7 @@ TEST_F(TestDiffDriveController, expect_configure_succeededs_tf_test_prefix_false ASSERT_EQ(test_base_frame_id, base_link_id); } -TEST_F(TestDiffDriveController, expect_configure_succeededs_tf_test_prefix_true_no_namespace) +TEST_F(TestDiffDriveController, configure_succeededs_tf_test_prefix_true_no_namespace) { std::string odom_id = "odom"; std::string base_link_id = "base_link"; @@ -335,7 +335,7 @@ TEST_F(TestDiffDriveController, expect_configure_succeededs_tf_test_prefix_true_ ASSERT_EQ(test_base_frame_id, frame_prefix + "/" + base_link_id); } -TEST_F(TestDiffDriveController, expect_configure_succeededs_tf_blank_prefix_true_no_namespace) +TEST_F(TestDiffDriveController, configure_succeededs_tf_blank_prefix_true_no_namespace) { std::string odom_id = "odom"; std::string base_link_id = "base_link"; @@ -361,7 +361,7 @@ TEST_F(TestDiffDriveController, expect_configure_succeededs_tf_blank_prefix_true ASSERT_EQ(test_base_frame_id, base_link_id); } -TEST_F(TestDiffDriveController, expect_configure_succeededs_tf_test_prefix_false_set_namespace) +TEST_F(TestDiffDriveController, configure_succeededs_tf_test_prefix_false_set_namespace) { std::string test_namespace = "/test_namespace"; @@ -389,7 +389,7 @@ TEST_F(TestDiffDriveController, expect_configure_succeededs_tf_test_prefix_false ASSERT_EQ(test_base_frame_id, base_link_id); } -TEST_F(TestDiffDriveController, expect_configure_succeededs_tf_test_prefix_true_set_namespace) +TEST_F(TestDiffDriveController, configure_succeededs_tf_test_prefix_true_set_namespace) { std::string test_namespace = "/test_namespace"; @@ -419,7 +419,7 @@ TEST_F(TestDiffDriveController, expect_configure_succeededs_tf_test_prefix_true_ ASSERT_EQ(test_base_frame_id, frame_prefix + "/" + base_link_id); } -TEST_F(TestDiffDriveController, expect_configure_succeededs_tf_blank_prefix_true_set_namespace) +TEST_F(TestDiffDriveController, configure_succeededs_tf_blank_prefix_true_set_namespace) { std::string test_namespace = "/test_namespace"; std::string odom_id = "odom"; @@ -457,7 +457,7 @@ TEST_F(TestDiffDriveController, activate_fails_without_resources_assigned) expect_activate_succeeded(controller_, false); } -TEST_F(TestDiffDriveController, expect_activate_succeededs_with_pos_resources_assigned) +TEST_F(TestDiffDriveController, activate_succeededs_with_pos_resources_assigned) { ASSERT_EQ(InitController(), controller_interface::return_type::OK); @@ -469,7 +469,7 @@ TEST_F(TestDiffDriveController, expect_activate_succeededs_with_pos_resources_as expect_activate_succeeded(controller_, true); } -TEST_F(TestDiffDriveController, expect_activate_succeededs_with_vel_resources_assigned) +TEST_F(TestDiffDriveController, activate_succeededs_with_vel_resources_assigned) { ASSERT_EQ( InitController( From 083cd7156fc709488d3cb372406887060cf8832f Mon Sep 17 00:00:00 2001 From: jsantoso Date: Wed, 14 May 2025 10:10:21 -0500 Subject: [PATCH 5/5] revert unexpected changes in test case names --- .../test/test_diff_drive_controller.cpp | 16 ++++++++-------- 1 file changed, 8 insertions(+), 8 deletions(-) diff --git a/diff_drive_controller/test/test_diff_drive_controller.cpp b/diff_drive_controller/test/test_diff_drive_controller.cpp index 483e054dfa..fe92868632 100644 --- a/diff_drive_controller/test/test_diff_drive_controller.cpp +++ b/diff_drive_controller/test/test_diff_drive_controller.cpp @@ -283,7 +283,7 @@ TEST_F( EXPECT_EQ(cmd_if_conf.type, controller_interface::interface_configuration_type::INDIVIDUAL); } -TEST_F(TestDiffDriveController, configure_succeededs_tf_test_prefix_false_no_namespace) +TEST_F(TestDiffDriveController, configure_succeeds_tf_test_prefix_false_no_namespace) { std::string odom_id = "odom"; std::string base_link_id = "base_link"; @@ -308,7 +308,7 @@ TEST_F(TestDiffDriveController, configure_succeededs_tf_test_prefix_false_no_nam ASSERT_EQ(test_base_frame_id, base_link_id); } -TEST_F(TestDiffDriveController, configure_succeededs_tf_test_prefix_true_no_namespace) +TEST_F(TestDiffDriveController, configure_succeeds_tf_test_prefix_true_no_namespace) { std::string odom_id = "odom"; std::string base_link_id = "base_link"; @@ -335,7 +335,7 @@ TEST_F(TestDiffDriveController, configure_succeededs_tf_test_prefix_true_no_name ASSERT_EQ(test_base_frame_id, frame_prefix + "/" + base_link_id); } -TEST_F(TestDiffDriveController, configure_succeededs_tf_blank_prefix_true_no_namespace) +TEST_F(TestDiffDriveController, configure_succeeds_tf_blank_prefix_true_no_namespace) { std::string odom_id = "odom"; std::string base_link_id = "base_link"; @@ -361,7 +361,7 @@ TEST_F(TestDiffDriveController, configure_succeededs_tf_blank_prefix_true_no_nam ASSERT_EQ(test_base_frame_id, base_link_id); } -TEST_F(TestDiffDriveController, configure_succeededs_tf_test_prefix_false_set_namespace) +TEST_F(TestDiffDriveController, configure_succeeds_tf_test_prefix_false_set_namespace) { std::string test_namespace = "/test_namespace"; @@ -389,7 +389,7 @@ TEST_F(TestDiffDriveController, configure_succeededs_tf_test_prefix_false_set_na ASSERT_EQ(test_base_frame_id, base_link_id); } -TEST_F(TestDiffDriveController, configure_succeededs_tf_test_prefix_true_set_namespace) +TEST_F(TestDiffDriveController, configure_succeeds_tf_test_prefix_true_set_namespace) { std::string test_namespace = "/test_namespace"; @@ -419,7 +419,7 @@ TEST_F(TestDiffDriveController, configure_succeededs_tf_test_prefix_true_set_nam ASSERT_EQ(test_base_frame_id, frame_prefix + "/" + base_link_id); } -TEST_F(TestDiffDriveController, configure_succeededs_tf_blank_prefix_true_set_namespace) +TEST_F(TestDiffDriveController, configure_succeeds_tf_blank_prefix_true_set_namespace) { std::string test_namespace = "/test_namespace"; std::string odom_id = "odom"; @@ -457,7 +457,7 @@ TEST_F(TestDiffDriveController, activate_fails_without_resources_assigned) expect_activate_succeeded(controller_, false); } -TEST_F(TestDiffDriveController, activate_succeededs_with_pos_resources_assigned) +TEST_F(TestDiffDriveController, activate_succeeds_with_pos_resources_assigned) { ASSERT_EQ(InitController(), controller_interface::return_type::OK); @@ -469,7 +469,7 @@ TEST_F(TestDiffDriveController, activate_succeededs_with_pos_resources_assigned) expect_activate_succeeded(controller_, true); } -TEST_F(TestDiffDriveController, activate_succeededs_with_vel_resources_assigned) +TEST_F(TestDiffDriveController, activate_succeeds_with_vel_resources_assigned) { ASSERT_EQ( InitController(