diff --git a/.github/workflows/humble-semi-binary-build.yml b/.github/workflows/humble-semi-binary-build.yml index 9d94aa3ba8..86fcac07e0 100644 --- a/.github/workflows/humble-semi-binary-build.yml +++ b/.github/workflows/humble-semi-binary-build.yml @@ -39,7 +39,7 @@ jobs: strategy: fail-fast: false matrix: - ROS_REPO: [main, testing] + ROS_REPO: [testing] with: ros_distro: humble ros_repo: ${{ matrix.ROS_REPO }} diff --git a/.github/workflows/iron-semi-binary-build.yml b/.github/workflows/iron-semi-binary-build.yml index 3aca5e5b70..a0782e50bb 100644 --- a/.github/workflows/iron-semi-binary-build.yml +++ b/.github/workflows/iron-semi-binary-build.yml @@ -40,7 +40,7 @@ jobs: strategy: fail-fast: false matrix: - ROS_REPO: [main, testing] + ROS_REPO: [testing] with: ros_distro: iron ros_repo: ${{ matrix.ROS_REPO }} diff --git a/.github/workflows/jazzy-semi-binary-build.yml b/.github/workflows/jazzy-semi-binary-build.yml index ffa4f914b9..a462d9040a 100644 --- a/.github/workflows/jazzy-semi-binary-build.yml +++ b/.github/workflows/jazzy-semi-binary-build.yml @@ -41,7 +41,7 @@ jobs: strategy: fail-fast: false matrix: - ROS_REPO: [main, testing] + ROS_REPO: [testing] with: ros_distro: jazzy ros_repo: ${{ matrix.ROS_REPO }} diff --git a/.github/workflows/rolling-compatibility-humble-binary-build.yml b/.github/workflows/rolling-compatibility-humble-binary-build.yml new file mode 100644 index 0000000000..7aab02fa62 --- /dev/null +++ b/.github/workflows/rolling-compatibility-humble-binary-build.yml @@ -0,0 +1,48 @@ +name: Check Rolling Compatibility on Humble +# author: Christoph Froehlich +# description: 'Build & test the rolling version on Humble distro.' + +on: + workflow_dispatch: + pull_request: + branches: + - master + - '*feature*' + - '*feature/**' + paths: + - '**.hpp' + - '**.h' + - '**.cpp' + - '**.py' + - '**.yaml' + - '.github/workflows/rolling-compatibility-humble-binary-build.yml' + - '**/package.xml' + - '**/CMakeLists.txt' + - 'ros2_controllers.rolling.repos' + push: + branches: + - master + paths: + - '**.hpp' + - '**.h' + - '**.cpp' + - '**.py' + - '**.yaml' + - '.github/workflows/rolling-compatibility-humble-binary-build.yml' + - '**/package.xml' + - '**/CMakeLists.txt' + - 'ros2_controllers.rolling.repos' + +jobs: + build-on-humble: + uses: ros-controls/ros2_control_ci/.github/workflows/reusable-industrial-ci-with-cache.yml@master + strategy: + fail-fast: false + matrix: + ROS_DISTRO: [humble] + ROS_REPO: [testing] + with: + ros_distro: ${{ matrix.ROS_DISTRO }} + ros_repo: ${{ matrix.ROS_REPO }} + upstream_workspace: ros2_controllers.rolling.repos + ref_for_scheduled_build: master diff --git a/.github/workflows/rolling-compatibility-jazzy-binary-build.yml b/.github/workflows/rolling-compatibility-jazzy-binary-build.yml new file mode 100644 index 0000000000..37ffbb84a4 --- /dev/null +++ b/.github/workflows/rolling-compatibility-jazzy-binary-build.yml @@ -0,0 +1,48 @@ +name: Check Rolling Compatibility on Jazzy +# author: Christoph Froehlich +# description: 'Build & test the rolling version on Jazzy distro.' + +on: + workflow_dispatch: + pull_request: + branches: + - master + - '*feature*' + - '*feature/**' + paths: + - '**.hpp' + - '**.h' + - '**.cpp' + - '**.py' + - '**.yaml' + - '.github/workflows/rolling-compatibility-jazzy-binary-build.yml' + - '**/package.xml' + - '**/CMakeLists.txt' + - 'ros2_controllers.rolling.repos' + push: + branches: + - master + paths: + - '**.hpp' + - '**.h' + - '**.cpp' + - '**.py' + - '**.yaml' + - '.github/workflows/rolling-compatibility-jazzy-binary-build.yml' + - '**/package.xml' + - '**/CMakeLists.txt' + - 'ros2_controllers.rolling.repos' + +jobs: + build-on-jazzy: + uses: ros-controls/ros2_control_ci/.github/workflows/reusable-industrial-ci-with-cache.yml@master + strategy: + fail-fast: false + matrix: + ROS_DISTRO: [jazzy] + ROS_REPO: [testing] + with: + ros_distro: ${{ matrix.ROS_DISTRO }} + ros_repo: ${{ matrix.ROS_REPO }} + upstream_workspace: ros2_controllers.rolling.repos + ref_for_scheduled_build: master diff --git a/.github/workflows/rolling-semi-binary-build.yml b/.github/workflows/rolling-semi-binary-build.yml index 872c509931..db0dd1fe64 100644 --- a/.github/workflows/rolling-semi-binary-build.yml +++ b/.github/workflows/rolling-semi-binary-build.yml @@ -41,7 +41,7 @@ jobs: strategy: fail-fast: false matrix: - ROS_REPO: [main, testing] + ROS_REPO: [testing] with: ros_distro: rolling ros_repo: ${{ matrix.ROS_REPO }} diff --git a/gpio_controllers/CMakeLists.txt b/gpio_controllers/CMakeLists.txt index 6ea97e04ca..37eb115c83 100644 --- a/gpio_controllers/CMakeLists.txt +++ b/gpio_controllers/CMakeLists.txt @@ -8,7 +8,7 @@ endif() if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") add_compile_options(-Wall -Wextra -Wpedantic -Werror=conversion -Werror=unused-but-set-variable - -Werror=return-type -Werror=shadow -Werror=format -Werror=range-loop-construct + -Werror=return-type -Werror=format -Werror=range-loop-construct -Werror=missing-braces) endif() @@ -76,7 +76,6 @@ if(BUILD_TESTING) target_include_directories(test_load_gpio_command_controller PRIVATE include) ament_target_dependencies(test_load_gpio_command_controller controller_manager - hardware_interface ros2_control_test_assets ) diff --git a/gpio_controllers/doc/userdoc.rst b/gpio_controllers/doc/userdoc.rst index 199a4bd208..799a53add8 100644 --- a/gpio_controllers/doc/userdoc.rst +++ b/gpio_controllers/doc/userdoc.rst @@ -23,12 +23,11 @@ This controller uses the `generate_parameter_library get_gpios_from_urdf() const; diff --git a/gpio_controllers/src/gpio_command_controller.cpp b/gpio_controllers/src/gpio_command_controller.cpp index a509eae774..646f2f8a5d 100644 --- a/gpio_controllers/src/gpio_command_controller.cpp +++ b/gpio_controllers/src/gpio_command_controller.cpp @@ -32,18 +32,6 @@ void print_interface(const rclcpp::Logger & logger, const T & command_interfaces RCLCPP_ERROR(logger, "Got %s", interface_name.c_str()); } } - -std::vector extract_gpios_from_hardware_info( - const std::vector & hardware_infos) -{ - std::vector result; - for (const auto & hardware_info : hardware_infos) - { - std::copy( - hardware_info.gpios.begin(), hardware_info.gpios.end(), std::back_insert_iterator(result)); - } - return result; -} } // namespace namespace gpio_controllers @@ -181,58 +169,8 @@ void GpioCommandController::store_command_interface_types() } } -bool GpioCommandController::should_broadcast_all_interfaces_of_configured_gpios() const -{ - auto are_interfaces_empty = [](const auto & interfaces) - { return interfaces.second.interfaces.empty(); }; - return std::all_of( - params_.state_interfaces.gpios_map.cbegin(), params_.state_interfaces.gpios_map.cend(), - are_interfaces_empty); -} - -std::vector GpioCommandController::get_gpios_from_urdf() const -try -{ - return extract_gpios_from_hardware_info( - hardware_interface::parse_control_resources_from_urdf(get_robot_description())); -} -catch (const std::exception & e) -{ - fprintf(stderr, "Exception thrown during extracting gpios info from urdf %s \n", e.what()); - return {}; -} - -void GpioCommandController::set_all_state_interfaces_of_configured_gpios() -{ - const auto gpios{get_gpios_from_urdf()}; - for (const auto & gpio_name : params_.gpios) - { - for (const auto & gpio : gpios) - { - if (gpio_name == gpio.name) - { - std::transform( - gpio.state_interfaces.begin(), gpio.state_interfaces.end(), - std::back_insert_iterator(state_interface_types_), - [&gpio_name](const auto & interface_name) - { return gpio_name + '/' + interface_name.name; }); - } - } - } -} - void GpioCommandController::store_state_interface_types() { - if (should_broadcast_all_interfaces_of_configured_gpios()) - { - RCLCPP_INFO( - get_node()->get_logger(), - "State interfaces are not configured. All available interfaces of configured GPIOs will be " - "broadcasted."); - set_all_state_interfaces_of_configured_gpios(); - return; - } - for (const auto & [gpio_name, interfaces] : params_.state_interfaces.gpios_map) { std::transform( diff --git a/gpio_controllers/test/test_gpio_command_controller.cpp b/gpio_controllers/test/test_gpio_command_controller.cpp index ec0e0f1228..14ff4e6a27 100644 --- a/gpio_controllers/test/test_gpio_command_controller.cpp +++ b/gpio_controllers/test/test_gpio_command_controller.cpp @@ -30,22 +30,6 @@ #include "rclcpp_lifecycle/node_interfaces/lifecycle_node_interface.hpp" #include "ros2_control_test_assets/descriptions.hpp" -namespace -{ -const auto hardware_resources_with_gpio = - R"( - - - - - -)"; - -const auto minimal_robot_urdf_with_gpio = std::string(ros2_control_test_assets::urdf_head) + - std::string(hardware_resources_with_gpio) + - std::string(ros2_control_test_assets::urdf_tail); -} // namespace - using CallbackReturn = controller_interface::CallbackReturn; using hardware_interface::LoanedCommandInterface; using hardware_interface::LoanedStateInterface; @@ -209,9 +193,7 @@ class GpioCommandControllerTestSuite : public ::testing::Test TEST_F(GpioCommandControllerTestSuite, WhenNoParametersAreSetInitShouldFail) { - const auto result = controller.init( - "test_gpio_command_controller", ros2_control_test_assets::minimal_robot_urdf, 0, "", - controller.define_custom_node_options()); + const auto result = controller.init("test_gpio_command_controller", ""); ASSERT_EQ(result, controller_interface::return_type::ERROR); } @@ -219,7 +201,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", "", node_options); ASSERT_EQ(result, controller_interface::return_type::ERROR); } @@ -230,7 +212,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", "", node_options); ASSERT_EQ(result, controller_interface::return_type::OK); } @@ -239,7 +221,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", "", node_options); ASSERT_EQ(result, controller_interface::return_type::OK); } @@ -255,7 +237,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", "", node_options); ASSERT_EQ(result, controller_interface::return_type::OK); } @@ -267,38 +249,7 @@ 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", 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); -} - -TEST_F( - GpioCommandControllerTestSuite, - WhenStateInterfaceAreNotConfiguredButAreSetInUrdfForConfiguredGpiosThenConfigureShouldSucceed) -{ - const auto node_options = create_node_options_with_overriden_parameters( - {{"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", 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); -} - -TEST_F( - GpioCommandControllerTestSuite, - WhenStateInterfaceAreNotConfiguredAndProvidedUrdfIsEmptyThenConfigureShouldFail) -{ - const auto node_options = create_node_options_with_overriden_parameters( - {{"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", "", node_options); ASSERT_EQ(result, controller_interface::return_type::OK); ASSERT_EQ(controller.on_configure(rclcpp_lifecycle::State()), CallbackReturn::ERROR); } @@ -311,7 +262,7 @@ 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", "", node_options); ASSERT_EQ(result, controller_interface::return_type::OK); ASSERT_EQ(controller.on_configure(rclcpp_lifecycle::State()), CallbackReturn::SUCCESS); @@ -329,7 +280,7 @@ 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", "", node_options); ASSERT_EQ(result, controller_interface::return_type::OK); ASSERT_EQ(controller.on_configure(rclcpp_lifecycle::State()), CallbackReturn::SUCCESS); @@ -357,7 +308,7 @@ 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", "", node_options); ASSERT_EQ(result, controller_interface::return_type::OK); ASSERT_EQ(controller.on_configure(rclcpp_lifecycle::State()), CallbackReturn::SUCCESS); @@ -386,7 +337,7 @@ 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", "", node_options); ASSERT_EQ(result, controller_interface::return_type::OK); ASSERT_EQ(controller.on_configure(rclcpp_lifecycle::State()), CallbackReturn::SUCCESS); @@ -416,7 +367,7 @@ 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", "", node_options)); assert_default_command_and_state_values(); update_controller_loop(); assert_default_command_and_state_values(); @@ -432,7 +383,7 @@ 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", "", node_options)); const auto command = createGpioCommand( {"gpio1", "gpio2"}, {createInterfaceValue({"dig.1", "dig.2"}, {0.0, 1.0, 1.0}), @@ -453,7 +404,7 @@ 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", "", node_options)); const auto command = createGpioCommand( {"gpio1", "gpio2"}, @@ -474,7 +425,7 @@ 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", "", node_options)); const auto command = createGpioCommand( {"gpio1", "gpio2"}, {createInterfaceValue({"dig.1", "dig.2"}, {0.0, 1.0}), @@ -497,7 +448,7 @@ 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", "", node_options)); const auto command = createGpioCommand( {"gpio2", "gpio1"}, {createInterfaceValue({"ana.1"}, {30.0}), @@ -520,7 +471,7 @@ 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", "", node_options)); const auto command = createGpioCommand({"gpio1"}, {createInterfaceValue({"dig.1", "dig.2"}, {0.0, 1.0})}); @@ -543,7 +494,7 @@ 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", "", node_options)); const auto command = createGpioCommand( {"gpio1", "gpio3"}, {createInterfaceValue({"dig.3", "dig.4"}, {20.0, 25.0}), @@ -567,7 +518,7 @@ 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", "", node_options)); auto command_pub = node->create_publisher( std::string(controller.get_node()->get_name()) + "/commands", rclcpp::SystemDefaultsQoS()); @@ -591,7 +542,7 @@ 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", "", node_options)); auto subscription = node->create_subscription( std::string(controller.get_node()->get_name()) + "/gpio_states", 10, @@ -614,45 +565,6 @@ TEST_F(GpioCommandControllerTestSuite, ControllerShouldPublishGpioStatesWithCurr ASSERT_EQ(gpio_state_msg.interface_values.at(1).values.at(0), 3.1); } -TEST_F( - GpioCommandControllerTestSuite, - WhenStateInterfaceAreNotConfiguredButSetInUrdfForConfiguredGpiosThenThatStatesShouldBePublished) -{ - const std::vector single_gpio{"gpio1"}; - const auto node_options = create_node_options_with_overriden_parameters( - {{"gpios", single_gpio}, - {"command_interfaces.gpio1.interfaces", std::vector{"dig.1"}}}); - - std::vector command_interfaces; - command_interfaces.emplace_back(gpio_1_1_dig_cmd); - - std::vector state_interfaces; - state_interfaces.emplace_back(gpio_1_1_dig_state); - state_interfaces.emplace_back(gpio_2_ana_state); - - 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); - - auto subscription = node->create_subscription( - std::string(controller.get_node()->get_name()) + "/gpio_states", 10, - [&](const StateType::SharedPtr) {}); - - stop_test_when_message_cannot_be_published(wait_for_subscription(subscription)); - - StateType gpio_state_msg; - rclcpp::MessageInfo msg_info; - ASSERT_TRUE(subscription->take(gpio_state_msg, msg_info)); - - ASSERT_EQ(gpio_state_msg.interface_groups.size(), single_gpio.size()); - ASSERT_EQ(gpio_state_msg.interface_groups.at(0), "gpio1"); - ASSERT_EQ(gpio_state_msg.interface_values.at(0).interface_names.at(0), "dig.1"); - ASSERT_EQ(gpio_state_msg.interface_values.at(0).values.at(0), 1.0); -} - TEST_F( GpioCommandControllerTestSuite, ControllerShouldPublishGpioStatesWithCurrentValuesWhenOnlyStateInterfacesAreSet) @@ -667,7 +579,7 @@ 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", "", 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)); diff --git a/gpio_controllers/test/test_load_gpio_command_controller.cpp b/gpio_controllers/test/test_load_gpio_command_controller.cpp index 379a53b048..491c13c6b3 100644 --- a/gpio_controllers/test/test_load_gpio_command_controller.cpp +++ b/gpio_controllers/test/test_load_gpio_command_controller.cpp @@ -13,25 +13,25 @@ // limitations under the License. #include + #include #include "controller_manager/controller_manager.hpp" -#include "hardware_interface/resource_manager.hpp" #include "rclcpp/executors/single_threaded_executor.hpp" #include "ros2_control_test_assets/descriptions.hpp" -TEST(TestLoadGpioCommandController, load_controller) +TEST(TestLoadGpioController, load_controller) { rclcpp::init(0, nullptr); std::shared_ptr executor = std::make_shared(); + controller_manager::ControllerManager control_mananger( + std::make_unique( + ros2_control_test_assets::minimal_robot_urdf), + executor, "test_controller_manager"); - controller_manager::ControllerManager cm( - executor, ros2_control_test_assets::minimal_robot_urdf, true, "test_controller_manager"); - - ASSERT_NO_THROW( - cm.load_controller("test_gpio_command_controller", "gpio_controllers/GpioCommandController")); - + ASSERT_NO_THROW(control_mananger.load_controller( + "test_gpio_command_controller", "gpio_controllers/GpioCommandController")); rclcpp::shutdown(); }