[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,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<double>::quiet_NaN();
+    bool read_success = false;
+    const auto& interface = state_interfaces_map_.at(interface_name).get();
+    auto opt_double = interface.get_optional<double>();
+    if(opt_double.has_value()){
+      state_value = opt_double.value();
+      read_success = true;
+    }
+    else{
+      auto opt_bool = interface.get_optional<bool>();
+      if(opt_bool.has_value()){
+        state_value = static_cast<double>( (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());
-    }
-    else
-    {
-      state_msg.interface_values[gpio_index].values[interface_index] =
-        state_msg_interface_value_op.value();
+        get_node()->get_logger(), "Unable to retrive the data from state(datatype neither bool nor double): %s \n", interface_name.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,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<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)));
+}

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();
-}
+}