fixed format

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:

gpio_controllers/include/gpio_controllers/gpio_command_controller.hpp

@@ -17,9 +17,10 @@
 
 #include <memory>
 #include <string>
-#include <vector>
 #include <unordered_map>
+#include <vector>
 
+#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
 {

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_

gpio_controllers/src/gpio_command_controller.cpp

@@ -43,9 +43,9 @@ CallbackReturn GpioCommandController::on_init()
   {
     auto_declare<std::vector<std::string>>("gpios", std::vector<std::string>());
     gpio_names_ = get_node()->get_parameter("gpios").as_string_array();
-    for(std::string &gpio : gpio_names_)
-      auto_declare<std::vector<std::string>>("command_interfaces."
-          + gpio, std::vector<std::string>());
+    for (std::string & gpio : gpio_names_)
+      auto_declare<std::vector<std::string>>(
+        "command_interfaces." + gpio, std::vector<std::string>());
   }
   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<CmdType>(
-          "~/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<StateType>(
-          "~/gpio_states", rclcpp::SystemDefaultsQoS());
+  try
+  {
+    gpios_command_subscriber_ = get_node()->create_subscription<CmdType>(
+      "~/commands", rclcpp::SystemDefaultsQoS(),
+      [this](const CmdType::SharedPtr msg) { rt_command_ptr_.writeFromNonRT(msg); });
 
-      realtime_gpio_state_publisher_ =
-        std::make_shared<realtime_tools::RealtimePublisher<StateType>>(
-          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<StateType>("~/gpio_states", rclcpp::SystemDefaultsQoS());
+
+    realtime_gpio_state_publisher_ =
+      std::make_shared<realtime_tools::RealtimePublisher<StateType>>(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<double>::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;
   }
 

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<std::string>{"gpio1", "gpio2"}});
   controller_->get_node()->set_parameter(
     {"command_interfaces.gpio1", std::vector<std::string>{"dig.1", "dig.2"}});
-  controller_->get_node()->set_parameter(
-    {"command_interfaces.gpio2", std::vector<std::string>()});
+  controller_->get_node()->set_parameter({"command_interfaces.gpio2", std::vector<std::string>()});
   // // 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<std::string>{"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<std::string>{"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<CmdType>();
   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<std::string>{"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<std::string>{"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<std::string>{"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);

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;

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