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..e4ac047052 100644 --- a/gps_sensor_broadcaster/test/test_gps_sensor_broadcaster.cpp +++ b/gps_sensor_broadcaster/test/test_gps_sensor_broadcaster.cpp @@ -62,6 +62,13 @@ class GPSSensorBroadcasterTest : public ::testing::Test ~GPSSensorBroadcasterTest() { rclcpp::shutdown(); } + void SetUp() + { + gps_broadcaster_ = std::make_unique(); + } + + void TearDown() { gps_broadcaster_.reset(nullptr); } + template < semantic_components::GPSSensorOption sensor_option = semantic_components::GPSSensorOption::WithoutCovariance> @@ -80,7 +87,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 +96,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 +122,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 +148,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 +163,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 +195,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 +223,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());