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(); }