1313// limitations under the License.
1414
1515#include < gmock/gmock.h>
16- #include < memory>
17- #include < fstream>
1816#include < cstdio>
19- #include < vector>
2017#include < filesystem>
18+ #include < fstream>
19+ #include < memory>
20+ #include < vector>
2121
2222#include " controller_manager/controller_manager.hpp"
23- #include " hardware_interface/resource_manager.hpp"
24- #include " rclcpp/executors/single_threaded_executor.hpp"
25- #include " ros2_control_test_assets/descriptions.hpp"
2623#include " controller_manager_msgs/srv/switch_controller.hpp"
24+ #include " gpio_controllers/gpio_command_controller.hpp"
2725#include " hardware_interface/loaned_command_interface.hpp"
2826#include " hardware_interface/loaned_state_interface.hpp"
29- #include " gpio_controllers/gpio_command_controller.hpp"
27+ #include " hardware_interface/resource_manager.hpp"
28+ #include " rclcpp/executors/single_threaded_executor.hpp"
29+ #include " ros2_control_test_assets/descriptions.hpp"
3030
3131const auto urdf_bool = R"(
3232<?xml version="1.0" encoding="utf-8"?>
@@ -72,9 +72,10 @@ class TestableGpioController : public gpio_controllers::GpioCommandController
7272 using gpio_controllers::GpioCommandController::assign_interfaces;
7373};
7474
75- TEST (TestLoadGpioCommandController, ReproduceBadCastCrash )
75+ TEST (TestLoadGpioCommandController, UpdateBoolGpioInterfaces )
7676{
77- if (!rclcpp::ok ()){
77+ if (!rclcpp::ok ())
78+ {
7879 rclcpp::init (0 , nullptr );
7980 }
8081
@@ -92,26 +93,30 @@ TEST(TestLoadGpioCommandController, ReproduceBadCastCrash)
9293 init_params.node_options = node_options;
9394
9495 ASSERT_EQ (controller->init (init_params), controller_interface::return_type::OK);
95- ASSERT_EQ (controller->on_configure (rclcpp_lifecycle::State ()), controller_interface::CallbackReturn::SUCCESS);
96-
96+ ASSERT_EQ (
97+ controller->on_configure (rclcpp_lifecycle::State ()),
98+ controller_interface::CallbackReturn::SUCCESS);
99+
97100 double dummy_double_value = 0.0 ;
98101
99- auto cmd_intf = std::make_shared<hardware_interface::CommandInterface>(" gpio1" , " dig_out_1" , &dummy_double_value);
102+ auto cmd_intf = std::make_shared<hardware_interface::CommandInterface>(
103+ " gpio1" , " dig_out_1" , &dummy_double_value);
100104 std::vector<hardware_interface::LoanedCommandInterface> cmd_loaned;
101105 cmd_loaned.emplace_back (cmd_intf);
102106
103- auto state_intf = std::make_shared<hardware_interface::StateInterface>(" gpio1" , " dig_in_1" , &dummy_double_value);
107+ auto state_intf =
108+ std::make_shared<hardware_interface::StateInterface>(" gpio1" , " dig_in_1" , &dummy_double_value);
104109 std::vector<hardware_interface::LoanedStateInterface> state_loaned;
105110 state_loaned.emplace_back (state_intf);
106111
107112 controller->assign_interfaces (std::move (cmd_loaned), std::move (state_loaned));
108113
109- ASSERT_EQ (controller->on_activate (rclcpp_lifecycle::State ()), controller_interface::CallbackReturn::SUCCESS);
110-
114+ ASSERT_EQ (
115+ controller->on_activate (rclcpp_lifecycle::State ()),
116+ controller_interface::CallbackReturn::SUCCESS);
117+
111118 // This verifies that the controller no longer crashes on update
112- EXPECT_NO_THROW (
113- controller->update (rclcpp::Time (0 ), rclcpp::Duration::from_seconds (0.01 ))
114- );
119+ EXPECT_NO_THROW (controller->update (rclcpp::Time (0 ), rclcpp::Duration::from_seconds (0.01 )));
115120
116121 rclcpp::shutdown ();
117- }
122+ }
0 commit comments