[gpio_controllers] Fix crash on NaN state values and sanitize inputs

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()
 

gpio_controllers/src/gpio_command_controller.cpp

@@ -15,6 +15,7 @@
 #include "gpio_controllers/gpio_command_controller.hpp"
 
 #include <algorithm>
+#include <cmath>
 
 #include "controller_interface/helpers.hpp"
 #include "hardware_interface/component_parser.hpp"
@@ -53,6 +54,7 @@ std::vector<hardware_interface::ComponentInfo> extract_gpios_from_hardware_info(
   }
   return result;
 }
+
 }  // namespace
 
 namespace gpio_controllers
@@ -417,20 +419,35 @@ 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<double>::quiet_NaN();
+    const auto & interface = state_interfaces_map_.at(interface_name).get();
+    const auto & type = interface.get_data_type();
 
-    if (!state_msg_interface_value_op.has_value())
+    if (type == hardware_interface::HandleDataType::DOUBLE)
+    {
+      const auto val_opt = interface.get_optional<double>();
+      if (val_opt.has_value())
+      {
+        state_value = val_opt.value();
+      }
+    }
+    else if (type == hardware_interface::HandleDataType::BOOL)
     {
-      RCLCPP_DEBUG(
-        get_node()->get_logger(), "Unable to retrieve the data from state: %s \n",
-        interface_name.c_str());
+      const auto val_opt = interface.get_optional<bool>();
+      if (val_opt.has_value())
+      {
+        state_value = val_opt.value() ? 1.0 : 0.0;
+      }
     }
     else
     {
-      state_msg.interface_values[gpio_index].values[interface_index] =
-        state_msg_interface_value_op.value();
+      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;
   }
   catch (const std::exception & e)
   {

gpio_controllers/test/test_gpio_command_controller.cpp

@@ -85,6 +85,7 @@ class FriendGpioCommandController : public gpio_controllers::GpioCommandControll
   FRIEND_TEST(
     GpioCommandControllerTestSuite,
     WhenGivenCmdContainsWrongGpioInterfacesOrWrongGpioNameThenCommandInterfacesShouldNotBeUpdated);
+  FRIEND_TEST(GpioCommandControllerTestSuite, UpdateBoolGpioInterfaces);
 };
 
 class GpioCommandControllerTestSuite : public ::testing::Test
@@ -708,3 +709,36 @@ 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<std::string>{"gpio1"}},
+     {"command_interfaces.gpio1.interfaces", std::vector<std::string>{"dig_out_1"}},
+     {"state_interfaces.gpio1.interfaces", std::vector<std::string>{"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<CommandInterface>("gpio1", "dig_out_1", &dummy_double_value);
+  std::vector<LoanedCommandInterface> cmd_loaned;
+  cmd_loaned.emplace_back(cmd_intf, nullptr);
+
+  auto state_intf = std::make_shared<StateInterface>("gpio1", "dig_in_1", &dummy_double_value);
+  std::vector<LoanedStateInterface> 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)));
+}