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

gpio_controllers/CMakeLists.txt

@@ -76,6 +76,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/include/gpio_controllers/gpio_command_controller.hpp

@@ -77,8 +77,7 @@ class GpioCommandController : public controller_interface::ControllerInterface
   void apply_state_value(
     StateType & state_msg, std::size_t gpio_index, std::size_t interface_index) const;
   void apply_command(
-    const CmdType & gpio_commands, std::size_t gpio_index,
-    std::size_t command_interface_index) const;
+    const CmdType & gpio_commands, std::size_t gpio_index, std::size_t command_interface_index);
   bool should_broadcast_all_interfaces_of_configured_gpios() const;
   void set_all_state_interfaces_of_configured_gpios();
   InterfacesNames get_gpios_state_interfaces_names(const std::string & gpio_name) const;

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
@@ -362,7 +364,7 @@ controller_interface::return_type GpioCommandController::update_gpios_commands()
 }
 
 void GpioCommandController::apply_command(
-  const CmdType & gpio_commands, std::size_t gpio_index, std::size_t command_interface_index) const
+  const CmdType & gpio_commands, std::size_t gpio_index, std::size_t command_interface_index)
 {
   const auto full_command_interface_name =
     gpio_commands.interface_groups[gpio_index] + '/' +
@@ -373,11 +375,34 @@ void GpioCommandController::apply_command(
 
   try
   {
-    if (!command_interfaces_map_.at(full_command_interface_name).get().set_value(command_value))
+    auto & interface = command_interfaces_map_.at(full_command_interface_name).get();
+    const auto & type = interface.get_data_type();
+    if (type == hardware_interface::HandleDataType::DOUBLE)
+    {
+      if (!interface.set_value<double>(command_value))
+      {
+        RCLCPP_WARN(
+          get_node()->get_logger(), "Unable to set the command for interface '%s' with value '%f'.",
+          full_command_interface_name.c_str(), command_value);
+      }
+    }
+    else if (type == hardware_interface::HandleDataType::BOOL)
+    {
+      const bool bool_command_value =
+        std::abs(command_value) > std::numeric_limits<double>::epsilon();
+      if (!interface.set_value<bool>(bool_command_value))
+      {
+        RCLCPP_WARN(
+          get_node()->get_logger(), "Unable to set the command for interface '%s' with value '%f'.",
+          full_command_interface_name.c_str(), command_value);
+      }
+    }
+    else
     {
       RCLCPP_WARN(
-        get_node()->get_logger(), "Unable to set the command for interface '%s' with value '%f'.",
-        full_command_interface_name.c_str(), command_value);
+        get_node()->get_logger(),
+        "Interface '%s' has unsupported type. Only 'double' and 'bool' are supported.",
+        full_command_interface_name.c_str());
     }
   }
   catch (const std::exception & e)
@@ -417,20 +442,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)
     {
-      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<double>();
+      if (val_opt.has_value())
+      {
+        state_value = val_opt.value();
+      }
+    }
+    else if (type == hardware_interface::HandleDataType::BOOL)
+    {
+      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_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;
   }
   catch (const std::exception & e)
   {

gpio_controllers/test/test_gpio_command_controller.cpp

@@ -85,6 +85,8 @@ class FriendGpioCommandController : public gpio_controllers::GpioCommandControll
   FRIEND_TEST(
     GpioCommandControllerTestSuite,
     WhenGivenCmdContainsWrongGpioInterfacesOrWrongGpioNameThenCommandInterfacesShouldNotBeUpdated);
+  FRIEND_TEST(GpioCommandControllerTestSuite, UpdateBoolGpioInterfaces);
+  FRIEND_TEST(GpioCommandControllerTestSuite, UpdateDoubleGpioInterfaces);
 };
 
 class GpioCommandControllerTestSuite : public ::testing::Test
