From 5fecf27c5494fd516f0fcfb1b00c493503a817f8 Mon Sep 17 00:00:00 2001 From: "m.bednarczyk" Date: Wed, 2 Nov 2022 15:30:19 +0100 Subject: [PATCH 01/10] 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/10] 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/10] 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/10] 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/10] 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/10] 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/10] 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/10] 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/10] 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/10] 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(); }