From 287161f28cfa246bdb6f894ba754869d3d254b7e Mon Sep 17 00:00:00 2001 From: Ishan1923 Date: Thu, 8 Jan 2026 00:46:58 +0530 Subject: [PATCH 1/8] fix: add missing cmath header for strict build Signed-off-by: Ishan1923 --- .../src/steering_controllers_library.cpp | 1 + 1 file changed, 1 insertion(+) diff --git a/steering_controllers_library/src/steering_controllers_library.cpp b/steering_controllers_library/src/steering_controllers_library.cpp index beae04762e..3d35dffd4f 100644 --- a/steering_controllers_library/src/steering_controllers_library.cpp +++ b/steering_controllers_library/src/steering_controllers_library.cpp @@ -19,6 +19,7 @@ #include #include #include +#include #include "controller_interface/tf_prefix.hpp" #include "hardware_interface/types/hardware_interface_type_values.hpp" From 86329bab613f769c138f3127368f52a6c4f36a86 Mon Sep 17 00:00:00 2001 From: Ishan1923 Date: Fri, 9 Jan 2026 17:26:51 +0530 Subject: [PATCH 2/8] reproduce crash with boolean iterface (issue #1970). Signed-off-by: Ishan1923 --- .../test_load_gpio_command_controller.cpp | 64 +++++++++++++++++++ 1 file changed, 64 insertions(+) diff --git a/gpio_controllers/test/test_load_gpio_command_controller.cpp b/gpio_controllers/test/test_load_gpio_command_controller.cpp index d47ad5af64..bcb6fd3bea 100644 --- a/gpio_controllers/test/test_load_gpio_command_controller.cpp +++ b/gpio_controllers/test/test_load_gpio_command_controller.cpp @@ -20,6 +20,28 @@ #include "rclcpp/executors/single_threaded_executor.hpp" #include "ros2_control_test_assets/descriptions.hpp" +const auto urdf_bool = R"( + + + + + + + + + + + + mock_components/GenericSystem + + + + + + + +)"; + TEST(TestLoadGpioCommandController, load_controller) { rclcpp::init(0, nullptr); @@ -35,3 +57,45 @@ TEST(TestLoadGpioCommandController, load_controller) rclcpp::shutdown(); } + +TEST(TestLoadGpioCommandController, load_controller_bool_crash) +{ + rclcpp::init(0, nullptr); + + std::shared_ptr executor = + std::make_shared(); + + rclcpp::NodeOptions node_options; + node_options.arguments({ + "--ros-args", + "-p", "test_gpio_command_controller_2.gpios:=['gpio1']", + "-p", "test_gpio_command_controller_2.command_interfaces.gpio1.interfaces:=['dig_out_1']", + "-p", "test_gpio_command_controller_2.state_interfaces.gpio1.interfaces:=['dig_in_1']" + }); + + controller_manager::ControllerManager cm( + executor, + urdf_bool, + true, + "test_controller_manager", + "", + node_options + ); + + ASSERT_NO_THROW( + cm.load_controller("test_gpio_command_controller_2", "gpio_controllers/GpioCommandController")); + + cm.configure_controller("test_gpio_command_controller_2"); + + cm.switch_controller( + {"test_gpio_command_controller_2"}, + {}, + controller_manager_msgs::srv::SwitchController::Request::STRICT, + true, + rclcpp::Duration(0, 0) + ); + + cm.update(rclcpp::Time(0), rclcpp::Duration::from_seconds(0.01)); + + rclcpp::shutdown(); +} \ No newline at end of file From 89bbdcb4e570a27b65a251b2eb6fd6245c69e270 Mon Sep 17 00:00:00 2001 From: Ishan1923 Date: Mon, 12 Jan 2026 14:52:48 +0530 Subject: [PATCH 3/8] fix crash on NaN state values and sanitize inputs Signed-off-by: Ishan1923 --- .../src/gpio_command_controller.cpp | 17 +++- .../test_load_gpio_command_controller.cpp | 82 +++++++++++-------- .../src/steering_controllers_library.cpp | 1 - 3 files changed, 64 insertions(+), 36 deletions(-) diff --git a/gpio_controllers/src/gpio_command_controller.cpp b/gpio_controllers/src/gpio_command_controller.cpp index 8ee414d7a0..d8f19c117b 100644 --- a/gpio_controllers/src/gpio_command_controller.cpp +++ b/gpio_controllers/src/gpio_command_controller.cpp @@ -15,6 +15,7 @@ #include "gpio_controllers/gpio_command_controller.hpp" #include +#include #include "controller_interface/helpers.hpp" #include "hardware_interface/component_parser.hpp" @@ -53,6 +54,16 @@ std::vector extract_gpios_from_hardware_info( } return result; } + + +double sanitize_double(double double_value) +{ + if (std::isnan(double_value)) + { + return 0.0; + } + return double_value; +} } // namespace namespace gpio_controllers @@ -264,7 +275,8 @@ void GpioCommandController::initialize_gpio_state_msg() get_gpios_state_interfaces_names(gpio_name); gpio_state_msg_.interface_values[gpio_index].values = std::vector( gpio_state_msg_.interface_values[gpio_index].interface_names.size(), - std::numeric_limits::quiet_NaN()); + sanitize_double(std::numeric_limits::quiet_NaN())); //////////////////////////////////////////////// + ///Change1 } } @@ -429,7 +441,8 @@ void GpioCommandController::apply_state_value( else { state_msg.interface_values[gpio_index].values[interface_index] = - state_msg_interface_value_op.value(); + + sanitize_double(state_msg_interface_value_op.value()); /////////////////////change2 } } catch (const std::exception & e) diff --git a/gpio_controllers/test/test_load_gpio_command_controller.cpp b/gpio_controllers/test/test_load_gpio_command_controller.cpp index bcb6fd3bea..63a0ea5645 100644 --- a/gpio_controllers/test/test_load_gpio_command_controller.cpp +++ b/gpio_controllers/test/test_load_gpio_command_controller.cpp @@ -14,11 +14,19 @@ #include #include +#include +#include +#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" +#include "controller_manager_msgs/srv/switch_controller.hpp" +#include "hardware_interface/loaned_command_interface.hpp" +#include "hardware_interface/loaned_state_interface.hpp" +#include "gpio_controllers/gpio_command_controller.hpp" const auto urdf_bool = R"( @@ -58,44 +66,52 @@ TEST(TestLoadGpioCommandController, load_controller) rclcpp::shutdown(); } -TEST(TestLoadGpioCommandController, load_controller_bool_crash) +class TestableGpioController : public gpio_controllers::GpioCommandController { - rclcpp::init(0, nullptr); +public: + using gpio_controllers::GpioCommandController::assign_interfaces; +}; - std::shared_ptr executor = - std::make_shared(); +TEST(TestLoadGpioCommandController, ReproduceBadCastCrash) +{ + if(!rclcpp::ok()){ + rclcpp::init(0, nullptr); + } rclcpp::NodeOptions node_options; - node_options.arguments({ - "--ros-args", - "-p", "test_gpio_command_controller_2.gpios:=['gpio1']", - "-p", "test_gpio_command_controller_2.command_interfaces.gpio1.interfaces:=['dig_out_1']", - "-p", "test_gpio_command_controller_2.state_interfaces.gpio1.interfaces:=['dig_in_1']" - }); - - controller_manager::ControllerManager cm( - executor, - urdf_bool, - true, - "test_controller_manager", - "", - node_options + std::vector params; + params.emplace_back("gpios", std::vector{"gpio1"}); + params.emplace_back("command_interfaces.gpio1.interfaces", std::vector{"dig_out_1"}); + params.emplace_back("state_interfaces.gpio1.interfaces", std::vector{"dig_in_1"}); + node_options.parameter_overrides(params); + + auto controller = std::make_shared(); + + controller_interface::ControllerInterfaceParams init_params; + init_params.controller_name = "test_gpio_controller"; + init_params.node_options = node_options; + + ASSERT_EQ(controller->init(init_params), controller_interface::return_type::OK); + ASSERT_EQ(controller->on_configure(rclcpp_lifecycle::State()), controller_interface::CallbackReturn::SUCCESS); + + double dummy_double_value = 0.0; + + auto cmd_intf = std::make_shared("gpio1", "dig_out_1", &dummy_double_value); + std::vector cmd_loaned; + cmd_loaned.emplace_back(cmd_intf); + + auto state_intf = std::make_shared("gpio1", "dig_in_1", &dummy_double_value); + std::vector state_loaned; + state_loaned.emplace_back(state_intf); + + controller->assign_interfaces(std::move(cmd_loaned), std::move(state_loaned)); + + ASSERT_EQ(controller->on_activate(rclcpp_lifecycle::State()), controller_interface::CallbackReturn::SUCCESS); + + // This verifies that the controller no longer crashes on update + EXPECT_NO_THROW( + controller->update(rclcpp::Time(0), rclcpp::Duration::from_seconds(0.01)) ); - ASSERT_NO_THROW( - cm.load_controller("test_gpio_command_controller_2", "gpio_controllers/GpioCommandController")); - - cm.configure_controller("test_gpio_command_controller_2"); - - cm.switch_controller( - {"test_gpio_command_controller_2"}, - {}, - controller_manager_msgs::srv::SwitchController::Request::STRICT, - true, - rclcpp::Duration(0, 0) - ); - - cm.update(rclcpp::Time(0), rclcpp::Duration::from_seconds(0.01)); - rclcpp::shutdown(); } \ No newline at end of file diff --git a/steering_controllers_library/src/steering_controllers_library.cpp b/steering_controllers_library/src/steering_controllers_library.cpp index 3d35dffd4f..beae04762e 100644 --- a/steering_controllers_library/src/steering_controllers_library.cpp +++ b/steering_controllers_library/src/steering_controllers_library.cpp @@ -19,7 +19,6 @@ #include #include #include -#include #include "controller_interface/tf_prefix.hpp" #include "hardware_interface/types/hardware_interface_type_values.hpp" From 5517039110fe62e71dd90149dab39ee2431507ad Mon Sep 17 00:00:00 2001 From: Ishan1923 Date: Wed, 21 Jan 2026 19:10:21 +0530 Subject: [PATCH 4/8] style: address review comments (remove comments, rename test) Signed-off-by: Ishan1923 --- .../src/gpio_command_controller.cpp | 6 +-- .../test_load_gpio_command_controller.cpp | 43 +++++++++++-------- 2 files changed, 26 insertions(+), 23 deletions(-) diff --git a/gpio_controllers/src/gpio_command_controller.cpp b/gpio_controllers/src/gpio_command_controller.cpp index d8f19c117b..821a0dbb8c 100644 --- a/gpio_controllers/src/gpio_command_controller.cpp +++ b/gpio_controllers/src/gpio_command_controller.cpp @@ -55,7 +55,6 @@ std::vector extract_gpios_from_hardware_info( return result; } - double sanitize_double(double double_value) { if (std::isnan(double_value)) @@ -275,8 +274,7 @@ void GpioCommandController::initialize_gpio_state_msg() get_gpios_state_interfaces_names(gpio_name); gpio_state_msg_.interface_values[gpio_index].values = std::vector( gpio_state_msg_.interface_values[gpio_index].interface_names.size(), - sanitize_double(std::numeric_limits::quiet_NaN())); //////////////////////////////////////////////// - ///Change1 + sanitize_double(std::numeric_limits::quiet_NaN())); } } @@ -442,7 +440,7 @@ void GpioCommandController::apply_state_value( { state_msg.interface_values[gpio_index].values[interface_index] = - sanitize_double(state_msg_interface_value_op.value()); /////////////////////change2 + sanitize_double(state_msg_interface_value_op.value()); } } catch (const std::exception & e) diff --git a/gpio_controllers/test/test_load_gpio_command_controller.cpp b/gpio_controllers/test/test_load_gpio_command_controller.cpp index 63a0ea5645..f6863735df 100644 --- a/gpio_controllers/test/test_load_gpio_command_controller.cpp +++ b/gpio_controllers/test/test_load_gpio_command_controller.cpp @@ -13,20 +13,20 @@ // limitations under the License. #include -#include -#include #include -#include #include +#include +#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" #include "controller_manager_msgs/srv/switch_controller.hpp" +#include "gpio_controllers/gpio_command_controller.hpp" #include "hardware_interface/loaned_command_interface.hpp" #include "hardware_interface/loaned_state_interface.hpp" -#include "gpio_controllers/gpio_command_controller.hpp" +#include "hardware_interface/resource_manager.hpp" +#include "rclcpp/executors/single_threaded_executor.hpp" +#include "ros2_control_test_assets/descriptions.hpp" const auto urdf_bool = R"( @@ -72,9 +72,10 @@ class TestableGpioController : public gpio_controllers::GpioCommandController using gpio_controllers::GpioCommandController::assign_interfaces; }; -TEST(TestLoadGpioCommandController, ReproduceBadCastCrash) +TEST(TestLoadGpioCommandController, UpdateBoolGpioInterfaces) { - if(!rclcpp::ok()){ + if (!rclcpp::ok()) + { rclcpp::init(0, nullptr); } @@ -92,26 +93,30 @@ TEST(TestLoadGpioCommandController, ReproduceBadCastCrash) init_params.node_options = node_options; ASSERT_EQ(controller->init(init_params), controller_interface::return_type::OK); - ASSERT_EQ(controller->on_configure(rclcpp_lifecycle::State()), controller_interface::CallbackReturn::SUCCESS); - + ASSERT_EQ( + controller->on_configure(rclcpp_lifecycle::State()), + controller_interface::CallbackReturn::SUCCESS); + double dummy_double_value = 0.0; - auto cmd_intf = std::make_shared("gpio1", "dig_out_1", &dummy_double_value); + auto cmd_intf = std::make_shared( + "gpio1", "dig_out_1", &dummy_double_value); std::vector cmd_loaned; cmd_loaned.emplace_back(cmd_intf); - auto state_intf = std::make_shared("gpio1", "dig_in_1", &dummy_double_value); + auto state_intf = + std::make_shared("gpio1", "dig_in_1", &dummy_double_value); std::vector state_loaned; state_loaned.emplace_back(state_intf); controller->assign_interfaces(std::move(cmd_loaned), std::move(state_loaned)); - ASSERT_EQ(controller->on_activate(rclcpp_lifecycle::State()), controller_interface::CallbackReturn::SUCCESS); - + ASSERT_EQ( + controller->on_activate(rclcpp_lifecycle::State()), + controller_interface::CallbackReturn::SUCCESS); + // This verifies that the controller no longer crashes on update - EXPECT_NO_THROW( - controller->update(rclcpp::Time(0), rclcpp::Duration::from_seconds(0.01)) - ); + EXPECT_NO_THROW(controller->update(rclcpp::Time(0), rclcpp::Duration::from_seconds(0.01))); rclcpp::shutdown(); -} \ No newline at end of file +} From 7ad400624250844ebbd90aaf06280f5284901c7a Mon Sep 17 00:00:00 2001 From: Ishan1923 Date: Sun, 1 Feb 2026 13:50:30 +0530 Subject: [PATCH 5/8] fix(gpio): handle double interfaces safely and fix tests Signed-off-by: Ishan1923 --- gpio_controllers/CMakeLists.txt | 1 + .../src/gpio_command_controller.cpp | 40 ++++----- .../test/test_gpio_command_controller.cpp | 39 +++++++++ .../test_load_gpio_command_controller.cpp | 87 +------------------ 4 files changed, 61 insertions(+), 106 deletions(-) diff --git a/gpio_controllers/CMakeLists.txt b/gpio_controllers/CMakeLists.txt index a19f7586af..462ac8ebfb 100644 --- a/gpio_controllers/CMakeLists.txt +++ b/gpio_controllers/CMakeLists.txt @@ -77,6 +77,7 @@ if(BUILD_TESTING) target_link_libraries(test_gpio_command_controller gpio_controllers ros2_control_test_assets::ros2_control_test_assets + gpio_command_controller_parameters ) endif() diff --git a/gpio_controllers/src/gpio_command_controller.cpp b/gpio_controllers/src/gpio_command_controller.cpp index 821a0dbb8c..2d2546d6c8 100644 --- a/gpio_controllers/src/gpio_command_controller.cpp +++ b/gpio_controllers/src/gpio_command_controller.cpp @@ -55,14 +55,6 @@ std::vector extract_gpios_from_hardware_info( return result; } -double sanitize_double(double double_value) -{ - if (std::isnan(double_value)) - { - return 0.0; - } - return double_value; -} } // namespace namespace gpio_controllers @@ -274,7 +266,7 @@ void GpioCommandController::initialize_gpio_state_msg() get_gpios_state_interfaces_names(gpio_name); gpio_state_msg_.interface_values[gpio_index].values = std::vector( gpio_state_msg_.interface_values[gpio_index].interface_names.size(), - sanitize_double(std::numeric_limits::quiet_NaN())); + std::numeric_limits::quiet_NaN()); } } @@ -427,21 +419,29 @@ void GpioCommandController::apply_state_value( state_msg.interface_values[gpio_index].interface_names[interface_index]; try { - auto state_msg_interface_value_op = - state_interfaces_map_.at(interface_name).get().get_optional(); + double state_value = std::numeric_limits::quiet_NaN(); + bool read_success = false; + const auto& interface = state_interfaces_map_.at(interface_name).get(); + auto opt_double = interface.get_optional(); + if(opt_double.has_value()){ + state_value = opt_double.value(); + read_success = true; + } + else{ + auto opt_bool = interface.get_optional(); + if(opt_bool.has_value()){ + state_value = static_cast( (opt_bool.value()) ? 1.0 : 0.0); + read_success = true; + } + } - if (!state_msg_interface_value_op.has_value()) - { + if(!read_success){ RCLCPP_DEBUG( - get_node()->get_logger(), "Unable to retrieve the data from state: %s \n", - interface_name.c_str()); + get_node()->get_logger(), "Unable to retrive the data from state(datatype neither bool nor double): %s \n", interface_name.c_str() + ); } - else - { - state_msg.interface_values[gpio_index].values[interface_index] = - sanitize_double(state_msg_interface_value_op.value()); - } + state_msg.interface_values[gpio_index].values[interface_index] = state_value; } catch (const std::exception & e) { diff --git a/gpio_controllers/test/test_gpio_command_controller.cpp b/gpio_controllers/test/test_gpio_command_controller.cpp index f3d8eeda55..7843f1915c 100644 --- a/gpio_controllers/test/test_gpio_command_controller.cpp +++ b/gpio_controllers/test/test_gpio_command_controller.cpp @@ -85,6 +85,10 @@ class FriendGpioCommandController : public gpio_controllers::GpioCommandControll FRIEND_TEST( GpioCommandControllerTestSuite, WhenGivenCmdContainsWrongGpioInterfacesOrWrongGpioNameThenCommandInterfacesShouldNotBeUpdated); + FRIEND_TEST( + GpioCommandControllerTestSuite, + UpdateBoolGpioInterfaces + ); }; class GpioCommandControllerTestSuite : public ::testing::Test @@ -708,3 +712,38 @@ TEST_F( ASSERT_EQ(gpio_state_msg.interface_values.at(0).values.at(0), 1.0); ASSERT_EQ(gpio_state_msg.interface_values.at(1).values.at(0), 3.1); } + +TEST_F(GpioCommandControllerTestSuite, UpdateBoolGpioInterfaces) +{ + const auto node_options = create_node_options_with_overriden_parameters( + {{"gpios", std::vector{"gpio1"}}, + {"command_interfaces.gpio1.interfaces", std::vector{"dig_out_1"}}, + {"state_interfaces.gpio1.interfaces", std::vector{"dig_in_1"}}} + ); + + ASSERT_EQ(controller_->init(create_ctrl_params(node_options)), controller_interface::return_type::OK); + ASSERT_EQ( + controller_->on_configure(rclcpp_lifecycle::State()), + controller_interface::CallbackReturn::SUCCESS); + + double dummy_double_value = 0.0; + + auto cmd_intf = std::make_shared( + "gpio1", "dig_out_1", &dummy_double_value); + std::vector cmd_loaned; + cmd_loaned.emplace_back(cmd_intf, nullptr); + + auto state_intf = + std::make_shared("gpio1", "dig_in_1", &dummy_double_value); + std::vector state_loaned; + state_loaned.emplace_back(state_intf, nullptr); + + controller_->assign_interfaces(std::move(cmd_loaned), std::move(state_loaned)); + + ASSERT_EQ( + controller_->on_activate(rclcpp_lifecycle::State()), + controller_interface::CallbackReturn::SUCCESS); + + // This verifies that the controller no longer crashes on update + EXPECT_NO_THROW(controller_->update(rclcpp::Time(0), rclcpp::Duration::from_seconds(0.01))); +} diff --git a/gpio_controllers/test/test_load_gpio_command_controller.cpp b/gpio_controllers/test/test_load_gpio_command_controller.cpp index f6863735df..9d12e01b28 100644 --- a/gpio_controllers/test/test_load_gpio_command_controller.cpp +++ b/gpio_controllers/test/test_load_gpio_command_controller.cpp @@ -13,43 +13,13 @@ // limitations under the License. #include -#include -#include -#include #include -#include #include "controller_manager/controller_manager.hpp" -#include "controller_manager_msgs/srv/switch_controller.hpp" -#include "gpio_controllers/gpio_command_controller.hpp" -#include "hardware_interface/loaned_command_interface.hpp" -#include "hardware_interface/loaned_state_interface.hpp" #include "hardware_interface/resource_manager.hpp" #include "rclcpp/executors/single_threaded_executor.hpp" #include "ros2_control_test_assets/descriptions.hpp" -const auto urdf_bool = R"( - - - - - - - - - - - - mock_components/GenericSystem - - - - - - - -)"; - TEST(TestLoadGpioCommandController, load_controller) { rclcpp::init(0, nullptr); @@ -64,59 +34,4 @@ TEST(TestLoadGpioCommandController, load_controller) cm.load_controller("test_gpio_command_controller", "gpio_controllers/GpioCommandController")); rclcpp::shutdown(); -} - -class TestableGpioController : public gpio_controllers::GpioCommandController -{ -public: - using gpio_controllers::GpioCommandController::assign_interfaces; -}; - -TEST(TestLoadGpioCommandController, UpdateBoolGpioInterfaces) -{ - if (!rclcpp::ok()) - { - rclcpp::init(0, nullptr); - } - - rclcpp::NodeOptions node_options; - std::vector params; - params.emplace_back("gpios", std::vector{"gpio1"}); - params.emplace_back("command_interfaces.gpio1.interfaces", std::vector{"dig_out_1"}); - params.emplace_back("state_interfaces.gpio1.interfaces", std::vector{"dig_in_1"}); - node_options.parameter_overrides(params); - - auto controller = std::make_shared(); - - controller_interface::ControllerInterfaceParams init_params; - init_params.controller_name = "test_gpio_controller"; - init_params.node_options = node_options; - - ASSERT_EQ(controller->init(init_params), controller_interface::return_type::OK); - ASSERT_EQ( - controller->on_configure(rclcpp_lifecycle::State()), - controller_interface::CallbackReturn::SUCCESS); - - double dummy_double_value = 0.0; - - auto cmd_intf = std::make_shared( - "gpio1", "dig_out_1", &dummy_double_value); - std::vector cmd_loaned; - cmd_loaned.emplace_back(cmd_intf); - - auto state_intf = - std::make_shared("gpio1", "dig_in_1", &dummy_double_value); - std::vector state_loaned; - state_loaned.emplace_back(state_intf); - - controller->assign_interfaces(std::move(cmd_loaned), std::move(state_loaned)); - - ASSERT_EQ( - controller->on_activate(rclcpp_lifecycle::State()), - controller_interface::CallbackReturn::SUCCESS); - - // This verifies that the controller no longer crashes on update - EXPECT_NO_THROW(controller->update(rclcpp::Time(0), rclcpp::Duration::from_seconds(0.01))); - - rclcpp::shutdown(); -} +} \ No newline at end of file From f1403c54e99226ffbfe573cd62e10add2b4f04b9 Mon Sep 17 00:00:00 2001 From: Ishan1923 Date: Mon, 2 Feb 2026 02:27:42 +0530 Subject: [PATCH 6/8] fix: use explicit HandleDataType checks and apply pre-commit Signed-off-by: Ishan1923 --- .../src/gpio_command_controller.cpp | 38 +++++++++++-------- .../test/test_gpio_command_controller.cpp | 19 ++++------ .../test_load_gpio_command_controller.cpp | 2 +- 3 files changed, 30 insertions(+), 29 deletions(-) diff --git a/gpio_controllers/src/gpio_command_controller.cpp b/gpio_controllers/src/gpio_command_controller.cpp index 2d2546d6c8..dc83392b9a 100644 --- a/gpio_controllers/src/gpio_command_controller.cpp +++ b/gpio_controllers/src/gpio_command_controller.cpp @@ -420,25 +420,31 @@ void GpioCommandController::apply_state_value( try { double state_value = std::numeric_limits::quiet_NaN(); - bool read_success = false; - const auto& interface = state_interfaces_map_.at(interface_name).get(); - auto opt_double = interface.get_optional(); - if(opt_double.has_value()){ - state_value = opt_double.value(); - read_success = true; + const auto & interface = state_interfaces_map_.at(interface_name).get(); + const auto & type = interface.get_data_type(); + + if (type == hardware_interface::HandleDataType::DOUBLE) + { + const auto val_opt = interface.get_optional(); + if (val_opt.has_value()) + { + state_value = val_opt.value(); + } } - else{ - auto opt_bool = interface.get_optional(); - if(opt_bool.has_value()){ - state_value = static_cast( (opt_bool.value()) ? 1.0 : 0.0); - read_success = true; + else if (type == hardware_interface::HandleDataType::BOOL) + { + const auto val_opt = interface.get_optional(); + if (val_opt.has_value()) + { + state_value = val_opt.value() ? 1.0 : 0.0; } } - - if(!read_success){ - RCLCPP_DEBUG( - get_node()->get_logger(), "Unable to retrive the data from state(datatype neither bool nor double): %s \n", interface_name.c_str() - ); + else + { + RCLCPP_DEBUG_ONCE( + get_node()->get_logger(), + "Interface '%s' has unsupported type '%s'. Only 'double' and 'bool' are supported.", + interface_name.c_str(), type.to_string().c_str()); } state_msg.interface_values[gpio_index].values[interface_index] = state_value; diff --git a/gpio_controllers/test/test_gpio_command_controller.cpp b/gpio_controllers/test/test_gpio_command_controller.cpp index 7843f1915c..137eab789e 100644 --- a/gpio_controllers/test/test_gpio_command_controller.cpp +++ b/gpio_controllers/test/test_gpio_command_controller.cpp @@ -85,10 +85,7 @@ class FriendGpioCommandController : public gpio_controllers::GpioCommandControll FRIEND_TEST( GpioCommandControllerTestSuite, WhenGivenCmdContainsWrongGpioInterfacesOrWrongGpioNameThenCommandInterfacesShouldNotBeUpdated); - FRIEND_TEST( - GpioCommandControllerTestSuite, - UpdateBoolGpioInterfaces - ); + FRIEND_TEST(GpioCommandControllerTestSuite, UpdateBoolGpioInterfaces); }; class GpioCommandControllerTestSuite : public ::testing::Test @@ -717,24 +714,22 @@ TEST_F(GpioCommandControllerTestSuite, UpdateBoolGpioInterfaces) { const auto node_options = create_node_options_with_overriden_parameters( {{"gpios", std::vector{"gpio1"}}, - {"command_interfaces.gpio1.interfaces", std::vector{"dig_out_1"}}, - {"state_interfaces.gpio1.interfaces", std::vector{"dig_in_1"}}} - ); + {"command_interfaces.gpio1.interfaces", std::vector{"dig_out_1"}}, + {"state_interfaces.gpio1.interfaces", std::vector{"dig_in_1"}}}); - ASSERT_EQ(controller_->init(create_ctrl_params(node_options)), controller_interface::return_type::OK); + ASSERT_EQ( + controller_->init(create_ctrl_params(node_options)), controller_interface::return_type::OK); ASSERT_EQ( controller_->on_configure(rclcpp_lifecycle::State()), controller_interface::CallbackReturn::SUCCESS); double dummy_double_value = 0.0; - auto cmd_intf = std::make_shared( - "gpio1", "dig_out_1", &dummy_double_value); + auto cmd_intf = std::make_shared("gpio1", "dig_out_1", &dummy_double_value); std::vector cmd_loaned; cmd_loaned.emplace_back(cmd_intf, nullptr); - auto state_intf = - std::make_shared("gpio1", "dig_in_1", &dummy_double_value); + auto state_intf = std::make_shared("gpio1", "dig_in_1", &dummy_double_value); std::vector state_loaned; state_loaned.emplace_back(state_intf, nullptr); diff --git a/gpio_controllers/test/test_load_gpio_command_controller.cpp b/gpio_controllers/test/test_load_gpio_command_controller.cpp index 9d12e01b28..d47ad5af64 100644 --- a/gpio_controllers/test/test_load_gpio_command_controller.cpp +++ b/gpio_controllers/test/test_load_gpio_command_controller.cpp @@ -34,4 +34,4 @@ TEST(TestLoadGpioCommandController, load_controller) cm.load_controller("test_gpio_command_controller", "gpio_controllers/GpioCommandController")); rclcpp::shutdown(); -} \ No newline at end of file +} From a9e90b1d322cf11385783af5a29fd546c9d2acd3 Mon Sep 17 00:00:00 2001 From: Ishan1923 Date: Mon, 9 Feb 2026 00:54:19 +0530 Subject: [PATCH 7/8] fix: use unordered_set for logging and remove unsafe to_string call --- .../gpio_controllers/gpio_command_controller.hpp | 2 ++ gpio_controllers/src/gpio_command_controller.cpp | 13 +++++++++---- 2 files changed, 11 insertions(+), 4 deletions(-) diff --git a/gpio_controllers/include/gpio_controllers/gpio_command_controller.hpp b/gpio_controllers/include/gpio_controllers/gpio_command_controller.hpp index 561abcce91..da91c4b1f2 100644 --- a/gpio_controllers/include/gpio_controllers/gpio_command_controller.hpp +++ b/gpio_controllers/include/gpio_controllers/gpio_command_controller.hpp @@ -18,6 +18,7 @@ #include #include #include +#include #include #include "control_msgs/msg/dynamic_interface_group_values.hpp" @@ -84,6 +85,7 @@ class GpioCommandController : public controller_interface::ControllerInterface InterfacesNames get_gpios_state_interfaces_names(const std::string & gpio_name) const; bool update_dynamic_map_parameters(); std::vector get_gpios_from_urdf() const; + mutable std::unordered_set nan_interfaces_warned_; protected: InterfacesNames command_interface_types_; diff --git a/gpio_controllers/src/gpio_command_controller.cpp b/gpio_controllers/src/gpio_command_controller.cpp index dc83392b9a..160309b436 100644 --- a/gpio_controllers/src/gpio_command_controller.cpp +++ b/gpio_controllers/src/gpio_command_controller.cpp @@ -441,10 +441,15 @@ void GpioCommandController::apply_state_value( } else { - RCLCPP_DEBUG_ONCE( - get_node()->get_logger(), - "Interface '%s' has unsupported type '%s'. Only 'double' and 'bool' are supported.", - interface_name.c_str(), type.to_string().c_str()); + if (nan_interfaces_warned_.find(interface_name) == nan_interfaces_warned_.end()) + { + RCLCPP_INFO( + get_node()->get_logger(), + "Interface '%s' has unsupported type. Only 'double' and 'bool' are supported.", + interface_name.c_str()); + + nan_interfaces_warned_.insert(interface_name); + } } state_msg.interface_values[gpio_index].values[interface_index] = state_value; From f7a84b2c91fd3ff9360e49e84affe087f7a9bb49 Mon Sep 17 00:00:00 2001 From: Ishan1923 Date: Tue, 17 Feb 2026 01:48:46 +0530 Subject: [PATCH 8/8] fix previously introduced ABI break in gpio_controllers Signed-off-by: Ishan1923 --- .../gpio_controllers/gpio_command_controller.hpp | 2 -- gpio_controllers/src/gpio_command_controller.cpp | 13 ++++--------- 2 files changed, 4 insertions(+), 11 deletions(-) diff --git a/gpio_controllers/include/gpio_controllers/gpio_command_controller.hpp b/gpio_controllers/include/gpio_controllers/gpio_command_controller.hpp index da91c4b1f2..561abcce91 100644 --- a/gpio_controllers/include/gpio_controllers/gpio_command_controller.hpp +++ b/gpio_controllers/include/gpio_controllers/gpio_command_controller.hpp @@ -18,7 +18,6 @@ #include #include #include -#include #include #include "control_msgs/msg/dynamic_interface_group_values.hpp" @@ -85,7 +84,6 @@ class GpioCommandController : public controller_interface::ControllerInterface InterfacesNames get_gpios_state_interfaces_names(const std::string & gpio_name) const; bool update_dynamic_map_parameters(); std::vector get_gpios_from_urdf() const; - mutable std::unordered_set nan_interfaces_warned_; protected: InterfacesNames command_interface_types_; diff --git a/gpio_controllers/src/gpio_command_controller.cpp b/gpio_controllers/src/gpio_command_controller.cpp index 160309b436..6f23ff9c47 100644 --- a/gpio_controllers/src/gpio_command_controller.cpp +++ b/gpio_controllers/src/gpio_command_controller.cpp @@ -441,15 +441,10 @@ void GpioCommandController::apply_state_value( } else { - if (nan_interfaces_warned_.find(interface_name) == nan_interfaces_warned_.end()) - { - RCLCPP_INFO( - get_node()->get_logger(), - "Interface '%s' has unsupported type. Only 'double' and 'bool' are supported.", - interface_name.c_str()); - - nan_interfaces_warned_.insert(interface_name); - } + RCLCPP_WARN_THROTTLE( + get_node()->get_logger(), *get_node()->get_clock(), 10000, + "Interface '%s' has unsupported type. Only 'double' and 'bool' are supported.", + interface_name.c_str()); } state_msg.interface_values[gpio_index].values[interface_index] = state_value;