diff --git a/gpio_controllers/CMakeLists.txt b/gpio_controllers/CMakeLists.txt index ed907d6229..8885c2eb05 100644 --- a/gpio_controllers/CMakeLists.txt +++ b/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() diff --git a/gpio_controllers/src/gpio_command_controller.cpp b/gpio_controllers/src/gpio_command_controller.cpp index 8ee414d7a0..6f23ff9c47 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,7 @@ std::vector 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::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(); + if (val_opt.has_value()) + { + state_value = val_opt.value(); + } + } + 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; + } } 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) { diff --git a/gpio_controllers/test/test_gpio_command_controller.cpp b/gpio_controllers/test/test_gpio_command_controller.cpp index f3d8eeda55..137eab789e 100644 --- a/gpio_controllers/test/test_gpio_command_controller.cpp +++ b/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{"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))); +}