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

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,16 @@ std::vector<hardware_interface::ComponentInfo> 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<double>(
       gpio_state_msg_.interface_values[gpio_index].interface_names.size(),
-      std::numeric_limits<double>::quiet_NaN());
+      sanitize_double(std::numeric_limits<double>::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)

gpio_controllers/test/test_load_gpio_command_controller.cpp

@@ -14,11 +14,41 @@
 
 #include <gmock/gmock.h>
 #include <memory>
+#include <fstream>
+#include <cstdio>
+#include <vector>
+#include <filesystem>
 
 #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"(
+<?xml version="1.0" encoding="utf-8"?>
+<robot name="BoolGpioBot">
+  <link name="world"/>
+  <link name="base_link"/>
+  <joint name="base_joint" type="fixed">
+    <origin rpy="0 0 0" xyz="0 0 0"/>
+    <parent link="world"/>
+    <child link="base_link"/>
+  </joint>
+  <ros2_control name="BoolGpioBot" type="system">
+    <hardware>
+      <plugin>mock_components/GenericSystem</plugin>
+    </hardware>
+    <gpio name="gpio1">
+      <command_interface name="dig_out_1" data_type="bool"/>
+      <state_interface name="dig_in_1" data_type="bool"/>
+    </gpio>
+  </ros2_control>
+</robot>
+)";
 
 TEST(TestLoadGpioCommandController, load_controller)
 {
@@ -35,3 +65,53 @@ TEST(TestLoadGpioCommandController, load_controller)
 
   rclcpp::shutdown();
 }
+
+class TestableGpioController : public gpio_controllers::GpioCommandController
+{
+public:
+  using gpio_controllers::GpioCommandController::assign_interfaces;
+};
+
+TEST(TestLoadGpioCommandController, ReproduceBadCastCrash)
+{
+  if(!rclcpp::ok()){
+    rclcpp::init(0, nullptr);
+  }
+
+  rclcpp::NodeOptions node_options;
+  std::vector<rclcpp::Parameter> params;
+  params.emplace_back("gpios", std::vector<std::string>{"gpio1"});
+  params.emplace_back("command_interfaces.gpio1.interfaces", std::vector<std::string>{"dig_out_1"});
+  params.emplace_back("state_interfaces.gpio1.interfaces", std::vector<std::string>{"dig_in_1"});
+  node_options.parameter_overrides(params);
+
+  auto controller = std::make_shared<TestableGpioController>();
+
+  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<hardware_interface::CommandInterface>("gpio1", "dig_out_1", &dummy_double_value);
+  std::vector<hardware_interface::LoanedCommandInterface> cmd_loaned;
+  cmd_loaned.emplace_back(cmd_intf);
+
+  auto state_intf = std::make_shared<hardware_interface::StateInterface>("gpio1", "dig_in_1", &dummy_double_value);
+  std::vector<hardware_interface::LoanedStateInterface> 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();
+}