diff --git a/gpio_controllers/include/gpio_controllers/gpio_command_controller.hpp b/gpio_controllers/include/gpio_controllers/gpio_command_controller.hpp index ad80cf4457..5149c74a1c 100644 --- a/gpio_controllers/include/gpio_controllers/gpio_command_controller.hpp +++ b/gpio_controllers/include/gpio_controllers/gpio_command_controller.hpp @@ -20,7 +20,7 @@ #include #include -#include "control_msgs/msg/dynamic_joint_state.hpp" +#include "control_msgs/msg/dynamic_interface_group_values.hpp" #include "controller_interface/controller_interface.hpp" #include "gpio_command_controller_parameters.hpp" #include "gpio_controllers/visibility_control.h" @@ -31,8 +31,8 @@ namespace gpio_controllers { -using CmdType = control_msgs::msg::DynamicJointState; -using StateType = control_msgs::msg::DynamicJointState; +using CmdType = control_msgs::msg::DynamicInterfaceGroupValues; +using StateType = control_msgs::msg::DynamicInterfaceGroupValues; using CallbackReturn = controller_interface::CallbackReturn; using InterfacesNames = std::vector; using MapOfReferencesToCommandInterfaces = std::unordered_map< diff --git a/gpio_controllers/package.xml b/gpio_controllers/package.xml index 888aaef3f6..9b98746545 100644 --- a/gpio_controllers/package.xml +++ b/gpio_controllers/package.xml @@ -16,6 +16,7 @@ rclcpp_lifecycle control_msgs realtime_tools + generate_parameter_library pluginlib diff --git a/gpio_controllers/src/gpio_command_controller.cpp b/gpio_controllers/src/gpio_command_controller.cpp index d3989eb122..a509eae774 100644 --- a/gpio_controllers/src/gpio_command_controller.cpp +++ b/gpio_controllers/src/gpio_command_controller.cpp @@ -246,13 +246,13 @@ void GpioCommandController::initialize_gpio_state_msg() { auto & gpio_state_msg = realtime_gpio_state_publisher_->msg_; gpio_state_msg.header.stamp = get_node()->now(); - gpio_state_msg.joint_names.resize(params_.gpios.size()); + gpio_state_msg.interface_groups.resize(params_.gpios.size()); gpio_state_msg.interface_values.resize(params_.gpios.size()); for (std::size_t gpio_index = 0; gpio_index < params_.gpios.size(); ++gpio_index) { const auto gpio_name = params_.gpios[gpio_index]; - gpio_state_msg.joint_names[gpio_index] = gpio_name; + gpio_state_msg.interface_groups[gpio_index] = gpio_name; gpio_state_msg.interface_values[gpio_index].interface_names = get_gpios_state_interfaces_names(gpio_name); gpio_state_msg.interface_values[gpio_index].values = std::vector( @@ -327,9 +327,9 @@ controller_interface::return_type GpioCommandController::update_gpios_commands() } const auto gpio_commands = *(*gpio_commands_ptr); - for (std::size_t gpio_index = 0; gpio_index < gpio_commands.joint_names.size(); ++gpio_index) + for (std::size_t gpio_index = 0; gpio_index < gpio_commands.interface_groups.size(); ++gpio_index) { - const auto & gpio_name = gpio_commands.joint_names[gpio_index]; + const auto & gpio_name = gpio_commands.interface_groups[gpio_index]; if ( gpio_commands.interface_values[gpio_index].values.size() != gpio_commands.interface_values[gpio_index].interface_names.size()) @@ -353,7 +353,7 @@ void GpioCommandController::apply_command( const CmdType & gpio_commands, std::size_t gpio_index, std::size_t command_interface_index) const { const auto full_command_interface_name = - gpio_commands.joint_names[gpio_index] + '/' + + gpio_commands.interface_groups[gpio_index] + '/' + gpio_commands.interface_values[gpio_index].interface_names[command_interface_index]; try { @@ -378,7 +378,8 @@ void GpioCommandController::update_gpios_states() auto & gpio_state_msg = realtime_gpio_state_publisher_->msg_; gpio_state_msg.header.stamp = get_node()->now(); - for (std::size_t gpio_index = 0; gpio_index < gpio_state_msg.joint_names.size(); ++gpio_index) + for (std::size_t gpio_index = 0; gpio_index < gpio_state_msg.interface_groups.size(); + ++gpio_index) { for (std::size_t interface_index = 0; interface_index < gpio_state_msg.interface_values[gpio_index].interface_names.size(); @@ -394,7 +395,7 @@ void GpioCommandController::apply_state_value( StateType & state_msg, std::size_t gpio_index, std::size_t interface_index) const { const auto interface_name = - state_msg.joint_names[gpio_index] + '/' + + state_msg.interface_groups[gpio_index] + '/' + state_msg.interface_values[gpio_index].interface_names[interface_index]; try { diff --git a/gpio_controllers/test/test_gpio_command_controller.cpp b/gpio_controllers/test/test_gpio_command_controller.cpp index 49b68e42b1..ec0e0f1228 100644 --- a/gpio_controllers/test/test_gpio_command_controller.cpp +++ b/gpio_controllers/test/test_gpio_command_controller.cpp @@ -15,6 +15,7 @@ #include #include +#include "control_msgs/msg/dynamic_interface_group_values.hpp" #include "gmock/gmock.h" #include "gpio_controllers/gpio_command_controller.hpp" #include "hardware_interface/handle.hpp" @@ -48,8 +49,8 @@ const auto minimal_robot_urdf_with_gpio = std::string(ros2_control_test_assets:: using CallbackReturn = controller_interface::CallbackReturn; using hardware_interface::LoanedCommandInterface; using hardware_interface::LoanedStateInterface; -using CmdType = control_msgs::msg::DynamicJointState; -using StateType = control_msgs::msg::DynamicJointState; +using CmdType = control_msgs::msg::DynamicInterfaceGroupValues; +using StateType = control_msgs::msg::DynamicInterfaceGroupValues; using hardware_interface::CommandInterface; using hardware_interface::StateInterface; @@ -146,7 +147,7 @@ class GpioCommandControllerTestSuite : public ::testing::Test const std::vector & interface_values) { CmdType command; - command.joint_names = gpios_names; + command.interface_groups = gpios_names; command.interface_values = interface_values; return command; } @@ -602,9 +603,9 @@ TEST_F(GpioCommandControllerTestSuite, ControllerShouldPublishGpioStatesWithCurr rclcpp::MessageInfo msg_info; ASSERT_TRUE(subscription->take(gpio_state_msg, msg_info)); - ASSERT_EQ(gpio_state_msg.joint_names.size(), gpio_names.size()); - ASSERT_EQ(gpio_state_msg.joint_names.at(0), "gpio1"); - ASSERT_EQ(gpio_state_msg.joint_names.at(1), "gpio2"); + ASSERT_EQ(gpio_state_msg.interface_groups.size(), gpio_names.size()); + ASSERT_EQ(gpio_state_msg.interface_groups.at(0), "gpio1"); + ASSERT_EQ(gpio_state_msg.interface_groups.at(1), "gpio2"); ASSERT_EQ(gpio_state_msg.interface_values.at(0).interface_names.at(0), "dig.1"); ASSERT_EQ(gpio_state_msg.interface_values.at(0).interface_names.at(1), "dig.2"); ASSERT_EQ(gpio_state_msg.interface_values.at(1).interface_names.at(0), "ana.1"); @@ -646,8 +647,8 @@ TEST_F( rclcpp::MessageInfo msg_info; ASSERT_TRUE(subscription->take(gpio_state_msg, msg_info)); - ASSERT_EQ(gpio_state_msg.joint_names.size(), single_gpio.size()); - ASSERT_EQ(gpio_state_msg.joint_names.at(0), "gpio1"); + ASSERT_EQ(gpio_state_msg.interface_groups.size(), single_gpio.size()); + ASSERT_EQ(gpio_state_msg.interface_groups.at(0), "gpio1"); ASSERT_EQ(gpio_state_msg.interface_values.at(0).interface_names.at(0), "dig.1"); ASSERT_EQ(gpio_state_msg.interface_values.at(0).values.at(0), 1.0); } @@ -682,9 +683,9 @@ TEST_F( rclcpp::MessageInfo msg_info; ASSERT_TRUE(subscription->take(gpio_state_msg, msg_info)); - ASSERT_EQ(gpio_state_msg.joint_names.size(), gpio_names.size()); - ASSERT_EQ(gpio_state_msg.joint_names.at(0), "gpio1"); - ASSERT_EQ(gpio_state_msg.joint_names.at(1), "gpio2"); + ASSERT_EQ(gpio_state_msg.interface_groups.size(), gpio_names.size()); + ASSERT_EQ(gpio_state_msg.interface_groups.at(0), "gpio1"); + ASSERT_EQ(gpio_state_msg.interface_groups.at(1), "gpio2"); ASSERT_EQ(gpio_state_msg.interface_values.at(0).interface_names.at(0), "dig.1"); ASSERT_EQ(gpio_state_msg.interface_values.at(1).interface_names.at(0), "ana.1"); ASSERT_EQ(gpio_state_msg.interface_values.at(0).values.at(0), 1.0);