From 5fecf27c5494fd516f0fcfb1b00c493503a817f8 Mon Sep 17 00:00:00 2001 From: "m.bednarczyk" Date: Wed, 2 Nov 2022 15:30:19 +0100 Subject: [PATCH 01/58] added gpio_command_controller --- gpio_controllers/CMakeLists.txt | 106 ++++++++ gpio_controllers/gpio_controllers_plugin.xml | 7 + .../gpio_command_controller.hpp | 76 ++++++ .../gpio_controllers/visibility_control.h | 35 +++ gpio_controllers/package.xml | 28 ++ .../src/gpio_command_controller.cpp | 175 ++++++++++++ .../test/test_gpio_command_controller.cpp | 248 ++++++++++++++++++ .../test/test_gpio_command_controller.hpp | 62 +++++ .../test_load_gpio_command_controller.cpp | 39 +++ 9 files changed, 776 insertions(+) create mode 100644 gpio_controllers/CMakeLists.txt create mode 100644 gpio_controllers/gpio_controllers_plugin.xml create mode 100644 gpio_controllers/include/gpio_controllers/gpio_command_controller.hpp create mode 100644 gpio_controllers/include/gpio_controllers/visibility_control.h create mode 100644 gpio_controllers/package.xml create mode 100644 gpio_controllers/src/gpio_command_controller.cpp create mode 100644 gpio_controllers/test/test_gpio_command_controller.cpp create mode 100644 gpio_controllers/test/test_gpio_command_controller.hpp create mode 100644 gpio_controllers/test/test_load_gpio_command_controller.cpp diff --git a/gpio_controllers/CMakeLists.txt b/gpio_controllers/CMakeLists.txt new file mode 100644 index 0000000000..3c1ec6b927 --- /dev/null +++ b/gpio_controllers/CMakeLists.txt @@ -0,0 +1,106 @@ +cmake_minimum_required(VERSION 3.8) +project(gpio_controllers) + +# Default to C++14 +if(NOT CMAKE_CXX_STANDARD) + set(CMAKE_CXX_STANDARD 14) +endif() + +if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") + add_compile_options(-Wall -Wextra -Wpedantic) +endif() + +# find dependencies +find_package(ament_cmake REQUIRED) +find_package(controller_interface REQUIRED) +find_package(hardware_interface REQUIRED) +find_package(pluginlib REQUIRED) +find_package(rclcpp REQUIRED) +find_package(rclcpp_lifecycle REQUIRED) +find_package(realtime_tools REQUIRED) +find_package(std_msgs REQUIRED) + +add_library(gpio_controllers + SHARED + src/gpio_command_controller.cpp +) +target_include_directories(gpio_controllers PRIVATE include) +ament_target_dependencies(gpio_controllers + builtin_interfaces + controller_interface + hardware_interface + pluginlib + rclcpp_lifecycle + rcutils + realtime_tools + std_msgs +) +# Causes the visibility macros to use dllexport rather than dllimport, +# which is appropriate when building the dll but not consuming it. +target_compile_definitions(gpio_controllers PRIVATE "GPIO_COMMAND_CONTROLLER_BUILDING_DLL") +pluginlib_export_plugin_description_file(controller_interface gpio_controllers_plugin.xml) + +install( + DIRECTORY include/ + DESTINATION include +) + +install( + TARGETS + gpio_controllers + RUNTIME DESTINATION bin + ARCHIVE DESTINATION lib + LIBRARY DESTINATION lib +) + +if(BUILD_TESTING) + find_package(ament_cmake_gmock REQUIRED) + find_package(controller_manager REQUIRED) + find_package(hardware_interface REQUIRED) + find_package(ros2_control_test_assets REQUIRED) + + ament_add_gmock( + test_load_gpio_command_controller + test/test_load_gpio_command_controller.cpp + ) + + target_include_directories(test_load_gpio_command_controller PRIVATE include) + ament_target_dependencies(test_load_gpio_command_controller + controller_manager + hardware_interface + ros2_control_test_assets + ) + + ament_add_gmock( + test_gpio_command_controller + test/test_gpio_command_controller.cpp + ) + target_include_directories(test_gpio_command_controller PRIVATE include) + target_link_libraries(test_gpio_command_controller + gpio_controllers + ) + ament_target_dependencies(test_gpio_command_controller + controller_interface + hardware_interface + rclcpp + rclcpp_lifecycle + realtime_tools + std_msgs + ) +endif() + +ament_export_dependencies( + controller_interface + hardware_interface + rclcpp + rclcpp_lifecycle + realtime_tools + std_msgs +) +ament_export_include_directories( + include +) +ament_export_libraries( + gpio_controllers +) +ament_package() \ No newline at end of file diff --git a/gpio_controllers/gpio_controllers_plugin.xml b/gpio_controllers/gpio_controllers_plugin.xml new file mode 100644 index 0000000000..ef7d59c9b5 --- /dev/null +++ b/gpio_controllers/gpio_controllers_plugin.xml @@ -0,0 +1,7 @@ + + + + The gpio command controller commands a group of gpios using multiple interfaces. + + + \ No newline at end of file diff --git a/gpio_controllers/include/gpio_controllers/gpio_command_controller.hpp b/gpio_controllers/include/gpio_controllers/gpio_command_controller.hpp new file mode 100644 index 0000000000..9ffd9adc73 --- /dev/null +++ b/gpio_controllers/include/gpio_controllers/gpio_command_controller.hpp @@ -0,0 +1,76 @@ +// Copyright 2022 ICUBE Laboratory, University of Strasbourg +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef GPIO_COMMAND_CONTROLLER__GPIO_COMMAND_CONTROLLER_HPP_ +#define GPIO_COMMAND_CONTROLLER__GPIO_COMMAND_CONTROLLER_HPP_ + +#include +#include +#include + +#include "controller_interface/controller_interface.hpp" +#include "gpio_controllers/visibility_control.h" +#include "rclcpp/subscription.hpp" +#include "rclcpp_lifecycle/node_interfaces/lifecycle_node_interface.hpp" +#include "rclcpp_lifecycle/state.hpp" +#include "realtime_tools/realtime_buffer.h" +#include "std_msgs/msg/float64_multi_array.hpp" + +namespace gpio_controllers +{ +using CmdType = std_msgs::msg::Float64MultiArray; +using CallbackReturn = rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn; + +class GpioCommandController : public controller_interface::ControllerInterface +{ +public: + GPIO_COMMAND_CONTROLLER_PUBLIC + GpioCommandController(); + + GPIO_COMMAND_CONTROLLER_PUBLIC + controller_interface::InterfaceConfiguration command_interface_configuration() const override; + + GPIO_COMMAND_CONTROLLER_PUBLIC + controller_interface::InterfaceConfiguration state_interface_configuration() const override; + + GPIO_COMMAND_CONTROLLER_PUBLIC + CallbackReturn on_init() override; + + GPIO_COMMAND_CONTROLLER_PUBLIC + CallbackReturn on_configure(const rclcpp_lifecycle::State & previous_state) override; + + GPIO_COMMAND_CONTROLLER_PUBLIC + CallbackReturn on_activate(const rclcpp_lifecycle::State & previous_state) override; + + GPIO_COMMAND_CONTROLLER_PUBLIC + CallbackReturn on_deactivate(const rclcpp_lifecycle::State & previous_state) override; + + GPIO_COMMAND_CONTROLLER_PUBLIC + controller_interface::return_type update( + const rclcpp::Time & time, const rclcpp::Duration & period) override; + +protected: + std::vector gpio_names_; + std::unordered_map> interface_names_; + std::vector command_interface_types_; + + realtime_tools::RealtimeBuffer> rt_command_ptr_; + rclcpp::Subscription::SharedPtr gpios_command_subscriber_; + + std::string logger_name_; +}; + +} // namespace gpio_controllers + +#endif // GPIO_COMMAND_CONTROLLER__GPIO_COMMAND_CONTROLLER_HPP_ diff --git a/gpio_controllers/include/gpio_controllers/visibility_control.h b/gpio_controllers/include/gpio_controllers/visibility_control.h new file mode 100644 index 0000000000..290da66e0d --- /dev/null +++ b/gpio_controllers/include/gpio_controllers/visibility_control.h @@ -0,0 +1,35 @@ +#ifndef GPIO_COMMAND_CONTROLLER__VISIBILITY_CONTROL_H_ +#define GPIO_COMMAND_CONTROLLER__VISIBILITY_CONTROL_H_ + +// This logic was borrowed (then namespaced) from the examples on the gcc wiki: +// https://gcc.gnu.org/wiki/Visibility + +#if defined _WIN32 || defined __CYGWIN__ + #ifdef __GNUC__ + #define GPIO_COMMAND_CONTROLLER_EXPORT __attribute__ ((dllexport)) + #define GPIO_COMMAND_CONTROLLER_IMPORT __attribute__ ((dllimport)) + #else + #define GPIO_COMMAND_CONTROLLER_EXPORT __declspec(dllexport) + #define GPIO_COMMAND_CONTROLLER_IMPORT __declspec(dllimport) + #endif + #ifdef GPIO_COMMAND_CONTROLLER_BUILDING_LIBRARY + #define GPIO_COMMAND_CONTROLLER_PUBLIC GPIO_COMMAND_CONTROLLER_EXPORT + #else + #define GPIO_COMMAND_CONTROLLER_PUBLIC GPIO_COMMAND_CONTROLLER_IMPORT + #endif + #define GPIO_COMMAND_CONTROLLER_PUBLIC_TYPE GPIO_COMMAND_CONTROLLER_PUBLIC + #define GPIO_COMMAND_CONTROLLER_LOCAL +#else + #define GPIO_COMMAND_CONTROLLER_EXPORT __attribute__ ((visibility("default"))) + #define GPIO_COMMAND_CONTROLLER_IMPORT + #if __GNUC__ >= 4 + #define GPIO_COMMAND_CONTROLLER_PUBLIC __attribute__ ((visibility("default"))) + #define GPIO_COMMAND_CONTROLLER_LOCAL __attribute__ ((visibility("hidden"))) + #else + #define GPIO_COMMAND_CONTROLLER_PUBLIC + #define GPIO_COMMAND_CONTROLLER_LOCAL + #endif + #define GPIO_COMMAND_CONTROLLER_PUBLIC_TYPE +#endif + +#endif // GPIO_COMMAND_CONTROLLER__VISIBILITY_CONTROL_H_ diff --git a/gpio_controllers/package.xml b/gpio_controllers/package.xml new file mode 100644 index 0000000000..7837b8b1c8 --- /dev/null +++ b/gpio_controllers/package.xml @@ -0,0 +1,28 @@ + + + + gpio_controllers + 0.0.0 + Controllers to interact with gpios. + Maciej Bednarczyk + Apache License 2.0 + + ament_cmake + + controller_interface + hardware_interface + rclcpp + rclcpp_lifecycle + realtime_tools + std_msgs + + pluginlib + + ament_cmake_gmock + controller_manager + ros2_control_test_assets + + + ament_cmake + + diff --git a/gpio_controllers/src/gpio_command_controller.cpp b/gpio_controllers/src/gpio_command_controller.cpp new file mode 100644 index 0000000000..59c4cd23b5 --- /dev/null +++ b/gpio_controllers/src/gpio_command_controller.cpp @@ -0,0 +1,175 @@ +// Copyright 2022 ICUBE Laboratory, University of Strasbourg +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include "gpio_controllers/gpio_command_controller.hpp" + +#include +#include +#include +#include +#include + +#include "controller_interface/helpers.hpp" +#include "rclcpp/logging.hpp" +#include "rclcpp/qos.hpp" + +#include "hardware_interface/loaned_command_interface.hpp" + +namespace gpio_controllers +{ +using hardware_interface::LoanedCommandInterface; + +GpioCommandController::GpioCommandController() +: controller_interface::ControllerInterface(), + rt_command_ptr_(nullptr), + gpios_command_subscriber_(nullptr) +{ +} + +CallbackReturn GpioCommandController::on_init() +{ + try + { + auto_declare>("gpios", std::vector()); + gpio_names_ = node_->get_parameter("gpios").as_string_array(); + for(std::string &gpio : gpio_names_) + auto_declare>("interface_names." + gpio, std::vector()); + } + catch (const std::exception & e) + { + fprintf(stderr, "Exception thrown during init stage with message: %s \n", e.what()); + return CallbackReturn::ERROR; + } + + return CallbackReturn::SUCCESS; +} + +CallbackReturn GpioCommandController::on_configure( + const rclcpp_lifecycle::State & /*previous_state*/) +{ + gpio_names_ = node_->get_parameter("gpios").as_string_array(); + + if (gpio_names_.empty()){ + RCLCPP_ERROR(get_node()->get_logger(), "'gpios' parameter was empty"); + return CallbackReturn::ERROR; + } + + for(std::string &gpio : gpio_names_){ + auto interfaces = node_->get_parameter("interface_names." + gpio).as_string_array(); + if (interfaces.empty()){ + RCLCPP_ERROR(get_node()->get_logger(), "'interface_names.%s' parameter was empty",gpio.c_str()); + return CallbackReturn::ERROR; + } + if ( !interface_names_.insert( std::make_pair( gpio, interfaces) ).second ) { + RCLCPP_ERROR(get_node()->get_logger(), "Trying to override existing gpio setup. Wrong controller parameters."); + return CallbackReturn::ERROR; + } + } + + for (const auto & gpio : gpio_names_){ + for (const auto & interface_name: interface_names_[gpio]){ + command_interface_types_.push_back(gpio + "/" + interface_name); + } + } + + gpios_command_subscriber_ = get_node()->create_subscription( + "~/commands", rclcpp::SystemDefaultsQoS(), + [this](const CmdType::SharedPtr msg) { rt_command_ptr_.writeFromNonRT(msg); }); + + RCLCPP_INFO(get_node()->get_logger(), "configure successful"); + return CallbackReturn::SUCCESS; +} + +controller_interface::InterfaceConfiguration +GpioCommandController::command_interface_configuration() const +{ + controller_interface::InterfaceConfiguration command_interfaces_config; + command_interfaces_config.type = controller_interface::interface_configuration_type::INDIVIDUAL; + command_interfaces_config.names = command_interface_types_; + + return command_interfaces_config; +} + +controller_interface::InterfaceConfiguration +GpioCommandController::state_interface_configuration() const +{ + return controller_interface::InterfaceConfiguration{ + controller_interface::interface_configuration_type::NONE}; +} + +CallbackReturn GpioCommandController::on_activate( + const rclcpp_lifecycle::State & /*previous_state*/) +{ + std::vector> ordered_interfaces; + if ( + !controller_interface::get_ordered_interfaces( + command_interfaces_, command_interface_types_, std::string(""), ordered_interfaces) || + command_interfaces_.size() != ordered_interfaces.size()) + { + RCLCPP_ERROR( + node_->get_logger(), "Expected %zu command interfaces, got %zu", command_interface_types_.size(), + ordered_interfaces.size()); + for(const auto & interface: command_interface_types_) + RCLCPP_ERROR(node_->get_logger(), "Got %s", interface.c_str()); + return CallbackReturn::ERROR; + } + + // reset command buffer if a command came through callback when controller was inactive + rt_command_ptr_.reset(); + + return CallbackReturn::SUCCESS; +} + +CallbackReturn GpioCommandController::on_deactivate( + const rclcpp_lifecycle::State & /*previous_state*/) +{ + // reset command buffer + rt_command_ptr_.reset(); + return CallbackReturn::SUCCESS; +} + +controller_interface::return_type GpioCommandController::update( + const rclcpp::Time & /*time*/, const rclcpp::Duration & /*period*/) +{ + auto gpio_commands = rt_command_ptr_.readFromRT(); + + // no command received yet + if (!gpio_commands || !(*gpio_commands)) + { + return controller_interface::return_type::OK; + } + + if ((*gpio_commands)->data.size() != command_interfaces_.size()) + { + RCLCPP_ERROR_THROTTLE( + get_node()->get_logger(), *node_->get_clock(), 1000, + "command size (%zu) does not match number of interfaces (%zu)", + (*gpio_commands)->data.size(), command_interfaces_.size()); + return controller_interface::return_type::ERROR; + } + + for (size_t index = 0; index < command_interfaces_.size(); ++index) + { + command_interfaces_[index].set_value((*gpio_commands)->data[index]); + } + + return controller_interface::return_type::OK; +} + +} // namespace gpio_command_controller + +#include "pluginlib/class_list_macros.hpp" + +PLUGINLIB_EXPORT_CLASS( + gpio_controllers::GpioCommandController, controller_interface::ControllerInterface) diff --git a/gpio_controllers/test/test_gpio_command_controller.cpp b/gpio_controllers/test/test_gpio_command_controller.cpp new file mode 100644 index 0000000000..34d5ab101b --- /dev/null +++ b/gpio_controllers/test/test_gpio_command_controller.cpp @@ -0,0 +1,248 @@ +// Copyright 2022 ICUBE Laboratory, University of Strasbourg +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include + +#include +#include +#include +#include +#include + +#include "hardware_interface/loaned_command_interface.hpp" +#include "hardware_interface/types/hardware_interface_return_values.hpp" +#include "lifecycle_msgs/msg/state.hpp" +#include "rclcpp/utilities.hpp" +#include "rclcpp_lifecycle/node_interfaces/lifecycle_node_interface.hpp" +#include "test_gpio_command_controller.hpp" + +using CallbackReturn = rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn; +using hardware_interface::LoanedCommandInterface; +using CmdType = std_msgs::msg::Float64MultiArray; + +namespace +{ +rclcpp::WaitResultKind wait_for(rclcpp::SubscriptionBase::SharedPtr subscription) +{ + rclcpp::WaitSet wait_set; + wait_set.add_subscription(subscription); + const auto timeout = std::chrono::seconds(10); + return wait_set.wait(timeout).kind(); +} +} // namespace + +void GpioCommandControllerTest::SetUpTestCase() { rclcpp::init(0, nullptr); } + +void GpioCommandControllerTest::TearDownTestCase() { rclcpp::shutdown(); } + +void GpioCommandControllerTest::SetUp() +{ + controller_ = std::make_unique(); +} + +void GpioCommandControllerTest::TearDown() { controller_.reset(nullptr); } + +void GpioCommandControllerTest::SetUpController() +{ + const auto result = controller_->init("test_gpio_command_controller"); + ASSERT_EQ(result, controller_interface::return_type::OK); + + std::vector command_ifs; + command_ifs.emplace_back(gpio_1_1_dig_cmd_); + command_ifs.emplace_back(gpio_1_2_dig_cmd_); + command_ifs.emplace_back(gpio_2_ana_cmd_); + controller_->assign_interfaces(std::move(command_ifs), {}); +} + +TEST_F(GpioCommandControllerTest, GpiosParameterNotSet) +{ + SetUpController(); + + // configure failed, 'gpios' parameter not set + ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), CallbackReturn::ERROR); +} + +TEST_F(GpioCommandControllerTest, GpiosParameterIsEmpty) +{ + SetUpController(); + controller_->get_node()->set_parameter({"gpios", std::vector()}); + + // configure failed, 'gpios' is empty + ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), CallbackReturn::ERROR); +} + +TEST_F(GpioCommandControllerTest, ConfigureAndActivateParamsSuccess) +{ + SetUpController(); + controller_->get_node()->set_parameter({"gpios", gpio_names_}); + controller_->get_node()->set_parameter( + {"interface_names.gpio1", std::vector{"dig.1", "dig.2"}}); + controller_->get_node()->set_parameter( + {"interface_names.gpio2", std::vector{"ana.1"}}); + + // configure successful + ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), CallbackReturn::SUCCESS); + ASSERT_EQ(controller_->on_activate(rclcpp_lifecycle::State()), CallbackReturn::SUCCESS); +} + +TEST_F(GpioCommandControllerTest, ActivateWithWrongGpiosNamesFails) +{ + SetUpController(); + controller_->get_node()->set_parameter({"gpios", std::vector{"gpio1", "gpio4"}}); + // controller_->get_node()->set_parameter( + // {"interface_names", std::vector{"dig.1", "dig.2", "ana.1"}}); + + controller_->get_node()->set_parameter( + {"interface_names.gpio1", std::vector{"dig.1", "dig.2"}}); + controller_->get_node()->set_parameter( + {"interface_names.gpio4", std::vector{"ana.1"}}); + // // activate failed, 'gpio4' is not a valid gpio name for the hardware + ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), CallbackReturn::SUCCESS); + ASSERT_EQ(controller_->on_activate(rclcpp_lifecycle::State()), CallbackReturn::ERROR); +} + +TEST_F(GpioCommandControllerTest, CommandSuccessTest) +{ + SetUpController(); + controller_->get_node()->set_parameter({"gpios", gpio_names_}); + controller_->get_node()->set_parameter( + {"interface_names.gpio1", std::vector{"dig.1", "dig.2"}}); + controller_->get_node()->set_parameter( + {"interface_names.gpio2", std::vector{"ana.1"}}); + + + ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), CallbackReturn::SUCCESS); + + // update successful though no command has been send yet + ASSERT_EQ( + controller_->update(rclcpp::Time(0), rclcpp::Duration::from_seconds(0.01)), + controller_interface::return_type::OK); + + // check gpio commands are still the default ones + ASSERT_EQ(gpio_1_1_dig_cmd_.get_value(), 1.0); + ASSERT_EQ(gpio_1_2_dig_cmd_.get_value(), 0.0); + ASSERT_EQ(gpio_2_ana_cmd_.get_value(), 3.1); + + // send command + auto command_ptr = std::make_shared(); + command_ptr->data = {0.0, 1.0, 30.0}; + controller_->rt_command_ptr_.writeFromNonRT(command_ptr); + + // update successful, command received + ASSERT_EQ( + controller_->update(rclcpp::Time(0), rclcpp::Duration::from_seconds(0.01)), + controller_interface::return_type::OK); + + // check gpio commands have been modified + ASSERT_EQ(gpio_1_1_dig_cmd_.get_value(), 0.0); + ASSERT_EQ(gpio_1_2_dig_cmd_.get_value(), 1.0); + ASSERT_EQ(gpio_2_ana_cmd_.get_value(), 30.0); +} + +TEST_F(GpioCommandControllerTest, WrongCommandCheckTest) +{ + SetUpController(); + controller_->get_node()->set_parameter({"gpios", gpio_names_}); + controller_->get_node()->set_parameter( + {"interface_names.gpio1", std::vector{"dig.1", "dig.2"}}); + controller_->get_node()->set_parameter( + {"interface_names.gpio2", std::vector{"ana.1"}}); + + + ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), CallbackReturn::SUCCESS); + + // send command with wrong number of gpios + auto command_ptr = std::make_shared(); + command_ptr->data = {0.0, 20.0}; + controller_->rt_command_ptr_.writeFromNonRT(command_ptr); + + // update failed, command size does not match number of gpios + ASSERT_EQ( + controller_->update(rclcpp::Time(0), rclcpp::Duration::from_seconds(0.01)), + controller_interface::return_type::ERROR); + + // check gpio commands are still the default ones + ASSERT_EQ(gpio_1_1_dig_cmd_.get_value(), 1.0); + ASSERT_EQ(gpio_1_2_dig_cmd_.get_value(), 0.0); + ASSERT_EQ(gpio_2_ana_cmd_.get_value(), 3.1); +} + +TEST_F(GpioCommandControllerTest, NoCommandCheckTest) +{ + SetUpController(); + controller_->get_node()->set_parameter({"gpios", gpio_names_}); + controller_->get_node()->set_parameter( + {"interface_names.gpio1", std::vector{"dig.1", "dig.2"}}); + controller_->get_node()->set_parameter( + {"interface_names.gpio2", std::vector{"ana.1"}}); + + + ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), CallbackReturn::SUCCESS); + + // update successful, no command received yet + ASSERT_EQ( + controller_->update(rclcpp::Time(0), rclcpp::Duration::from_seconds(0.01)), + controller_interface::return_type::OK); + + // check gpio commands are still the default ones + ASSERT_EQ(gpio_1_1_dig_cmd_.get_value(), 1.0); + ASSERT_EQ(gpio_1_2_dig_cmd_.get_value(), 0.0); + ASSERT_EQ(gpio_2_ana_cmd_.get_value(), 3.1); +} + +TEST_F(GpioCommandControllerTest, CommandCallbackTest) +{ + SetUpController(); + controller_->get_node()->set_parameter({"gpios", gpio_names_}); + controller_->get_node()->set_parameter( + {"interface_names.gpio1", std::vector{"dig.1", "dig.2"}}); + controller_->get_node()->set_parameter( + {"interface_names.gpio2", std::vector{"ana.1"}}); + + + // default values + ASSERT_EQ(gpio_1_1_dig_cmd_.get_value(), 1.0); + ASSERT_EQ(gpio_1_2_dig_cmd_.get_value(), 0.0); + ASSERT_EQ(gpio_2_ana_cmd_.get_value(), 3.1); + + auto node_state = controller_->configure(); + ASSERT_EQ(node_state.id(), lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE); + + node_state = controller_->activate(); + ASSERT_EQ(node_state.id(), lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE); + + // send a new command + rclcpp::Node test_node("test_node"); + auto command_pub = test_node.create_publisher( + std::string(controller_->get_node()->get_name()) + "/commands", rclcpp::SystemDefaultsQoS()); + std_msgs::msg::Float64MultiArray command_msg; + command_msg.data = {0.0, 1.0, 30.0}; + command_pub->publish(command_msg); + + // wait for command message to be passed + ASSERT_EQ(wait_for(controller_->gpios_command_subscriber_), rclcpp::WaitResultKind::Ready); + + // process callbacks + rclcpp::spin_some(controller_->get_node()->get_node_base_interface()); + + // update successful + ASSERT_EQ( + controller_->update(rclcpp::Time(0), rclcpp::Duration::from_seconds(0.01)), + controller_interface::return_type::OK); + + // check command in handle was set + ASSERT_EQ(gpio_1_1_dig_cmd_.get_value(), 0.0); + ASSERT_EQ(gpio_1_2_dig_cmd_.get_value(), 1.0); + ASSERT_EQ(gpio_2_ana_cmd_.get_value(), 30.0); +} \ No newline at end of file diff --git a/gpio_controllers/test/test_gpio_command_controller.hpp b/gpio_controllers/test/test_gpio_command_controller.hpp new file mode 100644 index 0000000000..d85d76e154 --- /dev/null +++ b/gpio_controllers/test/test_gpio_command_controller.hpp @@ -0,0 +1,62 @@ +// Copyright 2022 ICUBE Laboratory, University of Strasbourg +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef TEST_GPIO_COMMAND_CONTROLLER_HPP_ +#define TEST_GPIO_COMMAND_CONTROLLER_HPP_ + +#include +#include +#include + +#include "gmock/gmock.h" + +#include "hardware_interface/handle.hpp" +#include "hardware_interface/types/hardware_interface_type_values.hpp" +#include "gpio_controllers/gpio_command_controller.hpp" + +using hardware_interface::CommandInterface; + +// subclassing and friending so we can access member variables +class FriendGpioCommandController : public gpio_controllers::GpioCommandController +{ + FRIEND_TEST(GpioCommandControllerTest, CommandSuccessTest); + FRIEND_TEST(GpioCommandControllerTest, WrongCommandCheckTest); + FRIEND_TEST(GpioCommandControllerTest, CommandCallbackTest); +}; + +class GpioCommandControllerTest : public ::testing::Test +{ +public: + static void SetUpTestCase(); + static void TearDownTestCase(); + + void SetUp(); + void TearDown(); + + void SetUpController(); + void SetUpHandles(); + +protected: + std::unique_ptr controller_; + + // dummy gpio state values used for tests + const std::vector gpio_names_ = {"gpio1", "gpio2"}; + std::vector gpio_commands_ = {1.0, 0.0, 3.1}; + + CommandInterface gpio_1_1_dig_cmd_{gpio_names_[0], "dig.1", &gpio_commands_[0]}; + CommandInterface gpio_1_2_dig_cmd_{gpio_names_[0], "dig.2", &gpio_commands_[1]}; + CommandInterface gpio_2_ana_cmd_{gpio_names_[1], "ana.1", &gpio_commands_[2]}; +}; + +#endif // TEST_GPIO_COMMAND_CONTROLLER_HPP_ \ No newline at end of file diff --git a/gpio_controllers/test/test_load_gpio_command_controller.cpp b/gpio_controllers/test/test_load_gpio_command_controller.cpp new file mode 100644 index 0000000000..2b767ab234 --- /dev/null +++ b/gpio_controllers/test/test_load_gpio_command_controller.cpp @@ -0,0 +1,39 @@ +// Copyright 2022 ICUBE Laboratory, University of Strasbourg +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include +#include + +#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" + +TEST(TestLoadGpioCommandController, load_controller) +{ + rclcpp::init(0, nullptr); + + std::shared_ptr executor = + std::make_shared(); + + controller_manager::ControllerManager cm( + std::make_unique( + ros2_control_test_assets::minimal_robot_urdf), + executor, "test_controller_manager"); + + ASSERT_NO_THROW(cm.load_controller( + "test_gpio_command_controller", "gpio_controllers/GpioCommandController")); + + rclcpp::shutdown(); +} \ No newline at end of file From 6ffda9d1f95747108c5979f898a284b3e1f94cab Mon Sep 17 00:00:00 2001 From: "m.bednarczyk" Date: Wed, 2 Nov 2022 15:30:19 +0100 Subject: [PATCH 02/58] added doc to gpio controllers --- gpio_controllers/doc/userdoc.rst | 39 ++++++++++++++++++++++++++++++++ 1 file changed, 39 insertions(+) create mode 100644 gpio_controllers/doc/userdoc.rst diff --git a/gpio_controllers/doc/userdoc.rst b/gpio_controllers/doc/userdoc.rst new file mode 100644 index 0000000000..e8972f4479 --- /dev/null +++ b/gpio_controllers/doc/userdoc.rst @@ -0,0 +1,39 @@ +.. _gpio_controllers_userdoc: + +gpio_controllers +===================== + +This is a collection of controllers for gpios that work with multiple interfaces. + +Hardware interface type +----------------------- + +These controllers work with gpios using user defined command interfaces. + +Using GPIO Command Controller +----------------------------- +The controller expects at least one gpio interface abd the coresponding command interface names. +A yaml file for using it could be: + .. code-block:: yaml + + controller_manager: + ros__parameters: + update_rate: 100 # Hz + joint_state_broadcaster: + type: joint_state_broadcaster/JointStateBroadcaster + gpio_command_controller: + type: gpio_controllers/GpioCommandController + + gpio_command_controller: + ros__parameters: + gpios: + - Gpio1 + - Gpio2 + command_interfaces: + Gpio1: + - dig.1 + - dig.2 + - dig.3 + Gpio2: + - ana.1 + - ana.2 From f88fc7d37e85fbfe7e53581d33359625783871d2 Mon Sep 17 00:00:00 2001 From: "m.bednarczyk" Date: Wed, 2 Nov 2022 15:30:19 +0100 Subject: [PATCH 03/58] changed parameter interface_names to command_interfaces --- .../src/gpio_command_controller.cpp | 6 ++--- .../test/test_gpio_command_controller.cpp | 27 +++++++++---------- 2 files changed, 15 insertions(+), 18 deletions(-) diff --git a/gpio_controllers/src/gpio_command_controller.cpp b/gpio_controllers/src/gpio_command_controller.cpp index 59c4cd23b5..d4062395a7 100644 --- a/gpio_controllers/src/gpio_command_controller.cpp +++ b/gpio_controllers/src/gpio_command_controller.cpp @@ -44,7 +44,7 @@ CallbackReturn GpioCommandController::on_init() auto_declare>("gpios", std::vector()); gpio_names_ = node_->get_parameter("gpios").as_string_array(); for(std::string &gpio : gpio_names_) - auto_declare>("interface_names." + gpio, std::vector()); + auto_declare>("command_interfaces." + gpio, std::vector()); } catch (const std::exception & e) { @@ -66,9 +66,9 @@ CallbackReturn GpioCommandController::on_configure( } for(std::string &gpio : gpio_names_){ - auto interfaces = node_->get_parameter("interface_names." + gpio).as_string_array(); + auto interfaces = node_->get_parameter("command_interfaces." + gpio).as_string_array(); if (interfaces.empty()){ - RCLCPP_ERROR(get_node()->get_logger(), "'interface_names.%s' parameter was empty",gpio.c_str()); + RCLCPP_ERROR(get_node()->get_logger(), "'command_interfaces.%s' parameter was empty",gpio.c_str()); return CallbackReturn::ERROR; } if ( !interface_names_.insert( std::make_pair( gpio, interfaces) ).second ) { diff --git a/gpio_controllers/test/test_gpio_command_controller.cpp b/gpio_controllers/test/test_gpio_command_controller.cpp index 34d5ab101b..bb8c9da87f 100644 --- a/gpio_controllers/test/test_gpio_command_controller.cpp +++ b/gpio_controllers/test/test_gpio_command_controller.cpp @@ -87,9 +87,9 @@ TEST_F(GpioCommandControllerTest, ConfigureAndActivateParamsSuccess) SetUpController(); controller_->get_node()->set_parameter({"gpios", gpio_names_}); controller_->get_node()->set_parameter( - {"interface_names.gpio1", std::vector{"dig.1", "dig.2"}}); + {"command_interfaces.gpio1", std::vector{"dig.1", "dig.2"}}); controller_->get_node()->set_parameter( - {"interface_names.gpio2", std::vector{"ana.1"}}); + {"command_interfaces.gpio2", std::vector{"ana.1"}}); // configure successful ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), CallbackReturn::SUCCESS); @@ -100,13 +100,10 @@ TEST_F(GpioCommandControllerTest, ActivateWithWrongGpiosNamesFails) { SetUpController(); controller_->get_node()->set_parameter({"gpios", std::vector{"gpio1", "gpio4"}}); - // controller_->get_node()->set_parameter( - // {"interface_names", std::vector{"dig.1", "dig.2", "ana.1"}}); - controller_->get_node()->set_parameter( - {"interface_names.gpio1", std::vector{"dig.1", "dig.2"}}); + {"command_interfaces.gpio1", std::vector{"dig.1", "dig.2"}}); controller_->get_node()->set_parameter( - {"interface_names.gpio4", std::vector{"ana.1"}}); + {"command_interfaces.gpio4", std::vector{"ana.1"}}); // // activate failed, 'gpio4' is not a valid gpio name for the hardware ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), CallbackReturn::SUCCESS); ASSERT_EQ(controller_->on_activate(rclcpp_lifecycle::State()), CallbackReturn::ERROR); @@ -117,9 +114,9 @@ TEST_F(GpioCommandControllerTest, CommandSuccessTest) SetUpController(); controller_->get_node()->set_parameter({"gpios", gpio_names_}); controller_->get_node()->set_parameter( - {"interface_names.gpio1", std::vector{"dig.1", "dig.2"}}); + {"command_interfaces.gpio1", std::vector{"dig.1", "dig.2"}}); controller_->get_node()->set_parameter( - {"interface_names.gpio2", std::vector{"ana.1"}}); + {"command_interfaces.gpio2", std::vector{"ana.1"}}); ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), CallbackReturn::SUCCESS); @@ -155,9 +152,9 @@ TEST_F(GpioCommandControllerTest, WrongCommandCheckTest) SetUpController(); controller_->get_node()->set_parameter({"gpios", gpio_names_}); controller_->get_node()->set_parameter( - {"interface_names.gpio1", std::vector{"dig.1", "dig.2"}}); + {"command_interfaces.gpio1", std::vector{"dig.1", "dig.2"}}); controller_->get_node()->set_parameter( - {"interface_names.gpio2", std::vector{"ana.1"}}); + {"command_interfaces.gpio2", std::vector{"ana.1"}}); ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), CallbackReturn::SUCCESS); @@ -183,9 +180,9 @@ TEST_F(GpioCommandControllerTest, NoCommandCheckTest) SetUpController(); controller_->get_node()->set_parameter({"gpios", gpio_names_}); controller_->get_node()->set_parameter( - {"interface_names.gpio1", std::vector{"dig.1", "dig.2"}}); + {"command_interfaces.gpio1", std::vector{"dig.1", "dig.2"}}); controller_->get_node()->set_parameter( - {"interface_names.gpio2", std::vector{"ana.1"}}); + {"command_interfaces.gpio2", std::vector{"ana.1"}}); ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), CallbackReturn::SUCCESS); @@ -206,9 +203,9 @@ TEST_F(GpioCommandControllerTest, CommandCallbackTest) SetUpController(); controller_->get_node()->set_parameter({"gpios", gpio_names_}); controller_->get_node()->set_parameter( - {"interface_names.gpio1", std::vector{"dig.1", "dig.2"}}); + {"command_interfaces.gpio1", std::vector{"dig.1", "dig.2"}}); controller_->get_node()->set_parameter( - {"interface_names.gpio2", std::vector{"ana.1"}}); + {"command_interfaces.gpio2", std::vector{"ana.1"}}); // default values From 56fc1e5d97221984bb9f4982bd5c31009645e6b7 Mon Sep 17 00:00:00 2001 From: "m.bednarczyk" Date: Wed, 2 Nov 2022 15:30:19 +0100 Subject: [PATCH 04/58] added test --- .../test/test_gpio_command_controller.cpp | 12 ++++++++++++ 1 file changed, 12 insertions(+) diff --git a/gpio_controllers/test/test_gpio_command_controller.cpp b/gpio_controllers/test/test_gpio_command_controller.cpp index bb8c9da87f..24260e43c0 100644 --- a/gpio_controllers/test/test_gpio_command_controller.cpp +++ b/gpio_controllers/test/test_gpio_command_controller.cpp @@ -82,6 +82,18 @@ TEST_F(GpioCommandControllerTest, GpiosParameterIsEmpty) ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), CallbackReturn::ERROR); } +TEST_F(GpioCommandControllerTest, GpioWithMissingGpioParams) +{ + SetUpController(); + controller_->get_node()->set_parameter({"gpios", std::vector{"gpio1", "gpio2"}}); + controller_->get_node()->set_parameter( + {"command_interfaces.gpio1", std::vector{"dig.1", "dig.2"}}); + controller_->get_node()->set_parameter( + {"command_interfaces.gpio2", std::vector()}); + // // activate failed, command interface for 'gpio2' is not set up + ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), CallbackReturn::ERROR); +} + TEST_F(GpioCommandControllerTest, ConfigureAndActivateParamsSuccess) { SetUpController(); From 9e73ab797d09a61a28f6d40b2abfc6fe30c667e7 Mon Sep 17 00:00:00 2001 From: "m.bednarczyk" Date: Wed, 2 Nov 2022 15:30:19 +0100 Subject: [PATCH 05/58] code formating --- gpio_controllers/CMakeLists.txt | 4 +- gpio_controllers/doc/userdoc.rst | 50 +++++++++---------- gpio_controllers/gpio_controllers_plugin.xml | 2 +- .../gpio_command_controller.hpp | 9 ++-- .../gpio_controllers/visibility_control.h | 20 ++++++-- .../src/gpio_command_controller.cpp | 25 ++++++---- .../test/test_gpio_command_controller.cpp | 2 +- .../test/test_gpio_command_controller.hpp | 2 +- .../test_load_gpio_command_controller.cpp | 2 +- 9 files changed, 68 insertions(+), 48 deletions(-) diff --git a/gpio_controllers/CMakeLists.txt b/gpio_controllers/CMakeLists.txt index 3c1ec6b927..a36e86f2a2 100644 --- a/gpio_controllers/CMakeLists.txt +++ b/gpio_controllers/CMakeLists.txt @@ -63,7 +63,7 @@ if(BUILD_TESTING) test_load_gpio_command_controller test/test_load_gpio_command_controller.cpp ) - + target_include_directories(test_load_gpio_command_controller PRIVATE include) ament_target_dependencies(test_load_gpio_command_controller controller_manager @@ -103,4 +103,4 @@ ament_export_include_directories( ament_export_libraries( gpio_controllers ) -ament_package() \ No newline at end of file +ament_package() diff --git a/gpio_controllers/doc/userdoc.rst b/gpio_controllers/doc/userdoc.rst index e8972f4479..bccf15c2cb 100644 --- a/gpio_controllers/doc/userdoc.rst +++ b/gpio_controllers/doc/userdoc.rst @@ -3,7 +3,7 @@ gpio_controllers ===================== -This is a collection of controllers for gpios that work with multiple interfaces. +This is a collection of controllers for gpios that work with multiple interfaces. Hardware interface type ----------------------- @@ -12,28 +12,28 @@ These controllers work with gpios using user defined command interfaces. Using GPIO Command Controller ----------------------------- -The controller expects at least one gpio interface abd the coresponding command interface names. -A yaml file for using it could be: - .. code-block:: yaml - - controller_manager: - ros__parameters: - update_rate: 100 # Hz - joint_state_broadcaster: - type: joint_state_broadcaster/JointStateBroadcaster - gpio_command_controller: - type: gpio_controllers/GpioCommandController +The controller expects at least one gpio interface abd the corresponding command interface names. +A yaml file for using it could be: +.. code-block:: yaml - gpio_command_controller: - ros__parameters: - gpios: - - Gpio1 - - Gpio2 - command_interfaces: - Gpio1: - - dig.1 - - dig.2 - - dig.3 - Gpio2: - - ana.1 - - ana.2 + controller_manager: + ros__parameters: + update_rate: 100 # Hz + joint_state_broadcaster: + type: joint_state_broadcaster/JointStateBroadcaster + gpio_command_controller: + type: gpio_controllers/GpioCommandController + + gpio_command_controller: + ros__parameters: + gpios: + - Gpio1 + - Gpio2 + command_interfaces: + Gpio1: + - dig.1 + - dig.2 + - dig.3 + Gpio2: + - ana.1 + - ana.2 diff --git a/gpio_controllers/gpio_controllers_plugin.xml b/gpio_controllers/gpio_controllers_plugin.xml index ef7d59c9b5..03fd3e1ee0 100644 --- a/gpio_controllers/gpio_controllers_plugin.xml +++ b/gpio_controllers/gpio_controllers_plugin.xml @@ -4,4 +4,4 @@ The gpio command controller commands a group of gpios using multiple interfaces. - \ No newline at end of file + diff --git a/gpio_controllers/include/gpio_controllers/gpio_command_controller.hpp b/gpio_controllers/include/gpio_controllers/gpio_command_controller.hpp index 9ffd9adc73..f68c0e44ea 100644 --- a/gpio_controllers/include/gpio_controllers/gpio_command_controller.hpp +++ b/gpio_controllers/include/gpio_controllers/gpio_command_controller.hpp @@ -12,12 +12,13 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef GPIO_COMMAND_CONTROLLER__GPIO_COMMAND_CONTROLLER_HPP_ -#define GPIO_COMMAND_CONTROLLER__GPIO_COMMAND_CONTROLLER_HPP_ +#ifndef GPIO_CONTROLLERS__GPIO_COMMAND_CONTROLLER_HPP_ +#define GPIO_CONTROLLERS__GPIO_COMMAND_CONTROLLER_HPP_ #include #include #include +#include #include "controller_interface/controller_interface.hpp" #include "gpio_controllers/visibility_control.h" @@ -62,7 +63,7 @@ class GpioCommandController : public controller_interface::ControllerInterface protected: std::vector gpio_names_; - std::unordered_map> interface_names_; + std::unordered_map> interface_names_; std::vector command_interface_types_; realtime_tools::RealtimeBuffer> rt_command_ptr_; @@ -73,4 +74,4 @@ class GpioCommandController : public controller_interface::ControllerInterface } // namespace gpio_controllers -#endif // GPIO_COMMAND_CONTROLLER__GPIO_COMMAND_CONTROLLER_HPP_ +#endif // GPIO_CONTROLLERS__GPIO_COMMAND_CONTROLLER_HPP_ diff --git a/gpio_controllers/include/gpio_controllers/visibility_control.h b/gpio_controllers/include/gpio_controllers/visibility_control.h index 290da66e0d..22f960b91d 100644 --- a/gpio_controllers/include/gpio_controllers/visibility_control.h +++ b/gpio_controllers/include/gpio_controllers/visibility_control.h @@ -1,5 +1,19 @@ -#ifndef GPIO_COMMAND_CONTROLLER__VISIBILITY_CONTROL_H_ -#define GPIO_COMMAND_CONTROLLER__VISIBILITY_CONTROL_H_ +// Copyright 2022 ICUBE Laboratory, University of Strasbourg +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef GPIO_CONTROLLERS__VISIBILITY_CONTROL_H_ +#define GPIO_CONTROLLERS__VISIBILITY_CONTROL_H_ // This logic was borrowed (then namespaced) from the examples on the gcc wiki: // https://gcc.gnu.org/wiki/Visibility @@ -32,4 +46,4 @@ #define GPIO_COMMAND_CONTROLLER_PUBLIC_TYPE #endif -#endif // GPIO_COMMAND_CONTROLLER__VISIBILITY_CONTROL_H_ +#endif // GPIO_CONTROLLERS__VISIBILITY_CONTROL_H_ diff --git a/gpio_controllers/src/gpio_command_controller.cpp b/gpio_controllers/src/gpio_command_controller.cpp index d4062395a7..950d902256 100644 --- a/gpio_controllers/src/gpio_command_controller.cpp +++ b/gpio_controllers/src/gpio_command_controller.cpp @@ -43,8 +43,9 @@ CallbackReturn GpioCommandController::on_init() { auto_declare>("gpios", std::vector()); gpio_names_ = node_->get_parameter("gpios").as_string_array(); - for(std::string &gpio : gpio_names_) - auto_declare>("command_interfaces." + gpio, std::vector()); + for(std::string &gpio : gpio_names_) + auto_declare>("command_interfaces." + + gpio, std::vector()); } catch (const std::exception & e) { @@ -68,19 +69,21 @@ CallbackReturn GpioCommandController::on_configure( for(std::string &gpio : gpio_names_){ auto interfaces = node_->get_parameter("command_interfaces." + gpio).as_string_array(); if (interfaces.empty()){ - RCLCPP_ERROR(get_node()->get_logger(), "'command_interfaces.%s' parameter was empty",gpio.c_str()); + RCLCPP_ERROR(get_node()->get_logger(), + "'command_interfaces.%s' parameter was empty", gpio.c_str()); return CallbackReturn::ERROR; } if ( !interface_names_.insert( std::make_pair( gpio, interfaces) ).second ) { - RCLCPP_ERROR(get_node()->get_logger(), "Trying to override existing gpio setup. Wrong controller parameters."); + RCLCPP_ERROR(get_node()->get_logger(), + "Trying to override existing gpio setup. Wrong controller parameters."); return CallbackReturn::ERROR; } } - for (const auto & gpio : gpio_names_){ - for (const auto & interface_name: interface_names_[gpio]){ - command_interface_types_.push_back(gpio + "/" + interface_name); - } + for(const auto & gpio : gpio_names_){ + for(const auto & interface_name: interface_names_[gpio]){ + command_interface_types_.push_back(gpio + "/" + interface_name); + } } gpios_command_subscriber_ = get_node()->create_subscription( @@ -118,8 +121,10 @@ CallbackReturn GpioCommandController::on_activate( command_interfaces_.size() != ordered_interfaces.size()) { RCLCPP_ERROR( - node_->get_logger(), "Expected %zu command interfaces, got %zu", command_interface_types_.size(), + node_->get_logger(), + "Expected %zu command interfaces, got %zu", command_interface_types_.size(), ordered_interfaces.size()); + for(const auto & interface: command_interface_types_) RCLCPP_ERROR(node_->get_logger(), "Got %s", interface.c_str()); return CallbackReturn::ERROR; @@ -167,7 +172,7 @@ controller_interface::return_type GpioCommandController::update( return controller_interface::return_type::OK; } -} // namespace gpio_command_controller +} // namespace gpio_controllers #include "pluginlib/class_list_macros.hpp" diff --git a/gpio_controllers/test/test_gpio_command_controller.cpp b/gpio_controllers/test/test_gpio_command_controller.cpp index 24260e43c0..826c45f99e 100644 --- a/gpio_controllers/test/test_gpio_command_controller.cpp +++ b/gpio_controllers/test/test_gpio_command_controller.cpp @@ -254,4 +254,4 @@ TEST_F(GpioCommandControllerTest, CommandCallbackTest) ASSERT_EQ(gpio_1_1_dig_cmd_.get_value(), 0.0); ASSERT_EQ(gpio_1_2_dig_cmd_.get_value(), 1.0); ASSERT_EQ(gpio_2_ana_cmd_.get_value(), 30.0); -} \ No newline at end of file +} diff --git a/gpio_controllers/test/test_gpio_command_controller.hpp b/gpio_controllers/test/test_gpio_command_controller.hpp index d85d76e154..6627dee5b9 100644 --- a/gpio_controllers/test/test_gpio_command_controller.hpp +++ b/gpio_controllers/test/test_gpio_command_controller.hpp @@ -59,4 +59,4 @@ class GpioCommandControllerTest : public ::testing::Test CommandInterface gpio_2_ana_cmd_{gpio_names_[1], "ana.1", &gpio_commands_[2]}; }; -#endif // TEST_GPIO_COMMAND_CONTROLLER_HPP_ \ No newline at end of file +#endif // TEST_GPIO_COMMAND_CONTROLLER_HPP_ diff --git a/gpio_controllers/test/test_load_gpio_command_controller.cpp b/gpio_controllers/test/test_load_gpio_command_controller.cpp index 2b767ab234..81343db526 100644 --- a/gpio_controllers/test/test_load_gpio_command_controller.cpp +++ b/gpio_controllers/test/test_load_gpio_command_controller.cpp @@ -36,4 +36,4 @@ TEST(TestLoadGpioCommandController, load_controller) "test_gpio_command_controller", "gpio_controllers/GpioCommandController")); rclcpp::shutdown(); -} \ No newline at end of file +} From 1cbd3a6808805a91f143f4b55227e4c192b46bba Mon Sep 17 00:00:00 2001 From: Maciej Bednarczyk Date: Wed, 2 Nov 2022 15:30:19 +0100 Subject: [PATCH 06/58] added gpio state publisher --- .../gpio_command_controller.hpp | 8 +- .../src/gpio_command_controller.cpp | 80 ++++++++++++++----- 2 files changed, 66 insertions(+), 22 deletions(-) diff --git a/gpio_controllers/include/gpio_controllers/gpio_command_controller.hpp b/gpio_controllers/include/gpio_controllers/gpio_command_controller.hpp index f68c0e44ea..7f78c6f32e 100644 --- a/gpio_controllers/include/gpio_controllers/gpio_command_controller.hpp +++ b/gpio_controllers/include/gpio_controllers/gpio_command_controller.hpp @@ -26,11 +26,14 @@ #include "rclcpp_lifecycle/node_interfaces/lifecycle_node_interface.hpp" #include "rclcpp_lifecycle/state.hpp" #include "realtime_tools/realtime_buffer.h" +#include "realtime_tools/realtime_publisher.h" #include "std_msgs/msg/float64_multi_array.hpp" +#include "control_msgs/msg/interface_value.hpp" namespace gpio_controllers { using CmdType = std_msgs::msg::Float64MultiArray; +using StateType = control_msgs::msg::InterfaceValue; using CallbackReturn = rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn; class GpioCommandController : public controller_interface::ControllerInterface @@ -64,11 +67,14 @@ class GpioCommandController : public controller_interface::ControllerInterface protected: std::vector gpio_names_; std::unordered_map> interface_names_; - std::vector command_interface_types_; + std::vector interface_types_; realtime_tools::RealtimeBuffer> rt_command_ptr_; rclcpp::Subscription::SharedPtr gpios_command_subscriber_; + std::shared_ptr> gpio_state_publisher_; + std::shared_ptr> realtime_gpio_state_publisher_; + std::string logger_name_; }; diff --git a/gpio_controllers/src/gpio_command_controller.cpp b/gpio_controllers/src/gpio_command_controller.cpp index 950d902256..40ce7e5a71 100644 --- a/gpio_controllers/src/gpio_command_controller.cpp +++ b/gpio_controllers/src/gpio_command_controller.cpp @@ -42,7 +42,7 @@ CallbackReturn GpioCommandController::on_init() try { auto_declare>("gpios", std::vector()); - gpio_names_ = node_->get_parameter("gpios").as_string_array(); + gpio_names_ = get_node()->get_parameter("gpios").as_string_array(); for(std::string &gpio : gpio_names_) auto_declare>("command_interfaces." + gpio, std::vector()); @@ -59,7 +59,7 @@ CallbackReturn GpioCommandController::on_init() CallbackReturn GpioCommandController::on_configure( const rclcpp_lifecycle::State & /*previous_state*/) { - gpio_names_ = node_->get_parameter("gpios").as_string_array(); + gpio_names_ = get_node()->get_parameter("gpios").as_string_array(); if (gpio_names_.empty()){ RCLCPP_ERROR(get_node()->get_logger(), "'gpios' parameter was empty"); @@ -67,7 +67,7 @@ CallbackReturn GpioCommandController::on_configure( } for(std::string &gpio : gpio_names_){ - auto interfaces = node_->get_parameter("command_interfaces." + gpio).as_string_array(); + auto interfaces = get_node()->get_parameter("command_interfaces." + gpio).as_string_array(); if (interfaces.empty()){ RCLCPP_ERROR(get_node()->get_logger(), "'command_interfaces.%s' parameter was empty", gpio.c_str()); @@ -82,13 +82,30 @@ CallbackReturn GpioCommandController::on_configure( for(const auto & gpio : gpio_names_){ for(const auto & interface_name: interface_names_[gpio]){ - command_interface_types_.push_back(gpio + "/" + interface_name); + interface_types_.push_back(gpio + "/" + interface_name); } } - gpios_command_subscriber_ = get_node()->create_subscription( - "~/commands", rclcpp::SystemDefaultsQoS(), - [this](const CmdType::SharedPtr msg) { rt_command_ptr_.writeFromNonRT(msg); }); + try + { + gpios_command_subscriber_ = get_node()->create_subscription( + "~/commands", rclcpp::SystemDefaultsQoS(), + [this](const CmdType::SharedPtr msg) { rt_command_ptr_.writeFromNonRT(msg); }); + + gpio_state_publisher_ = + get_node()->create_publisher( + "~/gpio_states", rclcpp::SystemDefaultsQoS()); + + realtime_gpio_state_publisher_ = + std::make_shared>( + gpio_state_publisher_); + } + catch (const std::exception & e) + { + // get_node() may throw, logging raw here + fprintf(stderr, "Exception thrown during init stage with message: %s \n", e.what()); + return CallbackReturn::ERROR; + } RCLCPP_INFO(get_node()->get_logger(), "configure successful"); return CallbackReturn::SUCCESS; @@ -97,18 +114,21 @@ CallbackReturn GpioCommandController::on_configure( controller_interface::InterfaceConfiguration GpioCommandController::command_interface_configuration() const { - controller_interface::InterfaceConfiguration command_interfaces_config; - command_interfaces_config.type = controller_interface::interface_configuration_type::INDIVIDUAL; - command_interfaces_config.names = command_interface_types_; + controller_interface::InterfaceConfiguration command_interfaces_config; + command_interfaces_config.type = controller_interface::interface_configuration_type::INDIVIDUAL; + command_interfaces_config.names = interface_types_; - return command_interfaces_config; + return command_interfaces_config; } controller_interface::InterfaceConfiguration GpioCommandController::state_interface_configuration() const { - return controller_interface::InterfaceConfiguration{ - controller_interface::interface_configuration_type::NONE}; + controller_interface::InterfaceConfiguration state_interfaces_config; + state_interfaces_config.type = controller_interface::interface_configuration_type::INDIVIDUAL; + state_interfaces_config.names = interface_types_; + + return state_interfaces_config; } CallbackReturn GpioCommandController::on_activate( @@ -117,19 +137,26 @@ CallbackReturn GpioCommandController::on_activate( std::vector> ordered_interfaces; if ( !controller_interface::get_ordered_interfaces( - command_interfaces_, command_interface_types_, std::string(""), ordered_interfaces) || + command_interfaces_, interface_types_, std::string(""), ordered_interfaces) || command_interfaces_.size() != ordered_interfaces.size()) { RCLCPP_ERROR( - node_->get_logger(), - "Expected %zu command interfaces, got %zu", command_interface_types_.size(), + get_node()->get_logger(), + "Expected %zu command interfaces, got %zu", interface_types_.size(), ordered_interfaces.size()); - for(const auto & interface: command_interface_types_) - RCLCPP_ERROR(node_->get_logger(), "Got %s", interface.c_str()); + for(const auto & interface: interface_types_) + RCLCPP_ERROR(get_node()->get_logger(), "Got %s", interface.c_str()); return CallbackReturn::ERROR; } + //initialize gpio state msg + auto & gpio_state_msg = realtime_gpio_state_publisher_->msg_; + for(auto i = 0ul; i::quiet_NaN()); + } + // reset command buffer if a command came through callback when controller was inactive rt_command_ptr_.reset(); @@ -147,6 +174,17 @@ CallbackReturn GpioCommandController::on_deactivate( controller_interface::return_type GpioCommandController::update( const rclcpp::Time & /*time*/, const rclcpp::Duration & /*period*/) { + //publish gpio state msg + if (realtime_gpio_state_publisher_ && realtime_gpio_state_publisher_->trylock()) + { + auto & gpio_state_msg = realtime_gpio_state_publisher_->msg_; + + for (size_t iindex = 0; iindex < state_interfaces_.size(); ++iindex){ + gpio_state_msg.values[iindex] = state_interfaces_[iindex].get_value(); + } + realtime_gpio_state_publisher_->unlockAndPublish(); + } + auto gpio_commands = rt_command_ptr_.readFromRT(); // no command received yet @@ -158,15 +196,15 @@ controller_interface::return_type GpioCommandController::update( if ((*gpio_commands)->data.size() != command_interfaces_.size()) { RCLCPP_ERROR_THROTTLE( - get_node()->get_logger(), *node_->get_clock(), 1000, + get_node()->get_logger(), *get_node()->get_clock(), 1000, "command size (%zu) does not match number of interfaces (%zu)", (*gpio_commands)->data.size(), command_interfaces_.size()); return controller_interface::return_type::ERROR; } - for (size_t index = 0; index < command_interfaces_.size(); ++index) + for (size_t cindex = 0; cindex < command_interfaces_.size(); ++cindex) { - command_interfaces_[index].set_value((*gpio_commands)->data[index]); + command_interfaces_[cindex].set_value((*gpio_commands)->data[cindex]); } return controller_interface::return_type::OK; From 7228e0693e7c1974d43dc9a042bd5d7a27e99b88 Mon Sep 17 00:00:00 2001 From: Maciej Bednarczyk Date: Wed, 2 Nov 2022 15:30:19 +0100 Subject: [PATCH 07/58] fixed broken node_state --- gpio_controllers/test/test_gpio_command_controller.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/gpio_controllers/test/test_gpio_command_controller.cpp b/gpio_controllers/test/test_gpio_command_controller.cpp index 826c45f99e..c6a7340a32 100644 --- a/gpio_controllers/test/test_gpio_command_controller.cpp +++ b/gpio_controllers/test/test_gpio_command_controller.cpp @@ -228,7 +228,7 @@ TEST_F(GpioCommandControllerTest, CommandCallbackTest) auto node_state = controller_->configure(); ASSERT_EQ(node_state.id(), lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE); - node_state = controller_->activate(); + node_state = controller_->get_node()->activate(); ASSERT_EQ(node_state.id(), lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE); // send a new command From 880aeea91ba16a7f9c58ba11c4b1923595ca005d Mon Sep 17 00:00:00 2001 From: Maciej Bednarczyk Date: Wed, 2 Nov 2022 15:30:19 +0100 Subject: [PATCH 08/58] added DynamicJointState message for Gpio state --- .../gpio_command_controller.hpp | 6 ++-- .../src/gpio_command_controller.cpp | 30 ++++++++++++++----- 2 files changed, 25 insertions(+), 11 deletions(-) diff --git a/gpio_controllers/include/gpio_controllers/gpio_command_controller.hpp b/gpio_controllers/include/gpio_controllers/gpio_command_controller.hpp index 7f78c6f32e..05b9a05f9b 100644 --- a/gpio_controllers/include/gpio_controllers/gpio_command_controller.hpp +++ b/gpio_controllers/include/gpio_controllers/gpio_command_controller.hpp @@ -28,13 +28,13 @@ #include "realtime_tools/realtime_buffer.h" #include "realtime_tools/realtime_publisher.h" #include "std_msgs/msg/float64_multi_array.hpp" -#include "control_msgs/msg/interface_value.hpp" +#include "control_msgs/msg/dynamic_joint_state.hpp" namespace gpio_controllers { using CmdType = std_msgs::msg::Float64MultiArray; -using StateType = control_msgs::msg::InterfaceValue; -using CallbackReturn = rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn; +using StateType = control_msgs::msg::DynamicJointState; +using CallbackReturn = controller_interface::CallbackReturn; class GpioCommandController : public controller_interface::ControllerInterface { diff --git a/gpio_controllers/src/gpio_command_controller.cpp b/gpio_controllers/src/gpio_command_controller.cpp index 40ce7e5a71..99cf9c4ef7 100644 --- a/gpio_controllers/src/gpio_command_controller.cpp +++ b/gpio_controllers/src/gpio_command_controller.cpp @@ -150,16 +150,23 @@ CallbackReturn GpioCommandController::on_activate( return CallbackReturn::ERROR; } - //initialize gpio state msg + // initialize gpio state msg auto & gpio_state_msg = realtime_gpio_state_publisher_->msg_; - for(auto i = 0ul; i::quiet_NaN()); + gpio_state_msg.header.stamp = get_node()->now(); + gpio_state_msg.joint_names.resize(gpio_names_.size()); + gpio_state_msg.interface_values.resize(gpio_names_.size()); + for(auto g = 0ul; g < gpio_names_.size(); g++){ + gpio_state_msg.joint_names[g] = gpio_names_[g]; + for(const auto & interface_name: interface_names_[gpio_names_[g]]){ + gpio_state_msg.interface_values[g].interface_names.push_back(interface_name); + gpio_state_msg.interface_values[g].values.push_back(std::numeric_limits::quiet_NaN()); + } } // reset command buffer if a command came through callback when controller was inactive rt_command_ptr_.reset(); + RCLCPP_INFO(get_node()->get_logger(), "activate successful"); return CallbackReturn::SUCCESS; } @@ -179,12 +186,19 @@ controller_interface::return_type GpioCommandController::update( { auto & gpio_state_msg = realtime_gpio_state_publisher_->msg_; - for (size_t iindex = 0; iindex < state_interfaces_.size(); ++iindex){ - gpio_state_msg.values[iindex] = state_interfaces_[iindex].get_value(); + gpio_state_msg.header.stamp = get_node()->now(); + + auto sindex = 0ul; + for(auto g = 0ul; g < gpio_names_.size(); g++){ + for(auto i = 0ul; i < interface_names_[gpio_names_[g]].size(); i++){ + gpio_state_msg.interface_values[g].values[i] = state_interfaces_[sindex].get_value(); + sindex ++; + } } + realtime_gpio_state_publisher_->unlockAndPublish(); - } - + } + auto gpio_commands = rt_command_ptr_.readFromRT(); // no command received yet From 94befb055ca50ade74b19f37eb7fd496843fb307 Mon Sep 17 00:00:00 2001 From: Maciej Bednarczyk Date: Wed, 2 Nov 2022 15:30:19 +0100 Subject: [PATCH 09/58] fixed tests and added gpio state tests --- .../test/test_gpio_command_controller.cpp | 96 ++++++++++++++++++- .../test/test_gpio_command_controller.hpp | 6 ++ 2 files changed, 98 insertions(+), 4 deletions(-) diff --git a/gpio_controllers/test/test_gpio_command_controller.cpp b/gpio_controllers/test/test_gpio_command_controller.cpp index c6a7340a32..6e97c404bd 100644 --- a/gpio_controllers/test/test_gpio_command_controller.cpp +++ b/gpio_controllers/test/test_gpio_command_controller.cpp @@ -27,9 +27,11 @@ #include "rclcpp_lifecycle/node_interfaces/lifecycle_node_interface.hpp" #include "test_gpio_command_controller.hpp" -using CallbackReturn = rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn; +using CallbackReturn = controller_interface::CallbackReturn; using hardware_interface::LoanedCommandInterface; +using hardware_interface::LoanedStateInterface; using CmdType = std_msgs::msg::Float64MultiArray; +using StateType = control_msgs::msg::DynamicJointState; namespace { @@ -62,7 +64,13 @@ void GpioCommandControllerTest::SetUpController() command_ifs.emplace_back(gpio_1_1_dig_cmd_); command_ifs.emplace_back(gpio_1_2_dig_cmd_); command_ifs.emplace_back(gpio_2_ana_cmd_); - controller_->assign_interfaces(std::move(command_ifs), {}); + + std::vector state_ifs; + state_ifs.emplace_back(gpio_1_1_dig_state_); + state_ifs.emplace_back(gpio_1_2_dig_state_); + state_ifs.emplace_back(gpio_2_ana_state_); + + controller_->assign_interfaces(std::move(command_ifs), std::move(state_ifs)); } TEST_F(GpioCommandControllerTest, GpiosParameterNotSet) @@ -132,6 +140,7 @@ TEST_F(GpioCommandControllerTest, CommandSuccessTest) ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), CallbackReturn::SUCCESS); + ASSERT_EQ(controller_->on_activate(rclcpp_lifecycle::State()), CallbackReturn::SUCCESS); // update successful though no command has been send yet ASSERT_EQ( @@ -170,6 +179,8 @@ TEST_F(GpioCommandControllerTest, WrongCommandCheckTest) ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), CallbackReturn::SUCCESS); + ASSERT_EQ(controller_->on_activate(rclcpp_lifecycle::State()), CallbackReturn::SUCCESS); + // send command with wrong number of gpios auto command_ptr = std::make_shared(); @@ -198,6 +209,7 @@ TEST_F(GpioCommandControllerTest, NoCommandCheckTest) ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), CallbackReturn::SUCCESS); + ASSERT_EQ(controller_->on_activate(rclcpp_lifecycle::State()), CallbackReturn::SUCCESS); // update successful, no command received yet ASSERT_EQ( @@ -218,6 +230,10 @@ TEST_F(GpioCommandControllerTest, CommandCallbackTest) {"command_interfaces.gpio1", std::vector{"dig.1", "dig.2"}}); controller_->get_node()->set_parameter( {"command_interfaces.gpio2", std::vector{"ana.1"}}); + controller_->get_node()->set_parameter( + {"state_interfaces.gpio1", std::vector{"dig.1", "dig.2"}}); + controller_->get_node()->set_parameter( + {"state_interfaces.gpio2", std::vector{"ana.1"}}); // default values @@ -225,6 +241,10 @@ TEST_F(GpioCommandControllerTest, CommandCallbackTest) ASSERT_EQ(gpio_1_2_dig_cmd_.get_value(), 0.0); ASSERT_EQ(gpio_2_ana_cmd_.get_value(), 3.1); + ASSERT_EQ(gpio_1_1_dig_state_.get_value(), 1.0); + ASSERT_EQ(gpio_1_2_dig_state_.get_value(), 0.0); + ASSERT_EQ(gpio_2_ana_state_.get_value(), 3.1); + auto node_state = controller_->configure(); ASSERT_EQ(node_state.id(), lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE); @@ -233,9 +253,9 @@ TEST_F(GpioCommandControllerTest, CommandCallbackTest) // send a new command rclcpp::Node test_node("test_node"); - auto command_pub = test_node.create_publisher( + auto command_pub = test_node.create_publisher( std::string(controller_->get_node()->get_name()) + "/commands", rclcpp::SystemDefaultsQoS()); - std_msgs::msg::Float64MultiArray command_msg; + CmdType command_msg; command_msg.data = {0.0, 1.0, 30.0}; command_pub->publish(command_msg); @@ -255,3 +275,71 @@ TEST_F(GpioCommandControllerTest, CommandCallbackTest) ASSERT_EQ(gpio_1_2_dig_cmd_.get_value(), 1.0); ASSERT_EQ(gpio_2_ana_cmd_.get_value(), 30.0); } + +TEST_F(GpioCommandControllerTest, StateCallbackTest) +{ + SetUpController(); + controller_->get_node()->set_parameter({"gpios", gpio_names_}); + controller_->get_node()->set_parameter( + {"command_interfaces.gpio1", std::vector{"dig.1", "dig.2"}}); + controller_->get_node()->set_parameter( + {"command_interfaces.gpio2", std::vector{"ana.1"}}); + controller_->get_node()->set_parameter( + {"state_interfaces.gpio1", std::vector{"dig.1", "dig.2"}}); + controller_->get_node()->set_parameter( + {"state_interfaces.gpio2", std::vector{"ana.1"}}); + + + // default values + ASSERT_EQ(gpio_1_1_dig_cmd_.get_value(), 1.0); + ASSERT_EQ(gpio_1_2_dig_cmd_.get_value(), 0.0); + ASSERT_EQ(gpio_2_ana_cmd_.get_value(), 3.1); + + ASSERT_EQ(gpio_1_1_dig_state_.get_value(), 1.0); + ASSERT_EQ(gpio_1_2_dig_state_.get_value(), 0.0); + ASSERT_EQ(gpio_2_ana_state_.get_value(), 3.1); + + auto node_state = controller_->configure(); + ASSERT_EQ(node_state.id(), lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE); + + node_state = controller_->get_node()->activate(); + ASSERT_EQ(node_state.id(), lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE); + + // send a new command + rclcpp::Node test_node("test_node"); + auto subs_callback = [&](const StateType::SharedPtr) {}; + auto subscription = test_node.create_subscription( + std::string(controller_->get_node()->get_name()) + "/gpio_states", 10, subs_callback); + + // call update to publish the test value + // since update doesn't guarantee a published message, republish until received + int max_sub_check_loop_count = 5; // max number of tries for pub/sub loop + rclcpp::WaitSet wait_set; // block used to wait on message + wait_set.add_subscription(subscription); + while (max_sub_check_loop_count--) + { + controller_->update(rclcpp::Time(0), rclcpp::Duration::from_seconds(0.01)); + // check if message has been received + if (wait_set.wait(std::chrono::milliseconds(2)).kind() == rclcpp::WaitResultKind::Ready) + { + break; + } + } + ASSERT_GE(max_sub_check_loop_count, 0) << "Test was unable to publish a message through " + "controller/broadcaster update loop"; + + // take message from subscription + StateType gpio_state_msg; + 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[0], "gpio1"); + ASSERT_EQ(gpio_state_msg.joint_names[1], "gpio2"); + ASSERT_EQ(gpio_state_msg.interface_values[0].interface_names[0], "dig.1"); + ASSERT_EQ(gpio_state_msg.interface_values[0].interface_names[1], "dig.2"); + ASSERT_EQ(gpio_state_msg.interface_values[1].interface_names[0], "ana.1"); + ASSERT_EQ(gpio_state_msg.interface_values[0].values[0], 1.0); + ASSERT_EQ(gpio_state_msg.interface_values[0].values[1], 0.0); + ASSERT_EQ(gpio_state_msg.interface_values[1].values[0], 3.1); +} diff --git a/gpio_controllers/test/test_gpio_command_controller.hpp b/gpio_controllers/test/test_gpio_command_controller.hpp index 6627dee5b9..9841ed4add 100644 --- a/gpio_controllers/test/test_gpio_command_controller.hpp +++ b/gpio_controllers/test/test_gpio_command_controller.hpp @@ -26,6 +26,7 @@ #include "gpio_controllers/gpio_command_controller.hpp" using hardware_interface::CommandInterface; +using hardware_interface::StateInterface; // subclassing and friending so we can access member variables class FriendGpioCommandController : public gpio_controllers::GpioCommandController @@ -53,10 +54,15 @@ class GpioCommandControllerTest : public ::testing::Test // dummy gpio state values used for tests const std::vector gpio_names_ = {"gpio1", "gpio2"}; std::vector gpio_commands_ = {1.0, 0.0, 3.1}; + std::vector gpio_states_ = {1.0, 0.0, 3.1}; CommandInterface gpio_1_1_dig_cmd_{gpio_names_[0], "dig.1", &gpio_commands_[0]}; CommandInterface gpio_1_2_dig_cmd_{gpio_names_[0], "dig.2", &gpio_commands_[1]}; CommandInterface gpio_2_ana_cmd_{gpio_names_[1], "ana.1", &gpio_commands_[2]}; + + StateInterface gpio_1_1_dig_state_{gpio_names_[0], "dig.1", &gpio_states_[0]}; + StateInterface gpio_1_2_dig_state_{gpio_names_[0], "dig.2", &gpio_states_[1]}; + StateInterface gpio_2_ana_state_{gpio_names_[1], "ana.1", &gpio_states_[2]}; }; #endif // TEST_GPIO_COMMAND_CONTROLLER_HPP_ From 882c93685343e6c567a3194476cb79865282350c Mon Sep 17 00:00:00 2001 From: Maciej Bednarczyk Date: Wed, 16 Nov 2022 15:06:01 +0100 Subject: [PATCH 10/58] fixed format --- gpio_controllers/doc/userdoc.rst | 4 +- .../gpio_command_controller.hpp | 4 +- .../gpio_controllers/visibility_control.h | 48 +++---- .../src/gpio_command_controller.cpp | 120 ++++++++++-------- .../test/test_gpio_command_controller.cpp | 11 +- .../test/test_gpio_command_controller.hpp | 2 +- .../test_load_gpio_command_controller.cpp | 4 +- 7 files changed, 97 insertions(+), 96 deletions(-) diff --git a/gpio_controllers/doc/userdoc.rst b/gpio_controllers/doc/userdoc.rst index bccf15c2cb..7bf8d8b75b 100644 --- a/gpio_controllers/doc/userdoc.rst +++ b/gpio_controllers/doc/userdoc.rst @@ -20,9 +20,9 @@ A yaml file for using it could be: ros__parameters: update_rate: 100 # Hz joint_state_broadcaster: - type: joint_state_broadcaster/JointStateBroadcaster + type: joint_state_broadcaster/JointStateBroadcaster gpio_command_controller: - type: gpio_controllers/GpioCommandController + type: gpio_controllers/GpioCommandController gpio_command_controller: ros__parameters: diff --git a/gpio_controllers/include/gpio_controllers/gpio_command_controller.hpp b/gpio_controllers/include/gpio_controllers/gpio_command_controller.hpp index 05b9a05f9b..aef84f0a1e 100644 --- a/gpio_controllers/include/gpio_controllers/gpio_command_controller.hpp +++ b/gpio_controllers/include/gpio_controllers/gpio_command_controller.hpp @@ -17,9 +17,10 @@ #include #include -#include #include +#include +#include "control_msgs/msg/dynamic_joint_state.hpp" #include "controller_interface/controller_interface.hpp" #include "gpio_controllers/visibility_control.h" #include "rclcpp/subscription.hpp" @@ -28,7 +29,6 @@ #include "realtime_tools/realtime_buffer.h" #include "realtime_tools/realtime_publisher.h" #include "std_msgs/msg/float64_multi_array.hpp" -#include "control_msgs/msg/dynamic_joint_state.hpp" namespace gpio_controllers { diff --git a/gpio_controllers/include/gpio_controllers/visibility_control.h b/gpio_controllers/include/gpio_controllers/visibility_control.h index 22f960b91d..a735a1621c 100644 --- a/gpio_controllers/include/gpio_controllers/visibility_control.h +++ b/gpio_controllers/include/gpio_controllers/visibility_control.h @@ -19,31 +19,31 @@ // https://gcc.gnu.org/wiki/Visibility #if defined _WIN32 || defined __CYGWIN__ - #ifdef __GNUC__ - #define GPIO_COMMAND_CONTROLLER_EXPORT __attribute__ ((dllexport)) - #define GPIO_COMMAND_CONTROLLER_IMPORT __attribute__ ((dllimport)) - #else - #define GPIO_COMMAND_CONTROLLER_EXPORT __declspec(dllexport) - #define GPIO_COMMAND_CONTROLLER_IMPORT __declspec(dllimport) - #endif - #ifdef GPIO_COMMAND_CONTROLLER_BUILDING_LIBRARY - #define GPIO_COMMAND_CONTROLLER_PUBLIC GPIO_COMMAND_CONTROLLER_EXPORT - #else - #define GPIO_COMMAND_CONTROLLER_PUBLIC GPIO_COMMAND_CONTROLLER_IMPORT - #endif - #define GPIO_COMMAND_CONTROLLER_PUBLIC_TYPE GPIO_COMMAND_CONTROLLER_PUBLIC - #define GPIO_COMMAND_CONTROLLER_LOCAL +#ifdef __GNUC__ +#define GPIO_COMMAND_CONTROLLER_EXPORT __attribute__((dllexport)) +#define GPIO_COMMAND_CONTROLLER_IMPORT __attribute__((dllimport)) #else - #define GPIO_COMMAND_CONTROLLER_EXPORT __attribute__ ((visibility("default"))) - #define GPIO_COMMAND_CONTROLLER_IMPORT - #if __GNUC__ >= 4 - #define GPIO_COMMAND_CONTROLLER_PUBLIC __attribute__ ((visibility("default"))) - #define GPIO_COMMAND_CONTROLLER_LOCAL __attribute__ ((visibility("hidden"))) - #else - #define GPIO_COMMAND_CONTROLLER_PUBLIC - #define GPIO_COMMAND_CONTROLLER_LOCAL - #endif - #define GPIO_COMMAND_CONTROLLER_PUBLIC_TYPE +#define GPIO_COMMAND_CONTROLLER_EXPORT __declspec(dllexport) +#define GPIO_COMMAND_CONTROLLER_IMPORT __declspec(dllimport) +#endif +#ifdef GPIO_COMMAND_CONTROLLER_BUILDING_LIBRARY +#define GPIO_COMMAND_CONTROLLER_PUBLIC GPIO_COMMAND_CONTROLLER_EXPORT +#else +#define GPIO_COMMAND_CONTROLLER_PUBLIC GPIO_COMMAND_CONTROLLER_IMPORT +#endif +#define GPIO_COMMAND_CONTROLLER_PUBLIC_TYPE GPIO_COMMAND_CONTROLLER_PUBLIC +#define GPIO_COMMAND_CONTROLLER_LOCAL +#else +#define GPIO_COMMAND_CONTROLLER_EXPORT __attribute__((visibility("default"))) +#define GPIO_COMMAND_CONTROLLER_IMPORT +#if __GNUC__ >= 4 +#define GPIO_COMMAND_CONTROLLER_PUBLIC __attribute__((visibility("default"))) +#define GPIO_COMMAND_CONTROLLER_LOCAL __attribute__((visibility("hidden"))) +#else +#define GPIO_COMMAND_CONTROLLER_PUBLIC +#define GPIO_COMMAND_CONTROLLER_LOCAL +#endif +#define GPIO_COMMAND_CONTROLLER_PUBLIC_TYPE #endif #endif // GPIO_CONTROLLERS__VISIBILITY_CONTROL_H_ diff --git a/gpio_controllers/src/gpio_command_controller.cpp b/gpio_controllers/src/gpio_command_controller.cpp index 99cf9c4ef7..75c93d8ea2 100644 --- a/gpio_controllers/src/gpio_command_controller.cpp +++ b/gpio_controllers/src/gpio_command_controller.cpp @@ -43,9 +43,9 @@ CallbackReturn GpioCommandController::on_init() { auto_declare>("gpios", std::vector()); gpio_names_ = get_node()->get_parameter("gpios").as_string_array(); - for(std::string &gpio : gpio_names_) - auto_declare>("command_interfaces." - + gpio, std::vector()); + for (std::string & gpio : gpio_names_) + auto_declare>( + "command_interfaces." + gpio, std::vector()); } catch (const std::exception & e) { @@ -59,56 +59,61 @@ CallbackReturn GpioCommandController::on_init() CallbackReturn GpioCommandController::on_configure( const rclcpp_lifecycle::State & /*previous_state*/) { - gpio_names_ = get_node()->get_parameter("gpios").as_string_array(); + gpio_names_ = get_node()->get_parameter("gpios").as_string_array(); - if (gpio_names_.empty()){ - RCLCPP_ERROR(get_node()->get_logger(), "'gpios' parameter was empty"); - return CallbackReturn::ERROR; - } + if (gpio_names_.empty()) + { + RCLCPP_ERROR(get_node()->get_logger(), "'gpios' parameter was empty"); + return CallbackReturn::ERROR; + } - for(std::string &gpio : gpio_names_){ - auto interfaces = get_node()->get_parameter("command_interfaces." + gpio).as_string_array(); - if (interfaces.empty()){ - RCLCPP_ERROR(get_node()->get_logger(), - "'command_interfaces.%s' parameter was empty", gpio.c_str()); - return CallbackReturn::ERROR; - } - if ( !interface_names_.insert( std::make_pair( gpio, interfaces) ).second ) { - RCLCPP_ERROR(get_node()->get_logger(), - "Trying to override existing gpio setup. Wrong controller parameters."); - return CallbackReturn::ERROR; - } + for (std::string & gpio : gpio_names_) + { + auto interfaces = get_node()->get_parameter("command_interfaces." + gpio).as_string_array(); + if (interfaces.empty()) + { + RCLCPP_ERROR( + get_node()->get_logger(), "'command_interfaces.%s' parameter was empty", gpio.c_str()); + return CallbackReturn::ERROR; } - - for(const auto & gpio : gpio_names_){ - for(const auto & interface_name: interface_names_[gpio]){ - interface_types_.push_back(gpio + "/" + interface_name); - } + if (!interface_names_.insert(std::make_pair(gpio, interfaces)).second) + { + RCLCPP_ERROR( + get_node()->get_logger(), + "Trying to override existing gpio setup. Wrong controller parameters."); + return CallbackReturn::ERROR; } + } - try + for (const auto & gpio : gpio_names_) + { + for (const auto & interface_name : interface_names_[gpio]) { - gpios_command_subscriber_ = get_node()->create_subscription( - "~/commands", rclcpp::SystemDefaultsQoS(), - [this](const CmdType::SharedPtr msg) { rt_command_ptr_.writeFromNonRT(msg); }); + interface_types_.push_back(gpio + "/" + interface_name); + } + } - gpio_state_publisher_ = - get_node()->create_publisher( - "~/gpio_states", rclcpp::SystemDefaultsQoS()); + try + { + gpios_command_subscriber_ = get_node()->create_subscription( + "~/commands", rclcpp::SystemDefaultsQoS(), + [this](const CmdType::SharedPtr msg) { rt_command_ptr_.writeFromNonRT(msg); }); - realtime_gpio_state_publisher_ = - std::make_shared>( - gpio_state_publisher_); - } - catch (const std::exception & e) - { - // get_node() may throw, logging raw here - fprintf(stderr, "Exception thrown during init stage with message: %s \n", e.what()); - return CallbackReturn::ERROR; - } + gpio_state_publisher_ = + get_node()->create_publisher("~/gpio_states", rclcpp::SystemDefaultsQoS()); + + realtime_gpio_state_publisher_ = + std::make_shared>(gpio_state_publisher_); + } + catch (const std::exception & e) + { + // get_node() may throw, logging raw here + fprintf(stderr, "Exception thrown during init stage with message: %s \n", e.what()); + return CallbackReturn::ERROR; + } - RCLCPP_INFO(get_node()->get_logger(), "configure successful"); - return CallbackReturn::SUCCESS; + RCLCPP_INFO(get_node()->get_logger(), "configure successful"); + return CallbackReturn::SUCCESS; } controller_interface::InterfaceConfiguration @@ -121,8 +126,8 @@ GpioCommandController::command_interface_configuration() const return command_interfaces_config; } -controller_interface::InterfaceConfiguration -GpioCommandController::state_interface_configuration() const +controller_interface::InterfaceConfiguration GpioCommandController::state_interface_configuration() + const { controller_interface::InterfaceConfiguration state_interfaces_config; state_interfaces_config.type = controller_interface::interface_configuration_type::INDIVIDUAL; @@ -141,11 +146,10 @@ CallbackReturn GpioCommandController::on_activate( command_interfaces_.size() != ordered_interfaces.size()) { RCLCPP_ERROR( - get_node()->get_logger(), - "Expected %zu command interfaces, got %zu", interface_types_.size(), + get_node()->get_logger(), "Expected %zu command interfaces, got %zu", interface_types_.size(), ordered_interfaces.size()); - for(const auto & interface: interface_types_) + for (const auto & interface : interface_types_) RCLCPP_ERROR(get_node()->get_logger(), "Got %s", interface.c_str()); return CallbackReturn::ERROR; } @@ -155,9 +159,11 @@ CallbackReturn GpioCommandController::on_activate( gpio_state_msg.header.stamp = get_node()->now(); gpio_state_msg.joint_names.resize(gpio_names_.size()); gpio_state_msg.interface_values.resize(gpio_names_.size()); - for(auto g = 0ul; g < gpio_names_.size(); g++){ + for (auto g = 0ul; g < gpio_names_.size(); g++) + { gpio_state_msg.joint_names[g] = gpio_names_[g]; - for(const auto & interface_name: interface_names_[gpio_names_[g]]){ + for (const auto & interface_name : interface_names_[gpio_names_[g]]) + { gpio_state_msg.interface_values[g].interface_names.push_back(interface_name); gpio_state_msg.interface_values[g].values.push_back(std::numeric_limits::quiet_NaN()); } @@ -189,10 +195,12 @@ controller_interface::return_type GpioCommandController::update( gpio_state_msg.header.stamp = get_node()->now(); auto sindex = 0ul; - for(auto g = 0ul; g < gpio_names_.size(); g++){ - for(auto i = 0ul; i < interface_names_[gpio_names_[g]].size(); i++){ + for (auto g = 0ul; g < gpio_names_.size(); g++) + { + for (auto i = 0ul; i < interface_names_[gpio_names_[g]].size(); i++) + { gpio_state_msg.interface_values[g].values[i] = state_interfaces_[sindex].get_value(); - sindex ++; + sindex++; } } @@ -211,8 +219,8 @@ controller_interface::return_type GpioCommandController::update( { RCLCPP_ERROR_THROTTLE( get_node()->get_logger(), *get_node()->get_clock(), 1000, - "command size (%zu) does not match number of interfaces (%zu)", - (*gpio_commands)->data.size(), command_interfaces_.size()); + "command size (%zu) does not match number of interfaces (%zu)", (*gpio_commands)->data.size(), + command_interfaces_.size()); return controller_interface::return_type::ERROR; } diff --git a/gpio_controllers/test/test_gpio_command_controller.cpp b/gpio_controllers/test/test_gpio_command_controller.cpp index 6e97c404bd..57f936588a 100644 --- a/gpio_controllers/test/test_gpio_command_controller.cpp +++ b/gpio_controllers/test/test_gpio_command_controller.cpp @@ -69,7 +69,7 @@ void GpioCommandControllerTest::SetUpController() state_ifs.emplace_back(gpio_1_1_dig_state_); state_ifs.emplace_back(gpio_1_2_dig_state_); state_ifs.emplace_back(gpio_2_ana_state_); - + controller_->assign_interfaces(std::move(command_ifs), std::move(state_ifs)); } @@ -96,8 +96,7 @@ TEST_F(GpioCommandControllerTest, GpioWithMissingGpioParams) controller_->get_node()->set_parameter({"gpios", std::vector{"gpio1", "gpio2"}}); controller_->get_node()->set_parameter( {"command_interfaces.gpio1", std::vector{"dig.1", "dig.2"}}); - controller_->get_node()->set_parameter( - {"command_interfaces.gpio2", std::vector()}); + controller_->get_node()->set_parameter({"command_interfaces.gpio2", std::vector()}); // // activate failed, command interface for 'gpio2' is not set up ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), CallbackReturn::ERROR); } @@ -138,7 +137,6 @@ TEST_F(GpioCommandControllerTest, CommandSuccessTest) controller_->get_node()->set_parameter( {"command_interfaces.gpio2", std::vector{"ana.1"}}); - ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), CallbackReturn::SUCCESS); ASSERT_EQ(controller_->on_activate(rclcpp_lifecycle::State()), CallbackReturn::SUCCESS); @@ -177,11 +175,9 @@ TEST_F(GpioCommandControllerTest, WrongCommandCheckTest) controller_->get_node()->set_parameter( {"command_interfaces.gpio2", std::vector{"ana.1"}}); - ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), CallbackReturn::SUCCESS); ASSERT_EQ(controller_->on_activate(rclcpp_lifecycle::State()), CallbackReturn::SUCCESS); - // send command with wrong number of gpios auto command_ptr = std::make_shared(); command_ptr->data = {0.0, 20.0}; @@ -207,7 +203,6 @@ TEST_F(GpioCommandControllerTest, NoCommandCheckTest) controller_->get_node()->set_parameter( {"command_interfaces.gpio2", std::vector{"ana.1"}}); - ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), CallbackReturn::SUCCESS); ASSERT_EQ(controller_->on_activate(rclcpp_lifecycle::State()), CallbackReturn::SUCCESS); @@ -235,7 +230,6 @@ TEST_F(GpioCommandControllerTest, CommandCallbackTest) controller_->get_node()->set_parameter( {"state_interfaces.gpio2", std::vector{"ana.1"}}); - // default values ASSERT_EQ(gpio_1_1_dig_cmd_.get_value(), 1.0); ASSERT_EQ(gpio_1_2_dig_cmd_.get_value(), 0.0); @@ -289,7 +283,6 @@ TEST_F(GpioCommandControllerTest, StateCallbackTest) controller_->get_node()->set_parameter( {"state_interfaces.gpio2", std::vector{"ana.1"}}); - // default values ASSERT_EQ(gpio_1_1_dig_cmd_.get_value(), 1.0); ASSERT_EQ(gpio_1_2_dig_cmd_.get_value(), 0.0); diff --git a/gpio_controllers/test/test_gpio_command_controller.hpp b/gpio_controllers/test/test_gpio_command_controller.hpp index 9841ed4add..d7f8e67a11 100644 --- a/gpio_controllers/test/test_gpio_command_controller.hpp +++ b/gpio_controllers/test/test_gpio_command_controller.hpp @@ -21,9 +21,9 @@ #include "gmock/gmock.h" +#include "gpio_controllers/gpio_command_controller.hpp" #include "hardware_interface/handle.hpp" #include "hardware_interface/types/hardware_interface_type_values.hpp" -#include "gpio_controllers/gpio_command_controller.hpp" using hardware_interface::CommandInterface; using hardware_interface::StateInterface; diff --git a/gpio_controllers/test/test_load_gpio_command_controller.cpp b/gpio_controllers/test/test_load_gpio_command_controller.cpp index 81343db526..ee8089455d 100644 --- a/gpio_controllers/test/test_load_gpio_command_controller.cpp +++ b/gpio_controllers/test/test_load_gpio_command_controller.cpp @@ -32,8 +32,8 @@ TEST(TestLoadGpioCommandController, load_controller) ros2_control_test_assets::minimal_robot_urdf), executor, "test_controller_manager"); - ASSERT_NO_THROW(cm.load_controller( - "test_gpio_command_controller", "gpio_controllers/GpioCommandController")); + ASSERT_NO_THROW( + cm.load_controller("test_gpio_command_controller", "gpio_controllers/GpioCommandController")); rclcpp::shutdown(); } From efce87a9e8bd34ae612d3b7baf9beeb837cd5ffc Mon Sep 17 00:00:00 2001 From: Wiktor Bajor Date: Wed, 31 Jul 2024 19:11:30 +0200 Subject: [PATCH 11/58] Apply pre-commit fix --- gpio_controllers/src/gpio_command_controller.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/gpio_controllers/src/gpio_command_controller.cpp b/gpio_controllers/src/gpio_command_controller.cpp index 75c93d8ea2..50aa27fcbf 100644 --- a/gpio_controllers/src/gpio_command_controller.cpp +++ b/gpio_controllers/src/gpio_command_controller.cpp @@ -187,7 +187,7 @@ CallbackReturn GpioCommandController::on_deactivate( controller_interface::return_type GpioCommandController::update( const rclcpp::Time & /*time*/, const rclcpp::Duration & /*period*/) { - //publish gpio state msg + // publish gpio state msg if (realtime_gpio_state_publisher_ && realtime_gpio_state_publisher_->trylock()) { auto & gpio_state_msg = realtime_gpio_state_publisher_->msg_; From b53bff699ba7c6632b6ca97032664923a8bb7cfb Mon Sep 17 00:00:00 2001 From: Wiktor Bajor Date: Thu, 1 Aug 2024 20:16:26 +0000 Subject: [PATCH 12/58] Fix compilation on ros rolling --- gpio_controllers/test/test_gpio_command_controller.cpp | 3 ++- gpio_controllers/test/test_load_gpio_command_controller.cpp | 4 +--- 2 files changed, 3 insertions(+), 4 deletions(-) diff --git a/gpio_controllers/test/test_gpio_command_controller.cpp b/gpio_controllers/test/test_gpio_command_controller.cpp index 57f936588a..60ba0b4290 100644 --- a/gpio_controllers/test/test_gpio_command_controller.cpp +++ b/gpio_controllers/test/test_gpio_command_controller.cpp @@ -57,7 +57,8 @@ void GpioCommandControllerTest::TearDown() { controller_.reset(nullptr); } void GpioCommandControllerTest::SetUpController() { - const auto result = controller_->init("test_gpio_command_controller"); + const auto result = controller_->init( + "test_gpio_command_controller", "", 0, "", controller_->define_custom_node_options()); ASSERT_EQ(result, controller_interface::return_type::OK); std::vector command_ifs; diff --git a/gpio_controllers/test/test_load_gpio_command_controller.cpp b/gpio_controllers/test/test_load_gpio_command_controller.cpp index ee8089455d..379a53b048 100644 --- a/gpio_controllers/test/test_load_gpio_command_controller.cpp +++ b/gpio_controllers/test/test_load_gpio_command_controller.cpp @@ -28,9 +28,7 @@ TEST(TestLoadGpioCommandController, load_controller) std::make_shared(); controller_manager::ControllerManager cm( - std::make_unique( - ros2_control_test_assets::minimal_robot_urdf), - executor, "test_controller_manager"); + executor, ros2_control_test_assets::minimal_robot_urdf, true, "test_controller_manager"); ASSERT_NO_THROW( cm.load_controller("test_gpio_command_controller", "gpio_controllers/GpioCommandController")); From 67b6069490c9b3b1116b36b4c38213ff1e826255 Mon Sep 17 00:00:00 2001 From: Wiktor Bajor Date: Sat, 3 Aug 2024 14:08:03 +0000 Subject: [PATCH 13/58] Add parameters file, use generated gpio_command_controller_parameters params and align UTs --- gpio_controllers/CMakeLists.txt | 9 +- .../gpio_command_controller.hpp | 3 + .../src/gpio_command_controller.cpp | 60 ++-- .../gpio_command_controller_parameters.yaml | 20 ++ .../test/test_gpio_command_controller.cpp | 279 +++++++++++------- 5 files changed, 225 insertions(+), 146 deletions(-) create mode 100644 gpio_controllers/src/gpio_command_controller_parameters.yaml diff --git a/gpio_controllers/CMakeLists.txt b/gpio_controllers/CMakeLists.txt index a36e86f2a2..b96b6f96e2 100644 --- a/gpio_controllers/CMakeLists.txt +++ b/gpio_controllers/CMakeLists.txt @@ -19,13 +19,20 @@ find_package(rclcpp REQUIRED) find_package(rclcpp_lifecycle REQUIRED) find_package(realtime_tools REQUIRED) find_package(std_msgs REQUIRED) +find_package(generate_parameter_library REQUIRED) + + +generate_parameter_library(gpio_command_controller_parameters + src/gpio_command_controller_parameters.yaml +) add_library(gpio_controllers SHARED src/gpio_command_controller.cpp ) target_include_directories(gpio_controllers PRIVATE include) -ament_target_dependencies(gpio_controllers +target_link_libraries(gpio_controllers PUBLIC gpio_command_controller_parameters) +ament_target_dependencies(gpio_controllers PUBLIC builtin_interfaces controller_interface hardware_interface diff --git a/gpio_controllers/include/gpio_controllers/gpio_command_controller.hpp b/gpio_controllers/include/gpio_controllers/gpio_command_controller.hpp index aef84f0a1e..dd146fb865 100644 --- a/gpio_controllers/include/gpio_controllers/gpio_command_controller.hpp +++ b/gpio_controllers/include/gpio_controllers/gpio_command_controller.hpp @@ -22,6 +22,7 @@ #include "control_msgs/msg/dynamic_joint_state.hpp" #include "controller_interface/controller_interface.hpp" +#include "gpio_command_controller_parameters.hpp" #include "gpio_controllers/visibility_control.h" #include "rclcpp/subscription.hpp" #include "rclcpp_lifecycle/node_interfaces/lifecycle_node_interface.hpp" @@ -76,6 +77,8 @@ class GpioCommandController : public controller_interface::ControllerInterface std::shared_ptr> realtime_gpio_state_publisher_; std::string logger_name_; + std::shared_ptr param_listener_; + gpio_command_controller_parameters::Params params_; }; } // namespace gpio_controllers diff --git a/gpio_controllers/src/gpio_command_controller.cpp b/gpio_controllers/src/gpio_command_controller.cpp index 50aa27fcbf..87c03a626e 100644 --- a/gpio_controllers/src/gpio_command_controller.cpp +++ b/gpio_controllers/src/gpio_command_controller.cpp @@ -38,45 +38,23 @@ GpioCommandController::GpioCommandController() } CallbackReturn GpioCommandController::on_init() +try { - try - { - auto_declare>("gpios", std::vector()); - gpio_names_ = get_node()->get_parameter("gpios").as_string_array(); - for (std::string & gpio : gpio_names_) - auto_declare>( - "command_interfaces." + gpio, std::vector()); - } - catch (const std::exception & e) - { - fprintf(stderr, "Exception thrown during init stage with message: %s \n", e.what()); - return CallbackReturn::ERROR; - } - + param_listener_ = std::make_shared(get_node()); + params_ = param_listener_->get_params(); return CallbackReturn::SUCCESS; } - -CallbackReturn GpioCommandController::on_configure( - const rclcpp_lifecycle::State & /*previous_state*/) +catch (const std::exception & e) { - gpio_names_ = get_node()->get_parameter("gpios").as_string_array(); - - if (gpio_names_.empty()) - { - RCLCPP_ERROR(get_node()->get_logger(), "'gpios' parameter was empty"); - return CallbackReturn::ERROR; - } + fprintf(stderr, "Exception thrown during init stage with message: %s \n", e.what()); + return CallbackReturn::ERROR; +} - for (std::string & gpio : gpio_names_) +CallbackReturn GpioCommandController::on_configure(const rclcpp_lifecycle::State &) +{ + for (const auto & [gpio, ports] : params_.command_interfaces.gpios_map) { - auto interfaces = get_node()->get_parameter("command_interfaces." + gpio).as_string_array(); - if (interfaces.empty()) - { - RCLCPP_ERROR( - get_node()->get_logger(), "'command_interfaces.%s' parameter was empty", gpio.c_str()); - return CallbackReturn::ERROR; - } - if (!interface_names_.insert(std::make_pair(gpio, interfaces)).second) + if (!interface_names_.insert(std::make_pair(gpio, ports.ports)).second) { RCLCPP_ERROR( get_node()->get_logger(), @@ -85,7 +63,7 @@ CallbackReturn GpioCommandController::on_configure( } } - for (const auto & gpio : gpio_names_) + for (const auto & gpio : params_.gpios) { for (const auto & interface_name : interface_names_[gpio]) { @@ -157,12 +135,12 @@ CallbackReturn GpioCommandController::on_activate( // 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(gpio_names_.size()); - gpio_state_msg.interface_values.resize(gpio_names_.size()); - for (auto g = 0ul; g < gpio_names_.size(); g++) + gpio_state_msg.joint_names.resize(params_.gpios.size()); + gpio_state_msg.interface_values.resize(params_.gpios.size()); + for (auto g = 0ul; g < params_.gpios.size(); g++) { - gpio_state_msg.joint_names[g] = gpio_names_[g]; - for (const auto & interface_name : interface_names_[gpio_names_[g]]) + gpio_state_msg.joint_names[g] = params_.gpios[g]; + for (const auto & interface_name : interface_names_[params_.gpios[g]]) { gpio_state_msg.interface_values[g].interface_names.push_back(interface_name); gpio_state_msg.interface_values[g].values.push_back(std::numeric_limits::quiet_NaN()); @@ -195,9 +173,9 @@ controller_interface::return_type GpioCommandController::update( gpio_state_msg.header.stamp = get_node()->now(); auto sindex = 0ul; - for (auto g = 0ul; g < gpio_names_.size(); g++) + for (auto g = 0ul; g < params_.gpios.size(); g++) { - for (auto i = 0ul; i < interface_names_[gpio_names_[g]].size(); i++) + for (auto i = 0ul; i < interface_names_[params_.gpios[g]].size(); i++) { gpio_state_msg.interface_values[g].values[i] = state_interfaces_[sindex].get_value(); sindex++; diff --git a/gpio_controllers/src/gpio_command_controller_parameters.yaml b/gpio_controllers/src/gpio_command_controller_parameters.yaml new file mode 100644 index 0000000000..e6fe0c4ebd --- /dev/null +++ b/gpio_controllers/src/gpio_command_controller_parameters.yaml @@ -0,0 +1,20 @@ +gpio_command_controller_parameters: + gpios: { + type: string_array, + description: "List of gpios", + validation: { + size_gt<>: [0], + unique<>: null + } + } + + command_interfaces: + __map_gpios: + ports: { + type: string_array, + description: "List of ports: for each gpio", + validation: { + size_gt<>: [0], + unique<>: null + } + } diff --git a/gpio_controllers/test/test_gpio_command_controller.cpp b/gpio_controllers/test/test_gpio_command_controller.cpp index 60ba0b4290..2b63a1c553 100644 --- a/gpio_controllers/test/test_gpio_command_controller.cpp +++ b/gpio_controllers/test/test_gpio_command_controller.cpp @@ -33,17 +33,6 @@ using hardware_interface::LoanedStateInterface; using CmdType = std_msgs::msg::Float64MultiArray; using StateType = control_msgs::msg::DynamicJointState; -namespace -{ -rclcpp::WaitResultKind wait_for(rclcpp::SubscriptionBase::SharedPtr subscription) -{ - rclcpp::WaitSet wait_set; - wait_set.add_subscription(subscription); - const auto timeout = std::chrono::seconds(10); - return wait_set.wait(timeout).kind(); -} -} // namespace - void GpioCommandControllerTest::SetUpTestCase() { rclcpp::init(0, nullptr); } void GpioCommandControllerTest::TearDownTestCase() { rclcpp::shutdown(); } @@ -55,11 +44,48 @@ void GpioCommandControllerTest::SetUp() void GpioCommandControllerTest::TearDown() { controller_.reset(nullptr); } -void GpioCommandControllerTest::SetUpController() +TEST_F(GpioCommandControllerTest, GpiosParameterNotSet) { const auto result = controller_->init( "test_gpio_command_controller", "", 0, "", controller_->define_custom_node_options()); - ASSERT_EQ(result, controller_interface::return_type::OK); + ASSERT_EQ(result, controller_interface::return_type::ERROR); +} + +TEST_F(GpioCommandControllerTest, GpiosParameterIsEmpty) +{ + std::vector parameters; + parameters.emplace_back("gpios", std::vector{}); + auto node_options = rclcpp::NodeOptions(); + node_options.parameter_overrides(parameters); + const auto result = controller_->init("test_gpio_command_controller", "", 0, "", node_options); + + ASSERT_EQ(result, controller_interface::return_type::ERROR); +} + +TEST_F(GpioCommandControllerTest, GpioWithMissingGpioParams) +{ + std::vector parameters; + parameters.emplace_back("gpios", std::vector{"gpio1", "gpio2"}); + parameters.emplace_back( + "command_interfaces.gpio1.ports", std::vector{"dig.1", "dig.2"}); + parameters.emplace_back("command_interfaces.gpio2.ports", std::vector{}); + auto node_options = rclcpp::NodeOptions(); + node_options.parameter_overrides(parameters); + const auto result = controller_->init("test_gpio_command_controller", "", 0, "", node_options); + + ASSERT_EQ(result, controller_interface::return_type::ERROR); +} + +TEST_F(GpioCommandControllerTest, ConfigureAndActivateParamsSuccess) +{ + std::vector parameters; + parameters.emplace_back("gpios", gpio_names_); + parameters.emplace_back( + "command_interfaces.gpio1.ports", std::vector{"dig.1", "dig.2"}); + parameters.emplace_back("command_interfaces.gpio2.ports", std::vector{"ana.1"}); + auto node_options = rclcpp::NodeOptions(); + node_options.parameter_overrides(parameters); + const auto result = controller_->init("test_gpio_command_controller", "", 0, "", node_options); std::vector command_ifs; command_ifs.emplace_back(gpio_1_1_dig_cmd_); @@ -72,72 +98,63 @@ void GpioCommandControllerTest::SetUpController() state_ifs.emplace_back(gpio_2_ana_state_); controller_->assign_interfaces(std::move(command_ifs), std::move(state_ifs)); -} -TEST_F(GpioCommandControllerTest, GpiosParameterNotSet) -{ - SetUpController(); - - // configure failed, 'gpios' parameter not set - ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), CallbackReturn::ERROR); + ASSERT_EQ(result, controller_interface::return_type::OK); + ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), CallbackReturn::SUCCESS); + ASSERT_EQ(controller_->on_activate(rclcpp_lifecycle::State()), CallbackReturn::SUCCESS); } -TEST_F(GpioCommandControllerTest, GpiosParameterIsEmpty) +TEST_F(GpioCommandControllerTest, ActivateWithWrongGpiosNamesFails) { - SetUpController(); - controller_->get_node()->set_parameter({"gpios", std::vector()}); + std::vector parameters; + parameters.emplace_back("gpios", std::vector{"gpio1", "gpio4"}); + parameters.emplace_back( + "command_interfaces.gpio1.ports", std::vector{"dig.1", "dig.2"}); + parameters.emplace_back("command_interfaces.gpio4.ports", std::vector{"ana.1"}); + auto node_options = rclcpp::NodeOptions(); + node_options.parameter_overrides(parameters); + const auto result = controller_->init("test_gpio_command_controller", "", 0, "", node_options); - // configure failed, 'gpios' is empty - ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), CallbackReturn::ERROR); -} + std::vector command_ifs; + command_ifs.emplace_back(gpio_1_1_dig_cmd_); + command_ifs.emplace_back(gpio_1_2_dig_cmd_); + command_ifs.emplace_back(gpio_2_ana_cmd_); -TEST_F(GpioCommandControllerTest, GpioWithMissingGpioParams) -{ - SetUpController(); - controller_->get_node()->set_parameter({"gpios", std::vector{"gpio1", "gpio2"}}); - controller_->get_node()->set_parameter( - {"command_interfaces.gpio1", std::vector{"dig.1", "dig.2"}}); - controller_->get_node()->set_parameter({"command_interfaces.gpio2", std::vector()}); - // // activate failed, command interface for 'gpio2' is not set up - ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), CallbackReturn::ERROR); -} + std::vector state_ifs; + state_ifs.emplace_back(gpio_1_1_dig_state_); + state_ifs.emplace_back(gpio_1_2_dig_state_); + state_ifs.emplace_back(gpio_2_ana_state_); -TEST_F(GpioCommandControllerTest, ConfigureAndActivateParamsSuccess) -{ - SetUpController(); - controller_->get_node()->set_parameter({"gpios", gpio_names_}); - controller_->get_node()->set_parameter( - {"command_interfaces.gpio1", std::vector{"dig.1", "dig.2"}}); - controller_->get_node()->set_parameter( - {"command_interfaces.gpio2", std::vector{"ana.1"}}); - - // configure successful - ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), CallbackReturn::SUCCESS); - ASSERT_EQ(controller_->on_activate(rclcpp_lifecycle::State()), CallbackReturn::SUCCESS); -} + controller_->assign_interfaces(std::move(command_ifs), std::move(state_ifs)); -TEST_F(GpioCommandControllerTest, ActivateWithWrongGpiosNamesFails) -{ - SetUpController(); - controller_->get_node()->set_parameter({"gpios", std::vector{"gpio1", "gpio4"}}); - controller_->get_node()->set_parameter( - {"command_interfaces.gpio1", std::vector{"dig.1", "dig.2"}}); - controller_->get_node()->set_parameter( - {"command_interfaces.gpio4", std::vector{"ana.1"}}); - // // activate failed, 'gpio4' is not a valid gpio name for the hardware + ASSERT_EQ(result, controller_interface::return_type::OK); ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), CallbackReturn::SUCCESS); ASSERT_EQ(controller_->on_activate(rclcpp_lifecycle::State()), CallbackReturn::ERROR); } TEST_F(GpioCommandControllerTest, CommandSuccessTest) { - SetUpController(); - controller_->get_node()->set_parameter({"gpios", gpio_names_}); - controller_->get_node()->set_parameter( - {"command_interfaces.gpio1", std::vector{"dig.1", "dig.2"}}); - controller_->get_node()->set_parameter( - {"command_interfaces.gpio2", std::vector{"ana.1"}}); + std::vector parameters; + parameters.emplace_back("gpios", std::vector{"gpio1", "gpio2"}); + parameters.emplace_back( + "command_interfaces.gpio1.ports", std::vector{"dig.1", "dig.2"}); + parameters.emplace_back("command_interfaces.gpio2.ports", std::vector{"ana.1"}); + auto node_options = rclcpp::NodeOptions(); + node_options.parameter_overrides(parameters); + const auto result = controller_->init("test_gpio_command_controller", "", 0, "", node_options); + + std::vector command_ifs; + command_ifs.emplace_back(gpio_1_1_dig_cmd_); + command_ifs.emplace_back(gpio_1_2_dig_cmd_); + command_ifs.emplace_back(gpio_2_ana_cmd_); + + std::vector state_ifs; + state_ifs.emplace_back(gpio_1_1_dig_state_); + state_ifs.emplace_back(gpio_1_2_dig_state_); + state_ifs.emplace_back(gpio_2_ana_state_); + controller_->assign_interfaces(std::move(command_ifs), std::move(state_ifs)); + ASSERT_EQ(result, controller_interface::return_type::OK); ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), CallbackReturn::SUCCESS); ASSERT_EQ(controller_->on_activate(rclcpp_lifecycle::State()), CallbackReturn::SUCCESS); @@ -169,13 +186,27 @@ TEST_F(GpioCommandControllerTest, CommandSuccessTest) TEST_F(GpioCommandControllerTest, WrongCommandCheckTest) { - SetUpController(); - controller_->get_node()->set_parameter({"gpios", gpio_names_}); - controller_->get_node()->set_parameter( - {"command_interfaces.gpio1", std::vector{"dig.1", "dig.2"}}); - controller_->get_node()->set_parameter( - {"command_interfaces.gpio2", std::vector{"ana.1"}}); + std::vector parameters; + parameters.emplace_back("gpios", std::vector{"gpio1", "gpio2"}); + parameters.emplace_back( + "command_interfaces.gpio1.ports", std::vector{"dig.1", "dig.2"}); + parameters.emplace_back("command_interfaces.gpio2.ports", std::vector{"ana.1"}); + auto node_options = rclcpp::NodeOptions(); + node_options.parameter_overrides(parameters); + const auto result = controller_->init("test_gpio_command_controller", "", 0, "", node_options); + + std::vector command_ifs; + command_ifs.emplace_back(gpio_1_1_dig_cmd_); + command_ifs.emplace_back(gpio_1_2_dig_cmd_); + command_ifs.emplace_back(gpio_2_ana_cmd_); + std::vector state_ifs; + state_ifs.emplace_back(gpio_1_1_dig_state_); + state_ifs.emplace_back(gpio_1_2_dig_state_); + state_ifs.emplace_back(gpio_2_ana_state_); + + controller_->assign_interfaces(std::move(command_ifs), std::move(state_ifs)); + ASSERT_EQ(result, controller_interface::return_type::OK); ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), CallbackReturn::SUCCESS); ASSERT_EQ(controller_->on_activate(rclcpp_lifecycle::State()), CallbackReturn::SUCCESS); @@ -197,13 +228,28 @@ TEST_F(GpioCommandControllerTest, WrongCommandCheckTest) TEST_F(GpioCommandControllerTest, NoCommandCheckTest) { - SetUpController(); - controller_->get_node()->set_parameter({"gpios", gpio_names_}); - controller_->get_node()->set_parameter( - {"command_interfaces.gpio1", std::vector{"dig.1", "dig.2"}}); - controller_->get_node()->set_parameter( - {"command_interfaces.gpio2", std::vector{"ana.1"}}); + std::vector parameters; + parameters.emplace_back("gpios", std::vector{"gpio1", "gpio2"}); + parameters.emplace_back( + "command_interfaces.gpio1.ports", std::vector{"dig.1", "dig.2"}); + parameters.emplace_back("command_interfaces.gpio2.ports", std::vector{"ana.1"}); + auto node_options = rclcpp::NodeOptions(); + node_options.parameter_overrides(parameters); + const auto result = controller_->init("test_gpio_command_controller", "", 0, "", node_options); + + std::vector command_ifs; + command_ifs.emplace_back(gpio_1_1_dig_cmd_); + command_ifs.emplace_back(gpio_1_2_dig_cmd_); + command_ifs.emplace_back(gpio_2_ana_cmd_); + std::vector state_ifs; + state_ifs.emplace_back(gpio_1_1_dig_state_); + state_ifs.emplace_back(gpio_1_2_dig_state_); + state_ifs.emplace_back(gpio_2_ana_state_); + + controller_->assign_interfaces(std::move(command_ifs), std::move(state_ifs)); + + ASSERT_EQ(result, controller_interface::return_type::OK); ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), CallbackReturn::SUCCESS); ASSERT_EQ(controller_->on_activate(rclcpp_lifecycle::State()), CallbackReturn::SUCCESS); @@ -220,18 +266,29 @@ TEST_F(GpioCommandControllerTest, NoCommandCheckTest) TEST_F(GpioCommandControllerTest, CommandCallbackTest) { - SetUpController(); - controller_->get_node()->set_parameter({"gpios", gpio_names_}); - controller_->get_node()->set_parameter( - {"command_interfaces.gpio1", std::vector{"dig.1", "dig.2"}}); - controller_->get_node()->set_parameter( - {"command_interfaces.gpio2", std::vector{"ana.1"}}); - controller_->get_node()->set_parameter( - {"state_interfaces.gpio1", std::vector{"dig.1", "dig.2"}}); - controller_->get_node()->set_parameter( - {"state_interfaces.gpio2", std::vector{"ana.1"}}); + std::vector parameters; + parameters.emplace_back("gpios", std::vector{"gpio1", "gpio2"}); + parameters.emplace_back( + "command_interfaces.gpio1.ports", std::vector{"dig.1", "dig.2"}); + parameters.emplace_back("command_interfaces.gpio2.ports", std::vector{"ana.1"}); + auto node_options = rclcpp::NodeOptions(); + node_options.parameter_overrides(parameters); + const auto result = controller_->init("test_gpio_command_controller", "", 0, "", node_options); + + std::vector command_ifs; + command_ifs.emplace_back(gpio_1_1_dig_cmd_); + command_ifs.emplace_back(gpio_1_2_dig_cmd_); + command_ifs.emplace_back(gpio_2_ana_cmd_); + + std::vector state_ifs; + state_ifs.emplace_back(gpio_1_1_dig_state_); + state_ifs.emplace_back(gpio_1_2_dig_state_); + state_ifs.emplace_back(gpio_2_ana_state_); + + controller_->assign_interfaces(std::move(command_ifs), std::move(state_ifs)); + + ASSERT_EQ(result, controller_interface::return_type::OK); - // default values ASSERT_EQ(gpio_1_1_dig_cmd_.get_value(), 1.0); ASSERT_EQ(gpio_1_2_dig_cmd_.get_value(), 0.0); ASSERT_EQ(gpio_2_ana_cmd_.get_value(), 3.1); @@ -254,18 +311,20 @@ TEST_F(GpioCommandControllerTest, CommandCallbackTest) command_msg.data = {0.0, 1.0, 30.0}; command_pub->publish(command_msg); - // wait for command message to be passed - ASSERT_EQ(wait_for(controller_->gpios_command_subscriber_), rclcpp::WaitResultKind::Ready); - - // process callbacks - rclcpp::spin_some(controller_->get_node()->get_node_base_interface()); + rclcpp::executors::SingleThreadedExecutor executor; + executor.add_node(controller_->get_node()->get_node_base_interface()); + const auto timeout = std::chrono::milliseconds{1}; + const auto until = controller_->get_node()->get_clock()->now() + timeout; + while (controller_->get_node()->get_clock()->now() < until) + { + executor.spin_some(); + std::this_thread::sleep_for(std::chrono::microseconds(10)); + } - // update successful ASSERT_EQ( controller_->update(rclcpp::Time(0), rclcpp::Duration::from_seconds(0.01)), controller_interface::return_type::OK); - // check command in handle was set ASSERT_EQ(gpio_1_1_dig_cmd_.get_value(), 0.0); ASSERT_EQ(gpio_1_2_dig_cmd_.get_value(), 1.0); ASSERT_EQ(gpio_2_ana_cmd_.get_value(), 30.0); @@ -273,16 +332,28 @@ TEST_F(GpioCommandControllerTest, CommandCallbackTest) TEST_F(GpioCommandControllerTest, StateCallbackTest) { - SetUpController(); - controller_->get_node()->set_parameter({"gpios", gpio_names_}); - controller_->get_node()->set_parameter( - {"command_interfaces.gpio1", std::vector{"dig.1", "dig.2"}}); - controller_->get_node()->set_parameter( - {"command_interfaces.gpio2", std::vector{"ana.1"}}); - controller_->get_node()->set_parameter( - {"state_interfaces.gpio1", std::vector{"dig.1", "dig.2"}}); - controller_->get_node()->set_parameter( - {"state_interfaces.gpio2", std::vector{"ana.1"}}); + std::vector parameters; + parameters.emplace_back("gpios", std::vector{"gpio1", "gpio2"}); + parameters.emplace_back( + "command_interfaces.gpio1.ports", std::vector{"dig.1", "dig.2"}); + parameters.emplace_back("command_interfaces.gpio2.ports", std::vector{"ana.1"}); + auto node_options = rclcpp::NodeOptions(); + node_options.parameter_overrides(parameters); + const auto result = controller_->init("test_gpio_command_controller", "", 0, "", node_options); + + std::vector command_ifs; + command_ifs.emplace_back(gpio_1_1_dig_cmd_); + command_ifs.emplace_back(gpio_1_2_dig_cmd_); + command_ifs.emplace_back(gpio_2_ana_cmd_); + + std::vector state_ifs; + state_ifs.emplace_back(gpio_1_1_dig_state_); + state_ifs.emplace_back(gpio_1_2_dig_state_); + state_ifs.emplace_back(gpio_2_ana_state_); + + controller_->assign_interfaces(std::move(command_ifs), std::move(state_ifs)); + + ASSERT_EQ(result, controller_interface::return_type::OK); // default values ASSERT_EQ(gpio_1_1_dig_cmd_.get_value(), 1.0); From 30d260488eac248e7beb89e619db56e3d91f2e47 Mon Sep 17 00:00:00 2001 From: Wiktor Bajor Date: Sun, 4 Aug 2024 10:52:57 +0200 Subject: [PATCH 14/58] Remove unnecessary members: interface_names_, gpios_names_ and use params struct instead. Use default member initializer. --- .../gpio_command_controller.hpp | 14 +++--- .../src/gpio_command_controller.cpp | 45 +++++-------------- 2 files changed, 15 insertions(+), 44 deletions(-) diff --git a/gpio_controllers/include/gpio_controllers/gpio_command_controller.hpp b/gpio_controllers/include/gpio_controllers/gpio_command_controller.hpp index dd146fb865..6cda520113 100644 --- a/gpio_controllers/include/gpio_controllers/gpio_command_controller.hpp +++ b/gpio_controllers/include/gpio_controllers/gpio_command_controller.hpp @@ -66,18 +66,14 @@ class GpioCommandController : public controller_interface::ControllerInterface const rclcpp::Time & time, const rclcpp::Duration & period) override; protected: - std::vector gpio_names_; - std::unordered_map> interface_names_; std::vector interface_types_; + realtime_tools::RealtimeBuffer> rt_command_ptr_{}; + rclcpp::Subscription::SharedPtr gpios_command_subscriber_{}; - realtime_tools::RealtimeBuffer> rt_command_ptr_; - rclcpp::Subscription::SharedPtr gpios_command_subscriber_; + std::shared_ptr> gpio_state_publisher_{}; + std::shared_ptr> realtime_gpio_state_publisher_{}; - std::shared_ptr> gpio_state_publisher_; - std::shared_ptr> realtime_gpio_state_publisher_; - - std::string logger_name_; - std::shared_ptr param_listener_; + std::shared_ptr param_listener_{}; gpio_command_controller_parameters::Params params_; }; diff --git a/gpio_controllers/src/gpio_command_controller.cpp b/gpio_controllers/src/gpio_command_controller.cpp index 87c03a626e..7cb44013b4 100644 --- a/gpio_controllers/src/gpio_command_controller.cpp +++ b/gpio_controllers/src/gpio_command_controller.cpp @@ -30,12 +30,7 @@ namespace gpio_controllers { using hardware_interface::LoanedCommandInterface; -GpioCommandController::GpioCommandController() -: controller_interface::ControllerInterface(), - rt_command_ptr_(nullptr), - gpios_command_subscriber_(nullptr) -{ -} +GpioCommandController::GpioCommandController() : controller_interface::ControllerInterface() {} CallbackReturn GpioCommandController::on_init() try @@ -54,23 +49,11 @@ CallbackReturn GpioCommandController::on_configure(const rclcpp_lifecycle::State { for (const auto & [gpio, ports] : params_.command_interfaces.gpios_map) { - if (!interface_names_.insert(std::make_pair(gpio, ports.ports)).second) - { - RCLCPP_ERROR( - get_node()->get_logger(), - "Trying to override existing gpio setup. Wrong controller parameters."); - return CallbackReturn::ERROR; - } - } - - for (const auto & gpio : params_.gpios) - { - for (const auto & interface_name : interface_names_[gpio]) + for (const auto & port : ports.ports) { - interface_types_.push_back(gpio + "/" + interface_name); + interface_types_.push_back(gpio + "/" + port); } } - try { gpios_command_subscriber_ = get_node()->create_subscription( @@ -85,7 +68,6 @@ CallbackReturn GpioCommandController::on_configure(const rclcpp_lifecycle::State } catch (const std::exception & e) { - // get_node() may throw, logging raw here fprintf(stderr, "Exception thrown during init stage with message: %s \n", e.what()); return CallbackReturn::ERROR; } @@ -114,8 +96,7 @@ controller_interface::InterfaceConfiguration GpioCommandController::state_interf return state_interfaces_config; } -CallbackReturn GpioCommandController::on_activate( - const rclcpp_lifecycle::State & /*previous_state*/) +CallbackReturn GpioCommandController::on_activate(const rclcpp_lifecycle::State &) { std::vector> ordered_interfaces; if ( @@ -137,45 +118,40 @@ CallbackReturn GpioCommandController::on_activate( gpio_state_msg.header.stamp = get_node()->now(); gpio_state_msg.joint_names.resize(params_.gpios.size()); gpio_state_msg.interface_values.resize(params_.gpios.size()); + for (auto g = 0ul; g < params_.gpios.size(); g++) { gpio_state_msg.joint_names[g] = params_.gpios[g]; - for (const auto & interface_name : interface_names_[params_.gpios[g]]) + for (const auto & interface_name : params_.command_interfaces.gpios_map[params_.gpios[g]].ports) { gpio_state_msg.interface_values[g].interface_names.push_back(interface_name); gpio_state_msg.interface_values[g].values.push_back(std::numeric_limits::quiet_NaN()); } } - - // reset command buffer if a command came through callback when controller was inactive rt_command_ptr_.reset(); - RCLCPP_INFO(get_node()->get_logger(), "activate successful"); return CallbackReturn::SUCCESS; } -CallbackReturn GpioCommandController::on_deactivate( - const rclcpp_lifecycle::State & /*previous_state*/) +CallbackReturn GpioCommandController::on_deactivate(const rclcpp_lifecycle::State &) { - // reset command buffer rt_command_ptr_.reset(); return CallbackReturn::SUCCESS; } controller_interface::return_type GpioCommandController::update( - const rclcpp::Time & /*time*/, const rclcpp::Duration & /*period*/) + const rclcpp::Time &, const rclcpp::Duration &) { - // publish gpio state msg if (realtime_gpio_state_publisher_ && realtime_gpio_state_publisher_->trylock()) { auto & gpio_state_msg = realtime_gpio_state_publisher_->msg_; - gpio_state_msg.header.stamp = get_node()->now(); auto sindex = 0ul; for (auto g = 0ul; g < params_.gpios.size(); g++) { - for (auto i = 0ul; i < interface_names_[params_.gpios[g]].size(); i++) + for (auto i = 0ul; i < params_.command_interfaces.gpios_map[params_.gpios[g]].ports.size(); + i++) { gpio_state_msg.interface_values[g].values[i] = state_interfaces_[sindex].get_value(); sindex++; @@ -187,7 +163,6 @@ controller_interface::return_type GpioCommandController::update( auto gpio_commands = rt_command_ptr_.readFromRT(); - // no command received yet if (!gpio_commands || !(*gpio_commands)) { return controller_interface::return_type::OK; From 4c824e84d67bf57555752f0599b90bdc9a1fa19a Mon Sep 17 00:00:00 2001 From: Wiktor Bajor Date: Sun, 4 Aug 2024 11:05:18 +0200 Subject: [PATCH 15/58] Refactor of storing interface types --- .../gpio_command_controller.hpp | 4 +- .../src/gpio_command_controller.cpp | 48 +++++++++---------- 2 files changed, 27 insertions(+), 25 deletions(-) diff --git a/gpio_controllers/include/gpio_controllers/gpio_command_controller.hpp b/gpio_controllers/include/gpio_controllers/gpio_command_controller.hpp index 6cda520113..a8737c4944 100644 --- a/gpio_controllers/include/gpio_controllers/gpio_command_controller.hpp +++ b/gpio_controllers/include/gpio_controllers/gpio_command_controller.hpp @@ -17,7 +17,6 @@ #include #include -#include #include #include "control_msgs/msg/dynamic_joint_state.hpp" @@ -65,6 +64,9 @@ class GpioCommandController : public controller_interface::ControllerInterface controller_interface::return_type update( const rclcpp::Time & time, const rclcpp::Duration & period) override; +private: + void store_interface_types(); + protected: std::vector interface_types_; realtime_tools::RealtimeBuffer> rt_command_ptr_{}; diff --git a/gpio_controllers/src/gpio_command_controller.cpp b/gpio_controllers/src/gpio_command_controller.cpp index 7cb44013b4..433caf0ee6 100644 --- a/gpio_controllers/src/gpio_command_controller.cpp +++ b/gpio_controllers/src/gpio_command_controller.cpp @@ -46,35 +46,36 @@ catch (const std::exception & e) } CallbackReturn GpioCommandController::on_configure(const rclcpp_lifecycle::State &) +try { - for (const auto & [gpio, ports] : params_.command_interfaces.gpios_map) - { - for (const auto & port : ports.ports) - { - interface_types_.push_back(gpio + "/" + port); - } - } - try - { - gpios_command_subscriber_ = get_node()->create_subscription( - "~/commands", rclcpp::SystemDefaultsQoS(), - [this](const CmdType::SharedPtr msg) { rt_command_ptr_.writeFromNonRT(msg); }); + store_interface_types(); + gpios_command_subscriber_ = get_node()->create_subscription( + "~/commands", rclcpp::SystemDefaultsQoS(), + [this](const CmdType::SharedPtr msg) { rt_command_ptr_.writeFromNonRT(msg); }); - gpio_state_publisher_ = - get_node()->create_publisher("~/gpio_states", rclcpp::SystemDefaultsQoS()); - - realtime_gpio_state_publisher_ = - std::make_shared>(gpio_state_publisher_); - } - catch (const std::exception & e) - { - fprintf(stderr, "Exception thrown during init stage with message: %s \n", e.what()); - return CallbackReturn::ERROR; - } + gpio_state_publisher_ = + get_node()->create_publisher("~/gpio_states", rclcpp::SystemDefaultsQoS()); + realtime_gpio_state_publisher_ = + std::make_shared>(gpio_state_publisher_); RCLCPP_INFO(get_node()->get_logger(), "configure successful"); return CallbackReturn::SUCCESS; } +catch (const std::exception & e) +{ + fprintf(stderr, "Exception thrown during init stage with message: %s \n", e.what()); + return CallbackReturn::ERROR; +} + +void GpioCommandController::store_interface_types() +{ + for (const auto & [gpio_name, ports] : params_.command_interfaces.gpios_map) + { + std::transform( + ports.ports.begin(), ports.ports.end(), std::back_inserter(interface_types_), + [&](const auto & interface_name) { return gpio_name + "/" + interface_name; }); + } +} controller_interface::InterfaceConfiguration GpioCommandController::command_interface_configuration() const @@ -113,7 +114,6 @@ CallbackReturn GpioCommandController::on_activate(const rclcpp_lifecycle::State return CallbackReturn::ERROR; } - // 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()); From dd86d9eabbf38ddf4318c65d972744604a5fb8b6 Mon Sep 17 00:00:00 2001 From: Wiktor Bajor Date: Sun, 4 Aug 2024 11:09:13 +0200 Subject: [PATCH 16/58] Remove unnecessary includes --- .../include/gpio_controllers/gpio_command_controller.hpp | 1 - gpio_controllers/src/gpio_command_controller.cpp | 7 +------ 2 files changed, 1 insertion(+), 7 deletions(-) diff --git a/gpio_controllers/include/gpio_controllers/gpio_command_controller.hpp b/gpio_controllers/include/gpio_controllers/gpio_command_controller.hpp index a8737c4944..c59f5072ec 100644 --- a/gpio_controllers/include/gpio_controllers/gpio_command_controller.hpp +++ b/gpio_controllers/include/gpio_controllers/gpio_command_controller.hpp @@ -23,7 +23,6 @@ #include "controller_interface/controller_interface.hpp" #include "gpio_command_controller_parameters.hpp" #include "gpio_controllers/visibility_control.h" -#include "rclcpp/subscription.hpp" #include "rclcpp_lifecycle/node_interfaces/lifecycle_node_interface.hpp" #include "rclcpp_lifecycle/state.hpp" #include "realtime_tools/realtime_buffer.h" diff --git a/gpio_controllers/src/gpio_command_controller.cpp b/gpio_controllers/src/gpio_command_controller.cpp index 433caf0ee6..4d06319656 100644 --- a/gpio_controllers/src/gpio_command_controller.cpp +++ b/gpio_controllers/src/gpio_command_controller.cpp @@ -15,16 +15,11 @@ #include "gpio_controllers/gpio_command_controller.hpp" #include -#include -#include -#include -#include #include "controller_interface/helpers.hpp" #include "rclcpp/logging.hpp" #include "rclcpp/qos.hpp" - -#include "hardware_interface/loaned_command_interface.hpp" +#include "rclcpp/subscription.hpp" namespace gpio_controllers { From 4ac938de089c0e7d02c4a6facfec71b9ebcf1e27 Mon Sep 17 00:00:00 2001 From: Wiktor Bajor Date: Sun, 4 Aug 2024 11:41:05 +0200 Subject: [PATCH 17/58] Refacotr on_activate method. Add validate_configured_interfaces and initialize_gpio_state_msg methods --- .../gpio_command_controller.hpp | 2 + .../src/gpio_command_controller.cpp | 38 ++++++++++++++----- 2 files changed, 30 insertions(+), 10 deletions(-) diff --git a/gpio_controllers/include/gpio_controllers/gpio_command_controller.hpp b/gpio_controllers/include/gpio_controllers/gpio_command_controller.hpp index c59f5072ec..2202ffeabe 100644 --- a/gpio_controllers/include/gpio_controllers/gpio_command_controller.hpp +++ b/gpio_controllers/include/gpio_controllers/gpio_command_controller.hpp @@ -65,6 +65,8 @@ class GpioCommandController : public controller_interface::ControllerInterface private: void store_interface_types(); + void initialize_gpio_state_msg(); + CallbackReturn validate_configured_interfaces(); protected: std::vector interface_types_; diff --git a/gpio_controllers/src/gpio_command_controller.cpp b/gpio_controllers/src/gpio_command_controller.cpp index 4d06319656..6bfcc56cf0 100644 --- a/gpio_controllers/src/gpio_command_controller.cpp +++ b/gpio_controllers/src/gpio_command_controller.cpp @@ -93,6 +93,19 @@ controller_interface::InterfaceConfiguration GpioCommandController::state_interf } CallbackReturn GpioCommandController::on_activate(const rclcpp_lifecycle::State &) +{ + if (validate_configured_interfaces() != CallbackReturn::SUCCESS) + { + return CallbackReturn::ERROR; + } + + initialize_gpio_state_msg(); + rt_command_ptr_.reset(); + RCLCPP_INFO(get_node()->get_logger(), "activate successful"); + return CallbackReturn::SUCCESS; +} + +CallbackReturn GpioCommandController::validate_configured_interfaces() { std::vector> ordered_interfaces; if ( @@ -109,23 +122,28 @@ CallbackReturn GpioCommandController::on_activate(const rclcpp_lifecycle::State return CallbackReturn::ERROR; } + return CallbackReturn::SUCCESS; +} + +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_values.resize(params_.gpios.size()); - for (auto g = 0ul; g < params_.gpios.size(); g++) + for (std::size_t gpio_index = 0; gpio_index < params_.gpios.size(); ++gpio_index) { - gpio_state_msg.joint_names[g] = params_.gpios[g]; - for (const auto & interface_name : params_.command_interfaces.gpios_map[params_.gpios[g]].ports) - { - gpio_state_msg.interface_values[g].interface_names.push_back(interface_name); - gpio_state_msg.interface_values[g].values.push_back(std::numeric_limits::quiet_NaN()); - } + const auto gpio_name = params_.gpios[gpio_index]; + gpio_state_msg.joint_names[gpio_index] = gpio_name; + std::copy( + params_.command_interfaces.gpios_map[gpio_name].ports.begin(), + params_.command_interfaces.gpios_map[gpio_name].ports.end(), + std::back_insert_iterator(gpio_state_msg.interface_values[gpio_index].interface_names)); + gpio_state_msg.interface_values[gpio_index].values = std::vector( + params_.command_interfaces.gpios_map[gpio_name].ports.size(), + std::numeric_limits::quiet_NaN()); } - rt_command_ptr_.reset(); - RCLCPP_INFO(get_node()->get_logger(), "activate successful"); - return CallbackReturn::SUCCESS; } CallbackReturn GpioCommandController::on_deactivate(const rclcpp_lifecycle::State &) From aa592973acef5a9b951e76337f02669148b31704 Mon Sep 17 00:00:00 2001 From: Wiktor Bajor Date: Sun, 4 Aug 2024 12:00:08 +0200 Subject: [PATCH 18/58] Refactor update method, split update of state if and command if --- .../gpio_command_controller.hpp | 2 + .../src/gpio_command_controller.cpp | 49 ++++++++++--------- 2 files changed, 29 insertions(+), 22 deletions(-) diff --git a/gpio_controllers/include/gpio_controllers/gpio_command_controller.hpp b/gpio_controllers/include/gpio_controllers/gpio_command_controller.hpp index 2202ffeabe..b355984d2e 100644 --- a/gpio_controllers/include/gpio_controllers/gpio_command_controller.hpp +++ b/gpio_controllers/include/gpio_controllers/gpio_command_controller.hpp @@ -67,6 +67,8 @@ class GpioCommandController : public controller_interface::ControllerInterface void store_interface_types(); void initialize_gpio_state_msg(); CallbackReturn validate_configured_interfaces(); + void update_gpios_states(); + controller_interface::return_type update_gpios_commands(); protected: std::vector interface_types_; diff --git a/gpio_controllers/src/gpio_command_controller.cpp b/gpio_controllers/src/gpio_command_controller.cpp index 6bfcc56cf0..78bd152d37 100644 --- a/gpio_controllers/src/gpio_command_controller.cpp +++ b/gpio_controllers/src/gpio_command_controller.cpp @@ -155,27 +155,13 @@ CallbackReturn GpioCommandController::on_deactivate(const rclcpp_lifecycle::Stat controller_interface::return_type GpioCommandController::update( const rclcpp::Time &, const rclcpp::Duration &) { - if (realtime_gpio_state_publisher_ && realtime_gpio_state_publisher_->trylock()) - { - auto & gpio_state_msg = realtime_gpio_state_publisher_->msg_; - gpio_state_msg.header.stamp = get_node()->now(); - - auto sindex = 0ul; - for (auto g = 0ul; g < params_.gpios.size(); g++) - { - for (auto i = 0ul; i < params_.command_interfaces.gpios_map[params_.gpios[g]].ports.size(); - i++) - { - gpio_state_msg.interface_values[g].values[i] = state_interfaces_[sindex].get_value(); - sindex++; - } - } - - realtime_gpio_state_publisher_->unlockAndPublish(); - } + update_gpios_states(); + return update_gpios_commands(); +} +controller_interface::return_type GpioCommandController::update_gpios_commands() +{ auto gpio_commands = rt_command_ptr_.readFromRT(); - if (!gpio_commands || !(*gpio_commands)) { return controller_interface::return_type::OK; @@ -190,14 +176,33 @@ controller_interface::return_type GpioCommandController::update( return controller_interface::return_type::ERROR; } - for (size_t cindex = 0; cindex < command_interfaces_.size(); ++cindex) + for (std::size_t command_index = 0; command_index < command_interfaces_.size(); ++command_index) { - command_interfaces_[cindex].set_value((*gpio_commands)->data[cindex]); + command_interfaces_[command_index].set_value((*gpio_commands)->data[command_index]); } - return controller_interface::return_type::OK; } +void GpioCommandController::update_gpios_states() +{ + if (realtime_gpio_state_publisher_ && realtime_gpio_state_publisher_->trylock()) + { + auto & gpio_state_msg = realtime_gpio_state_publisher_->msg_; + gpio_state_msg.header.stamp = get_node()->now(); + + std::size_t sindex = 0; + for (std::size_t g = 0; g < params_.gpios.size(); g++) + { + for (auto & interface_value : gpio_state_msg.interface_values[g].values) + { + interface_value = state_interfaces_[sindex].get_value(); + sindex++; + } + } + realtime_gpio_state_publisher_->unlockAndPublish(); + } +} + } // namespace gpio_controllers #include "pluginlib/class_list_macros.hpp" From 6276617ea9ba366fb28c0473073f20ee74bc1970 Mon Sep 17 00:00:00 2001 From: Wiktor Bajor Date: Sun, 4 Aug 2024 12:10:39 +0200 Subject: [PATCH 19/58] Move test fixture form hpp to cpp --- .../test/test_gpio_command_controller.cpp | 63 +++++++++++------ .../test/test_gpio_command_controller.hpp | 68 ------------------- 2 files changed, 43 insertions(+), 88 deletions(-) delete mode 100644 gpio_controllers/test/test_gpio_command_controller.hpp diff --git a/gpio_controllers/test/test_gpio_command_controller.cpp b/gpio_controllers/test/test_gpio_command_controller.cpp index 2b63a1c553..ca8614ce44 100644 --- a/gpio_controllers/test/test_gpio_command_controller.cpp +++ b/gpio_controllers/test/test_gpio_command_controller.cpp @@ -20,18 +20,58 @@ #include #include +#include "gmock/gmock.h" +#include "gpio_controllers/gpio_command_controller.hpp" +#include "hardware_interface/handle.hpp" #include "hardware_interface/loaned_command_interface.hpp" #include "hardware_interface/types/hardware_interface_return_values.hpp" +#include "hardware_interface/types/hardware_interface_type_values.hpp" #include "lifecycle_msgs/msg/state.hpp" #include "rclcpp/utilities.hpp" #include "rclcpp_lifecycle/node_interfaces/lifecycle_node_interface.hpp" -#include "test_gpio_command_controller.hpp" using CallbackReturn = controller_interface::CallbackReturn; using hardware_interface::LoanedCommandInterface; using hardware_interface::LoanedStateInterface; using CmdType = std_msgs::msg::Float64MultiArray; using StateType = control_msgs::msg::DynamicJointState; +using hardware_interface::CommandInterface; +using hardware_interface::StateInterface; + +class FriendGpioCommandController : public gpio_controllers::GpioCommandController +{ + FRIEND_TEST(GpioCommandControllerTest, CommandSuccessTest); + FRIEND_TEST(GpioCommandControllerTest, WrongCommandCheckTest); + FRIEND_TEST(GpioCommandControllerTest, CommandCallbackTest); +}; + +class GpioCommandControllerTest : public ::testing::Test +{ +public: + static void SetUpTestCase(); + static void TearDownTestCase(); + + void SetUp(); + void TearDown(); + + void SetUpController(); + void SetUpHandles(); + +protected: + std::unique_ptr controller_; + + const std::vector gpio_names_ = {"gpio1", "gpio2"}; + std::vector gpio_commands_ = {1.0, 0.0, 3.1}; + std::vector gpio_states_ = {1.0, 0.0, 3.1}; + + CommandInterface gpio_1_1_dig_cmd_{gpio_names_[0], "dig.1", &gpio_commands_[0]}; + CommandInterface gpio_1_2_dig_cmd_{gpio_names_[0], "dig.2", &gpio_commands_[1]}; + CommandInterface gpio_2_ana_cmd_{gpio_names_[1], "ana.1", &gpio_commands_[2]}; + + StateInterface gpio_1_1_dig_state_{gpio_names_[0], "dig.1", &gpio_states_[0]}; + StateInterface gpio_1_2_dig_state_{gpio_names_[0], "dig.2", &gpio_states_[1]}; + StateInterface gpio_2_ana_state_{gpio_names_[1], "ana.1", &gpio_states_[2]}; +}; void GpioCommandControllerTest::SetUpTestCase() { rclcpp::init(0, nullptr); } @@ -158,27 +198,22 @@ TEST_F(GpioCommandControllerTest, CommandSuccessTest) ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), CallbackReturn::SUCCESS); ASSERT_EQ(controller_->on_activate(rclcpp_lifecycle::State()), CallbackReturn::SUCCESS); - // update successful though no command has been send yet ASSERT_EQ( controller_->update(rclcpp::Time(0), rclcpp::Duration::from_seconds(0.01)), controller_interface::return_type::OK); - // check gpio commands are still the default ones ASSERT_EQ(gpio_1_1_dig_cmd_.get_value(), 1.0); ASSERT_EQ(gpio_1_2_dig_cmd_.get_value(), 0.0); ASSERT_EQ(gpio_2_ana_cmd_.get_value(), 3.1); - // send command auto command_ptr = std::make_shared(); command_ptr->data = {0.0, 1.0, 30.0}; controller_->rt_command_ptr_.writeFromNonRT(command_ptr); - // update successful, command received ASSERT_EQ( controller_->update(rclcpp::Time(0), rclcpp::Duration::from_seconds(0.01)), controller_interface::return_type::OK); - // check gpio commands have been modified ASSERT_EQ(gpio_1_1_dig_cmd_.get_value(), 0.0); ASSERT_EQ(gpio_1_2_dig_cmd_.get_value(), 1.0); ASSERT_EQ(gpio_2_ana_cmd_.get_value(), 30.0); @@ -210,17 +245,14 @@ TEST_F(GpioCommandControllerTest, WrongCommandCheckTest) ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), CallbackReturn::SUCCESS); ASSERT_EQ(controller_->on_activate(rclcpp_lifecycle::State()), CallbackReturn::SUCCESS); - // send command with wrong number of gpios auto command_ptr = std::make_shared(); command_ptr->data = {0.0, 20.0}; controller_->rt_command_ptr_.writeFromNonRT(command_ptr); - // update failed, command size does not match number of gpios ASSERT_EQ( controller_->update(rclcpp::Time(0), rclcpp::Duration::from_seconds(0.01)), controller_interface::return_type::ERROR); - // check gpio commands are still the default ones ASSERT_EQ(gpio_1_1_dig_cmd_.get_value(), 1.0); ASSERT_EQ(gpio_1_2_dig_cmd_.get_value(), 0.0); ASSERT_EQ(gpio_2_ana_cmd_.get_value(), 3.1); @@ -253,12 +285,10 @@ TEST_F(GpioCommandControllerTest, NoCommandCheckTest) ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), CallbackReturn::SUCCESS); ASSERT_EQ(controller_->on_activate(rclcpp_lifecycle::State()), CallbackReturn::SUCCESS); - // update successful, no command received yet ASSERT_EQ( controller_->update(rclcpp::Time(0), rclcpp::Duration::from_seconds(0.01)), controller_interface::return_type::OK); - // check gpio commands are still the default ones ASSERT_EQ(gpio_1_1_dig_cmd_.get_value(), 1.0); ASSERT_EQ(gpio_1_2_dig_cmd_.get_value(), 0.0); ASSERT_EQ(gpio_2_ana_cmd_.get_value(), 3.1); @@ -303,7 +333,6 @@ TEST_F(GpioCommandControllerTest, CommandCallbackTest) node_state = controller_->get_node()->activate(); ASSERT_EQ(node_state.id(), lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE); - // send a new command rclcpp::Node test_node("test_node"); auto command_pub = test_node.create_publisher( std::string(controller_->get_node()->get_name()) + "/commands", rclcpp::SystemDefaultsQoS()); @@ -355,7 +384,6 @@ TEST_F(GpioCommandControllerTest, StateCallbackTest) ASSERT_EQ(result, controller_interface::return_type::OK); - // default values ASSERT_EQ(gpio_1_1_dig_cmd_.get_value(), 1.0); ASSERT_EQ(gpio_1_2_dig_cmd_.get_value(), 0.0); ASSERT_EQ(gpio_2_ana_cmd_.get_value(), 3.1); @@ -370,21 +398,17 @@ TEST_F(GpioCommandControllerTest, StateCallbackTest) node_state = controller_->get_node()->activate(); ASSERT_EQ(node_state.id(), lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE); - // send a new command rclcpp::Node test_node("test_node"); auto subs_callback = [&](const StateType::SharedPtr) {}; auto subscription = test_node.create_subscription( std::string(controller_->get_node()->get_name()) + "/gpio_states", 10, subs_callback); - // call update to publish the test value - // since update doesn't guarantee a published message, republish until received - int max_sub_check_loop_count = 5; // max number of tries for pub/sub loop - rclcpp::WaitSet wait_set; // block used to wait on message + int max_sub_check_loop_count = 5; + rclcpp::WaitSet wait_set; wait_set.add_subscription(subscription); while (max_sub_check_loop_count--) { controller_->update(rclcpp::Time(0), rclcpp::Duration::from_seconds(0.01)); - // check if message has been received if (wait_set.wait(std::chrono::milliseconds(2)).kind() == rclcpp::WaitResultKind::Ready) { break; @@ -393,7 +417,6 @@ TEST_F(GpioCommandControllerTest, StateCallbackTest) ASSERT_GE(max_sub_check_loop_count, 0) << "Test was unable to publish a message through " "controller/broadcaster update loop"; - // take message from subscription StateType gpio_state_msg; rclcpp::MessageInfo msg_info; ASSERT_TRUE(subscription->take(gpio_state_msg, msg_info)); diff --git a/gpio_controllers/test/test_gpio_command_controller.hpp b/gpio_controllers/test/test_gpio_command_controller.hpp deleted file mode 100644 index d7f8e67a11..0000000000 --- a/gpio_controllers/test/test_gpio_command_controller.hpp +++ /dev/null @@ -1,68 +0,0 @@ -// Copyright 2022 ICUBE Laboratory, University of Strasbourg -// -// Licensed under the Apache License, Version 2.0 (the "License"); -// you may not use this file except in compliance with the License. -// You may obtain a copy of the License at -// -// http://www.apache.org/licenses/LICENSE-2.0 -// -// Unless required by applicable law or agreed to in writing, software -// distributed under the License is distributed on an "AS IS" BASIS, -// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -// See the License for the specific language governing permissions and -// limitations under the License. - -#ifndef TEST_GPIO_COMMAND_CONTROLLER_HPP_ -#define TEST_GPIO_COMMAND_CONTROLLER_HPP_ - -#include -#include -#include - -#include "gmock/gmock.h" - -#include "gpio_controllers/gpio_command_controller.hpp" -#include "hardware_interface/handle.hpp" -#include "hardware_interface/types/hardware_interface_type_values.hpp" - -using hardware_interface::CommandInterface; -using hardware_interface::StateInterface; - -// subclassing and friending so we can access member variables -class FriendGpioCommandController : public gpio_controllers::GpioCommandController -{ - FRIEND_TEST(GpioCommandControllerTest, CommandSuccessTest); - FRIEND_TEST(GpioCommandControllerTest, WrongCommandCheckTest); - FRIEND_TEST(GpioCommandControllerTest, CommandCallbackTest); -}; - -class GpioCommandControllerTest : public ::testing::Test -{ -public: - static void SetUpTestCase(); - static void TearDownTestCase(); - - void SetUp(); - void TearDown(); - - void SetUpController(); - void SetUpHandles(); - -protected: - std::unique_ptr controller_; - - // dummy gpio state values used for tests - const std::vector gpio_names_ = {"gpio1", "gpio2"}; - std::vector gpio_commands_ = {1.0, 0.0, 3.1}; - std::vector gpio_states_ = {1.0, 0.0, 3.1}; - - CommandInterface gpio_1_1_dig_cmd_{gpio_names_[0], "dig.1", &gpio_commands_[0]}; - CommandInterface gpio_1_2_dig_cmd_{gpio_names_[0], "dig.2", &gpio_commands_[1]}; - CommandInterface gpio_2_ana_cmd_{gpio_names_[1], "ana.1", &gpio_commands_[2]}; - - StateInterface gpio_1_1_dig_state_{gpio_names_[0], "dig.1", &gpio_states_[0]}; - StateInterface gpio_1_2_dig_state_{gpio_names_[0], "dig.2", &gpio_states_[1]}; - StateInterface gpio_2_ana_state_{gpio_names_[1], "ana.1", &gpio_states_[2]}; -}; - -#endif // TEST_GPIO_COMMAND_CONTROLLER_HPP_ From 66bd9711a2820a436e50c76fca8ea895767b268b Mon Sep 17 00:00:00 2001 From: Wiktor Bajor Date: Sun, 4 Aug 2024 12:16:10 +0200 Subject: [PATCH 20/58] Update docks and package.xml --- gpio_controllers/doc/userdoc.rst | 12 +++++++----- gpio_controllers/package.xml | 1 + 2 files changed, 8 insertions(+), 5 deletions(-) diff --git a/gpio_controllers/doc/userdoc.rst b/gpio_controllers/doc/userdoc.rst index 7bf8d8b75b..442132b977 100644 --- a/gpio_controllers/doc/userdoc.rst +++ b/gpio_controllers/doc/userdoc.rst @@ -31,9 +31,11 @@ A yaml file for using it could be: - Gpio2 command_interfaces: Gpio1: - - dig.1 - - dig.2 - - dig.3 + - ports: + - dig.1 + - dig.2 + - dig.3 Gpio2: - - ana.1 - - ana.2 + -ports: + - ana.1 + - ana.2 diff --git a/gpio_controllers/package.xml b/gpio_controllers/package.xml index 7837b8b1c8..93a3dcb721 100644 --- a/gpio_controllers/package.xml +++ b/gpio_controllers/package.xml @@ -5,6 +5,7 @@ 0.0.0 Controllers to interact with gpios. Maciej Bednarczyk + Wiktor Bajor Apache License 2.0 ament_cmake From 7b70159aa062c0cb7c457344570553243e181361 Mon Sep 17 00:00:00 2001 From: Wiktor Bajor Date: Sun, 4 Aug 2024 12:29:35 +0200 Subject: [PATCH 21/58] Add state validation --- gpio_controllers/src/gpio_command_controller.cpp | 16 ++++++++++++++++ 1 file changed, 16 insertions(+) diff --git a/gpio_controllers/src/gpio_command_controller.cpp b/gpio_controllers/src/gpio_command_controller.cpp index 78bd152d37..fd9c62a299 100644 --- a/gpio_controllers/src/gpio_command_controller.cpp +++ b/gpio_controllers/src/gpio_command_controller.cpp @@ -24,6 +24,7 @@ namespace gpio_controllers { using hardware_interface::LoanedCommandInterface; +using hardware_interface::LoanedStateInterface; GpioCommandController::GpioCommandController() : controller_interface::ControllerInterface() {} @@ -122,6 +123,21 @@ CallbackReturn GpioCommandController::validate_configured_interfaces() return CallbackReturn::ERROR; } + std::vector> state_interface_order; + if ( + !controller_interface::get_ordered_interfaces( + state_interfaces_, interface_types_, std::string(""), state_interface_order) || + state_interfaces_.size() != state_interface_order.size()) + { + RCLCPP_ERROR( + get_node()->get_logger(), "Expected %zu state interfaces, got %zu", interface_types_.size(), + ordered_interfaces.size()); + + for (const auto & interface : interface_types_) + RCLCPP_ERROR(get_node()->get_logger(), "Got %s", interface.c_str()); + return CallbackReturn::ERROR; + } + return CallbackReturn::SUCCESS; } From 58c1afc6ad156dbb8bc16cfb3ba189d51d4b291f Mon Sep 17 00:00:00 2001 From: Wiktor Bajor Date: Mon, 5 Aug 2024 21:49:04 +0200 Subject: [PATCH 22/58] Store command and state ifs in the map --- .../gpio_command_controller.hpp | 17 ++++- .../src/gpio_command_controller.cpp | 75 +++++++++++-------- 2 files changed, 61 insertions(+), 31 deletions(-) diff --git a/gpio_controllers/include/gpio_controllers/gpio_command_controller.hpp b/gpio_controllers/include/gpio_controllers/gpio_command_controller.hpp index b355984d2e..ca5e75c538 100644 --- a/gpio_controllers/include/gpio_controllers/gpio_command_controller.hpp +++ b/gpio_controllers/include/gpio_controllers/gpio_command_controller.hpp @@ -17,6 +17,7 @@ #include #include +#include #include #include "control_msgs/msg/dynamic_joint_state.hpp" @@ -34,6 +35,11 @@ namespace gpio_controllers using CmdType = std_msgs::msg::Float64MultiArray; using StateType = control_msgs::msg::DynamicJointState; using CallbackReturn = controller_interface::CallbackReturn; +using InterfacesNames = std::vector; +template +using MapOfReferencesToInterfaces = std::unordered_map>; +template +using LoanedInterfaces = std::vector; class GpioCommandController : public controller_interface::ControllerInterface { @@ -66,12 +72,21 @@ class GpioCommandController : public controller_interface::ControllerInterface private: void store_interface_types(); void initialize_gpio_state_msg(); - CallbackReturn validate_configured_interfaces(); void update_gpios_states(); controller_interface::return_type update_gpios_commands(); + template + MapOfReferencesToInterfaces create_map_of_references_to_interfaces( + const InterfacesNames & interfaces_from_params, LoanedInterfaces & configured_interfaces); + template + bool check_if_configured_interfaces_matches_received( + const InterfacesNames & interfaces_from_params, + const MapOfReferencesToInterfaces & interfaces_map); protected: std::vector interface_types_; + MapOfReferencesToInterfaces command_interfaces_map_; + MapOfReferencesToInterfaces state_interfaces_map_; + realtime_tools::RealtimeBuffer> rt_command_ptr_{}; rclcpp::Subscription::SharedPtr gpios_command_subscriber_{}; diff --git a/gpio_controllers/src/gpio_command_controller.cpp b/gpio_controllers/src/gpio_command_controller.cpp index fd9c62a299..5c99e32d9b 100644 --- a/gpio_controllers/src/gpio_command_controller.cpp +++ b/gpio_controllers/src/gpio_command_controller.cpp @@ -23,8 +23,6 @@ namespace gpio_controllers { -using hardware_interface::LoanedCommandInterface; -using hardware_interface::LoanedStateInterface; GpioCommandController::GpioCommandController() : controller_interface::ControllerInterface() {} @@ -95,7 +93,14 @@ controller_interface::InterfaceConfiguration GpioCommandController::state_interf CallbackReturn GpioCommandController::on_activate(const rclcpp_lifecycle::State &) { - if (validate_configured_interfaces() != CallbackReturn::SUCCESS) + command_interfaces_map_ = + create_map_of_references_to_interfaces(interface_types_, command_interfaces_); + state_interfaces_map_ = + create_map_of_references_to_interfaces(interface_types_, state_interfaces_); + + if ( + !check_if_configured_interfaces_matches_received(interface_types_, command_interfaces_map_) || + !check_if_configured_interfaces_matches_received(interface_types_, state_interfaces_map_)) { return CallbackReturn::ERROR; } @@ -106,39 +111,49 @@ CallbackReturn GpioCommandController::on_activate(const rclcpp_lifecycle::State return CallbackReturn::SUCCESS; } -CallbackReturn GpioCommandController::validate_configured_interfaces() +template +MapOfReferencesToInterfaces GpioCommandController::create_map_of_references_to_interfaces( + const InterfacesNames & interfaces_from_params, LoanedInterfaces & configured_interfaces) { - std::vector> ordered_interfaces; - if ( - !controller_interface::get_ordered_interfaces( - command_interfaces_, interface_types_, std::string(""), ordered_interfaces) || - command_interfaces_.size() != ordered_interfaces.size()) + MapOfReferencesToInterfaces map; + for (const auto & interface_name : interfaces_from_params) { - RCLCPP_ERROR( - get_node()->get_logger(), "Expected %zu command interfaces, got %zu", interface_types_.size(), - ordered_interfaces.size()); - - for (const auto & interface : interface_types_) - RCLCPP_ERROR(get_node()->get_logger(), "Got %s", interface.c_str()); - return CallbackReturn::ERROR; + auto interface = std::find_if( + configured_interfaces.begin(), configured_interfaces.end(), + [&](const auto & configured_interface) + { + const auto full_name_interface_name = configured_interface.get_name(); + return full_name_interface_name == interface_name; + }); + if (interface != configured_interfaces.end()) + { + map.emplace(interface_name, std::ref(*interface)); + } } + return map; +} - std::vector> state_interface_order; - if ( - !controller_interface::get_ordered_interfaces( - state_interfaces_, interface_types_, std::string(""), state_interface_order) || - state_interfaces_.size() != state_interface_order.size()) +template +bool GpioCommandController::check_if_configured_interfaces_matches_received( + const InterfacesNames & interfaces_from_params, + const MapOfReferencesToInterfaces & interfaces_map) +{ + if (!(interfaces_map.size() == interfaces_from_params.size())) { - RCLCPP_ERROR( - get_node()->get_logger(), "Expected %zu state interfaces, got %zu", interface_types_.size(), - ordered_interfaces.size()); - - for (const auto & interface : interface_types_) - RCLCPP_ERROR(get_node()->get_logger(), "Got %s", interface.c_str()); - return CallbackReturn::ERROR; + RCLCPP_ERROR_STREAM( + get_node()->get_logger(), + "Expected " << interfaces_from_params.size() << " interfaces, got " << interfaces_map.size()); + for (const auto & interface : interfaces_from_params) + { + RCLCPP_ERROR_STREAM(get_node()->get_logger(), "Expected " << interface); + } + for (const auto & [interface, value] : interfaces_map) + { + RCLCPP_ERROR_STREAM(get_node()->get_logger(), "Got " << interface); + } + return false; } - - return CallbackReturn::SUCCESS; + return true; } void GpioCommandController::initialize_gpio_state_msg() From e5c65dbe49e899ad98964a0283a0a099b4810e78 Mon Sep 17 00:00:00 2001 From: Wiktor Bajor Date: Mon, 5 Aug 2024 22:45:45 +0200 Subject: [PATCH 23/58] Use interface maps for command and state update. Use DynamicJointState as cmd type --- .../gpio_command_controller.hpp | 3 +- .../src/gpio_command_controller.cpp | 52 ++++++++++++------- .../test/test_gpio_command_controller.cpp | 40 ++++++++++---- 3 files changed, 66 insertions(+), 29 deletions(-) diff --git a/gpio_controllers/include/gpio_controllers/gpio_command_controller.hpp b/gpio_controllers/include/gpio_controllers/gpio_command_controller.hpp index ca5e75c538..1dba8bb417 100644 --- a/gpio_controllers/include/gpio_controllers/gpio_command_controller.hpp +++ b/gpio_controllers/include/gpio_controllers/gpio_command_controller.hpp @@ -28,11 +28,10 @@ #include "rclcpp_lifecycle/state.hpp" #include "realtime_tools/realtime_buffer.h" #include "realtime_tools/realtime_publisher.h" -#include "std_msgs/msg/float64_multi_array.hpp" namespace gpio_controllers { -using CmdType = std_msgs::msg::Float64MultiArray; +using CmdType = control_msgs::msg::DynamicJointState; using StateType = control_msgs::msg::DynamicJointState; using CallbackReturn = controller_interface::CallbackReturn; using InterfacesNames = std::vector; diff --git a/gpio_controllers/src/gpio_command_controller.cpp b/gpio_controllers/src/gpio_command_controller.cpp index 5c99e32d9b..5f8b0a4ba5 100644 --- a/gpio_controllers/src/gpio_command_controller.cpp +++ b/gpio_controllers/src/gpio_command_controller.cpp @@ -192,24 +192,35 @@ controller_interface::return_type GpioCommandController::update( controller_interface::return_type GpioCommandController::update_gpios_commands() { - auto gpio_commands = rt_command_ptr_.readFromRT(); - if (!gpio_commands || !(*gpio_commands)) + auto gpio_commands_ptr = rt_command_ptr_.readFromRT(); + if (!gpio_commands_ptr || !(*gpio_commands_ptr)) { return controller_interface::return_type::OK; } - if ((*gpio_commands)->data.size() != command_interfaces_.size()) + const auto gpio_commands = *(*gpio_commands_ptr); + for (std::size_t gpio_index = 0; gpio_index < gpio_commands.joint_names.size(); ++gpio_index) { - RCLCPP_ERROR_THROTTLE( - get_node()->get_logger(), *get_node()->get_clock(), 1000, - "command size (%zu) does not match number of interfaces (%zu)", (*gpio_commands)->data.size(), - command_interfaces_.size()); - return controller_interface::return_type::ERROR; - } - - for (std::size_t command_index = 0; command_index < command_interfaces_.size(); ++command_index) - { - command_interfaces_[command_index].set_value((*gpio_commands)->data[command_index]); + const auto & gpio_name = gpio_commands.joint_names[gpio_index]; + for (std::size_t command_interface_index = 0; + command_interface_index < gpio_commands.interface_values[gpio_index].values.size(); + ++command_interface_index) + { + const auto & full_command_interface_name = + gpio_name + '/' + + gpio_commands.interface_values[gpio_index].interface_names[command_interface_index]; + try + { + command_interfaces_map_.at(full_command_interface_name) + .get() + .set_value(gpio_commands.interface_values[gpio_index].values[command_interface_index]); + } + catch (const std::exception & e) + { + fprintf( + stderr, "Exception thrown during applying command stage with message: %s \n", e.what()); + } + } } return controller_interface::return_type::OK; } @@ -221,13 +232,18 @@ void GpioCommandController::update_gpios_states() auto & gpio_state_msg = realtime_gpio_state_publisher_->msg_; gpio_state_msg.header.stamp = get_node()->now(); - std::size_t sindex = 0; - for (std::size_t g = 0; g < params_.gpios.size(); g++) + for (std::size_t gpio_index = 0; gpio_index < gpio_state_msg.joint_names.size(); ++gpio_index) { - for (auto & interface_value : gpio_state_msg.interface_values[g].values) + const auto & gpio_name = gpio_state_msg.joint_names[gpio_index]; + for (std::size_t interface_index = 0; + interface_index < gpio_state_msg.interface_values[gpio_index].interface_names.size(); + ++interface_index) { - interface_value = state_interfaces_[sindex].get_value(); - sindex++; + const auto & full_state_interface_name = + gpio_name + '/' + + gpio_state_msg.interface_values[gpio_index].interface_names[interface_index]; + gpio_state_msg.interface_values[gpio_index].values[interface_index] = + state_interfaces_map_.at(full_state_interface_name).get().get_value(); } } realtime_gpio_state_publisher_->unlockAndPublish(); diff --git a/gpio_controllers/test/test_gpio_command_controller.cpp b/gpio_controllers/test/test_gpio_command_controller.cpp index ca8614ce44..bf84ab6a3d 100644 --- a/gpio_controllers/test/test_gpio_command_controller.cpp +++ b/gpio_controllers/test/test_gpio_command_controller.cpp @@ -33,7 +33,7 @@ using CallbackReturn = controller_interface::CallbackReturn; using hardware_interface::LoanedCommandInterface; using hardware_interface::LoanedStateInterface; -using CmdType = std_msgs::msg::Float64MultiArray; +using CmdType = control_msgs::msg::DynamicJointState; using StateType = control_msgs::msg::DynamicJointState; using hardware_interface::CommandInterface; using hardware_interface::StateInterface; @@ -41,7 +41,7 @@ using hardware_interface::StateInterface; class FriendGpioCommandController : public gpio_controllers::GpioCommandController { FRIEND_TEST(GpioCommandControllerTest, CommandSuccessTest); - FRIEND_TEST(GpioCommandControllerTest, WrongCommandCheckTest); + FRIEND_TEST(GpioCommandControllerTest, CommandSuccessTestWithOnlyOneGpio); FRIEND_TEST(GpioCommandControllerTest, CommandCallbackTest); }; @@ -207,7 +207,15 @@ TEST_F(GpioCommandControllerTest, CommandSuccessTest) ASSERT_EQ(gpio_2_ana_cmd_.get_value(), 3.1); auto command_ptr = std::make_shared(); - command_ptr->data = {0.0, 1.0, 30.0}; + control_msgs::msg::InterfaceValue inteface_value_gpio1; + inteface_value_gpio1.interface_names = {"dig.1", "dig.2"}; + inteface_value_gpio1.values = {0.0, 1.0}; + control_msgs::msg::InterfaceValue inteface_value_gpio2; + inteface_value_gpio2.interface_names = {"ana.1"}; + inteface_value_gpio2.values = {30.0}; + + command_ptr->joint_names = {"gpio1", "gpio2"}; + command_ptr->interface_values = {inteface_value_gpio1, inteface_value_gpio2}; controller_->rt_command_ptr_.writeFromNonRT(command_ptr); ASSERT_EQ( @@ -219,7 +227,7 @@ TEST_F(GpioCommandControllerTest, CommandSuccessTest) ASSERT_EQ(gpio_2_ana_cmd_.get_value(), 30.0); } -TEST_F(GpioCommandControllerTest, WrongCommandCheckTest) +TEST_F(GpioCommandControllerTest, CommandSuccessTestWithOnlyOneGpio) { std::vector parameters; parameters.emplace_back("gpios", std::vector{"gpio1", "gpio2"}); @@ -246,15 +254,20 @@ TEST_F(GpioCommandControllerTest, WrongCommandCheckTest) ASSERT_EQ(controller_->on_activate(rclcpp_lifecycle::State()), CallbackReturn::SUCCESS); auto command_ptr = std::make_shared(); - command_ptr->data = {0.0, 20.0}; + control_msgs::msg::InterfaceValue inteface_value_gpio1; + inteface_value_gpio1.interface_names = {"dig.1", "dig.2"}; + inteface_value_gpio1.values = {0.0, 1.0}; + + command_ptr->joint_names = {"gpio1"}; + command_ptr->interface_values = {inteface_value_gpio1}; controller_->rt_command_ptr_.writeFromNonRT(command_ptr); ASSERT_EQ( controller_->update(rclcpp::Time(0), rclcpp::Duration::from_seconds(0.01)), - controller_interface::return_type::ERROR); + controller_interface::return_type::OK); - ASSERT_EQ(gpio_1_1_dig_cmd_.get_value(), 1.0); - ASSERT_EQ(gpio_1_2_dig_cmd_.get_value(), 0.0); + ASSERT_EQ(gpio_1_1_dig_cmd_.get_value(), 0.0); + ASSERT_EQ(gpio_1_2_dig_cmd_.get_value(), 1.0); ASSERT_EQ(gpio_2_ana_cmd_.get_value(), 3.1); } @@ -337,7 +350,16 @@ TEST_F(GpioCommandControllerTest, CommandCallbackTest) auto command_pub = test_node.create_publisher( std::string(controller_->get_node()->get_name()) + "/commands", rclcpp::SystemDefaultsQoS()); CmdType command_msg; - command_msg.data = {0.0, 1.0, 30.0}; + control_msgs::msg::InterfaceValue inteface_value_gpio1; + inteface_value_gpio1.interface_names = {"dig.1", "dig.2"}; + inteface_value_gpio1.values = {0.0, 1.0}; + control_msgs::msg::InterfaceValue inteface_value_gpio2; + inteface_value_gpio2.interface_names = {"ana.1"}; + inteface_value_gpio2.values = {30.0}; + + command_msg.joint_names = {"gpio1", "gpio2"}; + command_msg.interface_values = {inteface_value_gpio1, inteface_value_gpio2}; + command_pub->publish(command_msg); rclcpp::executors::SingleThreadedExecutor executor; From 0dc5c87a79c38b448a0fd37dd5904d1f6b24b45d Mon Sep 17 00:00:00 2001 From: Wiktor Bajor Date: Tue, 6 Aug 2024 20:01:35 +0200 Subject: [PATCH 24/58] Use ordered state if vector instead of map --- .../gpio_controllers/gpio_command_controller.hpp | 5 +++-- gpio_controllers/src/gpio_command_controller.cpp | 15 ++++++--------- 2 files changed, 9 insertions(+), 11 deletions(-) diff --git a/gpio_controllers/include/gpio_controllers/gpio_command_controller.hpp b/gpio_controllers/include/gpio_controllers/gpio_command_controller.hpp index 1dba8bb417..3a9e57e628 100644 --- a/gpio_controllers/include/gpio_controllers/gpio_command_controller.hpp +++ b/gpio_controllers/include/gpio_controllers/gpio_command_controller.hpp @@ -82,9 +82,10 @@ class GpioCommandController : public controller_interface::ControllerInterface const MapOfReferencesToInterfaces & interfaces_map); protected: - std::vector interface_types_; + InterfacesNames interface_types_; MapOfReferencesToInterfaces command_interfaces_map_; - MapOfReferencesToInterfaces state_interfaces_map_; + std::vector> + ordered_state_interfaces_; realtime_tools::RealtimeBuffer> rt_command_ptr_{}; rclcpp::Subscription::SharedPtr gpios_command_subscriber_{}; diff --git a/gpio_controllers/src/gpio_command_controller.cpp b/gpio_controllers/src/gpio_command_controller.cpp index 5f8b0a4ba5..16a2da134a 100644 --- a/gpio_controllers/src/gpio_command_controller.cpp +++ b/gpio_controllers/src/gpio_command_controller.cpp @@ -95,12 +95,10 @@ CallbackReturn GpioCommandController::on_activate(const rclcpp_lifecycle::State { command_interfaces_map_ = create_map_of_references_to_interfaces(interface_types_, command_interfaces_); - state_interfaces_map_ = - create_map_of_references_to_interfaces(interface_types_, state_interfaces_); + controller_interface::get_ordered_interfaces( + state_interfaces_, interface_types_, "", ordered_state_interfaces_); - if ( - !check_if_configured_interfaces_matches_received(interface_types_, command_interfaces_map_) || - !check_if_configured_interfaces_matches_received(interface_types_, state_interfaces_map_)) + if (!check_if_configured_interfaces_matches_received(interface_types_, command_interfaces_map_)) { return CallbackReturn::ERROR; } @@ -232,6 +230,7 @@ void GpioCommandController::update_gpios_states() auto & gpio_state_msg = realtime_gpio_state_publisher_->msg_; gpio_state_msg.header.stamp = get_node()->now(); + std::size_t state_index{}; for (std::size_t gpio_index = 0; gpio_index < gpio_state_msg.joint_names.size(); ++gpio_index) { const auto & gpio_name = gpio_state_msg.joint_names[gpio_index]; @@ -239,11 +238,9 @@ void GpioCommandController::update_gpios_states() interface_index < gpio_state_msg.interface_values[gpio_index].interface_names.size(); ++interface_index) { - const auto & full_state_interface_name = - gpio_name + '/' + - gpio_state_msg.interface_values[gpio_index].interface_names[interface_index]; gpio_state_msg.interface_values[gpio_index].values[interface_index] = - state_interfaces_map_.at(full_state_interface_name).get().get_value(); + ordered_state_interfaces_[state_index].get().get_value(); + ++state_index; } } realtime_gpio_state_publisher_->unlockAndPublish(); From 765a3cb13ea4a23d84734355c76da031c2051131 Mon Sep 17 00:00:00 2001 From: Wiktor Bajor Date: Tue, 6 Aug 2024 20:33:15 +0200 Subject: [PATCH 25/58] Restore state if validation --- .../gpio_command_controller.hpp | 21 ++++---- .../src/gpio_command_controller.cpp | 54 ++++++++++++------- 2 files changed, 44 insertions(+), 31 deletions(-) diff --git a/gpio_controllers/include/gpio_controllers/gpio_command_controller.hpp b/gpio_controllers/include/gpio_controllers/gpio_command_controller.hpp index 3a9e57e628..975680a663 100644 --- a/gpio_controllers/include/gpio_controllers/gpio_command_controller.hpp +++ b/gpio_controllers/include/gpio_controllers/gpio_command_controller.hpp @@ -35,10 +35,10 @@ using CmdType = control_msgs::msg::DynamicJointState; using StateType = control_msgs::msg::DynamicJointState; using CallbackReturn = controller_interface::CallbackReturn; using InterfacesNames = std::vector; -template -using MapOfReferencesToInterfaces = std::unordered_map>; -template -using LoanedInterfaces = std::vector; +using MapOfReferencesToCommandInterfaces = std::unordered_map< + std::string, std::reference_wrapper>; +using StateInterfaces = + std::vector>; class GpioCommandController : public controller_interface::ControllerInterface { @@ -73,19 +73,16 @@ class GpioCommandController : public controller_interface::ControllerInterface void initialize_gpio_state_msg(); void update_gpios_states(); controller_interface::return_type update_gpios_commands(); - template - MapOfReferencesToInterfaces create_map_of_references_to_interfaces( - const InterfacesNames & interfaces_from_params, LoanedInterfaces & configured_interfaces); + MapOfReferencesToCommandInterfaces create_map_of_references_to_interfaces( + const InterfacesNames & interfaces_from_params); template bool check_if_configured_interfaces_matches_received( - const InterfacesNames & interfaces_from_params, - const MapOfReferencesToInterfaces & interfaces_map); + const InterfacesNames & interfaces_from_params, const T & interfaces_map); protected: InterfacesNames interface_types_; - MapOfReferencesToInterfaces command_interfaces_map_; - std::vector> - ordered_state_interfaces_; + MapOfReferencesToCommandInterfaces command_interfaces_map_; + StateInterfaces ordered_state_interfaces_; realtime_tools::RealtimeBuffer> rt_command_ptr_{}; rclcpp::Subscription::SharedPtr gpios_command_subscriber_{}; diff --git a/gpio_controllers/src/gpio_command_controller.cpp b/gpio_controllers/src/gpio_command_controller.cpp index 16a2da134a..f2dbbe98e7 100644 --- a/gpio_controllers/src/gpio_command_controller.cpp +++ b/gpio_controllers/src/gpio_command_controller.cpp @@ -21,6 +21,27 @@ #include "rclcpp/qos.hpp" #include "rclcpp/subscription.hpp" +namespace +{ +void print_interface( + const rclcpp::Logger & logger, const gpio_controllers::StateInterfaces & state_interfaces) +{ + for (const auto & interface : state_interfaces) + { + RCLCPP_ERROR_STREAM(logger, "Got " << interface.get().get_name()); + } +} +void print_interface( + const rclcpp::Logger & logger, + const gpio_controllers::MapOfReferencesToCommandInterfaces & command_interfaces) +{ + for (const auto & [interface_name, value] : command_interfaces) + { + RCLCPP_ERROR_STREAM(logger, "Got " << interface_name); + } +} +} // namespace + namespace gpio_controllers { @@ -93,12 +114,13 @@ controller_interface::InterfaceConfiguration GpioCommandController::state_interf CallbackReturn GpioCommandController::on_activate(const rclcpp_lifecycle::State &) { - command_interfaces_map_ = - create_map_of_references_to_interfaces(interface_types_, command_interfaces_); + command_interfaces_map_ = create_map_of_references_to_interfaces(interface_types_); controller_interface::get_ordered_interfaces( state_interfaces_, interface_types_, "", ordered_state_interfaces_); - if (!check_if_configured_interfaces_matches_received(interface_types_, command_interfaces_map_)) + if ( + !check_if_configured_interfaces_matches_received(interface_types_, command_interfaces_map_) || + !check_if_configured_interfaces_matches_received(interface_types_, ordered_state_interfaces_)) { return CallbackReturn::ERROR; } @@ -109,21 +131,20 @@ CallbackReturn GpioCommandController::on_activate(const rclcpp_lifecycle::State return CallbackReturn::SUCCESS; } -template -MapOfReferencesToInterfaces GpioCommandController::create_map_of_references_to_interfaces( - const InterfacesNames & interfaces_from_params, LoanedInterfaces & configured_interfaces) +MapOfReferencesToCommandInterfaces GpioCommandController::create_map_of_references_to_interfaces( + const InterfacesNames & interfaces_from_params) { - MapOfReferencesToInterfaces map; + MapOfReferencesToCommandInterfaces map; for (const auto & interface_name : interfaces_from_params) { auto interface = std::find_if( - configured_interfaces.begin(), configured_interfaces.end(), + command_interfaces_.begin(), command_interfaces_.end(), [&](const auto & configured_interface) { const auto full_name_interface_name = configured_interface.get_name(); return full_name_interface_name == interface_name; }); - if (interface != configured_interfaces.end()) + if (interface != command_interfaces_.end()) { map.emplace(interface_name, std::ref(*interface)); } @@ -133,22 +154,18 @@ MapOfReferencesToInterfaces GpioCommandController::create_map_of_references_t template bool GpioCommandController::check_if_configured_interfaces_matches_received( - const InterfacesNames & interfaces_from_params, - const MapOfReferencesToInterfaces & interfaces_map) + const InterfacesNames & interfaces_from_params, const T & configured_interfaces) { - if (!(interfaces_map.size() == interfaces_from_params.size())) + if (!(configured_interfaces.size() == interfaces_from_params.size())) { RCLCPP_ERROR_STREAM( - get_node()->get_logger(), - "Expected " << interfaces_from_params.size() << " interfaces, got " << interfaces_map.size()); + get_node()->get_logger(), "Expected " << interfaces_from_params.size() << " interfaces, got " + << configured_interfaces.size()); for (const auto & interface : interfaces_from_params) { RCLCPP_ERROR_STREAM(get_node()->get_logger(), "Expected " << interface); } - for (const auto & [interface, value] : interfaces_map) - { - RCLCPP_ERROR_STREAM(get_node()->get_logger(), "Got " << interface); - } + print_interface(get_node()->get_logger(), configured_interfaces); return false; } return true; @@ -233,7 +250,6 @@ void GpioCommandController::update_gpios_states() std::size_t state_index{}; for (std::size_t gpio_index = 0; gpio_index < gpio_state_msg.joint_names.size(); ++gpio_index) { - const auto & gpio_name = gpio_state_msg.joint_names[gpio_index]; for (std::size_t interface_index = 0; interface_index < gpio_state_msg.interface_values[gpio_index].interface_names.size(); ++interface_index) From a1c8fa35de56ce13f89d642076eee1672cfc2d76 Mon Sep 17 00:00:00 2001 From: Wiktor Bajor Date: Tue, 6 Aug 2024 20:39:14 +0200 Subject: [PATCH 26/58] Align logging --- gpio_controllers/src/gpio_command_controller.cpp | 12 ++++++------ 1 file changed, 6 insertions(+), 6 deletions(-) diff --git a/gpio_controllers/src/gpio_command_controller.cpp b/gpio_controllers/src/gpio_command_controller.cpp index f2dbbe98e7..3fb582a784 100644 --- a/gpio_controllers/src/gpio_command_controller.cpp +++ b/gpio_controllers/src/gpio_command_controller.cpp @@ -28,7 +28,7 @@ void print_interface( { for (const auto & interface : state_interfaces) { - RCLCPP_ERROR_STREAM(logger, "Got " << interface.get().get_name()); + RCLCPP_ERROR(logger, "Got %s", interface.get().get_name().c_str()); } } void print_interface( @@ -37,7 +37,7 @@ void print_interface( { for (const auto & [interface_name, value] : command_interfaces) { - RCLCPP_ERROR_STREAM(logger, "Got " << interface_name); + RCLCPP_ERROR(logger, "Got %s", interface_name.c_str()); } } } // namespace @@ -158,12 +158,12 @@ bool GpioCommandController::check_if_configured_interfaces_matches_received( { if (!(configured_interfaces.size() == interfaces_from_params.size())) { - RCLCPP_ERROR_STREAM( - get_node()->get_logger(), "Expected " << interfaces_from_params.size() << " interfaces, got " - << configured_interfaces.size()); + RCLCPP_ERROR( + get_node()->get_logger(), "Expected %ld interfaces, got %ld", interfaces_from_params.size(), + configured_interfaces.size()); for (const auto & interface : interfaces_from_params) { - RCLCPP_ERROR_STREAM(get_node()->get_logger(), "Expected " << interface); + RCLCPP_ERROR(get_node()->get_logger(), "Expected %s", interface.c_str()); } print_interface(get_node()->get_logger(), configured_interfaces); return false; From de8acbaa685bf45fbe56485ab285c1c5541510b5 Mon Sep 17 00:00:00 2001 From: Wiktor Bajor Date: Wed, 7 Aug 2024 19:44:09 +0200 Subject: [PATCH 27/58] Update code layout --- .../src/gpio_command_controller.cpp | 88 +++++++++---------- 1 file changed, 44 insertions(+), 44 deletions(-) diff --git a/gpio_controllers/src/gpio_command_controller.cpp b/gpio_controllers/src/gpio_command_controller.cpp index 3fb582a784..82a1689b5a 100644 --- a/gpio_controllers/src/gpio_command_controller.cpp +++ b/gpio_controllers/src/gpio_command_controller.cpp @@ -82,16 +82,6 @@ catch (const std::exception & e) return CallbackReturn::ERROR; } -void GpioCommandController::store_interface_types() -{ - for (const auto & [gpio_name, ports] : params_.command_interfaces.gpios_map) - { - std::transform( - ports.ports.begin(), ports.ports.end(), std::back_inserter(interface_types_), - [&](const auto & interface_name) { return gpio_name + "/" + interface_name; }); - } -} - controller_interface::InterfaceConfiguration GpioCommandController::command_interface_configuration() const { @@ -131,6 +121,50 @@ CallbackReturn GpioCommandController::on_activate(const rclcpp_lifecycle::State return CallbackReturn::SUCCESS; } +CallbackReturn GpioCommandController::on_deactivate(const rclcpp_lifecycle::State &) +{ + rt_command_ptr_.reset(); + return CallbackReturn::SUCCESS; +} + +controller_interface::return_type GpioCommandController::update( + const rclcpp::Time &, const rclcpp::Duration &) +{ + update_gpios_states(); + return update_gpios_commands(); +} + +void GpioCommandController::store_interface_types() +{ + for (const auto & [gpio_name, ports] : params_.command_interfaces.gpios_map) + { + std::transform( + ports.ports.begin(), ports.ports.end(), std::back_inserter(interface_types_), + [&](const auto & interface_name) { return gpio_name + "/" + interface_name; }); + } +} + +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_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; + std::copy( + params_.command_interfaces.gpios_map[gpio_name].ports.begin(), + params_.command_interfaces.gpios_map[gpio_name].ports.end(), + std::back_insert_iterator(gpio_state_msg.interface_values[gpio_index].interface_names)); + gpio_state_msg.interface_values[gpio_index].values = std::vector( + params_.command_interfaces.gpios_map[gpio_name].ports.size(), + std::numeric_limits::quiet_NaN()); + } +} + MapOfReferencesToCommandInterfaces GpioCommandController::create_map_of_references_to_interfaces( const InterfacesNames & interfaces_from_params) { @@ -171,40 +205,6 @@ bool GpioCommandController::check_if_configured_interfaces_matches_received( return true; } -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_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; - std::copy( - params_.command_interfaces.gpios_map[gpio_name].ports.begin(), - params_.command_interfaces.gpios_map[gpio_name].ports.end(), - std::back_insert_iterator(gpio_state_msg.interface_values[gpio_index].interface_names)); - gpio_state_msg.interface_values[gpio_index].values = std::vector( - params_.command_interfaces.gpios_map[gpio_name].ports.size(), - std::numeric_limits::quiet_NaN()); - } -} - -CallbackReturn GpioCommandController::on_deactivate(const rclcpp_lifecycle::State &) -{ - rt_command_ptr_.reset(); - return CallbackReturn::SUCCESS; -} - -controller_interface::return_type GpioCommandController::update( - const rclcpp::Time &, const rclcpp::Duration &) -{ - update_gpios_states(); - return update_gpios_commands(); -} - controller_interface::return_type GpioCommandController::update_gpios_commands() { auto gpio_commands_ptr = rt_command_ptr_.readFromRT(); From aebbf0b966895fbdd28e187c0f875a911d3a9d03 Mon Sep 17 00:00:00 2001 From: Wiktor Bajor Date: Wed, 7 Aug 2024 19:45:55 +0200 Subject: [PATCH 28/58] Simplifie test fixture --- .../test/test_gpio_command_controller.cpp | 59 ++++++++----------- 1 file changed, 24 insertions(+), 35 deletions(-) diff --git a/gpio_controllers/test/test_gpio_command_controller.cpp b/gpio_controllers/test/test_gpio_command_controller.cpp index bf84ab6a3d..eafd4a73ad 100644 --- a/gpio_controllers/test/test_gpio_command_controller.cpp +++ b/gpio_controllers/test/test_gpio_command_controller.cpp @@ -12,8 +12,6 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include - #include #include #include @@ -40,22 +38,24 @@ using hardware_interface::StateInterface; class FriendGpioCommandController : public gpio_controllers::GpioCommandController { - FRIEND_TEST(GpioCommandControllerTest, CommandSuccessTest); - FRIEND_TEST(GpioCommandControllerTest, CommandSuccessTestWithOnlyOneGpio); - FRIEND_TEST(GpioCommandControllerTest, CommandCallbackTest); + FRIEND_TEST(GpioCommandControllerTestSuite, CommandSuccessTest); + FRIEND_TEST(GpioCommandControllerTestSuite, CommandSuccessTestWithOnlyOneGpio); + FRIEND_TEST(GpioCommandControllerTestSuite, CommandCallbackTest); }; -class GpioCommandControllerTest : public ::testing::Test +class GpioCommandControllerTestSuite : public ::testing::Test { public: - static void SetUpTestCase(); - static void TearDownTestCase(); - - void SetUp(); - void TearDown(); - - void SetUpController(); - void SetUpHandles(); + GpioCommandControllerTestSuite() + { + controller_ = std::make_unique(); + rclcpp::init(0, nullptr); + } + ~GpioCommandControllerTestSuite() + { + rclcpp::shutdown(); + controller_.reset(nullptr); + } protected: std::unique_ptr controller_; @@ -73,25 +73,14 @@ class GpioCommandControllerTest : public ::testing::Test StateInterface gpio_2_ana_state_{gpio_names_[1], "ana.1", &gpio_states_[2]}; }; -void GpioCommandControllerTest::SetUpTestCase() { rclcpp::init(0, nullptr); } - -void GpioCommandControllerTest::TearDownTestCase() { rclcpp::shutdown(); } - -void GpioCommandControllerTest::SetUp() -{ - controller_ = std::make_unique(); -} - -void GpioCommandControllerTest::TearDown() { controller_.reset(nullptr); } - -TEST_F(GpioCommandControllerTest, GpiosParameterNotSet) +TEST_F(GpioCommandControllerTestSuite, GpiosParameterNotSet) { const auto result = controller_->init( "test_gpio_command_controller", "", 0, "", controller_->define_custom_node_options()); ASSERT_EQ(result, controller_interface::return_type::ERROR); } -TEST_F(GpioCommandControllerTest, GpiosParameterIsEmpty) +TEST_F(GpioCommandControllerTestSuite, GpiosParameterIsEmpty) { std::vector parameters; parameters.emplace_back("gpios", std::vector{}); @@ -102,7 +91,7 @@ TEST_F(GpioCommandControllerTest, GpiosParameterIsEmpty) ASSERT_EQ(result, controller_interface::return_type::ERROR); } -TEST_F(GpioCommandControllerTest, GpioWithMissingGpioParams) +TEST_F(GpioCommandControllerTestSuite, GpioWithMissingGpioParams) { std::vector parameters; parameters.emplace_back("gpios", std::vector{"gpio1", "gpio2"}); @@ -116,7 +105,7 @@ TEST_F(GpioCommandControllerTest, GpioWithMissingGpioParams) ASSERT_EQ(result, controller_interface::return_type::ERROR); } -TEST_F(GpioCommandControllerTest, ConfigureAndActivateParamsSuccess) +TEST_F(GpioCommandControllerTestSuite, ConfigureAndActivateParamsSuccess) { std::vector parameters; parameters.emplace_back("gpios", gpio_names_); @@ -144,7 +133,7 @@ TEST_F(GpioCommandControllerTest, ConfigureAndActivateParamsSuccess) ASSERT_EQ(controller_->on_activate(rclcpp_lifecycle::State()), CallbackReturn::SUCCESS); } -TEST_F(GpioCommandControllerTest, ActivateWithWrongGpiosNamesFails) +TEST_F(GpioCommandControllerTestSuite, ActivateWithWrongGpiosNamesFails) { std::vector parameters; parameters.emplace_back("gpios", std::vector{"gpio1", "gpio4"}); @@ -172,7 +161,7 @@ TEST_F(GpioCommandControllerTest, ActivateWithWrongGpiosNamesFails) ASSERT_EQ(controller_->on_activate(rclcpp_lifecycle::State()), CallbackReturn::ERROR); } -TEST_F(GpioCommandControllerTest, CommandSuccessTest) +TEST_F(GpioCommandControllerTestSuite, CommandSuccessTest) { std::vector parameters; parameters.emplace_back("gpios", std::vector{"gpio1", "gpio2"}); @@ -227,7 +216,7 @@ TEST_F(GpioCommandControllerTest, CommandSuccessTest) ASSERT_EQ(gpio_2_ana_cmd_.get_value(), 30.0); } -TEST_F(GpioCommandControllerTest, CommandSuccessTestWithOnlyOneGpio) +TEST_F(GpioCommandControllerTestSuite, CommandSuccessTestWithOnlyOneGpio) { std::vector parameters; parameters.emplace_back("gpios", std::vector{"gpio1", "gpio2"}); @@ -271,7 +260,7 @@ TEST_F(GpioCommandControllerTest, CommandSuccessTestWithOnlyOneGpio) ASSERT_EQ(gpio_2_ana_cmd_.get_value(), 3.1); } -TEST_F(GpioCommandControllerTest, NoCommandCheckTest) +TEST_F(GpioCommandControllerTestSuite, NoCommandCheckTest) { std::vector parameters; parameters.emplace_back("gpios", std::vector{"gpio1", "gpio2"}); @@ -307,7 +296,7 @@ TEST_F(GpioCommandControllerTest, NoCommandCheckTest) ASSERT_EQ(gpio_2_ana_cmd_.get_value(), 3.1); } -TEST_F(GpioCommandControllerTest, CommandCallbackTest) +TEST_F(GpioCommandControllerTestSuite, CommandCallbackTest) { std::vector parameters; parameters.emplace_back("gpios", std::vector{"gpio1", "gpio2"}); @@ -381,7 +370,7 @@ TEST_F(GpioCommandControllerTest, CommandCallbackTest) ASSERT_EQ(gpio_2_ana_cmd_.get_value(), 30.0); } -TEST_F(GpioCommandControllerTest, StateCallbackTest) +TEST_F(GpioCommandControllerTestSuite, StateCallbackTest) { std::vector parameters; parameters.emplace_back("gpios", std::vector{"gpio1", "gpio2"}); From 0deb4177735f0ce4905d36d82b193642a902b8e6 Mon Sep 17 00:00:00 2001 From: Wiktor Bajor Date: Wed, 7 Aug 2024 19:55:15 +0200 Subject: [PATCH 29/58] Update tests. Stop using pointer, set members as public --- .../test/test_gpio_command_controller.cpp | 265 +++++++++--------- 1 file changed, 128 insertions(+), 137 deletions(-) diff --git a/gpio_controllers/test/test_gpio_command_controller.cpp b/gpio_controllers/test/test_gpio_command_controller.cpp index eafd4a73ad..55d1c1612b 100644 --- a/gpio_controllers/test/test_gpio_command_controller.cpp +++ b/gpio_controllers/test/test_gpio_command_controller.cpp @@ -46,37 +46,28 @@ class FriendGpioCommandController : public gpio_controllers::GpioCommandControll class GpioCommandControllerTestSuite : public ::testing::Test { public: - GpioCommandControllerTestSuite() - { - controller_ = std::make_unique(); - rclcpp::init(0, nullptr); - } - ~GpioCommandControllerTestSuite() - { - rclcpp::shutdown(); - controller_.reset(nullptr); - } + GpioCommandControllerTestSuite() { rclcpp::init(0, nullptr); } + ~GpioCommandControllerTestSuite() { rclcpp::shutdown(); } -protected: - std::unique_ptr controller_; + FriendGpioCommandController controller; - const std::vector gpio_names_ = {"gpio1", "gpio2"}; - std::vector gpio_commands_ = {1.0, 0.0, 3.1}; - std::vector gpio_states_ = {1.0, 0.0, 3.1}; + const std::vector gpio_names{"gpio1", "gpio2"}; + std::vector gpio_commands{1.0, 0.0, 3.1}; + std::vector gpio_states{1.0, 0.0, 3.1}; - CommandInterface gpio_1_1_dig_cmd_{gpio_names_[0], "dig.1", &gpio_commands_[0]}; - CommandInterface gpio_1_2_dig_cmd_{gpio_names_[0], "dig.2", &gpio_commands_[1]}; - CommandInterface gpio_2_ana_cmd_{gpio_names_[1], "ana.1", &gpio_commands_[2]}; + CommandInterface gpio_1_1_dig_cmd{gpio_names[0], "dig.1", &gpio_commands[0]}; + CommandInterface gpio_1_2_dig_cmd{gpio_names[0], "dig.2", &gpio_commands[1]}; + CommandInterface gpio_2_ana_cmd{gpio_names[1], "ana.1", &gpio_commands[2]}; - StateInterface gpio_1_1_dig_state_{gpio_names_[0], "dig.1", &gpio_states_[0]}; - StateInterface gpio_1_2_dig_state_{gpio_names_[0], "dig.2", &gpio_states_[1]}; - StateInterface gpio_2_ana_state_{gpio_names_[1], "ana.1", &gpio_states_[2]}; + StateInterface gpio_1_1_dig_state{gpio_names[0], "dig.1", &gpio_states[0]}; + StateInterface gpio_1_2_dig_state{gpio_names[0], "dig.2", &gpio_states[1]}; + StateInterface gpio_2_ana_state{gpio_names[1], "ana.1", &gpio_states[2]}; }; TEST_F(GpioCommandControllerTestSuite, GpiosParameterNotSet) { - const auto result = controller_->init( - "test_gpio_command_controller", "", 0, "", controller_->define_custom_node_options()); + const auto result = controller.init( + "test_gpio_command_controller", "", 0, "", controller.define_custom_node_options()); ASSERT_EQ(result, controller_interface::return_type::ERROR); } @@ -86,7 +77,7 @@ TEST_F(GpioCommandControllerTestSuite, GpiosParameterIsEmpty) parameters.emplace_back("gpios", std::vector{}); auto node_options = rclcpp::NodeOptions(); node_options.parameter_overrides(parameters); - const auto result = controller_->init("test_gpio_command_controller", "", 0, "", node_options); + const auto result = controller.init("test_gpio_command_controller", "", 0, "", node_options); ASSERT_EQ(result, controller_interface::return_type::ERROR); } @@ -100,7 +91,7 @@ TEST_F(GpioCommandControllerTestSuite, GpioWithMissingGpioParams) parameters.emplace_back("command_interfaces.gpio2.ports", std::vector{}); auto node_options = rclcpp::NodeOptions(); node_options.parameter_overrides(parameters); - const auto result = controller_->init("test_gpio_command_controller", "", 0, "", node_options); + const auto result = controller.init("test_gpio_command_controller", "", 0, "", node_options); ASSERT_EQ(result, controller_interface::return_type::ERROR); } @@ -108,29 +99,29 @@ TEST_F(GpioCommandControllerTestSuite, GpioWithMissingGpioParams) TEST_F(GpioCommandControllerTestSuite, ConfigureAndActivateParamsSuccess) { std::vector parameters; - parameters.emplace_back("gpios", gpio_names_); + parameters.emplace_back("gpios", gpio_names); parameters.emplace_back( "command_interfaces.gpio1.ports", std::vector{"dig.1", "dig.2"}); parameters.emplace_back("command_interfaces.gpio2.ports", std::vector{"ana.1"}); auto node_options = rclcpp::NodeOptions(); node_options.parameter_overrides(parameters); - const auto result = controller_->init("test_gpio_command_controller", "", 0, "", node_options); + const auto result = controller.init("test_gpio_command_controller", "", 0, "", node_options); std::vector command_ifs; - command_ifs.emplace_back(gpio_1_1_dig_cmd_); - command_ifs.emplace_back(gpio_1_2_dig_cmd_); - command_ifs.emplace_back(gpio_2_ana_cmd_); + command_ifs.emplace_back(gpio_1_1_dig_cmd); + command_ifs.emplace_back(gpio_1_2_dig_cmd); + command_ifs.emplace_back(gpio_2_ana_cmd); std::vector state_ifs; - state_ifs.emplace_back(gpio_1_1_dig_state_); - state_ifs.emplace_back(gpio_1_2_dig_state_); - state_ifs.emplace_back(gpio_2_ana_state_); + state_ifs.emplace_back(gpio_1_1_dig_state); + state_ifs.emplace_back(gpio_1_2_dig_state); + state_ifs.emplace_back(gpio_2_ana_state); - controller_->assign_interfaces(std::move(command_ifs), std::move(state_ifs)); + controller.assign_interfaces(std::move(command_ifs), std::move(state_ifs)); ASSERT_EQ(result, controller_interface::return_type::OK); - ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), CallbackReturn::SUCCESS); - ASSERT_EQ(controller_->on_activate(rclcpp_lifecycle::State()), CallbackReturn::SUCCESS); + ASSERT_EQ(controller.on_configure(rclcpp_lifecycle::State()), CallbackReturn::SUCCESS); + ASSERT_EQ(controller.on_activate(rclcpp_lifecycle::State()), CallbackReturn::SUCCESS); } TEST_F(GpioCommandControllerTestSuite, ActivateWithWrongGpiosNamesFails) @@ -142,23 +133,23 @@ TEST_F(GpioCommandControllerTestSuite, ActivateWithWrongGpiosNamesFails) parameters.emplace_back("command_interfaces.gpio4.ports", std::vector{"ana.1"}); auto node_options = rclcpp::NodeOptions(); node_options.parameter_overrides(parameters); - const auto result = controller_->init("test_gpio_command_controller", "", 0, "", node_options); + const auto result = controller.init("test_gpio_command_controller", "", 0, "", node_options); std::vector command_ifs; - command_ifs.emplace_back(gpio_1_1_dig_cmd_); - command_ifs.emplace_back(gpio_1_2_dig_cmd_); - command_ifs.emplace_back(gpio_2_ana_cmd_); + command_ifs.emplace_back(gpio_1_1_dig_cmd); + command_ifs.emplace_back(gpio_1_2_dig_cmd); + command_ifs.emplace_back(gpio_2_ana_cmd); std::vector state_ifs; - state_ifs.emplace_back(gpio_1_1_dig_state_); - state_ifs.emplace_back(gpio_1_2_dig_state_); - state_ifs.emplace_back(gpio_2_ana_state_); + state_ifs.emplace_back(gpio_1_1_dig_state); + state_ifs.emplace_back(gpio_1_2_dig_state); + state_ifs.emplace_back(gpio_2_ana_state); - controller_->assign_interfaces(std::move(command_ifs), std::move(state_ifs)); + controller.assign_interfaces(std::move(command_ifs), std::move(state_ifs)); ASSERT_EQ(result, controller_interface::return_type::OK); - ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), CallbackReturn::SUCCESS); - ASSERT_EQ(controller_->on_activate(rclcpp_lifecycle::State()), CallbackReturn::ERROR); + ASSERT_EQ(controller.on_configure(rclcpp_lifecycle::State()), CallbackReturn::SUCCESS); + ASSERT_EQ(controller.on_activate(rclcpp_lifecycle::State()), CallbackReturn::ERROR); } TEST_F(GpioCommandControllerTestSuite, CommandSuccessTest) @@ -170,30 +161,30 @@ TEST_F(GpioCommandControllerTestSuite, CommandSuccessTest) parameters.emplace_back("command_interfaces.gpio2.ports", std::vector{"ana.1"}); auto node_options = rclcpp::NodeOptions(); node_options.parameter_overrides(parameters); - const auto result = controller_->init("test_gpio_command_controller", "", 0, "", node_options); + const auto result = controller.init("test_gpio_command_controller", "", 0, "", node_options); std::vector command_ifs; - command_ifs.emplace_back(gpio_1_1_dig_cmd_); - command_ifs.emplace_back(gpio_1_2_dig_cmd_); - command_ifs.emplace_back(gpio_2_ana_cmd_); + command_ifs.emplace_back(gpio_1_1_dig_cmd); + command_ifs.emplace_back(gpio_1_2_dig_cmd); + command_ifs.emplace_back(gpio_2_ana_cmd); std::vector state_ifs; - state_ifs.emplace_back(gpio_1_1_dig_state_); - state_ifs.emplace_back(gpio_1_2_dig_state_); - state_ifs.emplace_back(gpio_2_ana_state_); + state_ifs.emplace_back(gpio_1_1_dig_state); + state_ifs.emplace_back(gpio_1_2_dig_state); + state_ifs.emplace_back(gpio_2_ana_state); - controller_->assign_interfaces(std::move(command_ifs), std::move(state_ifs)); + controller.assign_interfaces(std::move(command_ifs), std::move(state_ifs)); ASSERT_EQ(result, controller_interface::return_type::OK); - ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), CallbackReturn::SUCCESS); - ASSERT_EQ(controller_->on_activate(rclcpp_lifecycle::State()), CallbackReturn::SUCCESS); + ASSERT_EQ(controller.on_configure(rclcpp_lifecycle::State()), CallbackReturn::SUCCESS); + ASSERT_EQ(controller.on_activate(rclcpp_lifecycle::State()), CallbackReturn::SUCCESS); ASSERT_EQ( - controller_->update(rclcpp::Time(0), rclcpp::Duration::from_seconds(0.01)), + controller.update(rclcpp::Time(0), rclcpp::Duration::from_seconds(0.01)), controller_interface::return_type::OK); - ASSERT_EQ(gpio_1_1_dig_cmd_.get_value(), 1.0); - ASSERT_EQ(gpio_1_2_dig_cmd_.get_value(), 0.0); - ASSERT_EQ(gpio_2_ana_cmd_.get_value(), 3.1); + ASSERT_EQ(gpio_1_1_dig_cmd.get_value(), 1.0); + ASSERT_EQ(gpio_1_2_dig_cmd.get_value(), 0.0); + ASSERT_EQ(gpio_2_ana_cmd.get_value(), 3.1); auto command_ptr = std::make_shared(); control_msgs::msg::InterfaceValue inteface_value_gpio1; @@ -205,15 +196,15 @@ TEST_F(GpioCommandControllerTestSuite, CommandSuccessTest) command_ptr->joint_names = {"gpio1", "gpio2"}; command_ptr->interface_values = {inteface_value_gpio1, inteface_value_gpio2}; - controller_->rt_command_ptr_.writeFromNonRT(command_ptr); + controller.rt_command_ptr_.writeFromNonRT(command_ptr); ASSERT_EQ( - controller_->update(rclcpp::Time(0), rclcpp::Duration::from_seconds(0.01)), + controller.update(rclcpp::Time(0), rclcpp::Duration::from_seconds(0.01)), controller_interface::return_type::OK); - ASSERT_EQ(gpio_1_1_dig_cmd_.get_value(), 0.0); - ASSERT_EQ(gpio_1_2_dig_cmd_.get_value(), 1.0); - ASSERT_EQ(gpio_2_ana_cmd_.get_value(), 30.0); + ASSERT_EQ(gpio_1_1_dig_cmd.get_value(), 0.0); + ASSERT_EQ(gpio_1_2_dig_cmd.get_value(), 1.0); + ASSERT_EQ(gpio_2_ana_cmd.get_value(), 30.0); } TEST_F(GpioCommandControllerTestSuite, CommandSuccessTestWithOnlyOneGpio) @@ -225,22 +216,22 @@ TEST_F(GpioCommandControllerTestSuite, CommandSuccessTestWithOnlyOneGpio) parameters.emplace_back("command_interfaces.gpio2.ports", std::vector{"ana.1"}); auto node_options = rclcpp::NodeOptions(); node_options.parameter_overrides(parameters); - const auto result = controller_->init("test_gpio_command_controller", "", 0, "", node_options); + const auto result = controller.init("test_gpio_command_controller", "", 0, "", node_options); std::vector command_ifs; - command_ifs.emplace_back(gpio_1_1_dig_cmd_); - command_ifs.emplace_back(gpio_1_2_dig_cmd_); - command_ifs.emplace_back(gpio_2_ana_cmd_); + command_ifs.emplace_back(gpio_1_1_dig_cmd); + command_ifs.emplace_back(gpio_1_2_dig_cmd); + command_ifs.emplace_back(gpio_2_ana_cmd); std::vector state_ifs; - state_ifs.emplace_back(gpio_1_1_dig_state_); - state_ifs.emplace_back(gpio_1_2_dig_state_); - state_ifs.emplace_back(gpio_2_ana_state_); + state_ifs.emplace_back(gpio_1_1_dig_state); + state_ifs.emplace_back(gpio_1_2_dig_state); + state_ifs.emplace_back(gpio_2_ana_state); - controller_->assign_interfaces(std::move(command_ifs), std::move(state_ifs)); + controller.assign_interfaces(std::move(command_ifs), std::move(state_ifs)); ASSERT_EQ(result, controller_interface::return_type::OK); - ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), CallbackReturn::SUCCESS); - ASSERT_EQ(controller_->on_activate(rclcpp_lifecycle::State()), CallbackReturn::SUCCESS); + ASSERT_EQ(controller.on_configure(rclcpp_lifecycle::State()), CallbackReturn::SUCCESS); + ASSERT_EQ(controller.on_activate(rclcpp_lifecycle::State()), CallbackReturn::SUCCESS); auto command_ptr = std::make_shared(); control_msgs::msg::InterfaceValue inteface_value_gpio1; @@ -249,15 +240,15 @@ TEST_F(GpioCommandControllerTestSuite, CommandSuccessTestWithOnlyOneGpio) command_ptr->joint_names = {"gpio1"}; command_ptr->interface_values = {inteface_value_gpio1}; - controller_->rt_command_ptr_.writeFromNonRT(command_ptr); + controller.rt_command_ptr_.writeFromNonRT(command_ptr); ASSERT_EQ( - controller_->update(rclcpp::Time(0), rclcpp::Duration::from_seconds(0.01)), + controller.update(rclcpp::Time(0), rclcpp::Duration::from_seconds(0.01)), controller_interface::return_type::OK); - ASSERT_EQ(gpio_1_1_dig_cmd_.get_value(), 0.0); - ASSERT_EQ(gpio_1_2_dig_cmd_.get_value(), 1.0); - ASSERT_EQ(gpio_2_ana_cmd_.get_value(), 3.1); + ASSERT_EQ(gpio_1_1_dig_cmd.get_value(), 0.0); + ASSERT_EQ(gpio_1_2_dig_cmd.get_value(), 1.0); + ASSERT_EQ(gpio_2_ana_cmd.get_value(), 3.1); } TEST_F(GpioCommandControllerTestSuite, NoCommandCheckTest) @@ -269,31 +260,31 @@ TEST_F(GpioCommandControllerTestSuite, NoCommandCheckTest) parameters.emplace_back("command_interfaces.gpio2.ports", std::vector{"ana.1"}); auto node_options = rclcpp::NodeOptions(); node_options.parameter_overrides(parameters); - const auto result = controller_->init("test_gpio_command_controller", "", 0, "", node_options); + const auto result = controller.init("test_gpio_command_controller", "", 0, "", node_options); std::vector command_ifs; - command_ifs.emplace_back(gpio_1_1_dig_cmd_); - command_ifs.emplace_back(gpio_1_2_dig_cmd_); - command_ifs.emplace_back(gpio_2_ana_cmd_); + command_ifs.emplace_back(gpio_1_1_dig_cmd); + command_ifs.emplace_back(gpio_1_2_dig_cmd); + command_ifs.emplace_back(gpio_2_ana_cmd); std::vector state_ifs; - state_ifs.emplace_back(gpio_1_1_dig_state_); - state_ifs.emplace_back(gpio_1_2_dig_state_); - state_ifs.emplace_back(gpio_2_ana_state_); + state_ifs.emplace_back(gpio_1_1_dig_state); + state_ifs.emplace_back(gpio_1_2_dig_state); + state_ifs.emplace_back(gpio_2_ana_state); - controller_->assign_interfaces(std::move(command_ifs), std::move(state_ifs)); + controller.assign_interfaces(std::move(command_ifs), std::move(state_ifs)); ASSERT_EQ(result, controller_interface::return_type::OK); - ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), CallbackReturn::SUCCESS); - ASSERT_EQ(controller_->on_activate(rclcpp_lifecycle::State()), CallbackReturn::SUCCESS); + ASSERT_EQ(controller.on_configure(rclcpp_lifecycle::State()), CallbackReturn::SUCCESS); + ASSERT_EQ(controller.on_activate(rclcpp_lifecycle::State()), CallbackReturn::SUCCESS); ASSERT_EQ( - controller_->update(rclcpp::Time(0), rclcpp::Duration::from_seconds(0.01)), + controller.update(rclcpp::Time(0), rclcpp::Duration::from_seconds(0.01)), controller_interface::return_type::OK); - ASSERT_EQ(gpio_1_1_dig_cmd_.get_value(), 1.0); - ASSERT_EQ(gpio_1_2_dig_cmd_.get_value(), 0.0); - ASSERT_EQ(gpio_2_ana_cmd_.get_value(), 3.1); + ASSERT_EQ(gpio_1_1_dig_cmd.get_value(), 1.0); + ASSERT_EQ(gpio_1_2_dig_cmd.get_value(), 0.0); + ASSERT_EQ(gpio_2_ana_cmd.get_value(), 3.1); } TEST_F(GpioCommandControllerTestSuite, CommandCallbackTest) @@ -305,39 +296,39 @@ TEST_F(GpioCommandControllerTestSuite, CommandCallbackTest) parameters.emplace_back("command_interfaces.gpio2.ports", std::vector{"ana.1"}); auto node_options = rclcpp::NodeOptions(); node_options.parameter_overrides(parameters); - const auto result = controller_->init("test_gpio_command_controller", "", 0, "", node_options); + const auto result = controller.init("test_gpio_command_controller", "", 0, "", node_options); std::vector command_ifs; - command_ifs.emplace_back(gpio_1_1_dig_cmd_); - command_ifs.emplace_back(gpio_1_2_dig_cmd_); - command_ifs.emplace_back(gpio_2_ana_cmd_); + command_ifs.emplace_back(gpio_1_1_dig_cmd); + command_ifs.emplace_back(gpio_1_2_dig_cmd); + command_ifs.emplace_back(gpio_2_ana_cmd); std::vector state_ifs; - state_ifs.emplace_back(gpio_1_1_dig_state_); - state_ifs.emplace_back(gpio_1_2_dig_state_); - state_ifs.emplace_back(gpio_2_ana_state_); + state_ifs.emplace_back(gpio_1_1_dig_state); + state_ifs.emplace_back(gpio_1_2_dig_state); + state_ifs.emplace_back(gpio_2_ana_state); - controller_->assign_interfaces(std::move(command_ifs), std::move(state_ifs)); + controller.assign_interfaces(std::move(command_ifs), std::move(state_ifs)); ASSERT_EQ(result, controller_interface::return_type::OK); - ASSERT_EQ(gpio_1_1_dig_cmd_.get_value(), 1.0); - ASSERT_EQ(gpio_1_2_dig_cmd_.get_value(), 0.0); - ASSERT_EQ(gpio_2_ana_cmd_.get_value(), 3.1); + ASSERT_EQ(gpio_1_1_dig_cmd.get_value(), 1.0); + ASSERT_EQ(gpio_1_2_dig_cmd.get_value(), 0.0); + ASSERT_EQ(gpio_2_ana_cmd.get_value(), 3.1); - ASSERT_EQ(gpio_1_1_dig_state_.get_value(), 1.0); - ASSERT_EQ(gpio_1_2_dig_state_.get_value(), 0.0); - ASSERT_EQ(gpio_2_ana_state_.get_value(), 3.1); + ASSERT_EQ(gpio_1_1_dig_state.get_value(), 1.0); + ASSERT_EQ(gpio_1_2_dig_state.get_value(), 0.0); + ASSERT_EQ(gpio_2_ana_state.get_value(), 3.1); - auto node_state = controller_->configure(); + auto node_state = controller.configure(); ASSERT_EQ(node_state.id(), lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE); - node_state = controller_->get_node()->activate(); + node_state = controller.get_node()->activate(); ASSERT_EQ(node_state.id(), lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE); rclcpp::Node test_node("test_node"); auto command_pub = test_node.create_publisher( - std::string(controller_->get_node()->get_name()) + "/commands", rclcpp::SystemDefaultsQoS()); + std::string(controller.get_node()->get_name()) + "/commands", rclcpp::SystemDefaultsQoS()); CmdType command_msg; control_msgs::msg::InterfaceValue inteface_value_gpio1; inteface_value_gpio1.interface_names = {"dig.1", "dig.2"}; @@ -352,22 +343,22 @@ TEST_F(GpioCommandControllerTestSuite, CommandCallbackTest) command_pub->publish(command_msg); rclcpp::executors::SingleThreadedExecutor executor; - executor.add_node(controller_->get_node()->get_node_base_interface()); + executor.add_node(controller.get_node()->get_node_base_interface()); const auto timeout = std::chrono::milliseconds{1}; - const auto until = controller_->get_node()->get_clock()->now() + timeout; - while (controller_->get_node()->get_clock()->now() < until) + const auto until = controller.get_node()->get_clock()->now() + timeout; + while (controller.get_node()->get_clock()->now() < until) { executor.spin_some(); std::this_thread::sleep_for(std::chrono::microseconds(10)); } ASSERT_EQ( - controller_->update(rclcpp::Time(0), rclcpp::Duration::from_seconds(0.01)), + controller.update(rclcpp::Time(0), rclcpp::Duration::from_seconds(0.01)), controller_interface::return_type::OK); - ASSERT_EQ(gpio_1_1_dig_cmd_.get_value(), 0.0); - ASSERT_EQ(gpio_1_2_dig_cmd_.get_value(), 1.0); - ASSERT_EQ(gpio_2_ana_cmd_.get_value(), 30.0); + ASSERT_EQ(gpio_1_1_dig_cmd.get_value(), 0.0); + ASSERT_EQ(gpio_1_2_dig_cmd.get_value(), 1.0); + ASSERT_EQ(gpio_2_ana_cmd.get_value(), 30.0); } TEST_F(GpioCommandControllerTestSuite, StateCallbackTest) @@ -379,47 +370,47 @@ TEST_F(GpioCommandControllerTestSuite, StateCallbackTest) parameters.emplace_back("command_interfaces.gpio2.ports", std::vector{"ana.1"}); auto node_options = rclcpp::NodeOptions(); node_options.parameter_overrides(parameters); - const auto result = controller_->init("test_gpio_command_controller", "", 0, "", node_options); + const auto result = controller.init("test_gpio_command_controller", "", 0, "", node_options); std::vector command_ifs; - command_ifs.emplace_back(gpio_1_1_dig_cmd_); - command_ifs.emplace_back(gpio_1_2_dig_cmd_); - command_ifs.emplace_back(gpio_2_ana_cmd_); + command_ifs.emplace_back(gpio_1_1_dig_cmd); + command_ifs.emplace_back(gpio_1_2_dig_cmd); + command_ifs.emplace_back(gpio_2_ana_cmd); std::vector state_ifs; - state_ifs.emplace_back(gpio_1_1_dig_state_); - state_ifs.emplace_back(gpio_1_2_dig_state_); - state_ifs.emplace_back(gpio_2_ana_state_); + state_ifs.emplace_back(gpio_1_1_dig_state); + state_ifs.emplace_back(gpio_1_2_dig_state); + state_ifs.emplace_back(gpio_2_ana_state); - controller_->assign_interfaces(std::move(command_ifs), std::move(state_ifs)); + controller.assign_interfaces(std::move(command_ifs), std::move(state_ifs)); ASSERT_EQ(result, controller_interface::return_type::OK); - ASSERT_EQ(gpio_1_1_dig_cmd_.get_value(), 1.0); - ASSERT_EQ(gpio_1_2_dig_cmd_.get_value(), 0.0); - ASSERT_EQ(gpio_2_ana_cmd_.get_value(), 3.1); + ASSERT_EQ(gpio_1_1_dig_cmd.get_value(), 1.0); + ASSERT_EQ(gpio_1_2_dig_cmd.get_value(), 0.0); + ASSERT_EQ(gpio_2_ana_cmd.get_value(), 3.1); - ASSERT_EQ(gpio_1_1_dig_state_.get_value(), 1.0); - ASSERT_EQ(gpio_1_2_dig_state_.get_value(), 0.0); - ASSERT_EQ(gpio_2_ana_state_.get_value(), 3.1); + ASSERT_EQ(gpio_1_1_dig_state.get_value(), 1.0); + ASSERT_EQ(gpio_1_2_dig_state.get_value(), 0.0); + ASSERT_EQ(gpio_2_ana_state.get_value(), 3.1); - auto node_state = controller_->configure(); + auto node_state = controller.configure(); ASSERT_EQ(node_state.id(), lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE); - node_state = controller_->get_node()->activate(); + node_state = controller.get_node()->activate(); ASSERT_EQ(node_state.id(), lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE); rclcpp::Node test_node("test_node"); auto subs_callback = [&](const StateType::SharedPtr) {}; auto subscription = test_node.create_subscription( - std::string(controller_->get_node()->get_name()) + "/gpio_states", 10, subs_callback); + std::string(controller.get_node()->get_name()) + "/gpio_states", 10, subs_callback); int max_sub_check_loop_count = 5; rclcpp::WaitSet wait_set; wait_set.add_subscription(subscription); while (max_sub_check_loop_count--) { - controller_->update(rclcpp::Time(0), rclcpp::Duration::from_seconds(0.01)); + controller.update(rclcpp::Time(0), rclcpp::Duration::from_seconds(0.01)); if (wait_set.wait(std::chrono::milliseconds(2)).kind() == rclcpp::WaitResultKind::Ready) { break; @@ -432,7 +423,7 @@ TEST_F(GpioCommandControllerTestSuite, StateCallbackTest) 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.size(), gpio_names.size()); ASSERT_EQ(gpio_state_msg.joint_names[0], "gpio1"); ASSERT_EQ(gpio_state_msg.joint_names[1], "gpio2"); ASSERT_EQ(gpio_state_msg.interface_values[0].interface_names[0], "dig.1"); From 3e7a12adb5723daa1d1865b0df119cb19b52627d Mon Sep 17 00:00:00 2001 From: Wiktor Bajor Date: Fri, 9 Aug 2024 22:05:49 +0200 Subject: [PATCH 30/58] Add set of test for init --- .../test/test_gpio_command_controller.cpp | 37 +++++++++++++++---- 1 file changed, 30 insertions(+), 7 deletions(-) diff --git a/gpio_controllers/test/test_gpio_command_controller.cpp b/gpio_controllers/test/test_gpio_command_controller.cpp index 55d1c1612b..f6e7456d76 100644 --- a/gpio_controllers/test/test_gpio_command_controller.cpp +++ b/gpio_controllers/test/test_gpio_command_controller.cpp @@ -64,14 +64,14 @@ class GpioCommandControllerTestSuite : public ::testing::Test StateInterface gpio_2_ana_state{gpio_names[1], "ana.1", &gpio_states[2]}; }; -TEST_F(GpioCommandControllerTestSuite, GpiosParameterNotSet) +TEST_F(GpioCommandControllerTestSuite, WhenNoParametersAreSetInitShouldFail) { const auto result = controller.init( "test_gpio_command_controller", "", 0, "", controller.define_custom_node_options()); ASSERT_EQ(result, controller_interface::return_type::ERROR); } -TEST_F(GpioCommandControllerTestSuite, GpiosParameterIsEmpty) +TEST_F(GpioCommandControllerTestSuite, WhenGpiosParameterIsEmptyInitShouldFail) { std::vector parameters; parameters.emplace_back("gpios", std::vector{}); @@ -82,13 +82,22 @@ TEST_F(GpioCommandControllerTestSuite, GpiosParameterIsEmpty) ASSERT_EQ(result, controller_interface::return_type::ERROR); } -TEST_F(GpioCommandControllerTestSuite, GpioWithMissingGpioParams) +TEST_F(GpioCommandControllerTestSuite, WhenPortsParameterForGpioIsEmptyInitShouldFail) { std::vector parameters; - parameters.emplace_back("gpios", std::vector{"gpio1", "gpio2"}); - parameters.emplace_back( - "command_interfaces.gpio1.ports", std::vector{"dig.1", "dig.2"}); - parameters.emplace_back("command_interfaces.gpio2.ports", std::vector{}); + parameters.emplace_back("gpios", std::vector{"gpio1"}); + parameters.emplace_back("command_interfaces.gpio1.ports", std::vector{}); + auto node_options = rclcpp::NodeOptions(); + node_options.parameter_overrides(parameters); + const auto result = controller.init("test_gpio_command_controller", "", 0, "", node_options); + + ASSERT_EQ(result, controller_interface::return_type::ERROR); +} + +TEST_F(GpioCommandControllerTestSuite, WhenPortsParameterForGpioIsNotSetInitShouldFail) +{ + std::vector parameters; + parameters.emplace_back("gpios", std::vector{"gpio1"}); auto node_options = rclcpp::NodeOptions(); node_options.parameter_overrides(parameters); const auto result = controller.init("test_gpio_command_controller", "", 0, "", node_options); @@ -96,6 +105,20 @@ TEST_F(GpioCommandControllerTestSuite, GpioWithMissingGpioParams) ASSERT_EQ(result, controller_interface::return_type::ERROR); } +TEST_F( + GpioCommandControllerTestSuite, WhenGpiosAreSetAndPortsAreSetForAllGpiosThenInitShouldSuccess) +{ + std::vector parameters; + parameters.emplace_back("gpios", gpio_names); + parameters.emplace_back( + "command_interfaces.gpio1.ports", std::vector{"dig.1", "dig.2"}); + parameters.emplace_back("command_interfaces.gpio2.ports", std::vector{"ana.1"}); + auto node_options = rclcpp::NodeOptions(); + node_options.parameter_overrides(parameters); + const auto result = controller.init("test_gpio_command_controller", "", 0, "", node_options); + ASSERT_EQ(result, controller_interface::return_type::OK); +} + TEST_F(GpioCommandControllerTestSuite, ConfigureAndActivateParamsSuccess) { std::vector parameters; From 373053e8d3e1f17b285d8ccc90e75cf302ee8506 Mon Sep 17 00:00:00 2001 From: Wiktor Bajor Date: Tue, 13 Aug 2024 20:26:39 +0200 Subject: [PATCH 31/58] Add setup_command_and_state_interfaces as refactor UTs --- .../test/test_gpio_command_controller.cpp | 102 ++++-------------- 1 file changed, 21 insertions(+), 81 deletions(-) diff --git a/gpio_controllers/test/test_gpio_command_controller.cpp b/gpio_controllers/test/test_gpio_command_controller.cpp index f6e7456d76..a8f57049d2 100644 --- a/gpio_controllers/test/test_gpio_command_controller.cpp +++ b/gpio_controllers/test/test_gpio_command_controller.cpp @@ -48,6 +48,20 @@ class GpioCommandControllerTestSuite : public ::testing::Test public: GpioCommandControllerTestSuite() { rclcpp::init(0, nullptr); } ~GpioCommandControllerTestSuite() { rclcpp::shutdown(); } + void setup_command_and_state_interfaces() + { + std::vector command_ifs; + command_ifs.emplace_back(gpio_1_1_dig_cmd); + command_ifs.emplace_back(gpio_1_2_dig_cmd); + command_ifs.emplace_back(gpio_2_ana_cmd); + + std::vector state_ifs; + state_ifs.emplace_back(gpio_1_1_dig_state); + state_ifs.emplace_back(gpio_1_2_dig_state); + state_ifs.emplace_back(gpio_2_ana_state); + + controller.assign_interfaces(std::move(command_ifs), std::move(state_ifs)); + } FriendGpioCommandController controller; @@ -129,19 +143,7 @@ TEST_F(GpioCommandControllerTestSuite, ConfigureAndActivateParamsSuccess) auto node_options = rclcpp::NodeOptions(); node_options.parameter_overrides(parameters); const auto result = controller.init("test_gpio_command_controller", "", 0, "", node_options); - - std::vector command_ifs; - command_ifs.emplace_back(gpio_1_1_dig_cmd); - command_ifs.emplace_back(gpio_1_2_dig_cmd); - command_ifs.emplace_back(gpio_2_ana_cmd); - - std::vector state_ifs; - state_ifs.emplace_back(gpio_1_1_dig_state); - state_ifs.emplace_back(gpio_1_2_dig_state); - state_ifs.emplace_back(gpio_2_ana_state); - - controller.assign_interfaces(std::move(command_ifs), std::move(state_ifs)); - + setup_command_and_state_interfaces(); ASSERT_EQ(result, controller_interface::return_type::OK); ASSERT_EQ(controller.on_configure(rclcpp_lifecycle::State()), CallbackReturn::SUCCESS); ASSERT_EQ(controller.on_activate(rclcpp_lifecycle::State()), CallbackReturn::SUCCESS); @@ -158,17 +160,7 @@ TEST_F(GpioCommandControllerTestSuite, ActivateWithWrongGpiosNamesFails) node_options.parameter_overrides(parameters); const auto result = controller.init("test_gpio_command_controller", "", 0, "", node_options); - std::vector command_ifs; - command_ifs.emplace_back(gpio_1_1_dig_cmd); - command_ifs.emplace_back(gpio_1_2_dig_cmd); - command_ifs.emplace_back(gpio_2_ana_cmd); - - std::vector state_ifs; - state_ifs.emplace_back(gpio_1_1_dig_state); - state_ifs.emplace_back(gpio_1_2_dig_state); - state_ifs.emplace_back(gpio_2_ana_state); - - controller.assign_interfaces(std::move(command_ifs), std::move(state_ifs)); + setup_command_and_state_interfaces(); ASSERT_EQ(result, controller_interface::return_type::OK); ASSERT_EQ(controller.on_configure(rclcpp_lifecycle::State()), CallbackReturn::SUCCESS); @@ -186,17 +178,7 @@ TEST_F(GpioCommandControllerTestSuite, CommandSuccessTest) node_options.parameter_overrides(parameters); const auto result = controller.init("test_gpio_command_controller", "", 0, "", node_options); - std::vector command_ifs; - command_ifs.emplace_back(gpio_1_1_dig_cmd); - command_ifs.emplace_back(gpio_1_2_dig_cmd); - command_ifs.emplace_back(gpio_2_ana_cmd); - - std::vector state_ifs; - state_ifs.emplace_back(gpio_1_1_dig_state); - state_ifs.emplace_back(gpio_1_2_dig_state); - state_ifs.emplace_back(gpio_2_ana_state); - - controller.assign_interfaces(std::move(command_ifs), std::move(state_ifs)); + setup_command_and_state_interfaces(); ASSERT_EQ(result, controller_interface::return_type::OK); ASSERT_EQ(controller.on_configure(rclcpp_lifecycle::State()), CallbackReturn::SUCCESS); ASSERT_EQ(controller.on_activate(rclcpp_lifecycle::State()), CallbackReturn::SUCCESS); @@ -241,17 +223,7 @@ TEST_F(GpioCommandControllerTestSuite, CommandSuccessTestWithOnlyOneGpio) node_options.parameter_overrides(parameters); const auto result = controller.init("test_gpio_command_controller", "", 0, "", node_options); - std::vector command_ifs; - command_ifs.emplace_back(gpio_1_1_dig_cmd); - command_ifs.emplace_back(gpio_1_2_dig_cmd); - command_ifs.emplace_back(gpio_2_ana_cmd); - - std::vector state_ifs; - state_ifs.emplace_back(gpio_1_1_dig_state); - state_ifs.emplace_back(gpio_1_2_dig_state); - state_ifs.emplace_back(gpio_2_ana_state); - - controller.assign_interfaces(std::move(command_ifs), std::move(state_ifs)); + setup_command_and_state_interfaces(); ASSERT_EQ(result, controller_interface::return_type::OK); ASSERT_EQ(controller.on_configure(rclcpp_lifecycle::State()), CallbackReturn::SUCCESS); ASSERT_EQ(controller.on_activate(rclcpp_lifecycle::State()), CallbackReturn::SUCCESS); @@ -285,18 +257,7 @@ TEST_F(GpioCommandControllerTestSuite, NoCommandCheckTest) node_options.parameter_overrides(parameters); const auto result = controller.init("test_gpio_command_controller", "", 0, "", node_options); - std::vector command_ifs; - command_ifs.emplace_back(gpio_1_1_dig_cmd); - command_ifs.emplace_back(gpio_1_2_dig_cmd); - command_ifs.emplace_back(gpio_2_ana_cmd); - - std::vector state_ifs; - state_ifs.emplace_back(gpio_1_1_dig_state); - state_ifs.emplace_back(gpio_1_2_dig_state); - state_ifs.emplace_back(gpio_2_ana_state); - - controller.assign_interfaces(std::move(command_ifs), std::move(state_ifs)); - + setup_command_and_state_interfaces(); ASSERT_EQ(result, controller_interface::return_type::OK); ASSERT_EQ(controller.on_configure(rclcpp_lifecycle::State()), CallbackReturn::SUCCESS); ASSERT_EQ(controller.on_activate(rclcpp_lifecycle::State()), CallbackReturn::SUCCESS); @@ -321,18 +282,7 @@ TEST_F(GpioCommandControllerTestSuite, CommandCallbackTest) node_options.parameter_overrides(parameters); const auto result = controller.init("test_gpio_command_controller", "", 0, "", node_options); - std::vector command_ifs; - command_ifs.emplace_back(gpio_1_1_dig_cmd); - command_ifs.emplace_back(gpio_1_2_dig_cmd); - command_ifs.emplace_back(gpio_2_ana_cmd); - - std::vector state_ifs; - state_ifs.emplace_back(gpio_1_1_dig_state); - state_ifs.emplace_back(gpio_1_2_dig_state); - state_ifs.emplace_back(gpio_2_ana_state); - - controller.assign_interfaces(std::move(command_ifs), std::move(state_ifs)); - + setup_command_and_state_interfaces(); ASSERT_EQ(result, controller_interface::return_type::OK); ASSERT_EQ(gpio_1_1_dig_cmd.get_value(), 1.0); @@ -395,17 +345,7 @@ TEST_F(GpioCommandControllerTestSuite, StateCallbackTest) node_options.parameter_overrides(parameters); const auto result = controller.init("test_gpio_command_controller", "", 0, "", node_options); - std::vector command_ifs; - command_ifs.emplace_back(gpio_1_1_dig_cmd); - command_ifs.emplace_back(gpio_1_2_dig_cmd); - command_ifs.emplace_back(gpio_2_ana_cmd); - - std::vector state_ifs; - state_ifs.emplace_back(gpio_1_1_dig_state); - state_ifs.emplace_back(gpio_1_2_dig_state); - state_ifs.emplace_back(gpio_2_ana_state); - - controller.assign_interfaces(std::move(command_ifs), std::move(state_ifs)); + setup_command_and_state_interfaces(); ASSERT_EQ(result, controller_interface::return_type::OK); From 1dc481f56255ed028435fb17d706a337fbaf2715 Mon Sep 17 00:00:00 2001 From: Wiktor Bajor Date: Thu, 15 Aug 2024 21:38:23 +0200 Subject: [PATCH 32/58] Add two UTs to check wrong ifs assing --- .../test/test_gpio_command_controller.cpp | 76 ++++++++++++++++--- 1 file changed, 67 insertions(+), 9 deletions(-) diff --git a/gpio_controllers/test/test_gpio_command_controller.cpp b/gpio_controllers/test/test_gpio_command_controller.cpp index a8f57049d2..016024f37f 100644 --- a/gpio_controllers/test/test_gpio_command_controller.cpp +++ b/gpio_controllers/test/test_gpio_command_controller.cpp @@ -50,17 +50,17 @@ class GpioCommandControllerTestSuite : public ::testing::Test ~GpioCommandControllerTestSuite() { rclcpp::shutdown(); } void setup_command_and_state_interfaces() { - std::vector command_ifs; - command_ifs.emplace_back(gpio_1_1_dig_cmd); - command_ifs.emplace_back(gpio_1_2_dig_cmd); - command_ifs.emplace_back(gpio_2_ana_cmd); + std::vector command_interfaces; + command_interfaces.emplace_back(gpio_1_1_dig_cmd); + command_interfaces.emplace_back(gpio_1_2_dig_cmd); + command_interfaces.emplace_back(gpio_2_ana_cmd); - std::vector state_ifs; - state_ifs.emplace_back(gpio_1_1_dig_state); - state_ifs.emplace_back(gpio_1_2_dig_state); - state_ifs.emplace_back(gpio_2_ana_state); + std::vector state_interfaces; + state_interfaces.emplace_back(gpio_1_1_dig_state); + state_interfaces.emplace_back(gpio_1_2_dig_state); + state_interfaces.emplace_back(gpio_2_ana_state); - controller.assign_interfaces(std::move(command_ifs), std::move(state_ifs)); + controller.assign_interfaces(std::move(command_interfaces), std::move(state_interfaces)); } FriendGpioCommandController controller; @@ -167,6 +167,64 @@ TEST_F(GpioCommandControllerTestSuite, ActivateWithWrongGpiosNamesFails) ASSERT_EQ(controller.on_activate(rclcpp_lifecycle::State()), CallbackReturn::ERROR); } +TEST_F( + GpioCommandControllerTestSuite, + WhenAssignedCommandInterfacesDoNotMatchInterfacesFromParamsThenControllerShouldFailOnActivation) +{ + std::vector parameters; + parameters.emplace_back("gpios", std::vector{"gpio1", "gpio2"}); + parameters.emplace_back( + "command_interfaces.gpio1.ports", std::vector{"dig.1", "dig.2"}); + parameters.emplace_back("command_interfaces.gpio2.ports", std::vector{"ana.1"}); + auto node_options = rclcpp::NodeOptions(); + node_options.parameter_overrides(parameters); + const auto result = controller.init("test_gpio_command_controller", "", 0, "", node_options); + + std::vector command_interfaces; + command_interfaces.emplace_back(gpio_1_1_dig_cmd); + command_interfaces.emplace_back(gpio_1_2_dig_cmd); + + std::vector state_interfaces; + state_interfaces.emplace_back(gpio_1_1_dig_state); + state_interfaces.emplace_back(gpio_1_2_dig_state); + state_interfaces.emplace_back(gpio_2_ana_state); + + controller.assign_interfaces(std::move(command_interfaces), std::move(state_interfaces)); + + ASSERT_EQ(result, controller_interface::return_type::OK); + ASSERT_EQ(controller.on_configure(rclcpp_lifecycle::State()), CallbackReturn::SUCCESS); + ASSERT_EQ(controller.on_activate(rclcpp_lifecycle::State()), CallbackReturn::ERROR); +} + +TEST_F( + GpioCommandControllerTestSuite, + WhenAssignedSateInterfacesDoNotMatchInterfacesFromParamsThenControllerShouldFailOnActivation) +{ + std::vector parameters; + parameters.emplace_back("gpios", std::vector{"gpio1", "gpio2"}); + parameters.emplace_back( + "command_interfaces.gpio1.ports", std::vector{"dig.1", "dig.2"}); + parameters.emplace_back("command_interfaces.gpio2.ports", std::vector{"ana.1"}); + auto node_options = rclcpp::NodeOptions(); + node_options.parameter_overrides(parameters); + const auto result = controller.init("test_gpio_command_controller", "", 0, "", node_options); + + std::vector command_interfaces; + command_interfaces.emplace_back(gpio_1_1_dig_cmd); + command_interfaces.emplace_back(gpio_1_2_dig_cmd); + command_interfaces.emplace_back(gpio_2_ana_cmd); + + std::vector state_interfaces; + state_interfaces.emplace_back(gpio_1_1_dig_state); + state_interfaces.emplace_back(gpio_1_2_dig_state); + + controller.assign_interfaces(std::move(command_interfaces), std::move(state_interfaces)); + + ASSERT_EQ(result, controller_interface::return_type::OK); + ASSERT_EQ(controller.on_configure(rclcpp_lifecycle::State()), CallbackReturn::SUCCESS); + ASSERT_EQ(controller.on_activate(rclcpp_lifecycle::State()), CallbackReturn::ERROR); +} + TEST_F(GpioCommandControllerTestSuite, CommandSuccessTest) { std::vector parameters; From 2bad07b615f29e9d35b68b862225f679e7f0d140 Mon Sep 17 00:00:00 2001 From: Wiktor Bajor Date: Thu, 15 Aug 2024 22:04:23 +0200 Subject: [PATCH 33/58] Add create node option function and remove redundant test --- .../test/test_gpio_command_controller.cpp | 150 +++++++----------- 1 file changed, 55 insertions(+), 95 deletions(-) diff --git a/gpio_controllers/test/test_gpio_command_controller.cpp b/gpio_controllers/test/test_gpio_command_controller.cpp index 016024f37f..3662b8064a 100644 --- a/gpio_controllers/test/test_gpio_command_controller.cpp +++ b/gpio_controllers/test/test_gpio_command_controller.cpp @@ -11,7 +11,6 @@ // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. // See the License for the specific language governing permissions and // limitations under the License. - #include #include #include @@ -36,6 +35,17 @@ using StateType = control_msgs::msg::DynamicJointState; using hardware_interface::CommandInterface; using hardware_interface::StateInterface; +namespace +{ +rclcpp::NodeOptions create_node_options_with_overriden_parameters( + std::vector parameters) +{ + auto node_options = rclcpp::NodeOptions(); + node_options.parameter_overrides(parameters); + return node_options; +} +} // namespace + class FriendGpioCommandController : public gpio_controllers::GpioCommandController { FRIEND_TEST(GpioCommandControllerTestSuite, CommandSuccessTest); @@ -87,10 +97,8 @@ TEST_F(GpioCommandControllerTestSuite, WhenNoParametersAreSetInitShouldFail) TEST_F(GpioCommandControllerTestSuite, WhenGpiosParameterIsEmptyInitShouldFail) { - std::vector parameters; - parameters.emplace_back("gpios", std::vector{}); - auto node_options = rclcpp::NodeOptions(); - node_options.parameter_overrides(parameters); + const auto node_options = + create_node_options_with_overriden_parameters({{"gpios", std::vector{}}}); const auto result = controller.init("test_gpio_command_controller", "", 0, "", node_options); ASSERT_EQ(result, controller_interface::return_type::ERROR); @@ -98,11 +106,9 @@ TEST_F(GpioCommandControllerTestSuite, WhenGpiosParameterIsEmptyInitShouldFail) TEST_F(GpioCommandControllerTestSuite, WhenPortsParameterForGpioIsEmptyInitShouldFail) { - std::vector parameters; - parameters.emplace_back("gpios", std::vector{"gpio1"}); - parameters.emplace_back("command_interfaces.gpio1.ports", std::vector{}); - auto node_options = rclcpp::NodeOptions(); - node_options.parameter_overrides(parameters); + const auto node_options = create_node_options_with_overriden_parameters( + {{"gpios", std::vector{"gpio1"}}, + {"command_interfaces.gpio1.ports", std::vector{}}}); const auto result = controller.init("test_gpio_command_controller", "", 0, "", node_options); ASSERT_EQ(result, controller_interface::return_type::ERROR); @@ -110,10 +116,8 @@ TEST_F(GpioCommandControllerTestSuite, WhenPortsParameterForGpioIsEmptyInitShoul TEST_F(GpioCommandControllerTestSuite, WhenPortsParameterForGpioIsNotSetInitShouldFail) { - std::vector parameters; - parameters.emplace_back("gpios", std::vector{"gpio1"}); - auto node_options = rclcpp::NodeOptions(); - node_options.parameter_overrides(parameters); + const auto node_options = + create_node_options_with_overriden_parameters({{"gpios", std::vector{"gpio1"}}}); const auto result = controller.init("test_gpio_command_controller", "", 0, "", node_options); ASSERT_EQ(result, controller_interface::return_type::ERROR); @@ -122,26 +126,21 @@ TEST_F(GpioCommandControllerTestSuite, WhenPortsParameterForGpioIsNotSetInitShou TEST_F( GpioCommandControllerTestSuite, WhenGpiosAreSetAndPortsAreSetForAllGpiosThenInitShouldSuccess) { - std::vector parameters; - parameters.emplace_back("gpios", gpio_names); - parameters.emplace_back( - "command_interfaces.gpio1.ports", std::vector{"dig.1", "dig.2"}); - parameters.emplace_back("command_interfaces.gpio2.ports", std::vector{"ana.1"}); - auto node_options = rclcpp::NodeOptions(); - node_options.parameter_overrides(parameters); + const auto node_options = create_node_options_with_overriden_parameters( + {{"gpios", gpio_names}, + {"command_interfaces.gpio1.ports", std::vector{"dig.1", "dig.2"}}, + {"command_interfaces.gpio2.ports", std::vector{"ana.1"}}}); + const auto result = controller.init("test_gpio_command_controller", "", 0, "", node_options); ASSERT_EQ(result, controller_interface::return_type::OK); } TEST_F(GpioCommandControllerTestSuite, ConfigureAndActivateParamsSuccess) { - std::vector parameters; - parameters.emplace_back("gpios", gpio_names); - parameters.emplace_back( - "command_interfaces.gpio1.ports", std::vector{"dig.1", "dig.2"}); - parameters.emplace_back("command_interfaces.gpio2.ports", std::vector{"ana.1"}); - auto node_options = rclcpp::NodeOptions(); - node_options.parameter_overrides(parameters); + const auto node_options = create_node_options_with_overriden_parameters( + {{"gpios", gpio_names}, + {"command_interfaces.gpio1.ports", std::vector{"dig.1", "dig.2"}}, + {"command_interfaces.gpio2.ports", std::vector{"ana.1"}}}); const auto result = controller.init("test_gpio_command_controller", "", 0, "", node_options); setup_command_and_state_interfaces(); ASSERT_EQ(result, controller_interface::return_type::OK); @@ -149,35 +148,14 @@ TEST_F(GpioCommandControllerTestSuite, ConfigureAndActivateParamsSuccess) ASSERT_EQ(controller.on_activate(rclcpp_lifecycle::State()), CallbackReturn::SUCCESS); } -TEST_F(GpioCommandControllerTestSuite, ActivateWithWrongGpiosNamesFails) -{ - std::vector parameters; - parameters.emplace_back("gpios", std::vector{"gpio1", "gpio4"}); - parameters.emplace_back( - "command_interfaces.gpio1.ports", std::vector{"dig.1", "dig.2"}); - parameters.emplace_back("command_interfaces.gpio4.ports", std::vector{"ana.1"}); - auto node_options = rclcpp::NodeOptions(); - node_options.parameter_overrides(parameters); - const auto result = controller.init("test_gpio_command_controller", "", 0, "", node_options); - - setup_command_and_state_interfaces(); - - ASSERT_EQ(result, controller_interface::return_type::OK); - ASSERT_EQ(controller.on_configure(rclcpp_lifecycle::State()), CallbackReturn::SUCCESS); - ASSERT_EQ(controller.on_activate(rclcpp_lifecycle::State()), CallbackReturn::ERROR); -} - TEST_F( GpioCommandControllerTestSuite, WhenAssignedCommandInterfacesDoNotMatchInterfacesFromParamsThenControllerShouldFailOnActivation) { - std::vector parameters; - parameters.emplace_back("gpios", std::vector{"gpio1", "gpio2"}); - parameters.emplace_back( - "command_interfaces.gpio1.ports", std::vector{"dig.1", "dig.2"}); - parameters.emplace_back("command_interfaces.gpio2.ports", std::vector{"ana.1"}); - auto node_options = rclcpp::NodeOptions(); - node_options.parameter_overrides(parameters); + const auto node_options = create_node_options_with_overriden_parameters( + {{"gpios", gpio_names}, + {"command_interfaces.gpio1.ports", std::vector{"dig.1", "dig.2"}}, + {"command_interfaces.gpio2.ports", std::vector{"ana.1"}}}); const auto result = controller.init("test_gpio_command_controller", "", 0, "", node_options); std::vector command_interfaces; @@ -200,13 +178,10 @@ TEST_F( GpioCommandControllerTestSuite, WhenAssignedSateInterfacesDoNotMatchInterfacesFromParamsThenControllerShouldFailOnActivation) { - std::vector parameters; - parameters.emplace_back("gpios", std::vector{"gpio1", "gpio2"}); - parameters.emplace_back( - "command_interfaces.gpio1.ports", std::vector{"dig.1", "dig.2"}); - parameters.emplace_back("command_interfaces.gpio2.ports", std::vector{"ana.1"}); - auto node_options = rclcpp::NodeOptions(); - node_options.parameter_overrides(parameters); + const auto node_options = create_node_options_with_overriden_parameters( + {{"gpios", gpio_names}, + {"command_interfaces.gpio1.ports", std::vector{"dig.1", "dig.2"}}, + {"command_interfaces.gpio2.ports", std::vector{"ana.1"}}}); const auto result = controller.init("test_gpio_command_controller", "", 0, "", node_options); std::vector command_interfaces; @@ -227,13 +202,10 @@ TEST_F( TEST_F(GpioCommandControllerTestSuite, CommandSuccessTest) { - std::vector parameters; - parameters.emplace_back("gpios", std::vector{"gpio1", "gpio2"}); - parameters.emplace_back( - "command_interfaces.gpio1.ports", std::vector{"dig.1", "dig.2"}); - parameters.emplace_back("command_interfaces.gpio2.ports", std::vector{"ana.1"}); - auto node_options = rclcpp::NodeOptions(); - node_options.parameter_overrides(parameters); + const auto node_options = create_node_options_with_overriden_parameters( + {{"gpios", gpio_names}, + {"command_interfaces.gpio1.ports", std::vector{"dig.1", "dig.2"}}, + {"command_interfaces.gpio2.ports", std::vector{"ana.1"}}}); const auto result = controller.init("test_gpio_command_controller", "", 0, "", node_options); setup_command_and_state_interfaces(); @@ -272,13 +244,10 @@ TEST_F(GpioCommandControllerTestSuite, CommandSuccessTest) TEST_F(GpioCommandControllerTestSuite, CommandSuccessTestWithOnlyOneGpio) { - std::vector parameters; - parameters.emplace_back("gpios", std::vector{"gpio1", "gpio2"}); - parameters.emplace_back( - "command_interfaces.gpio1.ports", std::vector{"dig.1", "dig.2"}); - parameters.emplace_back("command_interfaces.gpio2.ports", std::vector{"ana.1"}); - auto node_options = rclcpp::NodeOptions(); - node_options.parameter_overrides(parameters); + const auto node_options = create_node_options_with_overriden_parameters( + {{"gpios", gpio_names}, + {"command_interfaces.gpio1.ports", std::vector{"dig.1", "dig.2"}}, + {"command_interfaces.gpio2.ports", std::vector{"ana.1"}}}); const auto result = controller.init("test_gpio_command_controller", "", 0, "", node_options); setup_command_and_state_interfaces(); @@ -306,13 +275,10 @@ TEST_F(GpioCommandControllerTestSuite, CommandSuccessTestWithOnlyOneGpio) TEST_F(GpioCommandControllerTestSuite, NoCommandCheckTest) { - std::vector parameters; - parameters.emplace_back("gpios", std::vector{"gpio1", "gpio2"}); - parameters.emplace_back( - "command_interfaces.gpio1.ports", std::vector{"dig.1", "dig.2"}); - parameters.emplace_back("command_interfaces.gpio2.ports", std::vector{"ana.1"}); - auto node_options = rclcpp::NodeOptions(); - node_options.parameter_overrides(parameters); + const auto node_options = create_node_options_with_overriden_parameters( + {{"gpios", gpio_names}, + {"command_interfaces.gpio1.ports", std::vector{"dig.1", "dig.2"}}, + {"command_interfaces.gpio2.ports", std::vector{"ana.1"}}}); const auto result = controller.init("test_gpio_command_controller", "", 0, "", node_options); setup_command_and_state_interfaces(); @@ -331,13 +297,10 @@ TEST_F(GpioCommandControllerTestSuite, NoCommandCheckTest) TEST_F(GpioCommandControllerTestSuite, CommandCallbackTest) { - std::vector parameters; - parameters.emplace_back("gpios", std::vector{"gpio1", "gpio2"}); - parameters.emplace_back( - "command_interfaces.gpio1.ports", std::vector{"dig.1", "dig.2"}); - parameters.emplace_back("command_interfaces.gpio2.ports", std::vector{"ana.1"}); - auto node_options = rclcpp::NodeOptions(); - node_options.parameter_overrides(parameters); + const auto node_options = create_node_options_with_overriden_parameters( + {{"gpios", gpio_names}, + {"command_interfaces.gpio1.ports", std::vector{"dig.1", "dig.2"}}, + {"command_interfaces.gpio2.ports", std::vector{"ana.1"}}}); const auto result = controller.init("test_gpio_command_controller", "", 0, "", node_options); setup_command_and_state_interfaces(); @@ -394,13 +357,10 @@ TEST_F(GpioCommandControllerTestSuite, CommandCallbackTest) TEST_F(GpioCommandControllerTestSuite, StateCallbackTest) { - std::vector parameters; - parameters.emplace_back("gpios", std::vector{"gpio1", "gpio2"}); - parameters.emplace_back( - "command_interfaces.gpio1.ports", std::vector{"dig.1", "dig.2"}); - parameters.emplace_back("command_interfaces.gpio2.ports", std::vector{"ana.1"}); - auto node_options = rclcpp::NodeOptions(); - node_options.parameter_overrides(parameters); + const auto node_options = create_node_options_with_overriden_parameters( + {{"gpios", gpio_names}, + {"command_interfaces.gpio1.ports", std::vector{"dig.1", "dig.2"}}, + {"command_interfaces.gpio2.ports", std::vector{"ana.1"}}}); const auto result = controller.init("test_gpio_command_controller", "", 0, "", node_options); setup_command_and_state_interfaces(); From f78158eb65478d54bf44c9bbc8f1bcb219feecee Mon Sep 17 00:00:00 2001 From: Wiktor Bajor Date: Thu, 15 Aug 2024 22:23:24 +0200 Subject: [PATCH 34/58] Extract redundant code to methods --- .../test/test_gpio_command_controller.cpp | 91 +++++++++---------- 1 file changed, 43 insertions(+), 48 deletions(-) diff --git a/gpio_controllers/test/test_gpio_command_controller.cpp b/gpio_controllers/test/test_gpio_command_controller.cpp index 3662b8064a..0aa4cb0bd7 100644 --- a/gpio_controllers/test/test_gpio_command_controller.cpp +++ b/gpio_controllers/test/test_gpio_command_controller.cpp @@ -73,6 +73,36 @@ class GpioCommandControllerTestSuite : public ::testing::Test controller.assign_interfaces(std::move(command_interfaces), std::move(state_interfaces)); } + void move_to_activate_state(controller_interface::return_type result_of_initialization) + { + ASSERT_EQ(result_of_initialization, controller_interface::return_type::OK); + ASSERT_EQ(controller.on_configure(rclcpp_lifecycle::State()), CallbackReturn::SUCCESS); + ASSERT_EQ(controller.on_activate(rclcpp_lifecycle::State()), CallbackReturn::SUCCESS); + } + + void move_to_activate_state() + { + ASSERT_EQ(controller.configure().id(), lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE); + ASSERT_EQ( + controller.get_node()->activate().id(), lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE); + } + + void stop_test_when_message_cannot_be_published(int max_sub_check_loop_count) + { + ASSERT_GE(max_sub_check_loop_count, 0) + << "Test was unable to publish a message through controller/broadcaster update loop"; + } + + void assert_default_command_and_state_values() + { + ASSERT_EQ(gpio_1_1_dig_cmd.get_value(), 1.0); + ASSERT_EQ(gpio_1_2_dig_cmd.get_value(), 0.0); + ASSERT_EQ(gpio_2_ana_cmd.get_value(), 3.1); + ASSERT_EQ(gpio_1_1_dig_state.get_value(), 1.0); + ASSERT_EQ(gpio_1_2_dig_state.get_value(), 0.0); + ASSERT_EQ(gpio_2_ana_state.get_value(), 3.1); + } + FriendGpioCommandController controller; const std::vector gpio_names{"gpio1", "gpio2"}; @@ -209,17 +239,13 @@ TEST_F(GpioCommandControllerTestSuite, CommandSuccessTest) const auto result = controller.init("test_gpio_command_controller", "", 0, "", node_options); setup_command_and_state_interfaces(); - ASSERT_EQ(result, controller_interface::return_type::OK); - ASSERT_EQ(controller.on_configure(rclcpp_lifecycle::State()), CallbackReturn::SUCCESS); - ASSERT_EQ(controller.on_activate(rclcpp_lifecycle::State()), CallbackReturn::SUCCESS); + move_to_activate_state(result); ASSERT_EQ( controller.update(rclcpp::Time(0), rclcpp::Duration::from_seconds(0.01)), controller_interface::return_type::OK); - ASSERT_EQ(gpio_1_1_dig_cmd.get_value(), 1.0); - ASSERT_EQ(gpio_1_2_dig_cmd.get_value(), 0.0); - ASSERT_EQ(gpio_2_ana_cmd.get_value(), 3.1); + assert_default_command_and_state_values(); auto command_ptr = std::make_shared(); control_msgs::msg::InterfaceValue inteface_value_gpio1; @@ -249,11 +275,8 @@ TEST_F(GpioCommandControllerTestSuite, CommandSuccessTestWithOnlyOneGpio) {"command_interfaces.gpio1.ports", std::vector{"dig.1", "dig.2"}}, {"command_interfaces.gpio2.ports", std::vector{"ana.1"}}}); const auto result = controller.init("test_gpio_command_controller", "", 0, "", node_options); - setup_command_and_state_interfaces(); - ASSERT_EQ(result, controller_interface::return_type::OK); - ASSERT_EQ(controller.on_configure(rclcpp_lifecycle::State()), CallbackReturn::SUCCESS); - ASSERT_EQ(controller.on_activate(rclcpp_lifecycle::State()), CallbackReturn::SUCCESS); + move_to_activate_state(result); auto command_ptr = std::make_shared(); control_msgs::msg::InterfaceValue inteface_value_gpio1; @@ -282,17 +305,13 @@ TEST_F(GpioCommandControllerTestSuite, NoCommandCheckTest) const auto result = controller.init("test_gpio_command_controller", "", 0, "", node_options); setup_command_and_state_interfaces(); - ASSERT_EQ(result, controller_interface::return_type::OK); - ASSERT_EQ(controller.on_configure(rclcpp_lifecycle::State()), CallbackReturn::SUCCESS); - ASSERT_EQ(controller.on_activate(rclcpp_lifecycle::State()), CallbackReturn::SUCCESS); + move_to_activate_state(result); ASSERT_EQ( controller.update(rclcpp::Time(0), rclcpp::Duration::from_seconds(0.01)), controller_interface::return_type::OK); - ASSERT_EQ(gpio_1_1_dig_cmd.get_value(), 1.0); - ASSERT_EQ(gpio_1_2_dig_cmd.get_value(), 0.0); - ASSERT_EQ(gpio_2_ana_cmd.get_value(), 3.1); + assert_default_command_and_state_values(); } TEST_F(GpioCommandControllerTestSuite, CommandCallbackTest) @@ -306,19 +325,8 @@ TEST_F(GpioCommandControllerTestSuite, CommandCallbackTest) setup_command_and_state_interfaces(); ASSERT_EQ(result, controller_interface::return_type::OK); - ASSERT_EQ(gpio_1_1_dig_cmd.get_value(), 1.0); - ASSERT_EQ(gpio_1_2_dig_cmd.get_value(), 0.0); - ASSERT_EQ(gpio_2_ana_cmd.get_value(), 3.1); - - ASSERT_EQ(gpio_1_1_dig_state.get_value(), 1.0); - ASSERT_EQ(gpio_1_2_dig_state.get_value(), 0.0); - ASSERT_EQ(gpio_2_ana_state.get_value(), 3.1); - - auto node_state = controller.configure(); - ASSERT_EQ(node_state.id(), lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE); - - node_state = controller.get_node()->activate(); - ASSERT_EQ(node_state.id(), lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE); + assert_default_command_and_state_values(); + move_to_activate_state(); rclcpp::Node test_node("test_node"); auto command_pub = test_node.create_publisher( @@ -361,25 +369,13 @@ TEST_F(GpioCommandControllerTestSuite, StateCallbackTest) {{"gpios", gpio_names}, {"command_interfaces.gpio1.ports", std::vector{"dig.1", "dig.2"}}, {"command_interfaces.gpio2.ports", std::vector{"ana.1"}}}); - const auto result = controller.init("test_gpio_command_controller", "", 0, "", node_options); - setup_command_and_state_interfaces(); + ASSERT_EQ( + controller.init("test_gpio_command_controller", "", 0, "", node_options), + controller_interface::return_type::OK); - ASSERT_EQ(result, controller_interface::return_type::OK); - - ASSERT_EQ(gpio_1_1_dig_cmd.get_value(), 1.0); - ASSERT_EQ(gpio_1_2_dig_cmd.get_value(), 0.0); - ASSERT_EQ(gpio_2_ana_cmd.get_value(), 3.1); - - ASSERT_EQ(gpio_1_1_dig_state.get_value(), 1.0); - ASSERT_EQ(gpio_1_2_dig_state.get_value(), 0.0); - ASSERT_EQ(gpio_2_ana_state.get_value(), 3.1); - - auto node_state = controller.configure(); - ASSERT_EQ(node_state.id(), lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE); - - node_state = controller.get_node()->activate(); - ASSERT_EQ(node_state.id(), lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE); + assert_default_command_and_state_values(); + move_to_activate_state(); rclcpp::Node test_node("test_node"); auto subs_callback = [&](const StateType::SharedPtr) {}; @@ -397,8 +393,7 @@ TEST_F(GpioCommandControllerTestSuite, StateCallbackTest) break; } } - ASSERT_GE(max_sub_check_loop_count, 0) << "Test was unable to publish a message through " - "controller/broadcaster update loop"; + stop_test_when_message_cannot_be_published(max_sub_check_loop_count); StateType gpio_state_msg; rclcpp::MessageInfo msg_info; From 815f85e828640ec9e4d00ba0bf40377eb104d9f1 Mon Sep 17 00:00:00 2001 From: Wiktor Bajor Date: Fri, 16 Aug 2024 23:25:33 +0200 Subject: [PATCH 35/58] Reafactor command UTs --- .../test/test_gpio_command_controller.cpp | 218 +++++++++--------- 1 file changed, 105 insertions(+), 113 deletions(-) diff --git a/gpio_controllers/test/test_gpio_command_controller.cpp b/gpio_controllers/test/test_gpio_command_controller.cpp index 0aa4cb0bd7..67c9bf426f 100644 --- a/gpio_controllers/test/test_gpio_command_controller.cpp +++ b/gpio_controllers/test/test_gpio_command_controller.cpp @@ -56,7 +56,11 @@ class FriendGpioCommandController : public gpio_controllers::GpioCommandControll class GpioCommandControllerTestSuite : public ::testing::Test { public: - GpioCommandControllerTestSuite() { rclcpp::init(0, nullptr); } + GpioCommandControllerTestSuite() + { + rclcpp::init(0, nullptr); + node = std::make_unique("node"); + } ~GpioCommandControllerTestSuite() { rclcpp::shutdown(); } void setup_command_and_state_interfaces() { @@ -95,12 +99,68 @@ class GpioCommandControllerTestSuite : public ::testing::Test void assert_default_command_and_state_values() { - ASSERT_EQ(gpio_1_1_dig_cmd.get_value(), 1.0); - ASSERT_EQ(gpio_1_2_dig_cmd.get_value(), 0.0); - ASSERT_EQ(gpio_2_ana_cmd.get_value(), 3.1); - ASSERT_EQ(gpio_1_1_dig_state.get_value(), 1.0); - ASSERT_EQ(gpio_1_2_dig_state.get_value(), 0.0); - ASSERT_EQ(gpio_2_ana_state.get_value(), 3.1); + ASSERT_EQ(gpio_1_1_dig_cmd.get_value(), gpio_commands[0]); + ASSERT_EQ(gpio_1_2_dig_cmd.get_value(), gpio_commands[1]); + ASSERT_EQ(gpio_2_ana_cmd.get_value(), gpio_commands[2]); + ASSERT_EQ(gpio_1_1_dig_state.get_value(), gpio_states[0]); + ASSERT_EQ(gpio_1_2_dig_state.get_value(), gpio_states[1]); + ASSERT_EQ(gpio_2_ana_state.get_value(), gpio_states[2]); + } + + void update_controller_loop() + { + ASSERT_EQ( + controller.update(rclcpp::Time(0), rclcpp::Duration::from_seconds(0.01)), + controller_interface::return_type::OK); + } + + CmdType createGpioCommand( + const std::vector & gpios_names, + const std::vector & interface_values) + { + CmdType command; + command.joint_names = gpios_names; + command.interface_values = interface_values; + return command; + } + + control_msgs::msg::InterfaceValue createInterfaceValue( + const std::vector & interfaces_names, + const std::vector & interfaces_values) + { + control_msgs::msg::InterfaceValue output; + output.interface_names = interfaces_names; + output.values = interfaces_values; + return output; + } + + void wait_one_miliseconds_for_timeout() + { + rclcpp::executors::SingleThreadedExecutor executor; + executor.add_node(controller.get_node()->get_node_base_interface()); + const auto timeout = std::chrono::milliseconds{1}; + const auto until = controller.get_node()->get_clock()->now() + timeout; + while (controller.get_node()->get_clock()->now() < until) + { + executor.spin_some(); + std::this_thread::sleep_for(std::chrono::microseconds(10)); + } + } + + int wait_for_subscription( + std::shared_ptr subscription, int max_sub_check_loop_count = 5) + { + rclcpp::WaitSet wait_set; + wait_set.add_subscription(subscription); + while (max_sub_check_loop_count--) + { + controller.update(rclcpp::Time(0), rclcpp::Duration::from_seconds(0.01)); + if (wait_set.wait(std::chrono::milliseconds(2)).kind() == rclcpp::WaitResultKind::Ready) + { + break; + } + } + return max_sub_check_loop_count; } FriendGpioCommandController controller; @@ -116,6 +176,7 @@ class GpioCommandControllerTestSuite : public ::testing::Test StateInterface gpio_1_1_dig_state{gpio_names[0], "dig.1", &gpio_states[0]}; StateInterface gpio_1_2_dig_state{gpio_names[0], "dig.2", &gpio_states[1]}; StateInterface gpio_2_ana_state{gpio_names[1], "ana.1", &gpio_states[2]}; + std::unique_ptr node; }; TEST_F(GpioCommandControllerTestSuite, WhenNoParametersAreSetInitShouldFail) @@ -230,88 +291,62 @@ TEST_F( ASSERT_EQ(controller.on_activate(rclcpp_lifecycle::State()), CallbackReturn::ERROR); } -TEST_F(GpioCommandControllerTestSuite, CommandSuccessTest) +TEST_F( + GpioCommandControllerTestSuite, + WhenThereWasNoCommandForGpiosThenCommandInterfacesShouldHaveDefaultValues) { const auto node_options = create_node_options_with_overriden_parameters( {{"gpios", gpio_names}, {"command_interfaces.gpio1.ports", std::vector{"dig.1", "dig.2"}}, {"command_interfaces.gpio2.ports", std::vector{"ana.1"}}}); - const auto result = controller.init("test_gpio_command_controller", "", 0, "", node_options); setup_command_and_state_interfaces(); - move_to_activate_state(result); - - ASSERT_EQ( - controller.update(rclcpp::Time(0), rclcpp::Duration::from_seconds(0.01)), - controller_interface::return_type::OK); - + move_to_activate_state(controller.init("test_gpio_command_controller", "", 0, "", node_options)); + assert_default_command_and_state_values(); + update_controller_loop(); assert_default_command_and_state_values(); - - auto command_ptr = std::make_shared(); - control_msgs::msg::InterfaceValue inteface_value_gpio1; - inteface_value_gpio1.interface_names = {"dig.1", "dig.2"}; - inteface_value_gpio1.values = {0.0, 1.0}; - control_msgs::msg::InterfaceValue inteface_value_gpio2; - inteface_value_gpio2.interface_names = {"ana.1"}; - inteface_value_gpio2.values = {30.0}; - - command_ptr->joint_names = {"gpio1", "gpio2"}; - command_ptr->interface_values = {inteface_value_gpio1, inteface_value_gpio2}; - controller.rt_command_ptr_.writeFromNonRT(command_ptr); - - ASSERT_EQ( - controller.update(rclcpp::Time(0), rclcpp::Duration::from_seconds(0.01)), - controller_interface::return_type::OK); - - ASSERT_EQ(gpio_1_1_dig_cmd.get_value(), 0.0); - ASSERT_EQ(gpio_1_2_dig_cmd.get_value(), 1.0); - ASSERT_EQ(gpio_2_ana_cmd.get_value(), 30.0); } -TEST_F(GpioCommandControllerTestSuite, CommandSuccessTestWithOnlyOneGpio) +TEST_F(GpioCommandControllerTestSuite, CommandSuccessTest) { const auto node_options = create_node_options_with_overriden_parameters( {{"gpios", gpio_names}, {"command_interfaces.gpio1.ports", std::vector{"dig.1", "dig.2"}}, {"command_interfaces.gpio2.ports", std::vector{"ana.1"}}}); - const auto result = controller.init("test_gpio_command_controller", "", 0, "", node_options); - setup_command_and_state_interfaces(); - move_to_activate_state(result); - - auto command_ptr = std::make_shared(); - control_msgs::msg::InterfaceValue inteface_value_gpio1; - inteface_value_gpio1.interface_names = {"dig.1", "dig.2"}; - inteface_value_gpio1.values = {0.0, 1.0}; - command_ptr->joint_names = {"gpio1"}; - command_ptr->interface_values = {inteface_value_gpio1}; - controller.rt_command_ptr_.writeFromNonRT(command_ptr); + setup_command_and_state_interfaces(); + move_to_activate_state(controller.init("test_gpio_command_controller", "", 0, "", node_options)); - ASSERT_EQ( - controller.update(rclcpp::Time(0), rclcpp::Duration::from_seconds(0.01)), - controller_interface::return_type::OK); + const auto command = createGpioCommand( + {"gpio1", "gpio2"}, {createInterfaceValue({"dig.1", "dig.2"}, {0.0, 1.0}), + createInterfaceValue({"ana.1"}, {30.0})}); + controller.rt_command_ptr_.writeFromNonRT(std::make_shared(command)); + update_controller_loop(); ASSERT_EQ(gpio_1_1_dig_cmd.get_value(), 0.0); ASSERT_EQ(gpio_1_2_dig_cmd.get_value(), 1.0); - ASSERT_EQ(gpio_2_ana_cmd.get_value(), 3.1); + ASSERT_EQ(gpio_2_ana_cmd.get_value(), 30.0); } -TEST_F(GpioCommandControllerTestSuite, NoCommandCheckTest) +TEST_F(GpioCommandControllerTestSuite, CommandSuccessTestWithOnlyOneGpio) { const auto node_options = create_node_options_with_overriden_parameters( {{"gpios", gpio_names}, {"command_interfaces.gpio1.ports", std::vector{"dig.1", "dig.2"}}, {"command_interfaces.gpio2.ports", std::vector{"ana.1"}}}); const auto result = controller.init("test_gpio_command_controller", "", 0, "", node_options); - setup_command_and_state_interfaces(); move_to_activate_state(result); - ASSERT_EQ( - controller.update(rclcpp::Time(0), rclcpp::Duration::from_seconds(0.01)), - controller_interface::return_type::OK); + const auto command = + createGpioCommand({"gpio1"}, {createInterfaceValue({"dig.1", "dig.2"}, {0.0, 1.0})}); - assert_default_command_and_state_values(); + controller.rt_command_ptr_.writeFromNonRT(std::make_shared(command)); + update_controller_loop(); + + ASSERT_EQ(gpio_1_1_dig_cmd.get_value(), 0.0); + ASSERT_EQ(gpio_1_2_dig_cmd.get_value(), 1.0); + ASSERT_EQ(gpio_2_ana_cmd.get_value(), gpio_commands[2]); } TEST_F(GpioCommandControllerTestSuite, CommandCallbackTest) @@ -320,43 +355,17 @@ TEST_F(GpioCommandControllerTestSuite, CommandCallbackTest) {{"gpios", gpio_names}, {"command_interfaces.gpio1.ports", std::vector{"dig.1", "dig.2"}}, {"command_interfaces.gpio2.ports", std::vector{"ana.1"}}}); - const auto result = controller.init("test_gpio_command_controller", "", 0, "", node_options); - setup_command_and_state_interfaces(); - ASSERT_EQ(result, controller_interface::return_type::OK); - - assert_default_command_and_state_values(); - move_to_activate_state(); + move_to_activate_state(controller.init("test_gpio_command_controller", "", 0, "", node_options)); - rclcpp::Node test_node("test_node"); - auto command_pub = test_node.create_publisher( + auto command_pub = node->create_publisher( std::string(controller.get_node()->get_name()) + "/commands", rclcpp::SystemDefaultsQoS()); - CmdType command_msg; - control_msgs::msg::InterfaceValue inteface_value_gpio1; - inteface_value_gpio1.interface_names = {"dig.1", "dig.2"}; - inteface_value_gpio1.values = {0.0, 1.0}; - control_msgs::msg::InterfaceValue inteface_value_gpio2; - inteface_value_gpio2.interface_names = {"ana.1"}; - inteface_value_gpio2.values = {30.0}; - - command_msg.joint_names = {"gpio1", "gpio2"}; - command_msg.interface_values = {inteface_value_gpio1, inteface_value_gpio2}; - - command_pub->publish(command_msg); - - rclcpp::executors::SingleThreadedExecutor executor; - executor.add_node(controller.get_node()->get_node_base_interface()); - const auto timeout = std::chrono::milliseconds{1}; - const auto until = controller.get_node()->get_clock()->now() + timeout; - while (controller.get_node()->get_clock()->now() < until) - { - executor.spin_some(); - std::this_thread::sleep_for(std::chrono::microseconds(10)); - } - - ASSERT_EQ( - controller.update(rclcpp::Time(0), rclcpp::Duration::from_seconds(0.01)), - controller_interface::return_type::OK); + const auto command = createGpioCommand( + {"gpio1", "gpio2"}, {createInterfaceValue({"dig.1", "dig.2"}, {0.0, 1.0}), + createInterfaceValue({"ana.1"}, {30.0})}); + command_pub->publish(command); + wait_one_miliseconds_for_timeout(); + update_controller_loop(); ASSERT_EQ(gpio_1_1_dig_cmd.get_value(), 0.0); ASSERT_EQ(gpio_1_2_dig_cmd.get_value(), 1.0); @@ -370,30 +379,13 @@ TEST_F(GpioCommandControllerTestSuite, StateCallbackTest) {"command_interfaces.gpio1.ports", std::vector{"dig.1", "dig.2"}}, {"command_interfaces.gpio2.ports", std::vector{"ana.1"}}}); setup_command_and_state_interfaces(); - ASSERT_EQ( - controller.init("test_gpio_command_controller", "", 0, "", node_options), - controller_interface::return_type::OK); - - assert_default_command_and_state_values(); - move_to_activate_state(); + move_to_activate_state(controller.init("test_gpio_command_controller", "", 0, "", node_options)); - rclcpp::Node test_node("test_node"); - auto subs_callback = [&](const StateType::SharedPtr) {}; - auto subscription = test_node.create_subscription( - std::string(controller.get_node()->get_name()) + "/gpio_states", 10, subs_callback); + auto subscription = node->create_subscription( + std::string(controller.get_node()->get_name()) + "/gpio_states", 10, + [&](const StateType::SharedPtr) {}); - int max_sub_check_loop_count = 5; - rclcpp::WaitSet wait_set; - wait_set.add_subscription(subscription); - while (max_sub_check_loop_count--) - { - controller.update(rclcpp::Time(0), rclcpp::Duration::from_seconds(0.01)); - if (wait_set.wait(std::chrono::milliseconds(2)).kind() == rclcpp::WaitResultKind::Ready) - { - break; - } - } - stop_test_when_message_cannot_be_published(max_sub_check_loop_count); + stop_test_when_message_cannot_be_published(wait_for_subscription(subscription)); StateType gpio_state_msg; rclcpp::MessageInfo msg_info; From 026550375b84854c7b6f59705179a134dd8a0871 Mon Sep 17 00:00:00 2001 From: Wiktor Bajor Date: Fri, 16 Aug 2024 23:48:07 +0200 Subject: [PATCH 36/58] Add two UTs for different commands --- .../src/gpio_command_controller.cpp | 10 ++++ .../test/test_gpio_command_controller.cpp | 48 +++++++++++++++++++ 2 files changed, 58 insertions(+) diff --git a/gpio_controllers/src/gpio_command_controller.cpp b/gpio_controllers/src/gpio_command_controller.cpp index 82a1689b5a..0bda77e84d 100644 --- a/gpio_controllers/src/gpio_command_controller.cpp +++ b/gpio_controllers/src/gpio_command_controller.cpp @@ -217,6 +217,16 @@ controller_interface::return_type GpioCommandController::update_gpios_commands() for (std::size_t gpio_index = 0; gpio_index < gpio_commands.joint_names.size(); ++gpio_index) { const auto & gpio_name = gpio_commands.joint_names[gpio_index]; + if ( + gpio_commands.interface_values[gpio_index].values.size() != + gpio_commands.interface_values[gpio_index].interface_names.size()) + { + RCLCPP_ERROR( + get_node()->get_logger(), "For gpio %s interfaces_names do not match values", + gpio_name.c_str()); + return controller_interface::return_type::ERROR; + } + for (std::size_t command_interface_index = 0; command_interface_index < gpio_commands.interface_values[gpio_index].values.size(); ++command_interface_index) diff --git a/gpio_controllers/test/test_gpio_command_controller.cpp b/gpio_controllers/test/test_gpio_command_controller.cpp index 67c9bf426f..cc8e0fb234 100644 --- a/gpio_controllers/test/test_gpio_command_controller.cpp +++ b/gpio_controllers/test/test_gpio_command_controller.cpp @@ -51,6 +51,12 @@ class FriendGpioCommandController : public gpio_controllers::GpioCommandControll FRIEND_TEST(GpioCommandControllerTestSuite, CommandSuccessTest); FRIEND_TEST(GpioCommandControllerTestSuite, CommandSuccessTestWithOnlyOneGpio); FRIEND_TEST(GpioCommandControllerTestSuite, CommandCallbackTest); + FRIEND_TEST( + GpioCommandControllerTestSuite, + WhenCommandContainsMoreValuesThenInterfacesNameForGpioUpdateShouldReturnFalse); + FRIEND_TEST( + GpioCommandControllerTestSuite, + WhenCommandContainsMoreInterfacesNameThenValuesForGpioUpdateShouldReturnFalse); }; class GpioCommandControllerTestSuite : public ::testing::Test @@ -307,6 +313,48 @@ TEST_F( assert_default_command_and_state_values(); } +TEST_F( + GpioCommandControllerTestSuite, + WhenCommandContainsMoreValuesThenInterfacesNameForGpioUpdateShouldReturnFalse) +{ + const auto node_options = create_node_options_with_overriden_parameters( + {{"gpios", gpio_names}, + {"command_interfaces.gpio1.ports", std::vector{"dig.1", "dig.2"}}, + {"command_interfaces.gpio2.ports", std::vector{"ana.1"}}}); + + setup_command_and_state_interfaces(); + move_to_activate_state(controller.init("test_gpio_command_controller", "", 0, "", node_options)); + + const auto command = createGpioCommand( + {"gpio1", "gpio2"}, {createInterfaceValue({"dig.1", "dig.2"}, {0.0, 1.0, 1.0}), + createInterfaceValue({"ana.1"}, {30.0})}); + controller.rt_command_ptr_.writeFromNonRT(std::make_shared(command)); + ASSERT_EQ( + controller.update(rclcpp::Time(0), rclcpp::Duration::from_seconds(0.01)), + controller_interface::return_type::ERROR); +} + +TEST_F( + GpioCommandControllerTestSuite, + WhenCommandContainsMoreInterfacesNameThenValuesForGpioUpdateShouldReturnFalse) +{ + const auto node_options = create_node_options_with_overriden_parameters( + {{"gpios", gpio_names}, + {"command_interfaces.gpio1.ports", std::vector{"dig.1", "dig.2"}}, + {"command_interfaces.gpio2.ports", std::vector{"ana.1"}}}); + + setup_command_and_state_interfaces(); + move_to_activate_state(controller.init("test_gpio_command_controller", "", 0, "", node_options)); + + const auto command = createGpioCommand( + {"gpio1", "gpio2"}, + {createInterfaceValue({"dig.1", "dig.2"}, {0.0}), createInterfaceValue({"ana.1"}, {30.0})}); + controller.rt_command_ptr_.writeFromNonRT(std::make_shared(command)); + ASSERT_EQ( + controller.update(rclcpp::Time(0), rclcpp::Duration::from_seconds(0.01)), + controller_interface::return_type::ERROR); +} + TEST_F(GpioCommandControllerTestSuite, CommandSuccessTest) { const auto node_options = create_node_options_with_overriden_parameters( From 28db5aae4490804fa73a33d3e84affcaaf84dfa4 Mon Sep 17 00:00:00 2001 From: Wiktor Bajor Date: Sat, 17 Aug 2024 00:00:55 +0200 Subject: [PATCH 37/58] Add one more test with command with different ports order --- .../test/test_gpio_command_controller.cpp | 77 ++++++++++++++----- 1 file changed, 56 insertions(+), 21 deletions(-) diff --git a/gpio_controllers/test/test_gpio_command_controller.cpp b/gpio_controllers/test/test_gpio_command_controller.cpp index cc8e0fb234..39a8ea7e1f 100644 --- a/gpio_controllers/test/test_gpio_command_controller.cpp +++ b/gpio_controllers/test/test_gpio_command_controller.cpp @@ -48,15 +48,21 @@ rclcpp::NodeOptions create_node_options_with_overriden_parameters( class FriendGpioCommandController : public gpio_controllers::GpioCommandController { - FRIEND_TEST(GpioCommandControllerTestSuite, CommandSuccessTest); - FRIEND_TEST(GpioCommandControllerTestSuite, CommandSuccessTestWithOnlyOneGpio); - FRIEND_TEST(GpioCommandControllerTestSuite, CommandCallbackTest); + FRIEND_TEST( + GpioCommandControllerTestSuite, + WhenGivenCommandWithValuesForAllInterfacesThenValueOfInterfacesShouldBeUpdated); + FRIEND_TEST( + GpioCommandControllerTestSuite, + WhenGivenCommandWithOnlyOneGpioThenInterfacesValuesShouldBeUpdated); FRIEND_TEST( GpioCommandControllerTestSuite, WhenCommandContainsMoreValuesThenInterfacesNameForGpioUpdateShouldReturnFalse); FRIEND_TEST( GpioCommandControllerTestSuite, WhenCommandContainsMoreInterfacesNameThenValuesForGpioUpdateShouldReturnFalse); + FRIEND_TEST( + GpioCommandControllerTestSuite, + WhenGivenCommandPortsInDifferentOrderThenValueOfInterfacesShouldBeUpdated); }; class GpioCommandControllerTestSuite : public ::testing::Test @@ -105,12 +111,12 @@ class GpioCommandControllerTestSuite : public ::testing::Test void assert_default_command_and_state_values() { - ASSERT_EQ(gpio_1_1_dig_cmd.get_value(), gpio_commands[0]); - ASSERT_EQ(gpio_1_2_dig_cmd.get_value(), gpio_commands[1]); - ASSERT_EQ(gpio_2_ana_cmd.get_value(), gpio_commands[2]); - ASSERT_EQ(gpio_1_1_dig_state.get_value(), gpio_states[0]); - ASSERT_EQ(gpio_1_2_dig_state.get_value(), gpio_states[1]); - ASSERT_EQ(gpio_2_ana_state.get_value(), gpio_states[2]); + ASSERT_EQ(gpio_1_1_dig_cmd.get_value(), gpio_commands.at(0)); + ASSERT_EQ(gpio_1_2_dig_cmd.get_value(), gpio_commands.at(1)); + ASSERT_EQ(gpio_2_ana_cmd.get_value(), gpio_commands.at(2)); + ASSERT_EQ(gpio_1_1_dig_state.get_value(), gpio_states.at(0)); + ASSERT_EQ(gpio_1_2_dig_state.get_value(), gpio_states.at(1)); + ASSERT_EQ(gpio_2_ana_state.get_value(), gpio_states.at(2)); } void update_controller_loop() @@ -355,7 +361,9 @@ TEST_F( controller_interface::return_type::ERROR); } -TEST_F(GpioCommandControllerTestSuite, CommandSuccessTest) +TEST_F( + GpioCommandControllerTestSuite, + WhenGivenCommandWithValuesForAllInterfacesThenValueOfInterfacesShouldBeUpdated) { const auto node_options = create_node_options_with_overriden_parameters( {{"gpios", gpio_names}, @@ -376,7 +384,32 @@ TEST_F(GpioCommandControllerTestSuite, CommandSuccessTest) ASSERT_EQ(gpio_2_ana_cmd.get_value(), 30.0); } -TEST_F(GpioCommandControllerTestSuite, CommandSuccessTestWithOnlyOneGpio) +TEST_F( + GpioCommandControllerTestSuite, + WhenGivenCommandPortsInDifferentOrderThenValueOfInterfacesShouldBeUpdated) +{ + const auto node_options = create_node_options_with_overriden_parameters( + {{"gpios", gpio_names}, + {"command_interfaces.gpio1.ports", std::vector{"dig.1", "dig.2"}}, + {"command_interfaces.gpio2.ports", std::vector{"ana.1"}}}); + + setup_command_and_state_interfaces(); + move_to_activate_state(controller.init("test_gpio_command_controller", "", 0, "", node_options)); + + const auto command = createGpioCommand( + {"gpio2", "gpio1"}, {createInterfaceValue({"ana.1"}, {30.0}), + createInterfaceValue({"dig.2", "dig.1"}, {1.0, 0.0})}); + controller.rt_command_ptr_.writeFromNonRT(std::make_shared(command)); + update_controller_loop(); + + ASSERT_EQ(gpio_1_1_dig_cmd.get_value(), 0.0); + ASSERT_EQ(gpio_1_2_dig_cmd.get_value(), 1.0); + ASSERT_EQ(gpio_2_ana_cmd.get_value(), 30.0); +} + +TEST_F( + GpioCommandControllerTestSuite, + WhenGivenCommandWithOnlyOneGpioThenInterfacesValuesShouldBeUpdated) { const auto node_options = create_node_options_with_overriden_parameters( {{"gpios", gpio_names}, @@ -397,7 +430,9 @@ TEST_F(GpioCommandControllerTestSuite, CommandSuccessTestWithOnlyOneGpio) ASSERT_EQ(gpio_2_ana_cmd.get_value(), gpio_commands[2]); } -TEST_F(GpioCommandControllerTestSuite, CommandCallbackTest) +TEST_F( + GpioCommandControllerTestSuite, + WhenGivenCommandWithValuesForAllInterfacesViaTopicThenValueOfInterfacesShouldBeUpdated) { const auto node_options = create_node_options_with_overriden_parameters( {{"gpios", gpio_names}, @@ -420,7 +455,7 @@ TEST_F(GpioCommandControllerTestSuite, CommandCallbackTest) ASSERT_EQ(gpio_2_ana_cmd.get_value(), 30.0); } -TEST_F(GpioCommandControllerTestSuite, StateCallbackTest) +TEST_F(GpioCommandControllerTestSuite, ControllerShouldPublishGpioStatesWithCurrentValues) { const auto node_options = create_node_options_with_overriden_parameters( {{"gpios", gpio_names}, @@ -440,12 +475,12 @@ TEST_F(GpioCommandControllerTestSuite, StateCallbackTest) 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[0], "gpio1"); - ASSERT_EQ(gpio_state_msg.joint_names[1], "gpio2"); - ASSERT_EQ(gpio_state_msg.interface_values[0].interface_names[0], "dig.1"); - ASSERT_EQ(gpio_state_msg.interface_values[0].interface_names[1], "dig.2"); - ASSERT_EQ(gpio_state_msg.interface_values[1].interface_names[0], "ana.1"); - ASSERT_EQ(gpio_state_msg.interface_values[0].values[0], 1.0); - ASSERT_EQ(gpio_state_msg.interface_values[0].values[1], 0.0); - ASSERT_EQ(gpio_state_msg.interface_values[1].values[0], 3.1); + 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_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"); + ASSERT_EQ(gpio_state_msg.interface_values.at(0).values.at(0), 1.0); + ASSERT_EQ(gpio_state_msg.interface_values.at(0).values.at(1), 0.0); + ASSERT_EQ(gpio_state_msg.interface_values.at(1).values.at(0), 3.1); } From 4be8fac2a9ffad50a1d8ae18024d515591311229 Mon Sep 17 00:00:00 2001 From: Wiktor Bajor Date: Sat, 17 Aug 2024 12:07:00 +0200 Subject: [PATCH 38/58] Add UT for the wrong gpio/port name in the command --- .../test/test_gpio_command_controller.cpp | 41 +++++++++++++++---- 1 file changed, 33 insertions(+), 8 deletions(-) diff --git a/gpio_controllers/test/test_gpio_command_controller.cpp b/gpio_controllers/test/test_gpio_command_controller.cpp index 39a8ea7e1f..63a9a45914 100644 --- a/gpio_controllers/test/test_gpio_command_controller.cpp +++ b/gpio_controllers/test/test_gpio_command_controller.cpp @@ -11,10 +11,8 @@ // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. // See the License for the specific language governing permissions and // limitations under the License. -#include #include #include -#include #include #include "gmock/gmock.h" @@ -63,6 +61,9 @@ class FriendGpioCommandController : public gpio_controllers::GpioCommandControll FRIEND_TEST( GpioCommandControllerTestSuite, WhenGivenCommandPortsInDifferentOrderThenValueOfInterfacesShouldBeUpdated); + FRIEND_TEST( + GpioCommandControllerTestSuite, + WhenGivenCommandContainsWrongGpioPortsOrWrongGpioNameThenCommandInterfacesShouldNotBeUpdated); }; class GpioCommandControllerTestSuite : public ::testing::Test @@ -181,13 +182,13 @@ class GpioCommandControllerTestSuite : public ::testing::Test std::vector gpio_commands{1.0, 0.0, 3.1}; std::vector gpio_states{1.0, 0.0, 3.1}; - CommandInterface gpio_1_1_dig_cmd{gpio_names[0], "dig.1", &gpio_commands[0]}; - CommandInterface gpio_1_2_dig_cmd{gpio_names[0], "dig.2", &gpio_commands[1]}; - CommandInterface gpio_2_ana_cmd{gpio_names[1], "ana.1", &gpio_commands[2]}; + CommandInterface gpio_1_1_dig_cmd{gpio_names.at(0), "dig.1", &gpio_commands.at(0)}; + CommandInterface gpio_1_2_dig_cmd{gpio_names.at(0), "dig.2", &gpio_commands.at(1)}; + CommandInterface gpio_2_ana_cmd{gpio_names.at(1), "ana.1", &gpio_commands.at(2)}; - StateInterface gpio_1_1_dig_state{gpio_names[0], "dig.1", &gpio_states[0]}; - StateInterface gpio_1_2_dig_state{gpio_names[0], "dig.2", &gpio_states[1]}; - StateInterface gpio_2_ana_state{gpio_names[1], "ana.1", &gpio_states[2]}; + StateInterface gpio_1_1_dig_state{gpio_names.at(0), "dig.1", &gpio_states.at(0)}; + StateInterface gpio_1_2_dig_state{gpio_names.at(0), "dig.2", &gpio_states.at(1)}; + StateInterface gpio_2_ana_state{gpio_names.at(1), "ana.1", &gpio_states.at(2)}; std::unique_ptr node; }; @@ -430,6 +431,30 @@ TEST_F( ASSERT_EQ(gpio_2_ana_cmd.get_value(), gpio_commands[2]); } +TEST_F( + GpioCommandControllerTestSuite, + WhenGivenCommandContainsWrongGpioPortsOrWrongGpioNameThenCommandInterfacesShouldNotBeUpdated) +{ + const auto node_options = create_node_options_with_overriden_parameters( + {{"gpios", gpio_names}, + {"command_interfaces.gpio1.ports", std::vector{"dig.1", "dig.2"}}, + {"command_interfaces.gpio2.ports", std::vector{"ana.1"}}}); + const auto result = controller.init("test_gpio_command_controller", "", 0, "", node_options); + setup_command_and_state_interfaces(); + move_to_activate_state(result); + + const auto command = createGpioCommand( + {"gpio1", "gpio3"}, {createInterfaceValue({"dig.3", "dig.4"}, {20.0, 25.0}), + createInterfaceValue({"ana.1"}, {21.0})}); + + controller.rt_command_ptr_.writeFromNonRT(std::make_shared(command)); + update_controller_loop(); + + ASSERT_EQ(gpio_1_1_dig_cmd.get_value(), gpio_commands.at(0)); + ASSERT_EQ(gpio_1_2_dig_cmd.get_value(), gpio_commands.at(1)); + ASSERT_EQ(gpio_2_ana_cmd.get_value(), gpio_commands.at(2)); +} + TEST_F( GpioCommandControllerTestSuite, WhenGivenCommandWithValuesForAllInterfacesViaTopicThenValueOfInterfacesShouldBeUpdated) From fc10ad5ab0bc740c29de9d20c0d9bd2fd15f326d Mon Sep 17 00:00:00 2001 From: Wiktor Bajor Date: Sat, 17 Aug 2024 12:08:14 +0200 Subject: [PATCH 39/58] Update package.xml --- gpio_controllers/package.xml | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/gpio_controllers/package.xml b/gpio_controllers/package.xml index 93a3dcb721..c7d3c95f97 100644 --- a/gpio_controllers/package.xml +++ b/gpio_controllers/package.xml @@ -2,7 +2,7 @@ gpio_controllers - 0.0.0 + 1.0.0 Controllers to interact with gpios. Maciej Bednarczyk Wiktor Bajor From e89d8141e13a1a2849c2ed299c1cbaddbe6ae9c9 Mon Sep 17 00:00:00 2001 From: Wiktor Bajor Date: Thu, 22 Aug 2024 18:42:34 +0200 Subject: [PATCH 40/58] Update pkg version --- gpio_controllers/package.xml | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/gpio_controllers/package.xml b/gpio_controllers/package.xml index c7d3c95f97..4746c98c46 100644 --- a/gpio_controllers/package.xml +++ b/gpio_controllers/package.xml @@ -2,7 +2,7 @@ gpio_controllers - 1.0.0 + 4.12.1 Controllers to interact with gpios. Maciej Bednarczyk Wiktor Bajor From 9c90fb2c3d22ed29d9c40bf2005e2f2edb585c3d Mon Sep 17 00:00:00 2001 From: Wiktor Bajor Date: Thu, 22 Aug 2024 22:51:07 +0200 Subject: [PATCH 41/58] Fix includes --- gpio_controllers/test/test_gpio_command_controller.cpp | 3 +++ 1 file changed, 3 insertions(+) diff --git a/gpio_controllers/test/test_gpio_command_controller.cpp b/gpio_controllers/test/test_gpio_command_controller.cpp index 63a9a45914..6af6b19225 100644 --- a/gpio_controllers/test/test_gpio_command_controller.cpp +++ b/gpio_controllers/test/test_gpio_command_controller.cpp @@ -24,6 +24,9 @@ #include "lifecycle_msgs/msg/state.hpp" #include "rclcpp/utilities.hpp" #include "rclcpp_lifecycle/node_interfaces/lifecycle_node_interface.hpp" +#include "rclcpp/executors/single_threaded_executor.hpp" +#include "rclcpp/wait_set.hpp" +#include "rclcpp/wait_result.hpp" using CallbackReturn = controller_interface::CallbackReturn; using hardware_interface::LoanedCommandInterface; From c165c1d42834fef114406f222b2002c1bb4591df Mon Sep 17 00:00:00 2001 From: Wiktor Bajor Date: Sat, 24 Aug 2024 20:51:33 +0200 Subject: [PATCH 42/58] Fix linters --- gpio_controllers/test/test_gpio_command_controller.cpp | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/gpio_controllers/test/test_gpio_command_controller.cpp b/gpio_controllers/test/test_gpio_command_controller.cpp index 6af6b19225..04b360c3fb 100644 --- a/gpio_controllers/test/test_gpio_command_controller.cpp +++ b/gpio_controllers/test/test_gpio_command_controller.cpp @@ -22,11 +22,11 @@ #include "hardware_interface/types/hardware_interface_return_values.hpp" #include "hardware_interface/types/hardware_interface_type_values.hpp" #include "lifecycle_msgs/msg/state.hpp" -#include "rclcpp/utilities.hpp" -#include "rclcpp_lifecycle/node_interfaces/lifecycle_node_interface.hpp" #include "rclcpp/executors/single_threaded_executor.hpp" -#include "rclcpp/wait_set.hpp" +#include "rclcpp/utilities.hpp" #include "rclcpp/wait_result.hpp" +#include "rclcpp/wait_set.hpp" +#include "rclcpp_lifecycle/node_interfaces/lifecycle_node_interface.hpp" using CallbackReturn = controller_interface::CallbackReturn; using hardware_interface::LoanedCommandInterface; From dd19879947f97643041ab53eaab7dfa9b7610ad1 Mon Sep 17 00:00:00 2001 From: Wiktor Bajor Date: Sat, 24 Aug 2024 20:58:04 +0200 Subject: [PATCH 43/58] Update toctree, add gpio controller info --- doc/controllers_index.rst | 1 + 1 file changed, 1 insertion(+) diff --git a/doc/controllers_index.rst b/doc/controllers_index.rst index 683f23c202..d6a0d625b1 100644 --- a/doc/controllers_index.rst +++ b/doc/controllers_index.rst @@ -56,6 +56,7 @@ The controllers are using `common hardware interface definitions`_, and may use PID Controller <../pid_controller/doc/userdoc.rst> Position Controllers <../position_controllers/doc/userdoc.rst> Velocity Controllers <../velocity_controllers/doc/userdoc.rst> + Gpio Command Controller <../gpio_controllers/doc/userdoc.rst> Broadcasters From 510d8471c2d6f5539ff41c7c92965c2980550921 Mon Sep 17 00:00:00 2001 From: Christoph Froehlich Date: Sun, 3 Nov 2024 21:20:35 +0000 Subject: [PATCH 44/58] Rename parameter and update docs --- gpio_controllers/doc/userdoc.rst | 63 ++++++++++--------- .../src/gpio_command_controller.cpp | 25 ++++++-- .../gpio_command_controller_parameters.yaml | 4 +- 3 files changed, 56 insertions(+), 36 deletions(-) diff --git a/gpio_controllers/doc/userdoc.rst b/gpio_controllers/doc/userdoc.rst index 442132b977..ccc7267e15 100644 --- a/gpio_controllers/doc/userdoc.rst +++ b/gpio_controllers/doc/userdoc.rst @@ -3,39 +3,44 @@ gpio_controllers ===================== -This is a collection of controllers for gpios that work with multiple interfaces. +This is a collection of controllers for hardware interfaces of type GPIO (```` tag in the URDF). -Hardware interface type ------------------------ +gpio_command_controller +----------------------------- +gpio_command_controller publishes all state interfaces of given GPIO interfaces and let the user expose command interfaces. -These controllers work with gpios using user defined command interfaces. +Description of controller's interfaces +^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +- ``//gpio_states`` [``control_msgs/msg/DynamicJointState``]: Publishes all state interfaces of the given GPIO interfaces. +- ``//commands`` [``control_msgs/msg/DynamicJointState``]: A subscriber for configured command interfaces. -Using GPIO Command Controller ------------------------------ -The controller expects at least one gpio interface abd the corresponding command interface names. + +Parameters +^^^^^^^^^^^^^^^^^^^^^^^^ + +This controller uses the `generate_parameter_library `_ to handle its parameters. The parameter `definition file located in the src folder `_ contains descriptions for all the parameters used by the controller. + +.. generate_parameter_library_details:: + ../src/gpio_command_controller_parameters.yaml + +The controller expects at least one gpio interface and the corresponding command interface names. A yaml file for using it could be: -.. code-block:: yaml - controller_manager: - ros__parameters: - update_rate: 100 # Hz - joint_state_broadcaster: - type: joint_state_broadcaster/JointStateBroadcaster - gpio_command_controller: - type: gpio_controllers/GpioCommandController +.. code-block:: yaml gpio_command_controller: - ros__parameters: - gpios: - - Gpio1 - - Gpio2 - command_interfaces: - Gpio1: - - ports: - - dig.1 - - dig.2 - - dig.3 - Gpio2: - -ports: - - ana.1 - - ana.2 + ros__parameters: + type: gpio_controllers/GpioCommandController + gpios: + - Gpio1 + - Gpio2 + command_interfaces: + Gpio1: + - interfaces: + - dig.1 + - dig.2 + - dig.3 + Gpio2: + - interfaces: + - ana.1 + - ana.2 diff --git a/gpio_controllers/src/gpio_command_controller.cpp b/gpio_controllers/src/gpio_command_controller.cpp index 0bda77e84d..389a949a71 100644 --- a/gpio_controllers/src/gpio_command_controller.cpp +++ b/gpio_controllers/src/gpio_command_controller.cpp @@ -63,6 +63,20 @@ catch (const std::exception & e) CallbackReturn GpioCommandController::on_configure(const rclcpp_lifecycle::State &) try { + auto logger = get_node()->get_logger(); + + if (!param_listener_) + { + RCLCPP_ERROR(logger, "Error encountered during init"); + return controller_interface::CallbackReturn::ERROR; + } + + // update the dynamic map parameters + param_listener_->refresh_dynamic_parameters(); + + // get parameters from the listener in case they were updated + params_ = param_listener_->get_params(); + store_interface_types(); gpios_command_subscriber_ = get_node()->create_subscription( "~/commands", rclcpp::SystemDefaultsQoS(), @@ -136,10 +150,11 @@ controller_interface::return_type GpioCommandController::update( void GpioCommandController::store_interface_types() { - for (const auto & [gpio_name, ports] : params_.command_interfaces.gpios_map) + for (const auto & [gpio_name, interfaces] : params_.command_interfaces.gpios_map) { std::transform( - ports.ports.begin(), ports.ports.end(), std::back_inserter(interface_types_), + interfaces.interfaces.begin(), interfaces.interfaces.end(), + std::back_inserter(interface_types_), [&](const auto & interface_name) { return gpio_name + "/" + interface_name; }); } } @@ -156,11 +171,11 @@ void GpioCommandController::initialize_gpio_state_msg() const auto gpio_name = params_.gpios[gpio_index]; gpio_state_msg.joint_names[gpio_index] = gpio_name; std::copy( - params_.command_interfaces.gpios_map[gpio_name].ports.begin(), - params_.command_interfaces.gpios_map[gpio_name].ports.end(), + params_.command_interfaces.gpios_map[gpio_name].interfaces.begin(), + params_.command_interfaces.gpios_map[gpio_name].interfaces.end(), std::back_insert_iterator(gpio_state_msg.interface_values[gpio_index].interface_names)); gpio_state_msg.interface_values[gpio_index].values = std::vector( - params_.command_interfaces.gpios_map[gpio_name].ports.size(), + params_.command_interfaces.gpios_map[gpio_name].interfaces.size(), std::numeric_limits::quiet_NaN()); } } diff --git a/gpio_controllers/src/gpio_command_controller_parameters.yaml b/gpio_controllers/src/gpio_command_controller_parameters.yaml index e6fe0c4ebd..0021df1452 100644 --- a/gpio_controllers/src/gpio_command_controller_parameters.yaml +++ b/gpio_controllers/src/gpio_command_controller_parameters.yaml @@ -10,9 +10,9 @@ gpio_command_controller_parameters: command_interfaces: __map_gpios: - ports: { + interfaces: { type: string_array, - description: "List of ports: for each gpio", + description: "List of interfaces for each gpio", validation: { size_gt<>: [0], unique<>: null From 4518b58431968d563bd4e9a0e69f1cf089329e15 Mon Sep 17 00:00:00 2001 From: Christoph Froehlich Date: Sun, 3 Nov 2024 21:52:31 +0000 Subject: [PATCH 45/58] Set parameter read-only --- gpio_controllers/src/gpio_command_controller_parameters.yaml | 2 ++ 1 file changed, 2 insertions(+) diff --git a/gpio_controllers/src/gpio_command_controller_parameters.yaml b/gpio_controllers/src/gpio_command_controller_parameters.yaml index 0021df1452..cbfafaf5f3 100644 --- a/gpio_controllers/src/gpio_command_controller_parameters.yaml +++ b/gpio_controllers/src/gpio_command_controller_parameters.yaml @@ -2,6 +2,7 @@ gpio_command_controller_parameters: gpios: { type: string_array, description: "List of gpios", + read_only: true, validation: { size_gt<>: [0], unique<>: null @@ -13,6 +14,7 @@ gpio_command_controller_parameters: interfaces: { type: string_array, description: "List of interfaces for each gpio", + read_only: true, validation: { size_gt<>: [0], unique<>: null From 071e190ae34fd6233134eb663aae195941e57374 Mon Sep 17 00:00:00 2001 From: Christoph Froehlich Date: Sun, 3 Nov 2024 21:52:45 +0000 Subject: [PATCH 46/58] Update docs and improve error message --- gpio_controllers/doc/userdoc.rst | 2 +- gpio_controllers/src/gpio_command_controller.cpp | 3 ++- 2 files changed, 3 insertions(+), 2 deletions(-) diff --git a/gpio_controllers/doc/userdoc.rst b/gpio_controllers/doc/userdoc.rst index ccc7267e15..f7bdea8c6e 100644 --- a/gpio_controllers/doc/userdoc.rst +++ b/gpio_controllers/doc/userdoc.rst @@ -7,7 +7,7 @@ This is a collection of controllers for hardware interfaces of type GPIO (`` Date: Sun, 3 Nov 2024 21:58:02 +0000 Subject: [PATCH 47/58] Fix tests --- .../src/gpio_command_controller.cpp | 2 +- .../test/test_gpio_command_controller.cpp | 69 ++++++++++--------- 2 files changed, 36 insertions(+), 35 deletions(-) diff --git a/gpio_controllers/src/gpio_command_controller.cpp b/gpio_controllers/src/gpio_command_controller.cpp index 3d00f4d058..0218c71342 100644 --- a/gpio_controllers/src/gpio_command_controller.cpp +++ b/gpio_controllers/src/gpio_command_controller.cpp @@ -259,7 +259,7 @@ controller_interface::return_type GpioCommandController::update_gpios_commands() { fprintf( stderr, "Exception thrown during applying command stage of %s with message: %s \n", - full_command_interface_name, e.what()); + full_command_interface_name.c_str(), e.what()); } } } diff --git a/gpio_controllers/test/test_gpio_command_controller.cpp b/gpio_controllers/test/test_gpio_command_controller.cpp index 04b360c3fb..ab6893d0e7 100644 --- a/gpio_controllers/test/test_gpio_command_controller.cpp +++ b/gpio_controllers/test/test_gpio_command_controller.cpp @@ -63,10 +63,10 @@ class FriendGpioCommandController : public gpio_controllers::GpioCommandControll WhenCommandContainsMoreInterfacesNameThenValuesForGpioUpdateShouldReturnFalse); FRIEND_TEST( GpioCommandControllerTestSuite, - WhenGivenCommandPortsInDifferentOrderThenValueOfInterfacesShouldBeUpdated); + WhenGivenCommandInterfacesInDifferentOrderThenValueOfInterfacesShouldBeUpdated); FRIEND_TEST( GpioCommandControllerTestSuite, - WhenGivenCommandContainsWrongGpioPortsOrWrongGpioNameThenCommandInterfacesShouldNotBeUpdated); + WhenGivenCmdContainsWrongGpioInterfacesOrWrongGpioNameThenCommandInterfacesShouldNotBeUpdated); }; class GpioCommandControllerTestSuite : public ::testing::Test @@ -211,17 +211,17 @@ TEST_F(GpioCommandControllerTestSuite, WhenGpiosParameterIsEmptyInitShouldFail) ASSERT_EQ(result, controller_interface::return_type::ERROR); } -TEST_F(GpioCommandControllerTestSuite, WhenPortsParameterForGpioIsEmptyInitShouldFail) +TEST_F(GpioCommandControllerTestSuite, WhenInterfacesParameterForGpioIsEmptyInitShouldFail) { const auto node_options = create_node_options_with_overriden_parameters( {{"gpios", std::vector{"gpio1"}}, - {"command_interfaces.gpio1.ports", std::vector{}}}); + {"command_interfaces.gpio1.interfaces", std::vector{}}}); const auto result = controller.init("test_gpio_command_controller", "", 0, "", node_options); ASSERT_EQ(result, controller_interface::return_type::ERROR); } -TEST_F(GpioCommandControllerTestSuite, WhenPortsParameterForGpioIsNotSetInitShouldFail) +TEST_F(GpioCommandControllerTestSuite, WhenInterfacesParameterForGpioIsNotSetInitShouldFail) { const auto node_options = create_node_options_with_overriden_parameters({{"gpios", std::vector{"gpio1"}}}); @@ -231,12 +231,13 @@ TEST_F(GpioCommandControllerTestSuite, WhenPortsParameterForGpioIsNotSetInitShou } TEST_F( - GpioCommandControllerTestSuite, WhenGpiosAreSetAndPortsAreSetForAllGpiosThenInitShouldSuccess) + GpioCommandControllerTestSuite, + WhenGpiosAreSetAndInterfacesAreSetForAllGpiosThenInitShouldSuccess) { const auto node_options = create_node_options_with_overriden_parameters( {{"gpios", gpio_names}, - {"command_interfaces.gpio1.ports", std::vector{"dig.1", "dig.2"}}, - {"command_interfaces.gpio2.ports", std::vector{"ana.1"}}}); + {"command_interfaces.gpio1.interfaces", std::vector{"dig.1", "dig.2"}}, + {"command_interfaces.gpio2.interfaces", std::vector{"ana.1"}}}); const auto result = controller.init("test_gpio_command_controller", "", 0, "", node_options); ASSERT_EQ(result, controller_interface::return_type::OK); @@ -246,8 +247,8 @@ TEST_F(GpioCommandControllerTestSuite, ConfigureAndActivateParamsSuccess) { const auto node_options = create_node_options_with_overriden_parameters( {{"gpios", gpio_names}, - {"command_interfaces.gpio1.ports", std::vector{"dig.1", "dig.2"}}, - {"command_interfaces.gpio2.ports", std::vector{"ana.1"}}}); + {"command_interfaces.gpio1.interfaces", std::vector{"dig.1", "dig.2"}}, + {"command_interfaces.gpio2.interfaces", std::vector{"ana.1"}}}); const auto result = controller.init("test_gpio_command_controller", "", 0, "", node_options); setup_command_and_state_interfaces(); ASSERT_EQ(result, controller_interface::return_type::OK); @@ -261,8 +262,8 @@ TEST_F( { const auto node_options = create_node_options_with_overriden_parameters( {{"gpios", gpio_names}, - {"command_interfaces.gpio1.ports", std::vector{"dig.1", "dig.2"}}, - {"command_interfaces.gpio2.ports", std::vector{"ana.1"}}}); + {"command_interfaces.gpio1.interfaces", std::vector{"dig.1", "dig.2"}}, + {"command_interfaces.gpio2.interfaces", std::vector{"ana.1"}}}); const auto result = controller.init("test_gpio_command_controller", "", 0, "", node_options); std::vector command_interfaces; @@ -287,8 +288,8 @@ TEST_F( { const auto node_options = create_node_options_with_overriden_parameters( {{"gpios", gpio_names}, - {"command_interfaces.gpio1.ports", std::vector{"dig.1", "dig.2"}}, - {"command_interfaces.gpio2.ports", std::vector{"ana.1"}}}); + {"command_interfaces.gpio1.interfaces", std::vector{"dig.1", "dig.2"}}, + {"command_interfaces.gpio2.interfaces", std::vector{"ana.1"}}}); const auto result = controller.init("test_gpio_command_controller", "", 0, "", node_options); std::vector command_interfaces; @@ -313,8 +314,8 @@ TEST_F( { const auto node_options = create_node_options_with_overriden_parameters( {{"gpios", gpio_names}, - {"command_interfaces.gpio1.ports", std::vector{"dig.1", "dig.2"}}, - {"command_interfaces.gpio2.ports", std::vector{"ana.1"}}}); + {"command_interfaces.gpio1.interfaces", std::vector{"dig.1", "dig.2"}}, + {"command_interfaces.gpio2.interfaces", std::vector{"ana.1"}}}); setup_command_and_state_interfaces(); move_to_activate_state(controller.init("test_gpio_command_controller", "", 0, "", node_options)); @@ -329,8 +330,8 @@ TEST_F( { const auto node_options = create_node_options_with_overriden_parameters( {{"gpios", gpio_names}, - {"command_interfaces.gpio1.ports", std::vector{"dig.1", "dig.2"}}, - {"command_interfaces.gpio2.ports", std::vector{"ana.1"}}}); + {"command_interfaces.gpio1.interfaces", std::vector{"dig.1", "dig.2"}}, + {"command_interfaces.gpio2.interfaces", std::vector{"ana.1"}}}); setup_command_and_state_interfaces(); move_to_activate_state(controller.init("test_gpio_command_controller", "", 0, "", node_options)); @@ -350,8 +351,8 @@ TEST_F( { const auto node_options = create_node_options_with_overriden_parameters( {{"gpios", gpio_names}, - {"command_interfaces.gpio1.ports", std::vector{"dig.1", "dig.2"}}, - {"command_interfaces.gpio2.ports", std::vector{"ana.1"}}}); + {"command_interfaces.gpio1.interfaces", std::vector{"dig.1", "dig.2"}}, + {"command_interfaces.gpio2.interfaces", std::vector{"ana.1"}}}); setup_command_and_state_interfaces(); move_to_activate_state(controller.init("test_gpio_command_controller", "", 0, "", node_options)); @@ -371,8 +372,8 @@ TEST_F( { const auto node_options = create_node_options_with_overriden_parameters( {{"gpios", gpio_names}, - {"command_interfaces.gpio1.ports", std::vector{"dig.1", "dig.2"}}, - {"command_interfaces.gpio2.ports", std::vector{"ana.1"}}}); + {"command_interfaces.gpio1.interfaces", std::vector{"dig.1", "dig.2"}}, + {"command_interfaces.gpio2.interfaces", std::vector{"ana.1"}}}); setup_command_and_state_interfaces(); move_to_activate_state(controller.init("test_gpio_command_controller", "", 0, "", node_options)); @@ -390,12 +391,12 @@ TEST_F( TEST_F( GpioCommandControllerTestSuite, - WhenGivenCommandPortsInDifferentOrderThenValueOfInterfacesShouldBeUpdated) + WhenGivenCommandInterfacesInDifferentOrderThenValueOfInterfacesShouldBeUpdated) { const auto node_options = create_node_options_with_overriden_parameters( {{"gpios", gpio_names}, - {"command_interfaces.gpio1.ports", std::vector{"dig.1", "dig.2"}}, - {"command_interfaces.gpio2.ports", std::vector{"ana.1"}}}); + {"command_interfaces.gpio1.interfaces", std::vector{"dig.1", "dig.2"}}, + {"command_interfaces.gpio2.interfaces", std::vector{"ana.1"}}}); setup_command_and_state_interfaces(); move_to_activate_state(controller.init("test_gpio_command_controller", "", 0, "", node_options)); @@ -417,8 +418,8 @@ TEST_F( { const auto node_options = create_node_options_with_overriden_parameters( {{"gpios", gpio_names}, - {"command_interfaces.gpio1.ports", std::vector{"dig.1", "dig.2"}}, - {"command_interfaces.gpio2.ports", std::vector{"ana.1"}}}); + {"command_interfaces.gpio1.interfaces", std::vector{"dig.1", "dig.2"}}, + {"command_interfaces.gpio2.interfaces", std::vector{"ana.1"}}}); const auto result = controller.init("test_gpio_command_controller", "", 0, "", node_options); setup_command_and_state_interfaces(); move_to_activate_state(result); @@ -436,12 +437,12 @@ TEST_F( TEST_F( GpioCommandControllerTestSuite, - WhenGivenCommandContainsWrongGpioPortsOrWrongGpioNameThenCommandInterfacesShouldNotBeUpdated) + WhenGivenCmdContainsWrongGpioInterfacesOrWrongGpioNameThenCommandInterfacesShouldNotBeUpdated) { const auto node_options = create_node_options_with_overriden_parameters( {{"gpios", gpio_names}, - {"command_interfaces.gpio1.ports", std::vector{"dig.1", "dig.2"}}, - {"command_interfaces.gpio2.ports", std::vector{"ana.1"}}}); + {"command_interfaces.gpio1.interfaces", std::vector{"dig.1", "dig.2"}}, + {"command_interfaces.gpio2.interfaces", std::vector{"ana.1"}}}); const auto result = controller.init("test_gpio_command_controller", "", 0, "", node_options); setup_command_and_state_interfaces(); move_to_activate_state(result); @@ -464,8 +465,8 @@ TEST_F( { const auto node_options = create_node_options_with_overriden_parameters( {{"gpios", gpio_names}, - {"command_interfaces.gpio1.ports", std::vector{"dig.1", "dig.2"}}, - {"command_interfaces.gpio2.ports", std::vector{"ana.1"}}}); + {"command_interfaces.gpio1.interfaces", std::vector{"dig.1", "dig.2"}}, + {"command_interfaces.gpio2.interfaces", std::vector{"ana.1"}}}); setup_command_and_state_interfaces(); move_to_activate_state(controller.init("test_gpio_command_controller", "", 0, "", node_options)); @@ -487,8 +488,8 @@ TEST_F(GpioCommandControllerTestSuite, ControllerShouldPublishGpioStatesWithCurr { const auto node_options = create_node_options_with_overriden_parameters( {{"gpios", gpio_names}, - {"command_interfaces.gpio1.ports", std::vector{"dig.1", "dig.2"}}, - {"command_interfaces.gpio2.ports", std::vector{"ana.1"}}}); + {"command_interfaces.gpio1.interfaces", std::vector{"dig.1", "dig.2"}}, + {"command_interfaces.gpio2.interfaces", std::vector{"ana.1"}}}); setup_command_and_state_interfaces(); move_to_activate_state(controller.init("test_gpio_command_controller", "", 0, "", node_options)); From 1e611119e6ee12c338cb5a3d8d2366d3063f44ea Mon Sep 17 00:00:00 2001 From: Wiktor Bajor Date: Mon, 4 Nov 2024 17:58:19 +0100 Subject: [PATCH 48/58] Parametrize state interfaces --- .../gpio_command_controller.hpp | 26 +- .../src/gpio_command_controller.cpp | 225 ++++++++++++------ .../gpio_command_controller_parameters.yaml | 14 +- .../test/test_gpio_command_controller.cpp | 82 +++++-- 4 files changed, 252 insertions(+), 95 deletions(-) diff --git a/gpio_controllers/include/gpio_controllers/gpio_command_controller.hpp b/gpio_controllers/include/gpio_controllers/gpio_command_controller.hpp index 975680a663..45f95c528c 100644 --- a/gpio_controllers/include/gpio_controllers/gpio_command_controller.hpp +++ b/gpio_controllers/include/gpio_controllers/gpio_command_controller.hpp @@ -37,6 +37,8 @@ using CallbackReturn = controller_interface::CallbackReturn; using InterfacesNames = std::vector; using MapOfReferencesToCommandInterfaces = std::unordered_map< std::string, std::reference_wrapper>; +using MapOfReferencesToStateInterfaces = + std::unordered_map>; using StateInterfaces = std::vector>; @@ -69,20 +71,32 @@ class GpioCommandController : public controller_interface::ControllerInterface const rclcpp::Time & time, const rclcpp::Duration & period) override; private: - void store_interface_types(); + void store_command_interface_types(); + void store_state_interface_types(); void initialize_gpio_state_msg(); void update_gpios_states(); controller_interface::return_type update_gpios_commands(); - MapOfReferencesToCommandInterfaces create_map_of_references_to_interfaces( - const InterfacesNames & interfaces_from_params); + template + std::unordered_map> create_map_of_references_to_interfaces( + const InterfacesNames & interfaces_from_params, std::vector & configured_interfaces); template bool check_if_configured_interfaces_matches_received( - const InterfacesNames & interfaces_from_params, const T & interfaces_map); + const InterfacesNames & interfaces_from_params, const T & configured_interfaces); + 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; + 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; + bool update_dynamic_map_parameters(); protected: - InterfacesNames interface_types_; + InterfacesNames command_interface_types_; + InterfacesNames state_interface_types_; MapOfReferencesToCommandInterfaces command_interfaces_map_; - StateInterfaces ordered_state_interfaces_; + MapOfReferencesToStateInterfaces state_interfaces_map_; realtime_tools::RealtimeBuffer> rt_command_ptr_{}; rclcpp::Subscription::SharedPtr gpios_command_subscriber_{}; diff --git a/gpio_controllers/src/gpio_command_controller.cpp b/gpio_controllers/src/gpio_command_controller.cpp index 0218c71342..9aed3bde82 100644 --- a/gpio_controllers/src/gpio_command_controller.cpp +++ b/gpio_controllers/src/gpio_command_controller.cpp @@ -23,17 +23,8 @@ namespace { -void print_interface( - const rclcpp::Logger & logger, const gpio_controllers::StateInterfaces & state_interfaces) -{ - for (const auto & interface : state_interfaces) - { - RCLCPP_ERROR(logger, "Got %s", interface.get().get_name().c_str()); - } -} -void print_interface( - const rclcpp::Logger & logger, - const gpio_controllers::MapOfReferencesToCommandInterfaces & command_interfaces) +template +void print_interface(const rclcpp::Logger & logger, const T & command_interfaces) { for (const auto & [interface_name, value] : command_interfaces) { @@ -63,21 +54,12 @@ catch (const std::exception & e) CallbackReturn GpioCommandController::on_configure(const rclcpp_lifecycle::State &) try { - auto logger = get_node()->get_logger(); - - if (!param_listener_) + if (!update_dynamic_map_parameters()) { - RCLCPP_ERROR(logger, "Error encountered during init"); return controller_interface::CallbackReturn::ERROR; } - - // update the dynamic map parameters - param_listener_->refresh_dynamic_parameters(); - - // get parameters from the listener in case they were updated - params_ = param_listener_->get_params(); - - store_interface_types(); + store_command_interface_types(); + store_state_interface_types(); gpios_command_subscriber_ = get_node()->create_subscription( "~/commands", rclcpp::SystemDefaultsQoS(), [this](const CmdType::SharedPtr msg) { rt_command_ptr_.writeFromNonRT(msg); }); @@ -101,7 +83,7 @@ GpioCommandController::command_interface_configuration() const { controller_interface::InterfaceConfiguration command_interfaces_config; command_interfaces_config.type = controller_interface::interface_configuration_type::INDIVIDUAL; - command_interfaces_config.names = interface_types_; + command_interfaces_config.names = command_interface_types_; return command_interfaces_config; } @@ -111,20 +93,21 @@ controller_interface::InterfaceConfiguration GpioCommandController::state_interf { controller_interface::InterfaceConfiguration state_interfaces_config; state_interfaces_config.type = controller_interface::interface_configuration_type::INDIVIDUAL; - state_interfaces_config.names = interface_types_; + state_interfaces_config.names = state_interface_types_; return state_interfaces_config; } CallbackReturn GpioCommandController::on_activate(const rclcpp_lifecycle::State &) { - command_interfaces_map_ = create_map_of_references_to_interfaces(interface_types_); - controller_interface::get_ordered_interfaces( - state_interfaces_, interface_types_, "", ordered_state_interfaces_); - + command_interfaces_map_ = + create_map_of_references_to_interfaces(command_interface_types_, command_interfaces_); + state_interfaces_map_ = + create_map_of_references_to_interfaces(state_interface_types_, state_interfaces_); if ( - !check_if_configured_interfaces_matches_received(interface_types_, command_interfaces_map_) || - !check_if_configured_interfaces_matches_received(interface_types_, ordered_state_interfaces_)) + !check_if_configured_interfaces_matches_received( + command_interface_types_, command_interfaces_map_) || + !check_if_configured_interfaces_matches_received(state_interface_types_, state_interfaces_map_)) { return CallbackReturn::ERROR; } @@ -148,13 +131,72 @@ controller_interface::return_type GpioCommandController::update( return update_gpios_commands(); } -void GpioCommandController::store_interface_types() +bool GpioCommandController::update_dynamic_map_parameters() +{ + auto logger = get_node()->get_logger(); + if (!param_listener_) + { + RCLCPP_ERROR(logger, "Error encountered during init"); + return false; + } + // update the dynamic map parameters + param_listener_->refresh_dynamic_parameters(); + // get parameters from the listener in case they were updated + params_ = param_listener_->get_params(); + return true; +} + +void GpioCommandController::store_command_interface_types() { for (const auto & [gpio_name, interfaces] : params_.command_interfaces.gpios_map) { std::transform( - interfaces.interfaces.begin(), interfaces.interfaces.end(), - std::back_inserter(interface_types_), + interfaces.interfaces.cbegin(), interfaces.interfaces.cend(), + std::back_inserter(command_interface_types_), + [&](const auto & interface_name) { return gpio_name + "/" + interface_name; }); + } +} + +bool GpioCommandController::should_broadcast_all_interfaces_of_configured_gpios() const +{ + auto are_interfaces_not_empty = [](const auto & interfaces) + { return !interfaces.second.interfaces.empty(); }; + return std::none_of( + params_.state_interfaces.gpios_map.cbegin(), params_.state_interfaces.gpios_map.cend(), + are_interfaces_not_empty); +} + +void GpioCommandController::set_all_state_interfaces_of_configured_gpios() +{ + for (const auto & gpio : params_.gpios) + { + for (auto & interface : state_interfaces_) + { + if (gpio == interface.get_prefix_name()) + { + state_interface_types_.push_back(interface.get_name()); + } + } + } +} + +void GpioCommandController::store_state_interface_types() +{ + if (should_broadcast_all_interfaces_of_configured_gpios()) + { + RCLCPP_INFO( + get_node()->get_logger(), + "State interfaces are not configured. All available interfaces of configured GPIOs will be " + "broadcasted."); + set_all_state_interfaces_of_configured_gpios(); + return; + } + + for (const auto & [gpio_name, interfaces] : params_.state_interfaces.gpios_map) + { + std::transform( + interfaces.interfaces.cbegin(), interfaces.interfaces.cend(), + std::back_inserter(state_interface_types_), [&](const auto & interface_name) { return gpio_name + "/" + interface_name; }); } } @@ -170,30 +212,45 @@ void GpioCommandController::initialize_gpio_state_msg() { const auto gpio_name = params_.gpios[gpio_index]; gpio_state_msg.joint_names[gpio_index] = gpio_name; - std::copy( - params_.command_interfaces.gpios_map[gpio_name].interfaces.begin(), - params_.command_interfaces.gpios_map[gpio_name].interfaces.end(), - std::back_insert_iterator(gpio_state_msg.interface_values[gpio_index].interface_names)); + 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( - params_.command_interfaces.gpios_map[gpio_name].interfaces.size(), + gpio_state_msg.interface_values[gpio_index].interface_names.size(), std::numeric_limits::quiet_NaN()); } } -MapOfReferencesToCommandInterfaces GpioCommandController::create_map_of_references_to_interfaces( - const InterfacesNames & interfaces_from_params) +InterfacesNames GpioCommandController::get_gpios_state_interfaces_names( + const std::string & gpio_name) const +{ + InterfacesNames result; + for (const auto & interface_name : state_interface_types_) + { + const auto it = state_interfaces_map_.find(interface_name); + if (it != state_interfaces_map_.cend() && it->second.get().get_prefix_name() == gpio_name) + { + result.emplace_back(it->second.get().get_interface_name()); + } + } + return result; +} + +template +std::unordered_map> +GpioCommandController::create_map_of_references_to_interfaces( + const InterfacesNames & interfaces_from_params, std::vector & configured_interfaces) { - MapOfReferencesToCommandInterfaces map; + std::unordered_map> map; for (const auto & interface_name : interfaces_from_params) { auto interface = std::find_if( - command_interfaces_.begin(), command_interfaces_.end(), + configured_interfaces.begin(), configured_interfaces.end(), [&](const auto & configured_interface) { const auto full_name_interface_name = configured_interface.get_name(); return full_name_interface_name == interface_name; }); - if (interface != command_interfaces_.end()) + if (interface != configured_interfaces.end()) { map.emplace(interface_name, std::ref(*interface)); } @@ -241,51 +298,71 @@ controller_interface::return_type GpioCommandController::update_gpios_commands() gpio_name.c_str()); return controller_interface::return_type::ERROR; } - for (std::size_t command_interface_index = 0; command_interface_index < gpio_commands.interface_values[gpio_index].values.size(); ++command_interface_index) { - const auto & full_command_interface_name = - gpio_name + '/' + - gpio_commands.interface_values[gpio_index].interface_names[command_interface_index]; - try - { - command_interfaces_map_.at(full_command_interface_name) - .get() - .set_value(gpio_commands.interface_values[gpio_index].values[command_interface_index]); - } - catch (const std::exception & e) - { - fprintf( - stderr, "Exception thrown during applying command stage of %s with message: %s \n", - full_command_interface_name.c_str(), e.what()); - } + apply_command(gpio_commands, gpio_index, command_interface_index); } } return controller_interface::return_type::OK; } +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_values[gpio_index].interface_names[command_interface_index]; + try + { + command_interfaces_map_.at(full_command_interface_name) + .get() + .set_value(gpio_commands.interface_values[gpio_index].values[command_interface_index]); + } + catch (const std::exception & e) + { + fprintf( + stderr, "Exception thrown during applying command stage of %s with message: %s \n", + full_command_interface_name.c_str(), e.what()); + } +} + void GpioCommandController::update_gpios_states() { - if (realtime_gpio_state_publisher_ && realtime_gpio_state_publisher_->trylock()) + if (!realtime_gpio_state_publisher_ || !realtime_gpio_state_publisher_->trylock()) { - auto & gpio_state_msg = realtime_gpio_state_publisher_->msg_; - gpio_state_msg.header.stamp = get_node()->now(); + return; + } - std::size_t state_index{}; - for (std::size_t gpio_index = 0; gpio_index < gpio_state_msg.joint_names.size(); ++gpio_index) + 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 interface_index = 0; + interface_index < gpio_state_msg.interface_values[gpio_index].interface_names.size(); + ++interface_index) { - for (std::size_t interface_index = 0; - interface_index < gpio_state_msg.interface_values[gpio_index].interface_names.size(); - ++interface_index) - { - gpio_state_msg.interface_values[gpio_index].values[interface_index] = - ordered_state_interfaces_[state_index].get().get_value(); - ++state_index; - } + apply_state_value(gpio_state_msg, gpio_index, interface_index); } - realtime_gpio_state_publisher_->unlockAndPublish(); + } + realtime_gpio_state_publisher_->unlockAndPublish(); +} + +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_values[gpio_index].interface_names[interface_index]; + try + { + state_msg.interface_values[gpio_index].values[interface_index] = + state_interfaces_map_.at(interface_name).get().get_value(); + } + catch (const std::exception & e) + { + fprintf(stderr, "Exception thrown during reading state of: %s \n", interface_name.c_str()); } } diff --git a/gpio_controllers/src/gpio_command_controller_parameters.yaml b/gpio_controllers/src/gpio_command_controller_parameters.yaml index cbfafaf5f3..3f8606ed2e 100644 --- a/gpio_controllers/src/gpio_command_controller_parameters.yaml +++ b/gpio_controllers/src/gpio_command_controller_parameters.yaml @@ -13,10 +13,22 @@ gpio_command_controller_parameters: __map_gpios: interfaces: { type: string_array, - description: "List of interfaces for each gpio", + description: "List of command interfaces for each gpio.", read_only: true, validation: { size_gt<>: [0], unique<>: null } } + + state_interfaces: + __map_gpios: + interfaces: { + type: string_array, + description: "List of state interfaces for each gpio. If empty all available gpios' states are used.", + read_only: true, + default_value: [], + validation: { + unique<>: null + } + } diff --git a/gpio_controllers/test/test_gpio_command_controller.cpp b/gpio_controllers/test/test_gpio_command_controller.cpp index ab6893d0e7..cbcb2c4fb7 100644 --- a/gpio_controllers/test/test_gpio_command_controller.cpp +++ b/gpio_controllers/test/test_gpio_command_controller.cpp @@ -237,7 +237,9 @@ TEST_F( const auto node_options = create_node_options_with_overriden_parameters( {{"gpios", gpio_names}, {"command_interfaces.gpio1.interfaces", std::vector{"dig.1", "dig.2"}}, - {"command_interfaces.gpio2.interfaces", std::vector{"ana.1"}}}); + {"command_interfaces.gpio2.interfaces", std::vector{"ana.1"}}, + {"state_interfaces.gpio1.interfaces", std::vector{"dig.1", "dig.2"}}, + {"state_interfaces.gpio2.interfaces", std::vector{"ana.1"}}}); const auto result = controller.init("test_gpio_command_controller", "", 0, "", node_options); ASSERT_EQ(result, controller_interface::return_type::OK); @@ -248,7 +250,9 @@ TEST_F(GpioCommandControllerTestSuite, ConfigureAndActivateParamsSuccess) const auto node_options = create_node_options_with_overriden_parameters( {{"gpios", gpio_names}, {"command_interfaces.gpio1.interfaces", std::vector{"dig.1", "dig.2"}}, - {"command_interfaces.gpio2.interfaces", std::vector{"ana.1"}}}); + {"command_interfaces.gpio2.interfaces", std::vector{"ana.1"}}, + {"state_interfaces.gpio1.interfaces", std::vector{"dig.1", "dig.2"}}, + {"state_interfaces.gpio2.interfaces", std::vector{"ana.1"}}}); const auto result = controller.init("test_gpio_command_controller", "", 0, "", node_options); setup_command_and_state_interfaces(); ASSERT_EQ(result, controller_interface::return_type::OK); @@ -263,7 +267,9 @@ TEST_F( const auto node_options = create_node_options_with_overriden_parameters( {{"gpios", gpio_names}, {"command_interfaces.gpio1.interfaces", std::vector{"dig.1", "dig.2"}}, - {"command_interfaces.gpio2.interfaces", std::vector{"ana.1"}}}); + {"command_interfaces.gpio2.interfaces", std::vector{"ana.1"}}, + {"state_interfaces.gpio1.interfaces", std::vector{"dig.1", "dig.2"}}, + {"state_interfaces.gpio2.interfaces", std::vector{"ana.1"}}}); const auto result = controller.init("test_gpio_command_controller", "", 0, "", node_options); std::vector command_interfaces; @@ -284,12 +290,14 @@ TEST_F( TEST_F( GpioCommandControllerTestSuite, - WhenAssignedSateInterfacesDoNotMatchInterfacesFromParamsThenControllerShouldFailOnActivation) + WhenAssignedStateInterfacesDoNotMatchInterfacesFromParamsThenControllerShouldFailOnActivation) { const auto node_options = create_node_options_with_overriden_parameters( {{"gpios", gpio_names}, {"command_interfaces.gpio1.interfaces", std::vector{"dig.1", "dig.2"}}, - {"command_interfaces.gpio2.interfaces", std::vector{"ana.1"}}}); + {"command_interfaces.gpio2.interfaces", std::vector{"ana.1"}}, + {"state_interfaces.gpio1.interfaces", std::vector{"dig.1", "dig.2"}}, + {"state_interfaces.gpio2.interfaces", std::vector{"ana.1"}}}); const auto result = controller.init("test_gpio_command_controller", "", 0, "", node_options); std::vector command_interfaces; @@ -308,6 +316,34 @@ TEST_F( ASSERT_EQ(controller.on_activate(rclcpp_lifecycle::State()), CallbackReturn::ERROR); } +TEST_F( + GpioCommandControllerTestSuite, + WhenCommandInterfacesDontMatchStatesButBothMatchAssignedOnesThenOnActivationShouldSucceed) +{ + const auto node_options = create_node_options_with_overriden_parameters( + {{"gpios", gpio_names}, + {"command_interfaces.gpio1.interfaces", std::vector{"dig.1", "dig.2"}}, + {"command_interfaces.gpio2.interfaces", std::vector{"ana.1"}}, + {"state_interfaces.gpio1.interfaces", std::vector{"dig.1"}}, + {"state_interfaces.gpio2.interfaces", std::vector{"ana.1"}}}); + const auto result = controller.init("test_gpio_command_controller", "", 0, "", node_options); + + std::vector command_interfaces; + command_interfaces.emplace_back(gpio_1_1_dig_cmd); + command_interfaces.emplace_back(gpio_1_2_dig_cmd); + command_interfaces.emplace_back(gpio_2_ana_cmd); + + std::vector state_interfaces; + state_interfaces.emplace_back(gpio_1_1_dig_state); + state_interfaces.emplace_back(gpio_2_ana_state); + + controller.assign_interfaces(std::move(command_interfaces), std::move(state_interfaces)); + + ASSERT_EQ(result, controller_interface::return_type::OK); + ASSERT_EQ(controller.on_configure(rclcpp_lifecycle::State()), CallbackReturn::SUCCESS); + ASSERT_EQ(controller.on_activate(rclcpp_lifecycle::State()), CallbackReturn::SUCCESS); +} + TEST_F( GpioCommandControllerTestSuite, WhenThereWasNoCommandForGpiosThenCommandInterfacesShouldHaveDefaultValues) @@ -315,7 +351,9 @@ TEST_F( const auto node_options = create_node_options_with_overriden_parameters( {{"gpios", gpio_names}, {"command_interfaces.gpio1.interfaces", std::vector{"dig.1", "dig.2"}}, - {"command_interfaces.gpio2.interfaces", std::vector{"ana.1"}}}); + {"command_interfaces.gpio2.interfaces", std::vector{"ana.1"}}, + {"state_interfaces.gpio1.interfaces", std::vector{"dig.1", "dig.2"}}, + {"state_interfaces.gpio2.interfaces", std::vector{"ana.1"}}}); setup_command_and_state_interfaces(); move_to_activate_state(controller.init("test_gpio_command_controller", "", 0, "", node_options)); @@ -331,7 +369,9 @@ TEST_F( const auto node_options = create_node_options_with_overriden_parameters( {{"gpios", gpio_names}, {"command_interfaces.gpio1.interfaces", std::vector{"dig.1", "dig.2"}}, - {"command_interfaces.gpio2.interfaces", std::vector{"ana.1"}}}); + {"command_interfaces.gpio2.interfaces", std::vector{"ana.1"}}, + {"state_interfaces.gpio1.interfaces", std::vector{"dig.1", "dig.2"}}, + {"state_interfaces.gpio2.interfaces", std::vector{"ana.1"}}}); setup_command_and_state_interfaces(); move_to_activate_state(controller.init("test_gpio_command_controller", "", 0, "", node_options)); @@ -352,7 +392,9 @@ TEST_F( const auto node_options = create_node_options_with_overriden_parameters( {{"gpios", gpio_names}, {"command_interfaces.gpio1.interfaces", std::vector{"dig.1", "dig.2"}}, - {"command_interfaces.gpio2.interfaces", std::vector{"ana.1"}}}); + {"command_interfaces.gpio2.interfaces", std::vector{"ana.1"}}, + {"state_interfaces.gpio1.interfaces", std::vector{"dig.1", "dig.2"}}, + {"state_interfaces.gpio2.interfaces", std::vector{"ana.1"}}}); setup_command_and_state_interfaces(); move_to_activate_state(controller.init("test_gpio_command_controller", "", 0, "", node_options)); @@ -373,7 +415,9 @@ TEST_F( const auto node_options = create_node_options_with_overriden_parameters( {{"gpios", gpio_names}, {"command_interfaces.gpio1.interfaces", std::vector{"dig.1", "dig.2"}}, - {"command_interfaces.gpio2.interfaces", std::vector{"ana.1"}}}); + {"command_interfaces.gpio2.interfaces", std::vector{"ana.1"}}, + {"state_interfaces.gpio1.interfaces", std::vector{"dig.1", "dig.2"}}, + {"state_interfaces.gpio2.interfaces", std::vector{"ana.1"}}}); setup_command_and_state_interfaces(); move_to_activate_state(controller.init("test_gpio_command_controller", "", 0, "", node_options)); @@ -396,7 +440,9 @@ TEST_F( const auto node_options = create_node_options_with_overriden_parameters( {{"gpios", gpio_names}, {"command_interfaces.gpio1.interfaces", std::vector{"dig.1", "dig.2"}}, - {"command_interfaces.gpio2.interfaces", std::vector{"ana.1"}}}); + {"command_interfaces.gpio2.interfaces", std::vector{"ana.1"}}, + {"state_interfaces.gpio1.interfaces", std::vector{"dig.1", "dig.2"}}, + {"state_interfaces.gpio2.interfaces", std::vector{"ana.1"}}}); setup_command_and_state_interfaces(); move_to_activate_state(controller.init("test_gpio_command_controller", "", 0, "", node_options)); @@ -419,7 +465,9 @@ TEST_F( const auto node_options = create_node_options_with_overriden_parameters( {{"gpios", gpio_names}, {"command_interfaces.gpio1.interfaces", std::vector{"dig.1", "dig.2"}}, - {"command_interfaces.gpio2.interfaces", std::vector{"ana.1"}}}); + {"command_interfaces.gpio2.interfaces", std::vector{"ana.1"}}, + {"state_interfaces.gpio1.interfaces", std::vector{"dig.1", "dig.2"}}, + {"state_interfaces.gpio2.interfaces", std::vector{"ana.1"}}}); const auto result = controller.init("test_gpio_command_controller", "", 0, "", node_options); setup_command_and_state_interfaces(); move_to_activate_state(result); @@ -442,7 +490,9 @@ TEST_F( const auto node_options = create_node_options_with_overriden_parameters( {{"gpios", gpio_names}, {"command_interfaces.gpio1.interfaces", std::vector{"dig.1", "dig.2"}}, - {"command_interfaces.gpio2.interfaces", std::vector{"ana.1"}}}); + {"command_interfaces.gpio2.interfaces", std::vector{"ana.1"}}, + {"state_interfaces.gpio1.interfaces", std::vector{"dig.1", "dig.2"}}, + {"state_interfaces.gpio2.interfaces", std::vector{"ana.1"}}}); const auto result = controller.init("test_gpio_command_controller", "", 0, "", node_options); setup_command_and_state_interfaces(); move_to_activate_state(result); @@ -466,7 +516,9 @@ TEST_F( const auto node_options = create_node_options_with_overriden_parameters( {{"gpios", gpio_names}, {"command_interfaces.gpio1.interfaces", std::vector{"dig.1", "dig.2"}}, - {"command_interfaces.gpio2.interfaces", std::vector{"ana.1"}}}); + {"command_interfaces.gpio2.interfaces", std::vector{"ana.1"}}, + {"state_interfaces.gpio1.interfaces", std::vector{"dig.1", "dig.2"}}, + {"state_interfaces.gpio2.interfaces", std::vector{"ana.1"}}}); setup_command_and_state_interfaces(); move_to_activate_state(controller.init("test_gpio_command_controller", "", 0, "", node_options)); @@ -489,7 +541,9 @@ TEST_F(GpioCommandControllerTestSuite, ControllerShouldPublishGpioStatesWithCurr const auto node_options = create_node_options_with_overriden_parameters( {{"gpios", gpio_names}, {"command_interfaces.gpio1.interfaces", std::vector{"dig.1", "dig.2"}}, - {"command_interfaces.gpio2.interfaces", std::vector{"ana.1"}}}); + {"command_interfaces.gpio2.interfaces", std::vector{"ana.1"}}, + {"state_interfaces.gpio1.interfaces", std::vector{"dig.1", "dig.2"}}, + {"state_interfaces.gpio2.interfaces", std::vector{"ana.1"}}}); setup_command_and_state_interfaces(); move_to_activate_state(controller.init("test_gpio_command_controller", "", 0, "", node_options)); From 9929eb18d3b0e2c2c7d8b78c3a0afcaa601fb914 Mon Sep 17 00:00:00 2001 From: Wiktor Bajor Date: Mon, 4 Nov 2024 19:17:46 +0100 Subject: [PATCH 49/58] Parse URDF to get gpio state interfaces, add test and docs --- gpio_controllers/CMakeLists.txt | 3 + gpio_controllers/doc/userdoc.rst | 11 +- .../gpio_command_controller.hpp | 3 + .../src/gpio_command_controller.cpp | 54 ++++- .../gpio_command_controller_parameters.yaml | 2 +- .../test/test_gpio_command_controller.cpp | 199 ++++++++++++++---- 6 files changed, 224 insertions(+), 48 deletions(-) diff --git a/gpio_controllers/CMakeLists.txt b/gpio_controllers/CMakeLists.txt index b96b6f96e2..52dd87ebf9 100644 --- a/gpio_controllers/CMakeLists.txt +++ b/gpio_controllers/CMakeLists.txt @@ -20,6 +20,7 @@ find_package(rclcpp_lifecycle REQUIRED) find_package(realtime_tools REQUIRED) find_package(std_msgs REQUIRED) find_package(generate_parameter_library REQUIRED) +find_package(urdf REQUIRED) generate_parameter_library(gpio_command_controller_parameters @@ -41,6 +42,7 @@ ament_target_dependencies(gpio_controllers PUBLIC rcutils realtime_tools std_msgs + urdf ) # Causes the visibility macros to use dllexport rather than dllimport, # which is appropriate when building the dll but not consuming it. @@ -93,6 +95,7 @@ if(BUILD_TESTING) rclcpp_lifecycle realtime_tools std_msgs + ros2_control_test_assets ) endif() diff --git a/gpio_controllers/doc/userdoc.rst b/gpio_controllers/doc/userdoc.rst index f7bdea8c6e..e3034b92df 100644 --- a/gpio_controllers/doc/userdoc.rst +++ b/gpio_controllers/doc/userdoc.rst @@ -23,7 +23,9 @@ This controller uses the `generate_parameter_library get_gpios_from_urdf() const; protected: InterfacesNames command_interface_types_; @@ -106,6 +108,7 @@ class GpioCommandController : public controller_interface::ControllerInterface std::shared_ptr param_listener_{}; gpio_command_controller_parameters::Params params_; + urdf::Model model_; }; } // namespace gpio_controllers diff --git a/gpio_controllers/src/gpio_command_controller.cpp b/gpio_controllers/src/gpio_command_controller.cpp index 9aed3bde82..df99518ae0 100644 --- a/gpio_controllers/src/gpio_command_controller.cpp +++ b/gpio_controllers/src/gpio_command_controller.cpp @@ -17,6 +17,7 @@ #include #include "controller_interface/helpers.hpp" +#include "hardware_interface/component_parser.hpp" #include "rclcpp/logging.hpp" #include "rclcpp/qos.hpp" #include "rclcpp/subscription.hpp" @@ -31,6 +32,18 @@ void print_interface(const rclcpp::Logger & logger, const T & command_interfaces RCLCPP_ERROR(logger, "Got %s", interface_name.c_str()); } } + +std::vector extract_gpios_from_hardware_info( + const std::vector & hardware_infos) +{ + std::vector result; + for (const auto & hardware_info : hardware_infos) + { + std::copy( + hardware_info.gpios.begin(), hardware_info.gpios.end(), std::back_insert_iterator(result)); + } + return result; +} } // namespace namespace gpio_controllers @@ -58,8 +71,16 @@ try { return controller_interface::CallbackReturn::ERROR; } + store_command_interface_types(); store_state_interface_types(); + + if (command_interface_types_.empty() && state_interface_types_.empty()) + { + RCLCPP_ERROR(get_node()->get_logger(), "No command or state interfaces are configured"); + return CallbackReturn::ERROR; + } + gpios_command_subscriber_ = get_node()->create_subscription( "~/commands", rclcpp::SystemDefaultsQoS(), [this](const CmdType::SharedPtr msg) { rt_command_ptr_.writeFromNonRT(msg); }); @@ -159,22 +180,39 @@ void GpioCommandController::store_command_interface_types() bool GpioCommandController::should_broadcast_all_interfaces_of_configured_gpios() const { - auto are_interfaces_not_empty = [](const auto & interfaces) - { return !interfaces.second.interfaces.empty(); }; - return std::none_of( + auto are_interfaces_empty = [](const auto & interfaces) + { return interfaces.second.interfaces.empty(); }; + return std::all_of( params_.state_interfaces.gpios_map.cbegin(), params_.state_interfaces.gpios_map.cend(), - are_interfaces_not_empty); + are_interfaces_empty); +} + +std::vector GpioCommandController::get_gpios_from_urdf() const +try +{ + return extract_gpios_from_hardware_info( + hardware_interface::parse_control_resources_from_urdf(get_robot_description())); +} +catch (const std::exception & e) +{ + fprintf(stderr, "Exception thrown during extracting gpios info from urdf %s \n", e.what()); + return {}; } void GpioCommandController::set_all_state_interfaces_of_configured_gpios() { - for (const auto & gpio : params_.gpios) + const auto gpios{get_gpios_from_urdf()}; + for (const auto & gpio_name : params_.gpios) { - for (auto & interface : state_interfaces_) + for (const auto & gpio : gpios) { - if (gpio == interface.get_prefix_name()) + if (gpio_name == gpio.name) { - state_interface_types_.push_back(interface.get_name()); + std::transform( + gpio.state_interfaces.begin(), gpio.state_interfaces.end(), + std::back_insert_iterator(state_interface_types_), + [&gpio_name](const auto & interface_name) + { return gpio_name + '/' + interface_name.name; }); } } } diff --git a/gpio_controllers/src/gpio_command_controller_parameters.yaml b/gpio_controllers/src/gpio_command_controller_parameters.yaml index 3f8606ed2e..0c6aade35f 100644 --- a/gpio_controllers/src/gpio_command_controller_parameters.yaml +++ b/gpio_controllers/src/gpio_command_controller_parameters.yaml @@ -15,8 +15,8 @@ gpio_command_controller_parameters: type: string_array, description: "List of command interfaces for each gpio.", read_only: true, + default_value: [], validation: { - size_gt<>: [0], unique<>: null } } diff --git a/gpio_controllers/test/test_gpio_command_controller.cpp b/gpio_controllers/test/test_gpio_command_controller.cpp index cbcb2c4fb7..49b68e42b1 100644 --- a/gpio_controllers/test/test_gpio_command_controller.cpp +++ b/gpio_controllers/test/test_gpio_command_controller.cpp @@ -27,6 +27,23 @@ #include "rclcpp/wait_result.hpp" #include "rclcpp/wait_set.hpp" #include "rclcpp_lifecycle/node_interfaces/lifecycle_node_interface.hpp" +#include "ros2_control_test_assets/descriptions.hpp" + +namespace +{ +const auto hardware_resources_with_gpio = + R"( + + + + + +)"; + +const auto minimal_robot_urdf_with_gpio = std::string(ros2_control_test_assets::urdf_head) + + std::string(hardware_resources_with_gpio) + + std::string(ros2_control_test_assets::urdf_tail); +} // namespace using CallbackReturn = controller_interface::CallbackReturn; using hardware_interface::LoanedCommandInterface; @@ -97,16 +114,10 @@ class GpioCommandControllerTestSuite : public ::testing::Test { ASSERT_EQ(result_of_initialization, controller_interface::return_type::OK); ASSERT_EQ(controller.on_configure(rclcpp_lifecycle::State()), CallbackReturn::SUCCESS); + setup_command_and_state_interfaces(); ASSERT_EQ(controller.on_activate(rclcpp_lifecycle::State()), CallbackReturn::SUCCESS); } - void move_to_activate_state() - { - ASSERT_EQ(controller.configure().id(), lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE); - ASSERT_EQ( - controller.get_node()->activate().id(), lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE); - } - void stop_test_when_message_cannot_be_published(int max_sub_check_loop_count) { ASSERT_GE(max_sub_check_loop_count, 0) @@ -198,7 +209,8 @@ class GpioCommandControllerTestSuite : public ::testing::Test TEST_F(GpioCommandControllerTestSuite, WhenNoParametersAreSetInitShouldFail) { const auto result = controller.init( - "test_gpio_command_controller", "", 0, "", controller.define_custom_node_options()); + "test_gpio_command_controller", ros2_control_test_assets::minimal_robot_urdf, 0, "", + controller.define_custom_node_options()); ASSERT_EQ(result, controller_interface::return_type::ERROR); } @@ -211,23 +223,24 @@ TEST_F(GpioCommandControllerTestSuite, WhenGpiosParameterIsEmptyInitShouldFail) ASSERT_EQ(result, controller_interface::return_type::ERROR); } -TEST_F(GpioCommandControllerTestSuite, WhenInterfacesParameterForGpioIsEmptyInitShouldFail) +TEST_F(GpioCommandControllerTestSuite, WhenInterfacesParameterForGpioIsEmptyInitShouldNotFail) { const auto node_options = create_node_options_with_overriden_parameters( {{"gpios", std::vector{"gpio1"}}, - {"command_interfaces.gpio1.interfaces", std::vector{}}}); + {"command_interfaces.gpio1.interfaces", std::vector{}}, + {"state_interfaces.gpio1.interfaces", std::vector{}}}); const auto result = controller.init("test_gpio_command_controller", "", 0, "", node_options); - ASSERT_EQ(result, controller_interface::return_type::ERROR); + ASSERT_EQ(result, controller_interface::return_type::OK); } -TEST_F(GpioCommandControllerTestSuite, WhenInterfacesParameterForGpioIsNotSetInitShouldFail) +TEST_F(GpioCommandControllerTestSuite, WhenInterfacesParameterForGpioIsNotSetInitShouldNotFail) { const auto node_options = create_node_options_with_overriden_parameters({{"gpios", std::vector{"gpio1"}}}); const auto result = controller.init("test_gpio_command_controller", "", 0, "", node_options); - ASSERT_EQ(result, controller_interface::return_type::ERROR); + ASSERT_EQ(result, controller_interface::return_type::OK); } TEST_F( @@ -245,6 +258,50 @@ TEST_F( ASSERT_EQ(result, controller_interface::return_type::OK); } +TEST_F( + GpioCommandControllerTestSuite, + WhenCommandAndStateInterfacesAreEmptyAndNoInterfacesAreSetInUrdfConfigurationShouldFail) +{ + const auto node_options = create_node_options_with_overriden_parameters( + {{"gpios", std::vector{"gpio1"}}, + {"command_interfaces.gpio1.interfaces", std::vector{}}, + {"state_interfaces.gpio1.interfaces", std::vector{}}}); + const auto result = controller.init( + "test_gpio_command_controller", ros2_control_test_assets::minimal_robot_urdf, 0, "", + node_options); + ASSERT_EQ(result, controller_interface::return_type::OK); + ASSERT_EQ(controller.on_configure(rclcpp_lifecycle::State()), CallbackReturn::ERROR); +} + +TEST_F( + GpioCommandControllerTestSuite, + WhenStateInterfaceAreNotConfiguredButAreSetInUrdfForConfiguredGpiosThenConfigureShouldSucceed) +{ + const auto node_options = create_node_options_with_overriden_parameters( + {{"gpios", std::vector{"gpio1"}}, + {"command_interfaces.gpio1.interfaces", std::vector{}}, + {"state_interfaces.gpio1.interfaces", std::vector{}}}); + const auto result = controller.init( + "test_gpio_command_controller", minimal_robot_urdf_with_gpio, 0, "", node_options); + + ASSERT_EQ(result, controller_interface::return_type::OK); + ASSERT_EQ(controller.on_configure(rclcpp_lifecycle::State()), CallbackReturn::SUCCESS); +} + +TEST_F( + GpioCommandControllerTestSuite, + WhenStateInterfaceAreNotConfiguredAndProvidedUrdfIsEmptyThenConfigureShouldFail) +{ + const auto node_options = create_node_options_with_overriden_parameters( + {{"gpios", std::vector{"gpio1"}}, + {"command_interfaces.gpio1.interfaces", std::vector{}}, + {"state_interfaces.gpio1.interfaces", std::vector{}}}); + const auto result = controller.init("test_gpio_command_controller", "", 0, "", node_options); + + ASSERT_EQ(result, controller_interface::return_type::OK); + ASSERT_EQ(controller.on_configure(rclcpp_lifecycle::State()), CallbackReturn::ERROR); +} + TEST_F(GpioCommandControllerTestSuite, ConfigureAndActivateParamsSuccess) { const auto node_options = create_node_options_with_overriden_parameters( @@ -254,9 +311,10 @@ TEST_F(GpioCommandControllerTestSuite, ConfigureAndActivateParamsSuccess) {"state_interfaces.gpio1.interfaces", std::vector{"dig.1", "dig.2"}}, {"state_interfaces.gpio2.interfaces", std::vector{"ana.1"}}}); const auto result = controller.init("test_gpio_command_controller", "", 0, "", node_options); - setup_command_and_state_interfaces(); + ASSERT_EQ(result, controller_interface::return_type::OK); ASSERT_EQ(controller.on_configure(rclcpp_lifecycle::State()), CallbackReturn::SUCCESS); + setup_command_and_state_interfaces(); ASSERT_EQ(controller.on_activate(rclcpp_lifecycle::State()), CallbackReturn::SUCCESS); } @@ -272,6 +330,9 @@ TEST_F( {"state_interfaces.gpio2.interfaces", std::vector{"ana.1"}}}); const auto result = controller.init("test_gpio_command_controller", "", 0, "", node_options); + ASSERT_EQ(result, controller_interface::return_type::OK); + ASSERT_EQ(controller.on_configure(rclcpp_lifecycle::State()), CallbackReturn::SUCCESS); + std::vector command_interfaces; command_interfaces.emplace_back(gpio_1_1_dig_cmd); command_interfaces.emplace_back(gpio_1_2_dig_cmd); @@ -282,9 +343,6 @@ TEST_F( state_interfaces.emplace_back(gpio_2_ana_state); controller.assign_interfaces(std::move(command_interfaces), std::move(state_interfaces)); - - ASSERT_EQ(result, controller_interface::return_type::OK); - ASSERT_EQ(controller.on_configure(rclcpp_lifecycle::State()), CallbackReturn::SUCCESS); ASSERT_EQ(controller.on_activate(rclcpp_lifecycle::State()), CallbackReturn::ERROR); } @@ -300,6 +358,9 @@ TEST_F( {"state_interfaces.gpio2.interfaces", std::vector{"ana.1"}}}); const auto result = controller.init("test_gpio_command_controller", "", 0, "", node_options); + ASSERT_EQ(result, controller_interface::return_type::OK); + ASSERT_EQ(controller.on_configure(rclcpp_lifecycle::State()), CallbackReturn::SUCCESS); + std::vector command_interfaces; command_interfaces.emplace_back(gpio_1_1_dig_cmd); command_interfaces.emplace_back(gpio_1_2_dig_cmd); @@ -311,8 +372,6 @@ TEST_F( controller.assign_interfaces(std::move(command_interfaces), std::move(state_interfaces)); - ASSERT_EQ(result, controller_interface::return_type::OK); - ASSERT_EQ(controller.on_configure(rclcpp_lifecycle::State()), CallbackReturn::SUCCESS); ASSERT_EQ(controller.on_activate(rclcpp_lifecycle::State()), CallbackReturn::ERROR); } @@ -328,6 +387,9 @@ TEST_F( {"state_interfaces.gpio2.interfaces", std::vector{"ana.1"}}}); const auto result = controller.init("test_gpio_command_controller", "", 0, "", node_options); + ASSERT_EQ(result, controller_interface::return_type::OK); + ASSERT_EQ(controller.on_configure(rclcpp_lifecycle::State()), CallbackReturn::SUCCESS); + std::vector command_interfaces; command_interfaces.emplace_back(gpio_1_1_dig_cmd); command_interfaces.emplace_back(gpio_1_2_dig_cmd); @@ -339,8 +401,6 @@ TEST_F( controller.assign_interfaces(std::move(command_interfaces), std::move(state_interfaces)); - ASSERT_EQ(result, controller_interface::return_type::OK); - ASSERT_EQ(controller.on_configure(rclcpp_lifecycle::State()), CallbackReturn::SUCCESS); ASSERT_EQ(controller.on_activate(rclcpp_lifecycle::State()), CallbackReturn::SUCCESS); } @@ -355,7 +415,6 @@ TEST_F( {"state_interfaces.gpio1.interfaces", std::vector{"dig.1", "dig.2"}}, {"state_interfaces.gpio2.interfaces", std::vector{"ana.1"}}}); - setup_command_and_state_interfaces(); move_to_activate_state(controller.init("test_gpio_command_controller", "", 0, "", node_options)); assert_default_command_and_state_values(); update_controller_loop(); @@ -372,8 +431,6 @@ TEST_F( {"command_interfaces.gpio2.interfaces", std::vector{"ana.1"}}, {"state_interfaces.gpio1.interfaces", std::vector{"dig.1", "dig.2"}}, {"state_interfaces.gpio2.interfaces", std::vector{"ana.1"}}}); - - setup_command_and_state_interfaces(); move_to_activate_state(controller.init("test_gpio_command_controller", "", 0, "", node_options)); const auto command = createGpioCommand( @@ -395,8 +452,6 @@ TEST_F( {"command_interfaces.gpio2.interfaces", std::vector{"ana.1"}}, {"state_interfaces.gpio1.interfaces", std::vector{"dig.1", "dig.2"}}, {"state_interfaces.gpio2.interfaces", std::vector{"ana.1"}}}); - - setup_command_and_state_interfaces(); move_to_activate_state(controller.init("test_gpio_command_controller", "", 0, "", node_options)); const auto command = createGpioCommand( @@ -418,8 +473,6 @@ TEST_F( {"command_interfaces.gpio2.interfaces", std::vector{"ana.1"}}, {"state_interfaces.gpio1.interfaces", std::vector{"dig.1", "dig.2"}}, {"state_interfaces.gpio2.interfaces", std::vector{"ana.1"}}}); - - setup_command_and_state_interfaces(); move_to_activate_state(controller.init("test_gpio_command_controller", "", 0, "", node_options)); const auto command = createGpioCommand( @@ -443,8 +496,6 @@ TEST_F( {"command_interfaces.gpio2.interfaces", std::vector{"ana.1"}}, {"state_interfaces.gpio1.interfaces", std::vector{"dig.1", "dig.2"}}, {"state_interfaces.gpio2.interfaces", std::vector{"ana.1"}}}); - - setup_command_and_state_interfaces(); move_to_activate_state(controller.init("test_gpio_command_controller", "", 0, "", node_options)); const auto command = createGpioCommand( @@ -468,9 +519,7 @@ TEST_F( {"command_interfaces.gpio2.interfaces", std::vector{"ana.1"}}, {"state_interfaces.gpio1.interfaces", std::vector{"dig.1", "dig.2"}}, {"state_interfaces.gpio2.interfaces", std::vector{"ana.1"}}}); - const auto result = controller.init("test_gpio_command_controller", "", 0, "", node_options); - setup_command_and_state_interfaces(); - move_to_activate_state(result); + move_to_activate_state(controller.init("test_gpio_command_controller", "", 0, "", node_options)); const auto command = createGpioCommand({"gpio1"}, {createInterfaceValue({"dig.1", "dig.2"}, {0.0, 1.0})}); @@ -493,9 +542,7 @@ TEST_F( {"command_interfaces.gpio2.interfaces", std::vector{"ana.1"}}, {"state_interfaces.gpio1.interfaces", std::vector{"dig.1", "dig.2"}}, {"state_interfaces.gpio2.interfaces", std::vector{"ana.1"}}}); - const auto result = controller.init("test_gpio_command_controller", "", 0, "", node_options); - setup_command_and_state_interfaces(); - move_to_activate_state(result); + move_to_activate_state(controller.init("test_gpio_command_controller", "", 0, "", node_options)); const auto command = createGpioCommand( {"gpio1", "gpio3"}, {createInterfaceValue({"dig.3", "dig.4"}, {20.0, 25.0}), @@ -519,7 +566,6 @@ TEST_F( {"command_interfaces.gpio2.interfaces", std::vector{"ana.1"}}, {"state_interfaces.gpio1.interfaces", std::vector{"dig.1", "dig.2"}}, {"state_interfaces.gpio2.interfaces", std::vector{"ana.1"}}}); - setup_command_and_state_interfaces(); move_to_activate_state(controller.init("test_gpio_command_controller", "", 0, "", node_options)); auto command_pub = node->create_publisher( @@ -544,7 +590,6 @@ TEST_F(GpioCommandControllerTestSuite, ControllerShouldPublishGpioStatesWithCurr {"command_interfaces.gpio2.interfaces", std::vector{"ana.1"}}, {"state_interfaces.gpio1.interfaces", std::vector{"dig.1", "dig.2"}}, {"state_interfaces.gpio2.interfaces", std::vector{"ana.1"}}}); - setup_command_and_state_interfaces(); move_to_activate_state(controller.init("test_gpio_command_controller", "", 0, "", node_options)); auto subscription = node->create_subscription( @@ -567,3 +612,81 @@ TEST_F(GpioCommandControllerTestSuite, ControllerShouldPublishGpioStatesWithCurr ASSERT_EQ(gpio_state_msg.interface_values.at(0).values.at(1), 0.0); ASSERT_EQ(gpio_state_msg.interface_values.at(1).values.at(0), 3.1); } + +TEST_F( + GpioCommandControllerTestSuite, + WhenStateInterfaceAreNotConfiguredButSetInUrdfForConfiguredGpiosThenThatStatesShouldBePublished) +{ + const std::vector single_gpio{"gpio1"}; + const auto node_options = create_node_options_with_overriden_parameters( + {{"gpios", single_gpio}, + {"command_interfaces.gpio1.interfaces", std::vector{"dig.1"}}}); + + std::vector command_interfaces; + command_interfaces.emplace_back(gpio_1_1_dig_cmd); + + std::vector state_interfaces; + state_interfaces.emplace_back(gpio_1_1_dig_state); + state_interfaces.emplace_back(gpio_2_ana_state); + + const auto result_of_initialization = controller.init( + "test_gpio_command_controller", minimal_robot_urdf_with_gpio, 0, "", node_options); + ASSERT_EQ(result_of_initialization, controller_interface::return_type::OK); + ASSERT_EQ(controller.on_configure(rclcpp_lifecycle::State()), CallbackReturn::SUCCESS); + controller.assign_interfaces(std::move(command_interfaces), std::move(state_interfaces)); + ASSERT_EQ(controller.on_activate(rclcpp_lifecycle::State()), CallbackReturn::SUCCESS); + + auto subscription = node->create_subscription( + std::string(controller.get_node()->get_name()) + "/gpio_states", 10, + [&](const StateType::SharedPtr) {}); + + stop_test_when_message_cannot_be_published(wait_for_subscription(subscription)); + + StateType gpio_state_msg; + 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_values.at(0).interface_names.at(0), "dig.1"); + ASSERT_EQ(gpio_state_msg.interface_values.at(0).values.at(0), 1.0); +} + +TEST_F( + GpioCommandControllerTestSuite, + ControllerShouldPublishGpioStatesWithCurrentValuesWhenOnlyStateInterfacesAreSet) +{ + const auto node_options = create_node_options_with_overriden_parameters( + {{"gpios", gpio_names}, + {"state_interfaces.gpio1.interfaces", std::vector{"dig.1"}}, + {"state_interfaces.gpio2.interfaces", std::vector{"ana.1"}}}); + + std::vector state_interfaces; + state_interfaces.emplace_back(gpio_1_1_dig_state); + state_interfaces.emplace_back(gpio_2_ana_state); + + const auto result_of_initialization = + controller.init("test_gpio_command_controller", "", 0, "", node_options); + ASSERT_EQ(result_of_initialization, controller_interface::return_type::OK); + ASSERT_EQ(controller.on_configure(rclcpp_lifecycle::State()), CallbackReturn::SUCCESS); + controller.assign_interfaces({}, std::move(state_interfaces)); + ASSERT_EQ(controller.on_activate(rclcpp_lifecycle::State()), CallbackReturn::SUCCESS); + + auto subscription = node->create_subscription( + std::string(controller.get_node()->get_name()) + "/gpio_states", 10, + [&](const StateType::SharedPtr) {}); + + stop_test_when_message_cannot_be_published(wait_for_subscription(subscription)); + + StateType gpio_state_msg; + 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_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); + ASSERT_EQ(gpio_state_msg.interface_values.at(1).values.at(0), 3.1); +} From 13ba72baa6b15e55010c42d43d1283eedfd53aa3 Mon Sep 17 00:00:00 2001 From: Wiktor Bajor Date: Tue, 5 Nov 2024 22:29:49 +0100 Subject: [PATCH 50/58] Remove urdf model from controller --- gpio_controllers/CMakeLists.txt | 2 -- .../include/gpio_controllers/gpio_command_controller.hpp | 2 -- 2 files changed, 4 deletions(-) diff --git a/gpio_controllers/CMakeLists.txt b/gpio_controllers/CMakeLists.txt index 52dd87ebf9..61ec55d75f 100644 --- a/gpio_controllers/CMakeLists.txt +++ b/gpio_controllers/CMakeLists.txt @@ -20,7 +20,6 @@ find_package(rclcpp_lifecycle REQUIRED) find_package(realtime_tools REQUIRED) find_package(std_msgs REQUIRED) find_package(generate_parameter_library REQUIRED) -find_package(urdf REQUIRED) generate_parameter_library(gpio_command_controller_parameters @@ -42,7 +41,6 @@ ament_target_dependencies(gpio_controllers PUBLIC rcutils realtime_tools std_msgs - urdf ) # Causes the visibility macros to use dllexport rather than dllimport, # which is appropriate when building the dll but not consuming it. diff --git a/gpio_controllers/include/gpio_controllers/gpio_command_controller.hpp b/gpio_controllers/include/gpio_controllers/gpio_command_controller.hpp index 26eb24d774..ad80cf4457 100644 --- a/gpio_controllers/include/gpio_controllers/gpio_command_controller.hpp +++ b/gpio_controllers/include/gpio_controllers/gpio_command_controller.hpp @@ -28,7 +28,6 @@ #include "rclcpp_lifecycle/state.hpp" #include "realtime_tools/realtime_buffer.h" #include "realtime_tools/realtime_publisher.h" -#include "urdf/model.hpp" namespace gpio_controllers { @@ -108,7 +107,6 @@ class GpioCommandController : public controller_interface::ControllerInterface std::shared_ptr param_listener_{}; gpio_command_controller_parameters::Params params_; - urdf::Model model_; }; } // namespace gpio_controllers From a8228afd7a0bbe30cfec6304b53b30e4b5bbce84 Mon Sep 17 00:00:00 2001 From: Wiktor Bajor Date: Thu, 7 Nov 2024 09:39:01 +0100 Subject: [PATCH 51/58] Update docs after review --- gpio_controllers/doc/userdoc.rst | 13 ++++++++----- 1 file changed, 8 insertions(+), 5 deletions(-) diff --git a/gpio_controllers/doc/userdoc.rst b/gpio_controllers/doc/userdoc.rst index e3034b92df..a963bd43fe 100644 --- a/gpio_controllers/doc/userdoc.rst +++ b/gpio_controllers/doc/userdoc.rst @@ -23,10 +23,13 @@ This controller uses the `generate_parameter_library Date: Thu, 7 Nov 2024 11:50:59 +0100 Subject: [PATCH 52/58] Create command subscriber only when any command interface is present. Update docs. --- gpio_controllers/doc/userdoc.rst | 3 +-- gpio_controllers/src/gpio_command_controller.cpp | 9 ++++++--- 2 files changed, 7 insertions(+), 5 deletions(-) diff --git a/gpio_controllers/doc/userdoc.rst b/gpio_controllers/doc/userdoc.rst index a963bd43fe..e24ff18116 100644 --- a/gpio_controllers/doc/userdoc.rst +++ b/gpio_controllers/doc/userdoc.rst @@ -23,8 +23,7 @@ This controller uses the `generate_parameter_library create_subscription( - "~/commands", rclcpp::SystemDefaultsQoS(), - [this](const CmdType::SharedPtr msg) { rt_command_ptr_.writeFromNonRT(msg); }); + if (!command_interface_types_.empty()) + { + gpios_command_subscriber_ = get_node()->create_subscription( + "~/commands", rclcpp::SystemDefaultsQoS(), + [this](const CmdType::SharedPtr msg) { rt_command_ptr_.writeFromNonRT(msg); }); + } gpio_state_publisher_ = get_node()->create_publisher("~/gpio_states", rclcpp::SystemDefaultsQoS()); From 9c30814075e852bf9d916a616a0142806ac6a4c8 Mon Sep 17 00:00:00 2001 From: Wiktor Bajor Date: Sat, 16 Nov 2024 16:56:22 +0100 Subject: [PATCH 53/58] Update pkg xmls --- gpio_controllers/package.xml | 4 ++-- ros2_controllers/package.xml | 1 + 2 files changed, 3 insertions(+), 2 deletions(-) diff --git a/gpio_controllers/package.xml b/gpio_controllers/package.xml index 4746c98c46..888aaef3f6 100644 --- a/gpio_controllers/package.xml +++ b/gpio_controllers/package.xml @@ -2,7 +2,7 @@ gpio_controllers - 4.12.1 + 4.15.0 Controllers to interact with gpios. Maciej Bednarczyk Wiktor Bajor @@ -14,8 +14,8 @@ hardware_interface rclcpp rclcpp_lifecycle + control_msgs realtime_tools - std_msgs pluginlib diff --git a/ros2_controllers/package.xml b/ros2_controllers/package.xml index 6eb4144647..05dde8c13b 100644 --- a/ros2_controllers/package.xml +++ b/ros2_controllers/package.xml @@ -19,6 +19,7 @@ effort_controllers force_torque_sensor_broadcaster forward_command_controller + gpio_controllers imu_sensor_broadcaster joint_state_broadcaster joint_trajectory_controller From 39d885adeadf791341b94c28dd9406c3b18e41fb Mon Sep 17 00:00:00 2001 From: Wiktor Bajor Date: Sat, 16 Nov 2024 17:17:33 +0100 Subject: [PATCH 54/58] Add control_msgs to CMakeLists --- gpio_controllers/CMakeLists.txt | 7 +++---- 1 file changed, 3 insertions(+), 4 deletions(-) diff --git a/gpio_controllers/CMakeLists.txt b/gpio_controllers/CMakeLists.txt index 61ec55d75f..78963eefd8 100644 --- a/gpio_controllers/CMakeLists.txt +++ b/gpio_controllers/CMakeLists.txt @@ -18,8 +18,8 @@ find_package(pluginlib REQUIRED) find_package(rclcpp REQUIRED) find_package(rclcpp_lifecycle REQUIRED) find_package(realtime_tools REQUIRED) -find_package(std_msgs REQUIRED) find_package(generate_parameter_library REQUIRED) +find_package(control_msgs REQUIRED) generate_parameter_library(gpio_command_controller_parameters @@ -40,7 +40,7 @@ ament_target_dependencies(gpio_controllers PUBLIC rclcpp_lifecycle rcutils realtime_tools - std_msgs + control_msgs ) # Causes the visibility macros to use dllexport rather than dllimport, # which is appropriate when building the dll but not consuming it. @@ -92,8 +92,8 @@ if(BUILD_TESTING) rclcpp rclcpp_lifecycle realtime_tools - std_msgs ros2_control_test_assets + control_msgs ) endif() @@ -103,7 +103,6 @@ ament_export_dependencies( rclcpp rclcpp_lifecycle realtime_tools - std_msgs ) ament_export_include_directories( include From 305ab619e7c0aeb29aa1d6e00860e7d015092564 Mon Sep 17 00:00:00 2001 From: Wiktor Bajor Date: Sat, 16 Nov 2024 17:20:09 +0100 Subject: [PATCH 55/58] Use DynamicInterfaceGroupValues instead of DynamicInterfaceValues --- .../gpio_command_controller.hpp | 6 ++--- gpio_controllers/package.xml | 1 + .../src/gpio_command_controller.cpp | 15 ++++++------ .../test/test_gpio_command_controller.cpp | 23 ++++++++++--------- 4 files changed, 24 insertions(+), 21 deletions(-) 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); From 5c32a72cdf232a99532570c101a2712929c5840b Mon Sep 17 00:00:00 2001 From: Wiktor Bajor Date: Mon, 18 Nov 2024 09:05:32 +0100 Subject: [PATCH 56/58] Add gpio_controllers to the release_notes and add minor fixes to the userdoc --- doc/release_notes.rst | 4 ++++ gpio_controllers/doc/userdoc.rst | 4 ++-- 2 files changed, 6 insertions(+), 2 deletions(-) diff --git a/doc/release_notes.rst b/doc/release_notes.rst index 688c35724d..b1db026df2 100644 --- a/doc/release_notes.rst +++ b/doc/release_notes.rst @@ -63,3 +63,7 @@ steering_controllers_library tricycle_controller ************************ * tricycle_controller now uses generate_parameter_library (`#957 `_). + +gpio_controllers +************************ +* The GPIO command controller was added 🎉 (`#1251 `_). diff --git a/gpio_controllers/doc/userdoc.rst b/gpio_controllers/doc/userdoc.rst index e24ff18116..199a4bd208 100644 --- a/gpio_controllers/doc/userdoc.rst +++ b/gpio_controllers/doc/userdoc.rst @@ -18,7 +18,7 @@ Description of controller's interfaces Parameters ^^^^^^^^^^^^^^^^^^^^^^^^ -This controller uses the `generate_parameter_library `_ to handle its parameters. The parameter `definition file located in the src folder `_ contains descriptions for all the parameters used by the controller. +This controller uses the `generate_parameter_library `_ to handle its parameters. The parameter `definition file located in the src folder `_ contains descriptions for all the parameters used by the controller. .. generate_parameter_library_details:: ../src/gpio_command_controller_parameters.yaml @@ -28,7 +28,7 @@ The controller expects at least one GPIO interface and the corresponding command .. note:: When no state interface is provided in the param file, the controller will try to use state_interfaces from ros2_control's config placed in the URDF for configured gpio interfaces. - However, Command interfaces will not be configured based on the available URDF setup. + However, command interfaces will not be configured based on the available URDF setup. .. code-block:: yaml From 4cd57721ebb6ba7fc4143128ca82b31d39d3503c Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Christoph=20Fr=C3=B6hlich?= Date: Mon, 18 Nov 2024 14:25:41 +0100 Subject: [PATCH 57/58] Update compile flags Co-authored-by: Sai Kishor Kothakota --- gpio_controllers/CMakeLists.txt | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/gpio_controllers/CMakeLists.txt b/gpio_controllers/CMakeLists.txt index 78963eefd8..61490badd1 100644 --- a/gpio_controllers/CMakeLists.txt +++ b/gpio_controllers/CMakeLists.txt @@ -7,7 +7,7 @@ if(NOT CMAKE_CXX_STANDARD) endif() if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") - add_compile_options(-Wall -Wextra -Wpedantic) + add_compile_options(-Wall -Wextra -Wpedantic -Werror=conversion -Werror=unused-but-set-variable -Werror=return-type -Werror=shadow -Werror=format -Werror=range-loop-construct -Werror=missing-braces) endif() # find dependencies From 6ef4023023196a1da0ad95bab6b03e65f306b82b Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Christoph=20Fr=C3=B6hlich?= Date: Mon, 18 Nov 2024 14:30:16 +0100 Subject: [PATCH 58/58] Fix format --- gpio_controllers/CMakeLists.txt | 4 +++- 1 file changed, 3 insertions(+), 1 deletion(-) diff --git a/gpio_controllers/CMakeLists.txt b/gpio_controllers/CMakeLists.txt index 61490badd1..6ea97e04ca 100644 --- a/gpio_controllers/CMakeLists.txt +++ b/gpio_controllers/CMakeLists.txt @@ -7,7 +7,9 @@ if(NOT CMAKE_CXX_STANDARD) endif() if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") - add_compile_options(-Wall -Wextra -Wpedantic -Werror=conversion -Werror=unused-but-set-variable -Werror=return-type -Werror=shadow -Werror=format -Werror=range-loop-construct -Werror=missing-braces) + add_compile_options(-Wall -Wextra -Wpedantic -Werror=conversion -Werror=unused-but-set-variable + -Werror=return-type -Werror=shadow -Werror=format -Werror=range-loop-construct + -Werror=missing-braces) endif() # find dependencies