@@ -708,3 +710,125 @@ 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, UpdateDoubleGpioInterfaces)
+{
+  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_cmd = 0.0;
+  double dummy_double_value_state = 0.0;
+
+  auto cmd_intf = std::make_shared<CommandInterface>("gpio1", "dig_out_1", &dummy_double_value_cmd);
+  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_state);
+  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);
+
+  CmdType cmd_msg;
+  cmd_msg.interface_groups = {"gpio1"};
+  cmd_msg.interface_values.resize(1);
+  cmd_msg.interface_values.at(0).interface_names = {"dig_out_1"};
+  cmd_msg.interface_values.at(0).values = {1.0};
+
+  auto publisher = node->create_publisher<CmdType>(
+    std::string(controller_->get_node()->get_name()) + "/commands", 10);
+  publisher->publish(cmd_msg);
+
+  publisher->publish(cmd_msg);
+  wait_one_miliseconds_for_timeout();
+  controller_->update(rclcpp::Time(0), rclcpp::Duration::from_seconds(0.01));
+
+  ASSERT_EQ(dummy_double_value_cmd, 1.0);
+
+  auto state_subscription = node->create_subscription<StateType>(
+    std::string(controller_->get_node()->get_name()) + "/gpio_states", 10,
+    [&](const StateType::SharedPtr) {});
+
+  stop_test_when_message_cannot_be_published(wait_for_subscription(state_subscription));
+
+  StateType gpio_state_msg;
+  rclcpp::MessageInfo msg_info;
+  ASSERT_TRUE(state_subscription->take(gpio_state_msg, msg_info));
+  ASSERT_EQ(gpio_state_msg.interface_values.at(0).values.at(0), 0.0);
+
+  // This verifies that the controller no longer crashes on update
+  EXPECT_NO_THROW(controller_->update(rclcpp::Time(0), rclcpp::Duration::from_seconds(0.01)));
+}
+
+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);
+
+  auto cmd_intf = std::make_shared<CommandInterface>("gpio1", "dig_out_1", "bool", "false");
+  std::vector<LoanedCommandInterface> cmd_loaned;
+  cmd_loaned.emplace_back(cmd_intf, nullptr);
+
+  auto state_intf = std::make_shared<StateInterface>("gpio1", "dig_in_1", "bool", "true");
+  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);
+
+  CmdType cmd_msg;
+  cmd_msg.interface_groups = {"gpio1"};
+  cmd_msg.interface_values.resize(1);
+  cmd_msg.interface_values.at(0).interface_names = {"dig_out_1"};
+  cmd_msg.interface_values.at(0).values = {1.0};
+
+  auto publisher = node->create_publisher<CmdType>(
+    std::string(controller_->get_node()->get_name()) + "/commands", 10);
+  publisher->publish(cmd_msg);
+
+  publisher->publish(cmd_msg);
+  wait_one_miliseconds_for_timeout();
+  controller_->update(rclcpp::Time(0), rclcpp::Duration::from_seconds(0.01));
+
+  const auto result = cmd_intf->get_optional<bool>();
+  ASSERT_TRUE(result.has_value());
+  ASSERT_TRUE(result.value());
+
+  auto state_subscription = node->create_subscription<StateType>(
+    std::string(controller_->get_node()->get_name()) + "/gpio_states", 10,
+    [&](const StateType::SharedPtr) {});
+
+  stop_test_when_message_cannot_be_published(wait_for_subscription(state_subscription));
+
+  StateType gpio_state_msg;
+  rclcpp::MessageInfo msg_info;
+  ASSERT_TRUE(state_subscription->take(gpio_state_msg, msg_info));
+  ASSERT_EQ(gpio_state_msg.interface_values.at(0).values.at(0), 1.0);
+
+  // This verifies that the controller no longer crashes on update
+  EXPECT_NO_THROW(controller_->update(rclcpp::Time(0), rclcpp::Duration::from_seconds(0.01)));
+}