diff --git a/.vscode/settings.json b/.vscode/settings.json
new file mode 100644
index 00000000..5e09a03c
--- /dev/null
+++ b/.vscode/settings.json
@@ -0,0 +1,5 @@
+{
+    "files.associations": {
+        "chrono": "cpp"
+    }
+}
\ No newline at end of file
diff --git a/bitbots_buttons/docs/conf.py b/bitbots_buttons/docs/conf.py
index 586c7734..10626d73 100644
--- a/bitbots_buttons/docs/conf.py
+++ b/bitbots_buttons/docs/conf.py
@@ -1,4 +1,3 @@
-# -*- coding: utf-8 -*-
 #
 # Full list of options at http://www.sphinx-doc.org/en/master/config
 
@@ -10,18 +9,20 @@
 #
 import os
 import sys
-import catkin_pkg.package
 
+import catkin_pkg.package
 from exhale import utils
 
 package_dir = os.path.dirname(os.path.dirname(os.path.abspath(__file__)))
 catkin_package = catkin_pkg.package.parse_package(
-    os.path.join(package_dir, catkin_pkg.package.PACKAGE_MANIFEST_FILENAME))
+    os.path.join(package_dir, catkin_pkg.package.PACKAGE_MANIFEST_FILENAME)
+)
 sys.path.insert(0, os.path.abspath(os.path.join(package_dir, "src")))
 
 
 # -- Helper functions --------------------------------------------------------
 
+
 def count_files():
     """:returns tuple of (num_py, num_cpp)"""
     num_py = 0
@@ -42,7 +43,7 @@ def count_files():
 # -- Project information -----------------------------------------------------
 
 project = catkin_package.name
-copyright = '2019, Bit-Bots'
+copyright = "2019, Bit-Bots"
 author = ", ".join([a.name for a in catkin_package.authors])
 
 # The short X.Y version
@@ -60,27 +61,27 @@ def count_files():
 # extensions coming with Sphinx (named 'sphinx.ext.*') or your custom
 # ones.
 extensions = [
-    'sphinx.ext.autodoc',
-    'sphinx.ext.doctest',
-    'sphinx.ext.intersphinx',
-    'sphinx.ext.todo',
-    'sphinx.ext.coverage',
-    'sphinx.ext.imgmath',
-    'sphinx.ext.viewcode',
-    'sphinx_rtd_theme',
+    "sphinx.ext.autodoc",
+    "sphinx.ext.doctest",
+    "sphinx.ext.intersphinx",
+    "sphinx.ext.todo",
+    "sphinx.ext.coverage",
+    "sphinx.ext.imgmath",
+    "sphinx.ext.viewcode",
+    "sphinx_rtd_theme",
 ]
 
 # Add any paths that contain templates here, relative to this directory.
-templates_path = ['_templates']
+templates_path = ["_templates"]
 
 # The suffix(es) of source filenames.
 # You can specify multiple suffix as a list of string:
 #
 # source_suffix = ['.rst', '.md']
-source_suffix = '.rst'
+source_suffix = ".rst"
 
 # The master toctree document.
-master_doc = 'index'
+master_doc = "index"
 
 # The language for content autogenerated by Sphinx. Refer to documentation
 # for a list of supported languages.
@@ -92,7 +93,7 @@ def count_files():
 # List of patterns, relative to source directory, that match files and
 # directories to ignore when looking for source files.
 # This pattern also affects html_static_path and html_extra_path.
-exclude_patterns = ['_build', 'Thumbs.db', '.DS_Store']
+exclude_patterns = ["_build", "Thumbs.db", ".DS_Store"]
 
 # The name of the Pygments (syntax highlighting) style to use.
 pygments_style = None
@@ -108,24 +109,17 @@ def count_files():
 
 if num_files_cpp > 0:
     extensions += [
-        'breathe',
-        'exhale',
+        "breathe",
+        "exhale",
     ]
 
-    breathe_projects = {
-        project: os.path.join("_build", "doxyoutput", "xml")
-    }
+    breathe_projects = {project: os.path.join("_build", "doxyoutput", "xml")}
     breathe_default_project = project
 
     def specifications_for_kind(kind):
         # Show all members for classes and structs
         if kind == "class" or kind == "struct":
-            return [
-              ":members:",
-              ":protected-members:",
-              ":private-members:",
-              ":undoc-members:"
-            ]
+            return [":members:", ":protected-members:", ":private-members:", ":undoc-members:"]
         # An empty list signals to Exhale to use the defaults
         else:
             return []
@@ -136,13 +130,11 @@ def specifications_for_kind(kind):
         "rootFileName": "library_root.rst",
         "rootFileTitle": "C++ Library API",
         "doxygenStripFromPath": "..",
-        "customSpecificationsMapping": utils.makeCustomSpecificationsMapping(
-            specifications_for_kind
-        ),
+        "customSpecificationsMapping": utils.makeCustomSpecificationsMapping(specifications_for_kind),
         # Suggested optional arguments
         "createTreeView": True,
         "exhaleExecutesDoxygen": True,
-        "exhaleDoxygenStdin": "INPUT = {}".format(os.path.join(package_dir, "include"))
+        "exhaleDoxygenStdin": "INPUT = {}".format(os.path.join(package_dir, "include")),
     }
 
 # -- Options for HTML output -------------------------------------------------
@@ -150,7 +142,7 @@ def specifications_for_kind(kind):
 # The theme to use for HTML and HTML Help pages.  See the documentation for
 # a list of builtin themes.
 #
-html_theme = 'sphinx_rtd_theme'
+html_theme = "sphinx_rtd_theme"
 
 # Theme options are theme-specific and customize the look and feel of a theme
 # further.  For a list of options available for each theme, see the
@@ -161,7 +153,7 @@ def specifications_for_kind(kind):
 # Add any paths that contain custom static files (such as style sheets) here,
 # relative to this directory. They are copied after the builtin static files,
 # so a file named "default.css" will overwrite the builtin "default.css".
-html_static_path = ['_static']
+html_static_path = ["_static"]
 
 # Custom sidebar templates, must be a dictionary that maps document names
 # to template names.
@@ -173,14 +165,14 @@ def specifications_for_kind(kind):
 #
 # html_sidebars = {}
 
-html_logo = os.path.join('_static', 'logo.png')
-html_favicon = os.path.join('_static', 'logo.png')
+html_logo = os.path.join("_static", "logo.png")
+html_favicon = os.path.join("_static", "logo.png")
 
 
 # -- Options for intersphinx extension ---------------------------------------
 
 # Example configuration for intersphinx: refer to the Python standard library.
-intersphinx_mapping = {'https://docs.python.org/': None}
+intersphinx_mapping = {"https://docs.python.org/": None}
 
 # -- Options for todo extension ----------------------------------------------
 
@@ -188,6 +180,8 @@ def specifications_for_kind(kind):
 todo_include_todos = True
 
 # -- RST Standard variables ---------------------------------------------------
-rst_prolog = ".. |project| replace:: {}\n".format(project)
+rst_prolog = f".. |project| replace:: {project}\n"
 rst_prolog += ".. |description| replace:: {}\n".format(catkin_package.description.replace("\n\n", "\n"))
-rst_prolog += ".. |modindex| replace:: {}\n".format(":ref:`modindex`" if num_files_py > 0 else "Python module index is not available")
+rst_prolog += ".. |modindex| replace:: {}\n".format(
+    ":ref:`modindex`" if num_files_py > 0 else "Python module index is not available"
+)
diff --git a/bitbots_buttons/src/button_node.cpp b/bitbots_buttons/src/button_node.cpp
old mode 100755
new mode 100644
index 6fc1ec4b..5ac72b07
--- a/bitbots_buttons/src/button_node.cpp
+++ b/bitbots_buttons/src/button_node.cpp
@@ -1,23 +1,23 @@
 #include <signal.h>
-#include <thread>
-#include <rclcpp/rclcpp.hpp>
+
+#include <bitbots_localization/srv/reset_filter.hpp>
 #include <bitbots_msgs/msg/buttons.hpp>
+#include <bitbots_msgs/srv/manual_penalize.hpp>
 #include <humanoid_league_msgs/msg/audio.hpp>
+#include <rclcpp/experimental/executors/events_executor/events_executor.hpp>
+#include <rclcpp/rclcpp.hpp>
 #include <std_msgs/msg/bool.hpp>
-#include <bitbots_msgs/srv/manual_penalize.hpp>
 #include <std_srvs/srv/empty.hpp>
 #include <std_srvs/srv/set_bool.hpp>
-#include <rclcpp/experimental/executors/events_executor/events_executor.hpp>
-#include <bitbots_localization/srv/reset_filter.hpp>
+#include <thread>
 
 using std::placeholders::_1;
 using namespace std::chrono_literals;
 namespace bitbots_buttons {
 
 class ButtonNode : public rclcpp::Node {
-public:
-  explicit ButtonNode(): Node("buttons",rclcpp::NodeOptions()) {
-      
+ public:
+  explicit ButtonNode() : Node("buttons", rclcpp::NodeOptions()) {
     this->declare_parameter<bool>("speak_active", true);
     this->declare_parameter<double>("short_time", 2.0);
     this->declare_parameter<bool>("manual_penalty", true);
@@ -44,12 +44,12 @@ class ButtonNode : public rclcpp::Node {
     speak_pub_ = this->create_publisher<humanoid_league_msgs::msg::Audio>("/speak", 1);
     shoot_publisher_ = this->create_publisher<std_msgs::msg::Bool>("/shoot_button", 1);
 
-    if (manual_penalty_mode_){
+    if (manual_penalty_mode_) {
       manual_penalize_client_ = this->create_client<bitbots_msgs::srv::ManualPenalize>("manual_penalize");
       manual_penalty_mode_ = manual_penalize_client_->wait_for_service(3s);
     }
-     
-    if(!in_game_){
+
+    if (!in_game_) {
       foot_zero_client_ = this->create_client<std_srvs::srv::Empty>("set_foot_zero");
       foot_zero_available_ = foot_zero_client_->wait_for_service(3s);
     }
@@ -65,47 +65,46 @@ class ButtonNode : public rclcpp::Node {
 
     localization_client_ = this->create_client<bitbots_localization::srv::ResetFilter>("reset_localization");
     localization_available_ = localization_client_->wait_for_service(3s);
-    buttons_sub_ = this->create_subscription<bitbots_msgs::msg::Buttons>("/buttons", 1, std::bind(&bitbots_buttons::ButtonNode::buttonCb, this, _1));
-
-
+    buttons_sub_ = this->create_subscription<bitbots_msgs::msg::Buttons>(
+        "/buttons", 1, std::bind(&bitbots_buttons::ButtonNode::buttonCb, this, _1));
   }
 
-  void buttonCb(const bitbots_msgs::msg::Buttons::SharedPtr msg){
-    //button1 - red
-    //button2 - green
-    //button3 - blue
+  void buttonCb(const bitbots_msgs::msg::Buttons::SharedPtr msg) {
+    // button1 - red
+    // button2 - green
+    // button3 - blue
 
-      if(msg->button1 && !button1_){
+    if (msg->button1 && !button1_) {
       button1_ = true;
       button1_time_ = this->get_clock()->now().seconds();
-    }else if(msg->button2 && !button2_){
+    } else if (msg->button2 && !button2_) {
       button2_ = true;
       button2_time_ = this->get_clock()->now().seconds();
-    }else if(msg->button3 && !button3_){
+    } else if (msg->button3 && !button3_) {
       button3_ = true;
       button3_time_ = this->get_clock()->now().seconds();
-    }else if(!msg->button1 && button1_){
+    } else if (!msg->button1 && button1_) {
       // button 1 not pressed anymore
       button1_ = false;
       double current_time = this->get_clock()->now().seconds();
-      if (current_time - button1_time_ > debounce_time_){
-        if(current_time - button1_time_ < short_time_ || in_game_){
+      if (current_time - button1_time_ > debounce_time_) {
+        if (current_time - button1_time_ < short_time_ || in_game_) {
           // button 1 short
           speak("1 short");
           setPower(false);
-        }else{
+        } else {
           // button 1 long
           speak("1 long");
           setPower(true);
         }
       }
       button1_time_ = 0;
-    }else if (! msg->button2 && button2_){
+    } else if (!msg->button2 && button2_) {
       // button2 not pressed anymore
       button2_ = false;
       double current_time = this->get_clock()->now().seconds();
-      if (current_time - button2_time_ > debounce_time_){
-        if(current_time - button2_time_ < short_time_ || in_game_){
+      if (current_time - button2_time_ > debounce_time_) {
+        if (current_time - button2_time_ < short_time_ || in_game_) {
           speak("2 short");
           setPenalty(false);
           resetLocalization();
@@ -115,12 +114,12 @@ class ButtonNode : public rclcpp::Node {
         }
       }
       button2_time_ = 0;
-    }else if (! msg->button3 && button3_){
+    } else if (!msg->button3 && button3_) {
       // button2 not pressed anymore
       button3_ = false;
       double current_time = this->get_clock()->now().seconds();
-      if (current_time - button3_time_ > debounce_time_){
-        if(current_time - button3_time_ < short_time_ || in_game_){
+      if (current_time - button3_time_ > debounce_time_) {
+        if (current_time - button3_time_ < short_time_ || in_game_) {
           speak("3 short");
           setPenalty(true);
         } else {
@@ -132,61 +131,58 @@ class ButtonNode : public rclcpp::Node {
     }
   }
 
-  void setPenalty(bool penalize){
+  void setPenalty(bool penalize) {
     // Penalizes the robot, if it is not penalized and manual penalty mode is true.
-    if(manual_penalty_mode_){
+    if (manual_penalty_mode_) {
       // switch penalty state by calling service on HCM
-        auto request = std::make_shared<bitbots_msgs::srv::ManualPenalize::Request>();
-        request->penalize = penalize;
-        auto result = manual_penalize_client_->async_send_request(request);
+      auto request = std::make_shared<bitbots_msgs::srv::ManualPenalize::Request>();
+      request->penalize = penalize;
+      manual_penalize_client_->async_send_request(request);
     }
   }
 
-  void zeroFootSensors(){
+  void zeroFootSensors() {
     // Zeroes foot sensors, if foot zero mode is true
     if (foot_zero_available_) {
-        if(!in_game_){
-            auto request = std::make_shared<std_srvs::srv::Empty::Request>();
-            auto result = foot_zero_client_->async_send_request(request);
-        }
-    }
-    else {
-        RCLCPP_WARN(this->get_logger(), "service not available");
+      if (!in_game_) {
+        auto request = std::make_shared<std_srvs::srv::Empty::Request>();
+        foot_zero_client_->async_send_request(request);
+      }
+    } else {
+      RCLCPP_WARN(this->get_logger(), "service not available");
     }
-
   }
 
-  void setPower(bool power){
+  void setPower(bool power) {
     // Set power to the servos
     auto request = std::make_shared<std_srvs::srv::SetBool::Request>();
     request->data = power;
-    auto result = power_client_->async_send_request(request);
+    power_client_->async_send_request(request);
   }
 
   void resetLocalization() {
-      if (localization_available_) {
-          auto request = std::make_shared<bitbots_localization::srv::ResetFilter::Request>();
-          request->init_mode = 0;
-          auto result = localization_client_->async_send_request(request);
-      }
-      else {
-          RCLCPP_WARN(this->get_logger(), "service not available");
-      }
+    if (localization_available_) {
+      auto request = std::make_shared<bitbots_localization::srv::ResetFilter::Request>();
+      request->init_mode = 0;
+      localization_client_->async_send_request(request);
+    } else {
+      RCLCPP_WARN(this->get_logger(), "service not available");
+    }
   }
 
-private:
-  void speak(std::string text) {
+ private:
+  void speak(const std::string& text) {
     /**
-      *  Helper method to send a message for text-to-speech output
-      */
-    if(speak_){
+     *  Helper method to send a message for text-to-speech output
+     */
+    if (speak_) {
       humanoid_league_msgs::msg::Audio msg = humanoid_league_msgs::msg::Audio();
       msg.text = text;
       msg.priority = 100;
       speak_pub_->publish(msg);
     }
   }
-  
+
   bool speaking_active_;
   double short_time_;
   bool manual_penalty_mode_;
@@ -211,7 +207,7 @@ class ButtonNode : public rclcpp::Node {
   rclcpp::Client<bitbots_localization::srv::ResetFilter>::SharedPtr localization_client_;
   rclcpp::Subscription<bitbots_msgs::msg::Buttons>::SharedPtr buttons_sub_;
 };
-}// namespace bitbots_buttons
+}  // namespace bitbots_buttons
 
 int main(int argc, char* argv[]) {
   // initialize ros
diff --git a/bitbots_ros_control/docs/conf.py b/bitbots_ros_control/docs/conf.py
index 586c7734..10626d73 100644
--- a/bitbots_ros_control/docs/conf.py
+++ b/bitbots_ros_control/docs/conf.py
@@ -1,4 +1,3 @@
-# -*- coding: utf-8 -*-
 #
 # Full list of options at http://www.sphinx-doc.org/en/master/config
 
@@ -10,18 +9,20 @@
 #
 import os
 import sys
-import catkin_pkg.package
 
+import catkin_pkg.package
 from exhale import utils
 
 package_dir = os.path.dirname(os.path.dirname(os.path.abspath(__file__)))
 catkin_package = catkin_pkg.package.parse_package(
-    os.path.join(package_dir, catkin_pkg.package.PACKAGE_MANIFEST_FILENAME))
+    os.path.join(package_dir, catkin_pkg.package.PACKAGE_MANIFEST_FILENAME)
+)
 sys.path.insert(0, os.path.abspath(os.path.join(package_dir, "src")))
 
 
 # -- Helper functions --------------------------------------------------------
 
+
 def count_files():
     """:returns tuple of (num_py, num_cpp)"""
     num_py = 0
@@ -42,7 +43,7 @@ def count_files():
 # -- Project information -----------------------------------------------------
 
 project = catkin_package.name
-copyright = '2019, Bit-Bots'
+copyright = "2019, Bit-Bots"
 author = ", ".join([a.name for a in catkin_package.authors])
 
 # The short X.Y version
@@ -60,27 +61,27 @@ def count_files():
 # extensions coming with Sphinx (named 'sphinx.ext.*') or your custom
 # ones.
 extensions = [
-    'sphinx.ext.autodoc',
-    'sphinx.ext.doctest',
-    'sphinx.ext.intersphinx',
-    'sphinx.ext.todo',
-    'sphinx.ext.coverage',
-    'sphinx.ext.imgmath',
-    'sphinx.ext.viewcode',
-    'sphinx_rtd_theme',
+    "sphinx.ext.autodoc",
+    "sphinx.ext.doctest",
+    "sphinx.ext.intersphinx",
+    "sphinx.ext.todo",
+    "sphinx.ext.coverage",
+    "sphinx.ext.imgmath",
+    "sphinx.ext.viewcode",
+    "sphinx_rtd_theme",
 ]
 
 # Add any paths that contain templates here, relative to this directory.
-templates_path = ['_templates']
+templates_path = ["_templates"]
 
 # The suffix(es) of source filenames.
 # You can specify multiple suffix as a list of string:
 #
 # source_suffix = ['.rst', '.md']
-source_suffix = '.rst'
+source_suffix = ".rst"
 
 # The master toctree document.
-master_doc = 'index'
+master_doc = "index"
 
 # The language for content autogenerated by Sphinx. Refer to documentation
 # for a list of supported languages.
@@ -92,7 +93,7 @@ def count_files():
 # List of patterns, relative to source directory, that match files and
 # directories to ignore when looking for source files.
 # This pattern also affects html_static_path and html_extra_path.
-exclude_patterns = ['_build', 'Thumbs.db', '.DS_Store']
+exclude_patterns = ["_build", "Thumbs.db", ".DS_Store"]
 
 # The name of the Pygments (syntax highlighting) style to use.
 pygments_style = None
@@ -108,24 +109,17 @@ def count_files():
 
 if num_files_cpp > 0:
     extensions += [
-        'breathe',
-        'exhale',
+        "breathe",
+        "exhale",
     ]
 
-    breathe_projects = {
-        project: os.path.join("_build", "doxyoutput", "xml")
-    }
+    breathe_projects = {project: os.path.join("_build", "doxyoutput", "xml")}
     breathe_default_project = project
 
     def specifications_for_kind(kind):
         # Show all members for classes and structs
         if kind == "class" or kind == "struct":
-            return [
-              ":members:",
-              ":protected-members:",
-              ":private-members:",
-              ":undoc-members:"
-            ]
+            return [":members:", ":protected-members:", ":private-members:", ":undoc-members:"]
         # An empty list signals to Exhale to use the defaults
         else:
             return []
@@ -136,13 +130,11 @@ def specifications_for_kind(kind):
         "rootFileName": "library_root.rst",
         "rootFileTitle": "C++ Library API",
         "doxygenStripFromPath": "..",
-        "customSpecificationsMapping": utils.makeCustomSpecificationsMapping(
-            specifications_for_kind
-        ),
+        "customSpecificationsMapping": utils.makeCustomSpecificationsMapping(specifications_for_kind),
         # Suggested optional arguments
         "createTreeView": True,
         "exhaleExecutesDoxygen": True,
-        "exhaleDoxygenStdin": "INPUT = {}".format(os.path.join(package_dir, "include"))
+        "exhaleDoxygenStdin": "INPUT = {}".format(os.path.join(package_dir, "include")),
     }
 
 # -- Options for HTML output -------------------------------------------------
@@ -150,7 +142,7 @@ def specifications_for_kind(kind):
 # The theme to use for HTML and HTML Help pages.  See the documentation for
 # a list of builtin themes.
 #
-html_theme = 'sphinx_rtd_theme'
+html_theme = "sphinx_rtd_theme"
 
 # Theme options are theme-specific and customize the look and feel of a theme
 # further.  For a list of options available for each theme, see the
@@ -161,7 +153,7 @@ def specifications_for_kind(kind):
 # Add any paths that contain custom static files (such as style sheets) here,
 # relative to this directory. They are copied after the builtin static files,
 # so a file named "default.css" will overwrite the builtin "default.css".
-html_static_path = ['_static']
+html_static_path = ["_static"]
 
 # Custom sidebar templates, must be a dictionary that maps document names
 # to template names.
@@ -173,14 +165,14 @@ def specifications_for_kind(kind):
 #
 # html_sidebars = {}
 
-html_logo = os.path.join('_static', 'logo.png')
-html_favicon = os.path.join('_static', 'logo.png')
+html_logo = os.path.join("_static", "logo.png")
+html_favicon = os.path.join("_static", "logo.png")
 
 
 # -- Options for intersphinx extension ---------------------------------------
 
 # Example configuration for intersphinx: refer to the Python standard library.
-intersphinx_mapping = {'https://docs.python.org/': None}
+intersphinx_mapping = {"https://docs.python.org/": None}
 
 # -- Options for todo extension ----------------------------------------------
 
@@ -188,6 +180,8 @@ def specifications_for_kind(kind):
 todo_include_todos = True
 
 # -- RST Standard variables ---------------------------------------------------
-rst_prolog = ".. |project| replace:: {}\n".format(project)
+rst_prolog = f".. |project| replace:: {project}\n"
 rst_prolog += ".. |description| replace:: {}\n".format(catkin_package.description.replace("\n\n", "\n"))
-rst_prolog += ".. |modindex| replace:: {}\n".format(":ref:`modindex`" if num_files_py > 0 else "Python module index is not available")
+rst_prolog += ".. |modindex| replace:: {}\n".format(
+    ":ref:`modindex`" if num_files_py > 0 else "Python module index is not available"
+)
diff --git a/bitbots_ros_control/include/bitbots_ros_control/bitfoot_hardware_interface.h b/bitbots_ros_control/include/bitbots_ros_control/bitfoot_hardware_interface.hpp
similarity index 76%
rename from bitbots_ros_control/include/bitbots_ros_control/bitfoot_hardware_interface.h
rename to bitbots_ros_control/include/bitbots_ros_control/bitfoot_hardware_interface.hpp
index cdfdd6ed..c05f56aa 100644
--- a/bitbots_ros_control/include/bitbots_ros_control/bitfoot_hardware_interface.h
+++ b/bitbots_ros_control/include/bitbots_ros_control/bitfoot_hardware_interface.hpp
@@ -1,26 +1,22 @@
 #ifndef BITBOTS_ROS_CONTROL_INCLUDE_BITBOTS_ROS_CONTROL_BITFOOT_HARDWARE_INTERFACE_H_
 #define BITBOTS_ROS_CONTROL_INCLUDE_BITBOTS_ROS_CONTROL_BITFOOT_HARDWARE_INTERFACE_H_
 
-#include <rclcpp/rclcpp.hpp>
-#include <string>
+#include <dynamixel_driver.h>
 
-#include <humanoid_league_msgs/msg/audio.hpp>
-#include <diagnostic_msgs/msg/diagnostic_status.hpp>
-#include <diagnostic_msgs/msg/diagnostic_array.hpp>
 #include <bitbots_msgs/msg/foot_pressure.hpp>
-#include <bitbots_ros_control/hardware_interface.h>
-
-#include <dynamixel_driver.h>
+#include <bitbots_ros_control/hardware_interface.hpp>
+#include <diagnostic_msgs/msg/diagnostic_array.hpp>
+#include <diagnostic_msgs/msg/diagnostic_status.hpp>
+#include <humanoid_league_msgs/msg/audio.hpp>
+#include <rclcpp/rclcpp.hpp>
+#include <string>
 
 namespace bitbots_ros_control {
 
-class BitFootHardwareInterface : public bitbots_ros_control::HardwareInterface{
+class BitFootHardwareInterface : public bitbots_ros_control::HardwareInterface {
  public:
-  explicit BitFootHardwareInterface(rclcpp::Node::SharedPtr nh,
-                                    std::shared_ptr<DynamixelDriver> &driver,
-                                    int id,
-                                    std::string topic_name,
-                                    std::string name);
+  explicit BitFootHardwareInterface(rclcpp::Node::SharedPtr nh, std::shared_ptr<DynamixelDriver> &driver, int id,
+                                    std::string topic_name, std::string name);
 
   bool init();
 
@@ -43,7 +39,6 @@ class BitFootHardwareInterface : public bitbots_ros_control::HardwareInterface{
   bitbots_msgs::msg::FootPressure msg_;
   rclcpp::Publisher<diagnostic_msgs::msg::DiagnosticArray>::SharedPtr diagnostic_pub_;
   uint8_t *data_;
-
 };
-}
+}  // namespace bitbots_ros_control
 #endif
diff --git a/bitbots_ros_control/include/bitbots_ros_control/button_hardware_interface.h b/bitbots_ros_control/include/bitbots_ros_control/button_hardware_interface.hpp
similarity index 73%
rename from bitbots_ros_control/include/bitbots_ros_control/button_hardware_interface.h
rename to bitbots_ros_control/include/bitbots_ros_control/button_hardware_interface.hpp
index 3ee9d2b2..d728c87f 100644
--- a/bitbots_ros_control/include/bitbots_ros_control/button_hardware_interface.h
+++ b/bitbots_ros_control/include/bitbots_ros_control/button_hardware_interface.hpp
@@ -1,30 +1,24 @@
 #ifndef BITBOTS_ROS_CONTROL_INCLUDE_BITBOTS_ROS_CONTROL_BUTTON_HARDWARE_INTERFACE_H_
 #define BITBOTS_ROS_CONTROL_INCLUDE_BITBOTS_ROS_CONTROL_BUTTON_HARDWARE_INTERFACE_H_
 
-#include <rclcpp/rclcpp.hpp>
-#include <string>
+#include <dynamixel_driver.h>
 
-#include <humanoid_league_msgs/msg/audio.hpp>
-#include <diagnostic_msgs/msg/diagnostic_status.hpp>
-#include <diagnostic_msgs/msg/diagnostic_array.hpp>
 #include <bitbots_msgs/msg/buttons.hpp>
-
+#include <bitbots_ros_control/hardware_interface.hpp>
+#include <bitbots_ros_control/utils.hpp>
+#include <diagnostic_msgs/msg/diagnostic_array.hpp>
+#include <diagnostic_msgs/msg/diagnostic_status.hpp>
 #include <hardware_interface/sensor_interface.hpp>
-
-#include <dynamixel_driver.h>
-
-#include <bitbots_ros_control/utils.h>
-#include <bitbots_ros_control/hardware_interface.h>
+#include <humanoid_league_msgs/msg/audio.hpp>
+#include <rclcpp/rclcpp.hpp>
+#include <string>
 
 namespace bitbots_ros_control {
 
-class ButtonHardwareInterface : public bitbots_ros_control::HardwareInterface{
+class ButtonHardwareInterface : public bitbots_ros_control::HardwareInterface {
  public:
-  explicit ButtonHardwareInterface(rclcpp::Node::SharedPtr nh,
-                                   std::shared_ptr<DynamixelDriver> &driver,
-                                   int id,
-                                   std::string topic,
-                                   int read_rate_);
+  explicit ButtonHardwareInterface(rclcpp::Node::SharedPtr nh, std::shared_ptr<DynamixelDriver> &driver, int id,
+                                   std::string topic, int read_rate_);
 
   bool init();
   void read(const rclcpp::Time &t, const rclcpp::Duration &dt);
@@ -42,6 +36,6 @@ class ButtonHardwareInterface : public bitbots_ros_control::HardwareInterface{
   rclcpp::Publisher<diagnostic_msgs::msg::DiagnosticArray>::SharedPtr diagnostic_pub_;
   uint8_t *data_;
 };
-}
+}  // namespace bitbots_ros_control
 
 #endif
diff --git a/bitbots_ros_control/include/bitbots_ros_control/core_hardware_interface.h b/bitbots_ros_control/include/bitbots_ros_control/core_hardware_interface.hpp
similarity index 83%
rename from bitbots_ros_control/include/bitbots_ros_control/core_hardware_interface.h
rename to bitbots_ros_control/include/bitbots_ros_control/core_hardware_interface.hpp
index f05f6707..a7311cae 100644
--- a/bitbots_ros_control/include/bitbots_ros_control/core_hardware_interface.h
+++ b/bitbots_ros_control/include/bitbots_ros_control/core_hardware_interface.hpp
@@ -1,26 +1,25 @@
 #ifndef BITBOTS_ROS_CONTROL_INCLUDE_BITBOTS_ROS_CONTROL_CORE_HARDWARE_INTERFACE_H_
 #define BITBOTS_ROS_CONTROL_INCLUDE_BITBOTS_ROS_CONTROL_CORE_HARDWARE_INTERFACE_H_
 
-#include <rclcpp/rclcpp.hpp>
-#include <string>
+#include <dynamixel_driver.h>
 
-#include <diagnostic_msgs/msg/diagnostic_status.hpp>
+#include <bitbots_ros_control/hardware_interface.hpp>
+#include <bitbots_ros_control/utils.hpp>
 #include <diagnostic_msgs/msg/diagnostic_array.hpp>
-
-#include <bitbots_ros_control/utils.h>
-
-#include <dynamixel_driver.h>
+#include <diagnostic_msgs/msg/diagnostic_status.hpp>
+#include <rclcpp/rclcpp.hpp>
 #include <std_msgs/msg/bool.hpp>
 #include <std_msgs/msg/float64.hpp>
 #include <std_msgs/msg/float64_multi_array.hpp>
 #include <std_srvs/srv/set_bool.hpp>
-#include <bitbots_ros_control/hardware_interface.h>
+#include <string>
 
 namespace bitbots_ros_control {
 
-class CoreHardwareInterface : public bitbots_ros_control::HardwareInterface{
+class CoreHardwareInterface : public bitbots_ros_control::HardwareInterface {
  public:
-  explicit CoreHardwareInterface(rclcpp::Node::SharedPtr nh,std::shared_ptr<DynamixelDriver> &driver, int id, int read_rate);
+  explicit CoreHardwareInterface(rclcpp::Node::SharedPtr nh, std::shared_ptr<DynamixelDriver> &driver, int id,
+                                 int read_rate);
 
   bool get_power_status();
 
@@ -32,7 +31,8 @@ class CoreHardwareInterface : public bitbots_ros_control::HardwareInterface{
 
  private:
   rclcpp::Node::SharedPtr nh_;
-  bool switch_power(std::shared_ptr<std_srvs::srv::SetBool::Request> req, std::shared_ptr<std_srvs::srv::SetBool::Response> resp);
+  bool switch_power(std::shared_ptr<std_srvs::srv::SetBool::Request> req,
+                    std::shared_ptr<std_srvs::srv::SetBool::Response> resp);
 
   std::shared_ptr<DynamixelDriver> driver_;
 
@@ -63,5 +63,5 @@ class CoreHardwareInterface : public bitbots_ros_control::HardwareInterface{
 
   rclcpp::Service<std_srvs::srv::SetBool>::SharedPtr power_switch_service_;
 };
-}
+}  // namespace bitbots_ros_control
 #endif
diff --git a/bitbots_ros_control/include/bitbots_ros_control/dynamixel_servo_hardware_interface.h b/bitbots_ros_control/include/bitbots_ros_control/dynamixel_servo_hardware_interface.hpp
old mode 100755
new mode 100644
similarity index 93%
rename from bitbots_ros_control/include/bitbots_ros_control/dynamixel_servo_hardware_interface.h
rename to bitbots_ros_control/include/bitbots_ros_control/dynamixel_servo_hardware_interface.hpp
index 70fc5658..f639884c
--- a/bitbots_ros_control/include/bitbots_ros_control/dynamixel_servo_hardware_interface.h
+++ b/bitbots_ros_control/include/bitbots_ros_control/dynamixel_servo_hardware_interface.hpp
@@ -1,28 +1,25 @@
 #ifndef BITBOTS_ROS_CONTROL_INCLUDE_BITBOTS_ROS_CONTROL_DYNAMIXEL_SERVO_HARDWARE_INTERFACE_H_
 #define BITBOTS_ROS_CONTROL_INCLUDE_BITBOTS_ROS_CONTROL_DYNAMIXEL_SERVO_HARDWARE_INTERFACE_H_
 
-#include <rclcpp/rclcpp.hpp>
-#include <string>
+#include <bitbots_ros_control/utils.h>
+#include <dynamixel_driver.h>
 
-#include <std_msgs/msg/bool.hpp>
+#include <bitbots_msgs/msg/joint_command.hpp>
+#include <bitbots_msgs/msg/joint_torque.hpp>
+#include <bitbots_ros_control/hardware_interface.hpp>
+#include <bitbots_ros_control/servo_bus_interface.hpp>
+#include <bitset>
+#include <diagnostic_msgs/msg/diagnostic_array.hpp>
+#include <diagnostic_msgs/msg/diagnostic_status.hpp>
 #include <humanoid_league_msgs/msg/audio.hpp>
+#include <rclcpp/rclcpp.hpp>
 #include <sensor_msgs/msg/joint_state.hpp>
-#include <diagnostic_msgs/msg/diagnostic_status.hpp>
-#include <diagnostic_msgs/msg/diagnostic_array.hpp>
 #include <std_msgs/msg/bool.hpp>
 #include <std_msgs/msg/int32_multi_array.hpp>
-#include <bitbots_msgs/msg/joint_torque.hpp>
-#include <bitbots_msgs/msg/joint_command.hpp>
-
-#include <bitbots_ros_control/utils.h>
-#include <bitbots_ros_control/hardware_interface.h>
-
-#include <bitbots_ros_control/servo_bus_interface.h>
-#include <dynamixel_driver.h>
-#include <bitset>
+#include <string>
 
 namespace bitbots_ros_control {
-template<typename T>
+template <typename T>
 std::string vecToString(const std::vector<T> &vec) {
   std::stringstream ss;
   ss << "[";
@@ -49,7 +46,7 @@ struct Joint {
   State goal;
 };
 
-class DynamixelServoHardwareInterface : public bitbots_ros_control::HardwareInterface{
+class DynamixelServoHardwareInterface : public bitbots_ros_control::HardwareInterface {
  public:
   explicit DynamixelServoHardwareInterface(rclcpp::Node::SharedPtr nh);
 
@@ -59,7 +56,7 @@ class DynamixelServoHardwareInterface : public bitbots_ros_control::HardwareInte
   void addBusInterface(ServoBusInterface *bus);
   void writeROMRAM(bool first_time);
 
-private:
+ private:
   rclcpp::Node::SharedPtr nh_;
   std::vector<ServoBusInterface *> bus_interfaces_;
 
@@ -102,6 +99,6 @@ class DynamixelServoHardwareInterface : public bitbots_ros_control::HardwareInte
   sensor_msgs::msg::JointState joint_state_msg_;
   sensor_msgs::msg::JointState pwm_msg_;
 };
-}
+}  // namespace bitbots_ros_control
 
 #endif
\ No newline at end of file
diff --git a/bitbots_ros_control/include/bitbots_ros_control/hardware_interface.h b/bitbots_ros_control/include/bitbots_ros_control/hardware_interface.hpp
similarity index 71%
rename from bitbots_ros_control/include/bitbots_ros_control/hardware_interface.h
rename to bitbots_ros_control/include/bitbots_ros_control/hardware_interface.hpp
index 01299eff..16fabe92 100644
--- a/bitbots_ros_control/include/bitbots_ros_control/hardware_interface.h
+++ b/bitbots_ros_control/include/bitbots_ros_control/hardware_interface.hpp
@@ -10,11 +10,11 @@ class HardwareInterface {
  public:
   virtual bool init() = 0;
 
-  virtual void read(const rclcpp::Time &t, const rclcpp::Duration &dt) {};
+  virtual void read(const rclcpp::Time &t, const rclcpp::Duration &dt){};
 
-  virtual void write(const rclcpp::Time &t, const rclcpp::Duration &dt) {};
+  virtual void write(const rclcpp::Time &t, const rclcpp::Duration &dt){};
 
-  virtual ~HardwareInterface() {};
+  virtual ~HardwareInterface(){};
 };
-}
-#endif //BITBOTS_ROS_CONTROL_INCLUDE_BITBOTS_ROS_CONTROL_HARDWARE_INTERFACE_H_
+}  // namespace bitbots_ros_control
+#endif  // BITBOTS_ROS_CONTROL_INCLUDE_BITBOTS_ROS_CONTROL_HARDWARE_INTERFACE_H_
diff --git a/bitbots_ros_control/include/bitbots_ros_control/imu_hardware_interface.hpp b/bitbots_ros_control/include/bitbots_ros_control/imu_hardware_interface.hpp
index 6304c3d1..1b73601d 100644
--- a/bitbots_ros_control/include/bitbots_ros_control/imu_hardware_interface.hpp
+++ b/bitbots_ros_control/include/bitbots_ros_control/imu_hardware_interface.hpp
@@ -83,9 +83,12 @@ class ImuHardwareInterface : public bitbots_ros_control::HardwareInterface {
 
   int diag_counter_;
 
-  void setIMURanges(const std::shared_ptr<bitbots_msgs::srv::IMURanges::Request> req, std::shared_ptr<bitbots_msgs::srv::IMURanges::Response> resp);
-  void calibrateGyro(const std::shared_ptr<std_srvs::srv::Empty::Request> req, std::shared_ptr<std_srvs::srv::Empty::Response> resp);
-  void resetGyroCalibration(const std::shared_ptr<std_srvs::srv::Empty::Request> req, std::shared_ptr<std_srvs::srv::Empty::Response> resp);
+  void setIMURanges(const std::shared_ptr<bitbots_msgs::srv::IMURanges::Request> req,
+                    std::shared_ptr<bitbots_msgs::srv::IMURanges::Response> resp);
+  void calibrateGyro(const std::shared_ptr<std_srvs::srv::Empty::Request> req,
+                     std::shared_ptr<std_srvs::srv::Empty::Response> resp);
+  void resetGyroCalibration(const std::shared_ptr<std_srvs::srv::Empty::Request> req,
+                            std::shared_ptr<std_srvs::srv::Empty::Response> resp);
   void setComplementaryFilterParams(const std::shared_ptr<bitbots_msgs::srv::ComplementaryFilterParams::Request> req,
                                     std::shared_ptr<bitbots_msgs::srv::ComplementaryFilterParams::Response> resp);
   void calibrateAccel(const std::shared_ptr<std_srvs::srv::Empty::Request> req,
@@ -94,8 +97,9 @@ class ImuHardwareInterface : public bitbots_ros_control::HardwareInterface {
                             std::shared_ptr<std_srvs::srv::Empty::Response> resp);
   void readAccelCalibration(const std::shared_ptr<bitbots_msgs::srv::AccelerometerCalibration::Request> req,
                             std::shared_ptr<bitbots_msgs::srv::AccelerometerCalibration::Response> resp);
-  void setAccelCalibrationThreshold(const std::shared_ptr<bitbots_msgs::srv::SetAccelerometerCalibrationThreshold::Request> req,
-                                    std::shared_ptr<bitbots_msgs::srv::SetAccelerometerCalibrationThreshold::Response> resp);
+  void setAccelCalibrationThreshold(
+      const std::shared_ptr<bitbots_msgs::srv::SetAccelerometerCalibrationThreshold::Request> req,
+      std::shared_ptr<bitbots_msgs::srv::SetAccelerometerCalibrationThreshold::Response> resp);
 };
 }  // namespace bitbots_ros_control
 #endif
diff --git a/bitbots_ros_control/include/bitbots_ros_control/leds_hardware_interface.h b/bitbots_ros_control/include/bitbots_ros_control/leds_hardware_interface.hpp
similarity index 76%
rename from bitbots_ros_control/include/bitbots_ros_control/leds_hardware_interface.h
rename to bitbots_ros_control/include/bitbots_ros_control/leds_hardware_interface.hpp
index 7bb05bc9..19d689e6 100644
--- a/bitbots_ros_control/include/bitbots_ros_control/leds_hardware_interface.h
+++ b/bitbots_ros_control/include/bitbots_ros_control/leds_hardware_interface.hpp
@@ -1,22 +1,19 @@
 #ifndef BITBOTS_ROS_CONTROL_INCLUDE_BITBOTS_ROS_CONTROL_LEDS_HARDWARE_INTERFACE_H_
 #define BITBOTS_ROS_CONTROL_INCLUDE_BITBOTS_ROS_CONTROL_LEDS_HARDWARE_INTERFACE_H_
 
-#include <rclcpp/rclcpp.hpp>
-#include <string>
 #include <dynamixel_driver.h>
-#include <bitbots_ros_control/hardware_interface.h>
 
 #include <bitbots_msgs/srv/leds.hpp>
+#include <bitbots_ros_control/hardware_interface.hpp>
+#include <rclcpp/rclcpp.hpp>
+#include <string>
 
 namespace bitbots_ros_control {
 
-class LedsHardwareInterface : public bitbots_ros_control::HardwareInterface{
+class LedsHardwareInterface : public bitbots_ros_control::HardwareInterface {
  public:
-  LedsHardwareInterface(rclcpp::Node::SharedPtr nh,
-                        std::shared_ptr<DynamixelDriver> &driver,
-                        uint8_t id,
-                        uint8_t num_leds,
-                        uint8_t start_number);
+  LedsHardwareInterface(rclcpp::Node::SharedPtr nh, std::shared_ptr<DynamixelDriver> &driver, uint8_t id,
+                        uint8_t num_leds, uint8_t start_number);
 
   bool init();
   void read(const rclcpp::Time &t, const rclcpp::Duration &dt);
@@ -32,7 +29,8 @@ class LedsHardwareInterface : public bitbots_ros_control::HardwareInterface{
   std::vector<std_msgs::msg::ColorRGBA> leds_;
 
   rclcpp::Service<bitbots_msgs::srv::Leds>::SharedPtr leds_service_;
-  void setLeds(const std::shared_ptr<bitbots_msgs::srv::Leds::Request> req, std::shared_ptr<bitbots_msgs::srv::Leds::Response> resp);
+  void setLeds(const std::shared_ptr<bitbots_msgs::srv::Leds::Request> req,
+               std::shared_ptr<bitbots_msgs::srv::Leds::Response> resp);
   void ledCb0(std_msgs::msg::ColorRGBA msg);
   void ledCb1(std_msgs::msg::ColorRGBA msg);
   void ledCb2(std_msgs::msg::ColorRGBA msg);
@@ -41,5 +39,5 @@ class LedsHardwareInterface : public bitbots_ros_control::HardwareInterface{
   rclcpp::Subscription<std_msgs::msg::ColorRGBA>::SharedPtr sub1_;
   rclcpp::Subscription<std_msgs::msg::ColorRGBA>::SharedPtr sub2_;
 };
-}
+}  // namespace bitbots_ros_control
 #endif
\ No newline at end of file
diff --git a/bitbots_ros_control/include/bitbots_ros_control/pressure_converter.hpp b/bitbots_ros_control/include/bitbots_ros_control/pressure_converter.hpp
index 68562aad..0e0cef13 100644
--- a/bitbots_ros_control/include/bitbots_ros_control/pressure_converter.hpp
+++ b/bitbots_ros_control/include/bitbots_ros_control/pressure_converter.hpp
@@ -15,7 +15,6 @@
 #include <std_srvs/srv/empty.hpp>
 
 class PressureConverter {
-
  public:
   PressureConverter(rclcpp::Node::SharedPtr nh, char side);
 
diff --git a/bitbots_ros_control/include/bitbots_ros_control/servo_bus_interface.h b/bitbots_ros_control/include/bitbots_ros_control/servo_bus_interface.hpp
similarity index 83%
rename from bitbots_ros_control/include/bitbots_ros_control/servo_bus_interface.h
rename to bitbots_ros_control/include/bitbots_ros_control/servo_bus_interface.hpp
index 2646a56c..939b3a2a 100644
--- a/bitbots_ros_control/include/bitbots_ros_control/servo_bus_interface.h
+++ b/bitbots_ros_control/include/bitbots_ros_control/servo_bus_interface.hpp
@@ -1,30 +1,27 @@
 #ifndef BITBOTS_ROS_CONTROL_INCLUDE_BITBOTS_ROS_CONTROL_SERVO_BUS_INTERFACE_H_
 #define BITBOTS_ROS_CONTROL_INCLUDE_BITBOTS_ROS_CONTROL_SERVO_BUS_INTERFACE_H_
 
-#include <rclcpp/rclcpp.hpp>
-#include <string>
+#include <dynamixel_driver.h>
 
-#include <std_msgs/msg/bool.hpp>
+#include <bitbots_msgs/msg/joint_torque.hpp>
+#include <bitbots_ros_control/hardware_interface.hpp>
+#include <bitbots_ros_control/utils.hpp>
+#include <bitset>
+#include <diagnostic_msgs/msg/diagnostic_array.hpp>
+#include <diagnostic_msgs/msg/diagnostic_status.hpp>
 #include <humanoid_league_msgs/msg/audio.hpp>
+#include <rcl_interfaces/msg/list_parameters_result.hpp>
+#include <rclcpp/rclcpp.hpp>
 #include <sensor_msgs/msg/joint_state.hpp>
-#include <diagnostic_msgs/msg/diagnostic_status.hpp>
-#include <diagnostic_msgs/msg/diagnostic_array.hpp>
 #include <std_msgs/msg/bool.hpp>
 #include <std_msgs/msg/int32_multi_array.hpp>
-#include <bitbots_msgs/msg/joint_torque.hpp>
-#include <rcl_interfaces/msg/list_parameters_result.hpp>
-#include <bitbots_ros_control/hardware_interface.h>
-
-#include <bitbots_ros_control/utils.h>
-#include <dynamixel_driver.h>
-#include <bitset>
-
+#include <string>
 
-namespace bitbots_ros_control  {
+namespace bitbots_ros_control {
 
 class ServoBusInterface : public bitbots_ros_control::HardwareInterface {
  public:
-  explicit ServoBusInterface(rclcpp::Node::SharedPtr nh,std::shared_ptr<DynamixelDriver> &driver,
+  explicit ServoBusInterface(rclcpp::Node::SharedPtr nh, std::shared_ptr<DynamixelDriver> &driver,
                              std::vector<std::tuple<int, std::string, float, float, std::string>> servos);
   ~ServoBusInterface();
   bool init();
@@ -37,11 +34,8 @@ class ServoBusInterface : public bitbots_ros_control::HardwareInterface {
   void syncWritePWM();
 
   void switchDynamixelControlMode();
-  diagnostic_msgs::msg::DiagnosticStatus createServoDiagMsg(int id,
-                                                       char level,
-                                                       std::string message,
-                                                       std::map<std::string, std::string> map,
-                                                       std::string name);
+  diagnostic_msgs::msg::DiagnosticStatus createServoDiagMsg(int id, char level, std::string message,
+                                                            std::map<std::string, std::string> map, std::string name);
   void processVte(bool success);
 
   bool goal_torque_;
@@ -94,7 +88,7 @@ class ServoBusInterface : public bitbots_ros_control::HardwareInterface {
   std::vector<uint8_t> joint_ids_;
   std::vector<double> joint_mounting_offsets_;
   std::vector<double> joint_offsets_;
-  std::vector<std::string> joint_groups_; // The group name for each joint
+  std::vector<std::string> joint_groups_;  // The group name for each joint
 
   std::vector<double> goal_position_;
   std::vector<double> goal_effort_;
@@ -129,7 +123,6 @@ class ServoBusInterface : public bitbots_ros_control::HardwareInterface {
   int reading_successes_;
   rclcpp::Publisher<diagnostic_msgs::msg::DiagnosticArray>::SharedPtr diagnostic_pub_;
   rclcpp::Publisher<humanoid_league_msgs::msg::Audio>::SharedPtr speak_pub_;
-
 };
-}
-#endif //BITBOTS_ROS_CONTROL_INCLUDE_BITBOTS_ROS_CONTROL_SERVO_BUS_INTERFACE_H_
+}  // namespace bitbots_ros_control
+#endif  // BITBOTS_ROS_CONTROL_INCLUDE_BITBOTS_ROS_CONTROL_SERVO_BUS_INTERFACE_H_
diff --git a/bitbots_ros_control/include/bitbots_ros_control/utils.h b/bitbots_ros_control/include/bitbots_ros_control/utils.hpp
similarity index 76%
rename from bitbots_ros_control/include/bitbots_ros_control/utils.h
rename to bitbots_ros_control/include/bitbots_ros_control/utils.hpp
index 4f3d3199..5cfb6e67 100644
--- a/bitbots_ros_control/include/bitbots_ros_control/utils.h
+++ b/bitbots_ros_control/include/bitbots_ros_control/utils.hpp
@@ -1,17 +1,12 @@
 #ifndef BITBOTS_ROS_CONTROL_INCLUDE_BITBOTS_ROS_CONTROL_UTILS_H_
 #define BITBOTS_ROS_CONTROL_INCLUDE_BITBOTS_ROS_CONTROL_UTILS_H_
 
-#include "rclcpp/rclcpp.hpp"
 #include "humanoid_league_msgs/msg/audio.hpp"
+#include "rclcpp/rclcpp.hpp"
 
 namespace bitbots_ros_control {
 
-enum ControlMode {
-  POSITION_CONTROL,
-  VELOCITY_CONTROL,
-  EFFORT_CONTROL,
-  CURRENT_BASED_POSITION_CONTROL
-};
+enum ControlMode { POSITION_CONTROL, VELOCITY_CONTROL, EFFORT_CONTROL, CURRENT_BASED_POSITION_CONTROL };
 
 bool stringToControlMode(rclcpp::Node::SharedPtr nh, std::string control_modestr, ControlMode &control_mode);
 void speakError(rclcpp::Publisher<humanoid_league_msgs::msg::Audio>::SharedPtr speak_pub, std::string text);
@@ -22,6 +17,6 @@ float dxlMakeFloat(uint8_t *data);
 
 std::string gyroRangeToString(uint8_t range);
 std::string accelRangeToString(uint8_t range);
-}
+}  // namespace bitbots_ros_control
 
-#endif  //BITBOTS_ROS_CONTROL_INCLUDE_BITBOTS_ROS_CONTROL_UTILS_H_
+#endif  // BITBOTS_ROS_CONTROL_INCLUDE_BITBOTS_ROS_CONTROL_UTILS_H_
diff --git a/bitbots_ros_control/include/bitbots_ros_control/wolfgang_hardware_interface.h b/bitbots_ros_control/include/bitbots_ros_control/wolfgang_hardware_interface.hpp
similarity index 65%
rename from bitbots_ros_control/include/bitbots_ros_control/wolfgang_hardware_interface.h
rename to bitbots_ros_control/include/bitbots_ros_control/wolfgang_hardware_interface.hpp
index 0027fb08..1e163a93 100644
--- a/bitbots_ros_control/include/bitbots_ros_control/wolfgang_hardware_interface.h
+++ b/bitbots_ros_control/include/bitbots_ros_control/wolfgang_hardware_interface.hpp
@@ -1,25 +1,23 @@
 #ifndef BITBOTS_ROS_CONTROL_INCLUDE_BITBOTS_ROS_CONTROL_WOLFGANG_HARDWARE_INTERFACE_H_
 #define BITBOTS_ROS_CONTROL_INCLUDE_BITBOTS_ROS_CONTROL_WOLFGANG_HARDWARE_INTERFACE_H_
 
-#include <thread>
-
-#include <rclcpp/rclcpp.hpp>
-#include <bitbots_ros_control/utils.h>
-#include <bitbots_ros_control/core_hardware_interface.h>
-#include <bitbots_ros_control/imu_hardware_interface.h>
-#include <bitbots_ros_control/leds_hardware_interface.h>
-#include <bitbots_ros_control/dynamixel_servo_hardware_interface.h>
-#include <bitbots_ros_control/bitfoot_hardware_interface.h>
-#include <bitbots_ros_control/button_hardware_interface.h>
-#include <bitbots_ros_control/leds_hardware_interface.h>
-#include <bitbots_ros_control/hardware_interface.h>
+#include <bitbots_ros_control/bitfoot_hardware_interface.hpp>
+#include <bitbots_ros_control/button_hardware_interface.hpp>
+#include <bitbots_ros_control/core_hardware_interface.hpp>
+#include <bitbots_ros_control/dynamixel_servo_hardware_interface.hpp>
+#include <bitbots_ros_control/hardware_interface.hpp>
+#include <bitbots_ros_control/imu_hardware_interface.hpp>
+#include <bitbots_ros_control/leds_hardware_interface.hpp>
+#include <bitbots_ros_control/utils.hpp>
 #include <rcl_interfaces/msg/list_parameters_result.hpp>
+#include <rclcpp/rclcpp.hpp>
+#include <thread>
 
 namespace bitbots_ros_control {
 
 class WolfgangHardwareInterface {
  public:
-  WolfgangHardwareInterface(rclcpp::Node::SharedPtr nh);
+  explicit WolfgangHardwareInterface(rclcpp::Node::SharedPtr nh);
 
   bool init();
 
@@ -44,8 +42,8 @@ class WolfgangHardwareInterface {
   bool core_present_;
   bool current_power_status_;
   bool last_power_status_;
-  CoreHardwareInterface* core_interface_;
+  CoreHardwareInterface *core_interface_;
 };
-}
+}  // namespace bitbots_ros_control
 
-#endif //BITBOTS_ROS_CONTROL_INCLUDE_BITBOTS_ROS_CONTROL_WOLFGANG_HARDWARE_INTERFACE_H_
\ No newline at end of file
+#endif  // BITBOTS_ROS_CONTROL_INCLUDE_BITBOTS_ROS_CONTROL_WOLFGANG_HARDWARE_INTERFACE_H_
\ No newline at end of file
diff --git a/bitbots_ros_control/scripts/battery_led.py b/bitbots_ros_control/scripts/battery_led.py
index 6be375e9..b34b0413 100755
--- a/bitbots_ros_control/scripts/battery_led.py
+++ b/bitbots_ros_control/scripts/battery_led.py
@@ -3,10 +3,9 @@
 import rclpy
 from rclpy.node import Node
 from std_msgs.msg import ColorRGBA, Float64
-from rclpy.node import Node
 
 rclpy.init(args=None)
-node = Node('battery_led')
+node = Node("battery_led")
 
 pub = node.create_publisher(ColorRGBA, "/led2", 1)
 
@@ -15,6 +14,7 @@
 led_low = ColorRGBA(a=1.0, r=1.0, g=0.0, b=0.0)
 led_no = ColorRGBA(a=1.0, r=0.0, g=0.0, b=0.0)
 
+
 def vbat_cb(msg: Float64):
     if msg.data > 16:
         pub.publish(led_full)
diff --git a/bitbots_ros_control/scripts/led_error_blink.py b/bitbots_ros_control/scripts/led_error_blink.py
index 6b6deaf3..95c0c854 100755
--- a/bitbots_ros_control/scripts/led_error_blink.py
+++ b/bitbots_ros_control/scripts/led_error_blink.py
@@ -1,18 +1,17 @@
 #!/usr/bin/env python3
 
 import rclpy
-from rclpy.node import Node
-from diagnostic_msgs.msg import DiagnosticArray, DiagnosticStatus
 from bitbots_msgs.srv import Leds
-from std_msgs.msg import ColorRGBA
-from rclpy.node import Node
+from diagnostic_msgs.msg import DiagnosticStatus
 from rclpy.duration import Duration
+from rclpy.node import Node
+from std_msgs.msg import ColorRGBA
 
 BLINK_DURATION = 0.2
 ERROR_TIMEOUT = 1
 
 rclpy.init(args=None)
-node = Node('led_error_blink')
+node = Node("led_error_blink")
 
 last_hardware_error_time = None
 # true means warning, false error
diff --git a/bitbots_ros_control/scripts/pressure_calibration.py b/bitbots_ros_control/scripts/pressure_calibration.py
index d83d098c..d61e878f 100755
--- a/bitbots_ros_control/scripts/pressure_calibration.py
+++ b/bitbots_ros_control/scripts/pressure_calibration.py
@@ -1,9 +1,8 @@
 #!/usr/bin/env python3
 
 import rclpy
-from rclpy.node import Node
-from bitbots_msgs.msg import FootPressure
 from bitbots_msgs.srv import FootScale
+from rclpy.node import Node
 from std_srvs.srv import Empty
 
 CALIBRATION_WEIGHT = 0.850 * 9.81
@@ -12,8 +11,13 @@
 node = Node("calibration_node")
 
 
-node.get_logger().info("Waiting for " + "/foot_pressure_left/set_foot_zero" + "/foot_pressure_left/set_foot_scale"
-              + "/foot_pressure_right/set_foot_zero" + "/foot_pressure_right/set_foot_scale")
+node.get_logger().info(
+    "Waiting for "
+    + "/foot_pressure_left/set_foot_zero"
+    + "/foot_pressure_left/set_foot_scale"
+    + "/foot_pressure_right/set_foot_zero"
+    + "/foot_pressure_right/set_foot_scale"
+)
 
 zero_l = node.create_client(Empty, "/foot_pressure_left/set_foot_zero")
 zero_r = node.create_client(Empty, "/foot_pressure_right/set_foot_zero")
@@ -28,7 +32,9 @@
 node.get_logger().info("found all services")
 
 node.get_logger().info("Welcome to the foot calibration suite. ")
-node.get_logger().info("default calibration weight is {} kg. Change the code when using a different weight.".format(CALIBRATION_WEIGHT))
+node.get_logger().info(
+    f"default calibration weight is {CALIBRATION_WEIGHT} kg. Change the code when using a different weight."
+)
 
 
 node.get_logger().info("Press enter when there is no load on the weight cells of the LEFT foot")
@@ -38,7 +44,12 @@
 request = Empty.Request()
 req = zero_l.call_async(request)
 node.get_logger().info("Successfully set the zero position for all sensors")
-sensors = ['left front','left back','right front','right back',]
+sensors = [
+    "left front",
+    "left back",
+    "right front",
+    "right back",
+]
 
 request = FootScale.Request()
 request.weight = CALIBRATION_WEIGHT
@@ -69,6 +80,7 @@
     node.get_logger().info("Load cell for " + sensors[i] + " calibrated")
 
 
-node.get_logger().info("Thank you for your patience. The configuration has been automatically saved by the pressure_converter already")
+node.get_logger().info(
+    "Thank you for your patience. The configuration has been automatically saved by the pressure_converter already"
+)
 node.get_logger().info("Shutting down now.")
-
diff --git a/bitbots_ros_control/scripts/send_joint_command.py b/bitbots_ros_control/scripts/send_joint_command.py
index 80984a1f..0e0113ee 100755
--- a/bitbots_ros_control/scripts/send_joint_command.py
+++ b/bitbots_ros_control/scripts/send_joint_command.py
@@ -3,11 +3,9 @@
 import argparse
 
 import rclpy
-from rclpy.node import Node
-from rclpy.duration import Duration
-
 from bitbots_msgs.msg import JointCommand
-
+from rclpy.duration import Duration
+from rclpy.node import Node
 
 DYNAMIXEL_CMD_TOPIC = "/DynamixelController/command"
 
@@ -33,7 +31,7 @@ class PredefinedCommands:
         "RHipPitch",
         "RKnee",
         "RAnklePitch",
-        "RAnkleRoll"
+        "RAnkleRoll",
     ]
     __velocity__ = 5.0
     __accelerations__ = -1.0
@@ -44,7 +42,8 @@ class PredefinedCommands:
         velocities=[__velocity__] * len(__ids__),
         accelerations=[__accelerations__] * len(__ids__),
         max_currents=[__max_currents__] * len(__ids__),
-        positions=[0.0] * len(__ids__))
+        positions=[0.0] * len(__ids__),
+    )
 
     Walkready = JointCommand(
         joint_names=__ids__,
@@ -72,16 +71,23 @@ class PredefinedCommands:
             -1.0059,  # RKnee
             0.4512,  # RAnklePitch
             -0.0625,  # RAnkleRoll
-        ])
+        ],
+    )
 
 
 def parse_args():
     parser = argparse.ArgumentParser(description="Send a bitbots_msgs/JointCommand to our ros-control node in a loop")
-    parser.add_argument("--once", action="store_true", default=False,
-                        help="Only send the message once instead of in a loop")
-    parser.add_argument("-c", "--command", type=str, help="Command to send. Use one of the available choices",
-                        choices=[c for c in PredefinedCommands.__dict__ if not c.startswith("__")],
-                        default="Zero")
+    parser.add_argument(
+        "--once", action="store_true", default=False, help="Only send the message once instead of in a loop"
+    )
+    parser.add_argument(
+        "-c",
+        "--command",
+        type=str,
+        help="Command to send. Use one of the available choices",
+        choices=[c for c in PredefinedCommands.__dict__ if not c.startswith("__")],
+        default="Zero",
+    )
 
     return parser.parse_args()
 
@@ -95,12 +101,12 @@ def main():
     pub = node.create_publisher(JointCommand, DYNAMIXEL_CMD_TOPIC, 1)
 
     while pub.get_subscription_count() < 1:
-        node.get_logger().info("Waiting until subscribers connect to {}".format(DYNAMIXEL_CMD_TOPIC), once=True)
+        node.get_logger().info(f"Waiting until subscribers connect to {DYNAMIXEL_CMD_TOPIC}", once=True)
         node.get_clock().sleep_for(Duration(seconds=0.5))
     # just to make sure
     node.get_clock().sleep_for(Duration(seconds=1))
 
-    node.get_logger().info("Sending controller commands of type {} now.".format(args.command))
+    node.get_logger().info(f"Sending controller commands of type {args.command} now.")
     print(joint_command)
 
     while rclpy.ok():
@@ -111,5 +117,6 @@ def main():
         if args.once:
             return
 
+
 if __name__ == "__main__":
     main()
diff --git a/bitbots_ros_control/scripts/send_sinus_command.py b/bitbots_ros_control/scripts/send_sinus_command.py
index bb648155..a30ce5b8 100755
--- a/bitbots_ros_control/scripts/send_sinus_command.py
+++ b/bitbots_ros_control/scripts/send_sinus_command.py
@@ -2,9 +2,6 @@
 
 import math
 
-import rclpy
-from rclpy.node import Node
-
 import rclpy
 from bitbots_msgs.msg import JointCommand
 from rclpy.node import Node
@@ -21,7 +18,7 @@
     msg = JointCommand(joint_names=[JOINT_NAME], velocities=[-1], accelerations=[-1], max_currents=[-1])
 
     rclpy.init(args=None)
-    node = Node('sinus_command')
+    node = Node("sinus_command")
     pub = node.create_publisher(JointCommand, DYNAMIXEL_CMD_TOPIC, 1)
 
     def tick():
diff --git a/bitbots_ros_control/scripts/zero_on_button.py b/bitbots_ros_control/scripts/zero_on_button.py
index 36089cd1..fe474a90 100755
--- a/bitbots_ros_control/scripts/zero_on_button.py
+++ b/bitbots_ros_control/scripts/zero_on_button.py
@@ -1,19 +1,19 @@
 #!/usr/bin/env python3
 import rclpy
-from rclpy.node import Node
-from std_srvs.srv import Empty
 from bitbots_msgs.msg import Buttons
-from rclpy.node import Node
 from rclpy.duration import Duration
+from rclpy.node import Node
+from std_srvs.srv import Empty
 
 rclpy.init(args=None)
-node = Node('zero_on_button')
+node = Node("zero_on_button")
 
 zero_l = node.create_client(Empty, "/foot_pressure_left/set_foot_zero")
 zero_r = node.create_client(Empty, "/foot_pressure_right/set_foot_zero")
 button_prev_state = False
 press_time = node.get_clock().now() - Duration(seconds=1.0)
 
+
 def cb(msg):
     global button_prev_state, press_time
     print("New msg")
diff --git a/bitbots_ros_control/src/bitfoot_hardware_interface.cpp b/bitbots_ros_control/src/bitfoot_hardware_interface.cpp
index 24c5946e..6354eab1 100644
--- a/bitbots_ros_control/src/bitfoot_hardware_interface.cpp
+++ b/bitbots_ros_control/src/bitfoot_hardware_interface.cpp
@@ -1,13 +1,10 @@
-#include <bitbots_ros_control/bitfoot_hardware_interface.h>
-#include <bitbots_ros_control/utils.h>
+#include <bitbots_ros_control/bitfoot_hardware_interface.hpp>
+#include <bitbots_ros_control/utils.hpp>
 
 namespace bitbots_ros_control {
 
-BitFootHardwareInterface::BitFootHardwareInterface(rclcpp::Node::SharedPtr nh,
-                                                   std::shared_ptr<DynamixelDriver> &driver,
-                                                   int id,
-                                                   std::string topic_name,
-                                                   std::string name) {
+BitFootHardwareInterface::BitFootHardwareInterface(rclcpp::Node::SharedPtr nh, std::shared_ptr<DynamixelDriver> &driver,
+                                                   int id, std::string topic_name, std::string name) {
   nh_ = nh;
   driver_ = driver;
   id_ = id;
@@ -17,7 +14,7 @@ BitFootHardwareInterface::BitFootHardwareInterface(rclcpp::Node::SharedPtr nh,
 
 bool BitFootHardwareInterface::init() {
   current_pressure_.resize(4, std::vector<double>());
-  data_ = (uint8_t *) malloc(16 * sizeof(uint8_t));
+  data_ = (uint8_t *)malloc(16 * sizeof(uint8_t));
   pressure_pub_ = nh_->create_publisher<bitbots_msgs::msg::FootPressure>(topic_name_, 1);
   diagnostic_pub_ = nh_->create_publisher<diagnostic_msgs::msg::DiagnosticArray>("/diagnostics", 10);
   return true;
@@ -32,11 +29,11 @@ void BitFootHardwareInterface::read(const rclcpp::Time &t, const rclcpp::Duratio
   bool read_successful = true;
   if (driver_->readMultipleRegisters(id_, 36, 16, data_)) {
     for (int i = 0; i < 4; i++) {
-      int32_t pres = dxlMakedword(dxlMakeword(data_[i * 4], data_[i * 4 + 1]),
-                                  dxlMakeword(data_[i * 4 + 2], data_[i * 4 + 3]));
-      float pres_d = (float) pres;
+      int32_t pres =
+          dxlMakedword(dxlMakeword(data_[i * 4], data_[i * 4 + 1]), dxlMakeword(data_[i * 4 + 2], data_[i * 4 + 3]));
+      float pres_d = (float)pres;
       // we directly provide raw data since the scaling has to be calibrated by another node for every robot anyway
-      current_pressure_[i].push_back((double) pres_d);
+      current_pressure_[i].push_back((double)pres_d);
     }
   } else {
     RCLCPP_ERROR_THROTTLE(nh_->get_logger(), *nh_->get_clock(), 3000, "Could not read %s", name_.c_str());
@@ -107,7 +104,7 @@ void BitFootHardwareInterface::read(const rclcpp::Time &t, const rclcpp::Duratio
     }
     std::vector<diagnostic_msgs::msg::KeyValue> keyValues = std::vector<diagnostic_msgs::msg::KeyValue>();
     // iterate through map and save it into values
-    for (auto const &ent1: map) {
+    for (auto const &ent1 : map) {
       diagnostic_msgs::msg::KeyValue key_value = diagnostic_msgs::msg::KeyValue();
       key_value.key = ent1.first;
       key_value.value = ent1.second;
@@ -122,4 +119,4 @@ void BitFootHardwareInterface::read(const rclcpp::Time &t, const rclcpp::Duratio
 
 // we dont write anything to the pressure sensors
 void BitFootHardwareInterface::write(const rclcpp::Time &t, const rclcpp::Duration &dt) {}
-}
\ No newline at end of file
+}  // namespace bitbots_ros_control
\ No newline at end of file
diff --git a/bitbots_ros_control/src/button_hardware_interface.cpp b/bitbots_ros_control/src/button_hardware_interface.cpp
index e2151948..cc85b694 100644
--- a/bitbots_ros_control/src/button_hardware_interface.cpp
+++ b/bitbots_ros_control/src/button_hardware_interface.cpp
@@ -1,11 +1,8 @@
-#include <bitbots_ros_control/button_hardware_interface.h>
+#include <bitbots_ros_control/button_hardware_interface.hpp>
 
 namespace bitbots_ros_control {
-ButtonHardwareInterface::ButtonHardwareInterface(rclcpp::Node::SharedPtr nh,
-                                                 std::shared_ptr<DynamixelDriver> &driver,
-                                                 int id,
-                                                 std::string topic,
-                                                 int read_rate) {
+ButtonHardwareInterface::ButtonHardwareInterface(rclcpp::Node::SharedPtr nh, std::shared_ptr<DynamixelDriver> &driver,
+                                                 int id, std::string topic, int read_rate) {
   nh_ = nh;
   driver_ = driver;
   id_ = id;
@@ -14,7 +11,7 @@ ButtonHardwareInterface::ButtonHardwareInterface(rclcpp::Node::SharedPtr nh,
 }
 
 bool ButtonHardwareInterface::init() {
-  data_ = (uint8_t *) malloc(3 * sizeof(uint8_t));
+  data_ = (uint8_t *)malloc(3 * sizeof(uint8_t));
   button_pub_ = nh_->create_publisher<bitbots_msgs::msg::Buttons>(topic_, 1);
   diagnostic_pub_ = nh_->create_publisher<diagnostic_msgs::msg::DiagnosticArray>("/diagnostics", 10);
   return true;
@@ -25,8 +22,7 @@ void ButtonHardwareInterface::read(const rclcpp::Time &t, const rclcpp::Duration
    * Reads the buttons
    */
   counter_ = (counter_ + 1) % read_rate_;
-  if (counter_ != 0)
-    return;
+  if (counter_ != 0) return;
   bool read_successful = true;
   if (driver_->readMultipleRegisters(id_, 76, 3, data_)) {
     bitbots_msgs::msg::Buttons msg;
@@ -62,4 +58,4 @@ void ButtonHardwareInterface::read(const rclcpp::Time &t, const rclcpp::Duration
 
 // we don't write anything to the buttons
 void ButtonHardwareInterface::write(const rclcpp::Time &t, const rclcpp::Duration &dt) {}
-}
+}  // namespace bitbots_ros_control
diff --git a/bitbots_ros_control/src/core_hardware_interface.cpp b/bitbots_ros_control/src/core_hardware_interface.cpp
index 0a8a000f..4c4483df 100644
--- a/bitbots_ros_control/src/core_hardware_interface.cpp
+++ b/bitbots_ros_control/src/core_hardware_interface.cpp
@@ -1,4 +1,4 @@
-#include <bitbots_ros_control/core_hardware_interface.h>
+#include <bitbots_ros_control/core_hardware_interface.hpp>
 
 namespace bitbots_ros_control {
 
@@ -15,7 +15,8 @@ CoreHardwareInterface::CoreHardwareInterface(rclcpp::Node::SharedPtr nh, std::sh
   last_read_successful_ = false;
 }
 
-bool CoreHardwareInterface::switch_power(std::shared_ptr<std_srvs::srv::SetBool::Request> req, std::shared_ptr<std_srvs::srv::SetBool::Response> resp) {
+bool CoreHardwareInterface::switch_power(std::shared_ptr<std_srvs::srv::SetBool::Request> req,
+                                         std::shared_ptr<std_srvs::srv::SetBool::Response> resp) {
   requested_power_status_ = req->data;
   // wait for main loop to set value
   resp->success = true;
@@ -24,7 +25,7 @@ bool CoreHardwareInterface::switch_power(std::shared_ptr<std_srvs::srv::SetBool:
 
 bool CoreHardwareInterface::init() {
   VBAT_individual_.data.resize(6);
-  data_ = (uint8_t *) malloc(16 * sizeof(uint8_t));
+  data_ = (uint8_t *)malloc(16 * sizeof(uint8_t));
   power_pub_ = nh_->create_publisher<std_msgs::msg::Bool>("/core/power_switch_status", 1);
   vcc_pub_ = nh_->create_publisher<std_msgs::msg::Float64>("/core/vcc", 1);
   vbat_pub_ = nh_->create_publisher<std_msgs::msg::Float64>("/core/vbat", 1);
@@ -36,16 +37,13 @@ bool CoreHardwareInterface::init() {
   diagnostic_pub_ = nh_->create_publisher<diagnostic_msgs::msg::DiagnosticArray>("/diagnostics", 10);
 
   // service to switch power
-  power_switch_service_ = nh_->create_service<std_srvs::srv::SetBool>("/core/switch_power",
-                                                                      std::bind(
-                                                                          &CoreHardwareInterface::switch_power,
-                                                                          this,
-                                                                          std::placeholders::_1,
-                                                                          std::placeholders::_2));
+  power_switch_service_ = nh_->create_service<std_srvs::srv::SetBool>(
+      "/core/switch_power",
+      std::bind(&CoreHardwareInterface::switch_power, this, std::placeholders::_1, std::placeholders::_2));
   return true;
 }
 
-bool CoreHardwareInterface::get_power_status(){
+bool CoreHardwareInterface::get_power_status() {
   // this is only true when the physical switch and the soft status are true
   return power_control_status_.data && power_switch_status_.data;
 }
@@ -63,26 +61,26 @@ void CoreHardwareInterface::read(const rclcpp::Time &t, const rclcpp::Duration &
       // we read one string of bytes. see CORE firmware for definition of registers
       // convert to volt
       power_control_status_.data = data_[0];
-      VEXT_.data = (((float) dxlMakeword(data_[5], data_[6])) * (3.3 / 1024)) * 6;
-      VCC_.data = (((float) dxlMakeword(data_[7], data_[8])) * (3.3 / 1024)) * 6;
-      VDXL_.data = (((float) dxlMakeword(data_[9], data_[10])) * (3.3 / 1024)) * 6;
+      VEXT_.data = (((float)dxlMakeword(data_[5], data_[6])) * (3.3 / 1024)) * 6;
+      VCC_.data = (((float)dxlMakeword(data_[7], data_[8])) * (3.3 / 1024)) * 6;
+      VDXL_.data = (((float)dxlMakeword(data_[9], data_[10])) * (3.3 / 1024)) * 6;
       // convert to ampere. first go to voltage by 1024*3.3. shift by 2.5 and mulitply by volt/ampere
-      current_.data = ((((float) dxlMakeword(data_[11], data_[12])) * (3.3 / 1024)) - 2.5) / -0.066;
+      current_.data = ((((float)dxlMakeword(data_[11], data_[12])) * (3.3 / 1024)) - 2.5) / -0.066;
       // we need to apply a threshold on this to see if power is on or off
       power_switch_status_.data = dxlMakeword(data_[13], data_[14]);
       // calculate cell voltages as voltages read * voltage divider ratio - previous cell voltage sum
-      VBAT_individual_.data[0] = ((float) dxlMakeword(data_[15], data_[16])) * (3.3 / 1024) * (3.3 / (1.2 + 3.3));
+      VBAT_individual_.data[0] = ((float)dxlMakeword(data_[15], data_[16])) * (3.3 / 1024) * (3.3 / (1.2 + 3.3));
       VBAT_individual_.data[1] =
-          ((float) dxlMakeword(data_[17], data_[18])) * (3.3 / 1024) * (3.6 / (6.2 + 3.6)) - VBAT_individual_.data[0];
+          ((float)dxlMakeword(data_[17], data_[18])) * (3.3 / 1024) * (3.6 / (6.2 + 3.6)) - VBAT_individual_.data[0];
       VBAT_individual_.data[2] =
-          ((float) dxlMakeword(data_[19], data_[20])) * (3.3 / 1024) * (2.2 / (6.8 + 2.2)) - VBAT_individual_.data[1];
+          ((float)dxlMakeword(data_[19], data_[20])) * (3.3 / 1024) * (2.2 / (6.8 + 2.2)) - VBAT_individual_.data[1];
       VBAT_individual_.data[3] =
-          ((float) dxlMakeword(data_[21], data_[22])) * (3.3 / 1024) * (3.6 / (16.0 + 3.6)) - VBAT_individual_.data[2];
+          ((float)dxlMakeword(data_[21], data_[22])) * (3.3 / 1024) * (3.6 / (16.0 + 3.6)) - VBAT_individual_.data[2];
       VBAT_individual_.data[4] =
-          ((float) dxlMakeword(data_[23], data_[24])) * (3.3 / 1024) * (6.2 / (36.0 + 6.2)) - VBAT_individual_.data[3];
+          ((float)dxlMakeword(data_[23], data_[24])) * (3.3 / 1024) * (6.2 / (36.0 + 6.2)) - VBAT_individual_.data[3];
       VBAT_individual_.data[5] =
-          ((float) dxlMakeword(data_[25], data_[26])) * (3.3 / 1024) * (1.8 / (13.0 + 1.8)) - VBAT_individual_.data[4];
-      VBAT_.data = ((float) dxlMakeword(data_[25], data_[26])) * (3.3 / 1024) * (1.8 / (13.0 + 1.8));
+          ((float)dxlMakeword(data_[25], data_[26])) * (3.3 / 1024) * (1.8 / (13.0 + 1.8)) - VBAT_individual_.data[4];
+      VBAT_.data = ((float)dxlMakeword(data_[25], data_[26])) * (3.3 / 1024) * (1.8 / (13.0 + 1.8));
 
       power_pub_->publish(power_switch_status_);
       vcc_pub_->publish(VCC_);
@@ -131,7 +129,7 @@ void CoreHardwareInterface::read(const rclcpp::Time &t, const rclcpp::Duration &
     }
     std::vector<diagnostic_msgs::msg::KeyValue> keyValues = std::vector<diagnostic_msgs::msg::KeyValue>();
     // iterate through map and save it into values
-    for (auto const &ent1: map) {
+    for (auto const &ent1 : map) {
       diagnostic_msgs::msg::KeyValue key_value = diagnostic_msgs::msg::KeyValue();
       key_value.key = ent1.first;
       key_value.value = ent1.second;
@@ -152,4 +150,4 @@ void CoreHardwareInterface::write(const rclcpp::Time &t, const rclcpp::Duration
     driver_->writeRegister(id_, "Power", requested_power_status_);
   }
 }
-}
+}  // namespace bitbots_ros_control
diff --git a/bitbots_ros_control/src/dynamixel_servo_hardware_interface.cpp b/bitbots_ros_control/src/dynamixel_servo_hardware_interface.cpp
old mode 100755
new mode 100644
index 73914420..ccfbae2e
--- a/bitbots_ros_control/src/dynamixel_servo_hardware_interface.cpp
+++ b/bitbots_ros_control/src/dynamixel_servo_hardware_interface.cpp
@@ -1,41 +1,35 @@
-#include <bitbots_ros_control/dynamixel_servo_hardware_interface.h>
-#include <bitbots_ros_control/utils.h>
-
+#include <bitbots_ros_control/dynamixel_servo_hardware_interface.hpp>
+#include <bitbots_ros_control/utils.hpp>
 #include <utility>
 
 namespace bitbots_ros_control {
 using std::placeholders::_1;
 
-DynamixelServoHardwareInterface::DynamixelServoHardwareInterface(rclcpp::Node::SharedPtr nh) {
-  nh_ = nh;
-}
+DynamixelServoHardwareInterface::DynamixelServoHardwareInterface(rclcpp::Node::SharedPtr nh) { nh_ = nh; }
 
-void DynamixelServoHardwareInterface::addBusInterface(ServoBusInterface *bus) {
-  bus_interfaces_.push_back(bus);
-}
+void DynamixelServoHardwareInterface::addBusInterface(ServoBusInterface *bus) { bus_interfaces_.push_back(bus); }
 
-void DynamixelServoHardwareInterface::writeROMRAM(bool first_time){
-  for (ServoBusInterface *bus: bus_interfaces_) {
+void DynamixelServoHardwareInterface::writeROMRAM(bool first_time) {
+  for (ServoBusInterface *bus : bus_interfaces_) {
     bus->writeROMRAM(first_time);
   }
 }
 
 bool DynamixelServoHardwareInterface::init() {
   /*
-  * This initializes the hardware interface based on the values set in the config.
-  * The servos are pinged to verify that a connection is present and to know which type of servo it is.
-  */
+   * This initializes the hardware interface based on the values set in the config.
+   * The servos are pinged to verify that a connection is present and to know which type of servo it is.
+   */
 
   // Init subscriber / publisher
   set_torque_sub_ = nh_->create_subscription<std_msgs::msg::Bool>(
       "set_torque", 1, std::bind(&DynamixelServoHardwareInterface::setTorqueCb, this, _1));
   set_torque_indiv_sub_ = nh_->create_subscription<bitbots_msgs::msg::JointTorque>(
-      "set_torque_individual", 1, std::bind(
-          &DynamixelServoHardwareInterface::individualTorqueCb, this, _1));
+      "set_torque_individual", 1, std::bind(&DynamixelServoHardwareInterface::individualTorqueCb, this, _1));
   // todo we could change the command topic to something better
   sub_command_ = nh_->create_subscription<bitbots_msgs::msg::JointCommand>(
-      "/DynamixelController/command", 1, std::bind(&DynamixelServoHardwareInterface::commandCb,
-                                                   this, std::placeholders::_1));
+      "/DynamixelController/command", 1,
+      std::bind(&DynamixelServoHardwareInterface::commandCb, this, std::placeholders::_1));
   pwm_pub_ = nh_->create_publisher<sensor_msgs::msg::JointState>("/servo_PWM", 10);
   joint_pub_ = nh_->create_publisher<sensor_msgs::msg::JointState>("/joint_states", 10);
 
@@ -43,7 +37,7 @@ bool DynamixelServoHardwareInterface::init() {
 
   // init merged vectors for controller
   joint_count_ = 0;
-  for (ServoBusInterface *bus: bus_interfaces_) {
+  for (ServoBusInterface *bus : bus_interfaces_) {
     joint_count_ = joint_count_ + bus->joint_count_;
     for (int i = 0; i < bus->joint_count_; i++) {
       joint_names_.push_back(bus->joint_names_[i]);
@@ -85,10 +79,10 @@ bool DynamixelServoHardwareInterface::init() {
 }
 
 void DynamixelServoHardwareInterface::commandCb(const bitbots_msgs::msg::JointCommand &command_msg) {
-  if (!(command_msg.joint_names.size()==command_msg.positions.size() &&
-      command_msg.joint_names.size()==command_msg.velocities.size() &&
-      command_msg.joint_names.size()==command_msg.accelerations.size() &&
-      command_msg.joint_names.size()==command_msg.max_currents.size())) {
+  if (!(command_msg.joint_names.size() == command_msg.positions.size() &&
+        command_msg.joint_names.size() == command_msg.velocities.size() &&
+        command_msg.joint_names.size() == command_msg.accelerations.size() &&
+        command_msg.joint_names.size() == command_msg.max_currents.size())) {
     RCLCPP_ERROR(nh_->get_logger(), "Dynamixel Controller got command with inconsistent array lengths.");
     return;
   }
@@ -115,7 +109,7 @@ void DynamixelServoHardwareInterface::individualTorqueCb(bitbots_msgs::msg::Join
   for (size_t i = 0; i < msg.joint_names.size(); i++) {
     bool success = false;
     for (size_t j = 0; j < joint_names_.size(); j++) {
-      if (msg.joint_names[i]==joint_names_[j]) {
+      if (msg.joint_names[i] == joint_names_[j]) {
         if (i < msg.joint_names.size()) {
           goal_torque_individual_[j] = msg.on[i];
           success = true;
@@ -128,7 +122,7 @@ void DynamixelServoHardwareInterface::individualTorqueCb(bitbots_msgs::msg::Join
       RCLCPP_WARN(nh_->get_logger(), "Couldn't set torque for servo %s ", msg.joint_names[i].c_str());
     }
   }
-  for (ServoBusInterface *bus: bus_interfaces_) {
+  for (ServoBusInterface *bus : bus_interfaces_) {
     bus->switch_individual_torque_ = true;
   }
 }
@@ -137,7 +131,7 @@ void DynamixelServoHardwareInterface::setTorqueCb(std_msgs::msg::Bool::SharedPtr
   /**
    * This saves the given required value, so that it can be written to the servos in the write method
    */
-  for (ServoBusInterface *bus: bus_interfaces_) {
+  for (ServoBusInterface *bus : bus_interfaces_) {
     bus->goal_torque_ = enabled->data;
   }
   for (size_t j = 0; j < joint_names_.size(); j++) {
@@ -147,9 +141,9 @@ void DynamixelServoHardwareInterface::setTorqueCb(std_msgs::msg::Bool::SharedPtr
 
 void DynamixelServoHardwareInterface::read(const rclcpp::Time &t, const rclcpp::Duration &dt) {
   // retrieve values from the buses and set controller vector accordingly
-  //todo improve performance
+  // todo improve performance
   int i = 0;
-  for (ServoBusInterface *bus: bus_interfaces_) {
+  for (ServoBusInterface *bus : bus_interfaces_) {
     for (int j = 0; j < bus->joint_count_; j++) {
       current_position_[i] = bus->current_position_[j];
       current_velocity_[i] = bus->current_velocity_[j];
@@ -174,9 +168,9 @@ void DynamixelServoHardwareInterface::read(const rclcpp::Time &t, const rclcpp::
 
 void DynamixelServoHardwareInterface::write(const rclcpp::Time &t, const rclcpp::Duration &dt) {
   // set all values from controller to the buses
-  //todo improve performance
+  // todo improve performance
   int i = 0;
-  for (ServoBusInterface *bus: bus_interfaces_) {
+  for (ServoBusInterface *bus : bus_interfaces_) {
     for (int j = 0; j < bus->joint_count_; j++) {
       bus->goal_position_[j] = goal_position_[i];
       bus->goal_velocity_[j] = goal_velocity_[i];
@@ -187,4 +181,4 @@ void DynamixelServoHardwareInterface::write(const rclcpp::Time &t, const rclcpp:
     }
   }
 }
-}
+}  // namespace bitbots_ros_control
diff --git a/bitbots_ros_control/src/imu_hardware_interface.cpp b/bitbots_ros_control/src/imu_hardware_interface.cpp
index 90785d0f..db9ca16c 100644
--- a/bitbots_ros_control/src/imu_hardware_interface.cpp
+++ b/bitbots_ros_control/src/imu_hardware_interface.cpp
@@ -1,5 +1,5 @@
-#include <bitbots_ros_control/imu_hardware_interface.h>
-#include <bitbots_ros_control/utils.h>
+#include <bitbots_ros_control/imu_hardware_interface.hpp>
+#include <bitbots_ros_control/utils.hpp>
 
 #define gravity 9.80665
 
@@ -7,12 +7,8 @@ namespace bitbots_ros_control {
 using std::placeholders::_1;
 using std::placeholders::_2;
 
-ImuHardwareInterface::ImuHardwareInterface(rclcpp::Node::SharedPtr nh,
-                                           std::shared_ptr<DynamixelDriver> &driver,
-                                           int id,
-                                           std::string topic,
-                                           std::string frame,
-                                           std::string name) {
+ImuHardwareInterface::ImuHardwareInterface(rclcpp::Node::SharedPtr nh, std::shared_ptr<DynamixelDriver> &driver, int id,
+                                           std::string topic, std::string frame, std::string name) {
   nh_ = nh;
   driver_ = driver;
   id_ = id;
@@ -28,23 +24,22 @@ bool ImuHardwareInterface::init() {
   status_imu_.name = name_;
   status_imu_.hardware_id = std::to_string(id_);
 
-
   // alloc memory for imu values
-  orientation_ = (double *) malloc(4 * sizeof(double));
+  orientation_ = (double *)malloc(4 * sizeof(double));
   std::fill(orientation_, orientation_ + 4, 0);
-  orientation_covariance_ = (double *) malloc(9 * sizeof(double));
+  orientation_covariance_ = (double *)malloc(9 * sizeof(double));
   std::fill(orientation_covariance_, orientation_covariance_ + 9, 0);
-  angular_velocity_ = (double *) malloc(3 * sizeof(double));
+  angular_velocity_ = (double *)malloc(3 * sizeof(double));
   std::fill(angular_velocity_, angular_velocity_ + 3, 0);
-  angular_velocity_covariance_ = (double *) malloc(9 * sizeof(double));
+  angular_velocity_covariance_ = (double *)malloc(9 * sizeof(double));
   std::fill(angular_velocity_covariance_, angular_velocity_covariance_ + 9, 0);
-  linear_acceleration_ = (double *) malloc(3 * sizeof(double));
+  linear_acceleration_ = (double *)malloc(3 * sizeof(double));
   std::fill(linear_acceleration_, linear_acceleration_ + 3, 0);
-  linear_acceleration_covariance_ = (double *) malloc(9 * sizeof(double));
+  linear_acceleration_covariance_ = (double *)malloc(9 * sizeof(double));
   std::fill(linear_acceleration_covariance_, linear_acceleration_covariance_ + 9, 0);
 
-  data_ = (uint8_t *) malloc(40 * sizeof(uint8_t));
-  accel_calib_data_ = (uint8_t *) malloc(28 * sizeof(uint8_t));
+  data_ = (uint8_t *)malloc(40 * sizeof(uint8_t));
+  accel_calib_data_ = (uint8_t *)malloc(28 * sizeof(uint8_t));
 
   // make services
   imu_ranges_service_ = nh_->create_service<bitbots_msgs::srv::IMURanges>(
@@ -54,7 +49,8 @@ bool ImuHardwareInterface::init() {
   reset_gyro_calibration_service_ = nh_->create_service<std_srvs::srv::Empty>(
       "/imu/reset_gyro_calibration", std::bind(&ImuHardwareInterface::resetGyroCalibration, this, _1, _2));
   complementary_filter_params_service_ = nh_->create_service<bitbots_msgs::srv::ComplementaryFilterParams>(
-      "/imu/set_complementary_filter_params", std::bind(&ImuHardwareInterface::setComplementaryFilterParams, this, _1, _2));
+      "/imu/set_complementary_filter_params",
+      std::bind(&ImuHardwareInterface::setComplementaryFilterParams, this, _1, _2));
   calibrate_accel_service_ = nh_->create_service<std_srvs::srv::Empty>(
       "/imu/calibrate_accel", std::bind(&ImuHardwareInterface::calibrateAccel, this, _1, _2));
   read_accel_calibration_service_ = nh_->create_service<bitbots_msgs::srv::AccelerometerCalibration>(
@@ -62,14 +58,17 @@ bool ImuHardwareInterface::init() {
   reset_accel_calibration_service_ = nh_->create_service<std_srvs::srv::Empty>(
       "/imu/reset_accel_calibration", std::bind(&ImuHardwareInterface::resetAccelCalibraton, this, _1, _2));
   set_accel_calib_threshold_service_ = nh_->create_service<bitbots_msgs::srv::SetAccelerometerCalibrationThreshold>(
-      "/imu/set_accel_calibration_threshold", std::bind(&ImuHardwareInterface::setAccelCalibrationThreshold, this, _1, _2));
+      "/imu/set_accel_calibration_threshold",
+      std::bind(&ImuHardwareInterface::setAccelCalibrationThreshold, this, _1, _2));
 
   imu_pub_ = nh_->create_publisher<sensor_msgs::msg::Imu>(topic_, 10);
   diagnostic_pub_ = nh_->create_publisher<diagnostic_msgs::msg::DiagnosticArray>("/diagnostics", 10);
 
   // read the current values in the IMU module so that they can later be displayed in diagnostic message
-  const std::shared_ptr<bitbots_msgs::srv::AccelerometerCalibration::Request> req = std::make_shared<bitbots_msgs::srv::AccelerometerCalibration::Request>();
-  std::shared_ptr<bitbots_msgs::srv::AccelerometerCalibration::Response> resp = std::make_shared<bitbots_msgs::srv::AccelerometerCalibration::Response>();
+  const std::shared_ptr<bitbots_msgs::srv::AccelerometerCalibration::Request> req =
+      std::make_shared<bitbots_msgs::srv::AccelerometerCalibration::Request>();
+  std::shared_ptr<bitbots_msgs::srv::AccelerometerCalibration::Response> resp =
+      std::make_shared<bitbots_msgs::srv::AccelerometerCalibration::Response>();
   readAccelCalibration(req, resp);
   if (driver_->readMultipleRegisters(id_, 102, 16, data_)) {
     gyro_range_ = data_[0];
@@ -84,7 +83,7 @@ bool ImuHardwareInterface::init() {
     RCLCPP_WARN(nh_->get_logger(), "Could not read IMU %s config values in init", name_.c_str());
   }
 
-  //set filter values differently if specified in the config and write them
+  // set filter values differently if specified in the config and write them
   do_adaptive_gain_ = nh_->get_parameter("imu.do_adaptive_gain").as_bool();
   do_bias_estimation_ = nh_->get_parameter("imu.do_bias_estimation").as_bool();
   accel_gain_ = nh_->get_parameter("imu.accel_gain").as_double();
@@ -104,8 +103,8 @@ void ImuHardwareInterface::read(const rclcpp::Time &t, const rclcpp::Duration &d
   if (driver_->readMultipleRegisters(id_, 36, 40, data_)) {
     // sometimes we only get 0 right after power on, don't use that data
     // test on orientation is sufficient as 0,0,0,0 would not be a valid quaternion
-    if (dxlMakeFloat(data_ + 24) + dxlMakeFloat(data_ + 28) + dxlMakeFloat(data_ + 32) + dxlMakeFloat(data_ + 36)
-        != 0) {
+    if (dxlMakeFloat(data_ + 24) + dxlMakeFloat(data_ + 28) + dxlMakeFloat(data_ + 32) + dxlMakeFloat(data_ + 36) !=
+        0) {
       angular_velocity_[0] = dxlMakeFloat(data_ + 0);
       angular_velocity_[1] = dxlMakeFloat(data_ + 4);
       angular_velocity_[2] = dxlMakeFloat(data_ + 8);
@@ -173,7 +172,7 @@ void ImuHardwareInterface::read(const rclcpp::Time &t, const rclcpp::Duration &d
     }
     std::vector<diagnostic_msgs::msg::KeyValue> keyValues = std::vector<diagnostic_msgs::msg::KeyValue>();
     // itarate through map and save it into values
-    for (auto const &ent1: map) {
+    for (auto const &ent1 : map) {
       diagnostic_msgs::msg::KeyValue key_value = diagnostic_msgs::msg::KeyValue();
       key_value.key = ent1.first;
       key_value.value = ent1.second;
@@ -194,7 +193,8 @@ void ImuHardwareInterface::setIMURanges(const std::shared_ptr<bitbots_msgs::srv:
   write_ranges_ = true;
 }
 
-void ImuHardwareInterface::calibrateGyro(const std::shared_ptr<std_srvs::srv::Empty::Request> req, std::shared_ptr<std_srvs::srv::Empty::Response> resp) {
+void ImuHardwareInterface::calibrateGyro(const std::shared_ptr<std_srvs::srv::Empty::Request> req,
+                                         std::shared_ptr<std_srvs::srv::Empty::Response> resp) {
   calibrate_gyro_ = true;
 }
 
@@ -203,9 +203,9 @@ void ImuHardwareInterface::resetGyroCalibration(const std::shared_ptr<std_srvs::
   reset_gyro_calibration_ = true;
 }
 
-void ImuHardwareInterface::setComplementaryFilterParams(const std::shared_ptr<bitbots_msgs::srv::ComplementaryFilterParams::Request> req,
-                                                        std::shared_ptr<bitbots_msgs::srv::ComplementaryFilterParams::Response> resp) {
-
+void ImuHardwareInterface::setComplementaryFilterParams(
+    const std::shared_ptr<bitbots_msgs::srv::ComplementaryFilterParams::Request> req,
+    std::shared_ptr<bitbots_msgs::srv::ComplementaryFilterParams::Response> resp) {
   do_adaptive_gain_ = req->do_adaptive_gain;
   do_bias_estimation_ = req->do_bias_estimation;
   accel_gain_ = req->accel_gain;
@@ -213,7 +213,8 @@ void ImuHardwareInterface::setComplementaryFilterParams(const std::shared_ptr<bi
   write_complementary_filter_params_ = true;
 }
 
-void ImuHardwareInterface::calibrateAccel(const std::shared_ptr<std_srvs::srv::Empty::Request> req, std::shared_ptr<std_srvs::srv::Empty::Response> resp) {
+void ImuHardwareInterface::calibrateAccel(const std::shared_ptr<std_srvs::srv::Empty::Request> req,
+                                          std::shared_ptr<std_srvs::srv::Empty::Response> resp) {
   calibrate_accel_ = true;
 }
 
@@ -222,8 +223,9 @@ void ImuHardwareInterface::resetAccelCalibraton(const std::shared_ptr<std_srvs::
   reset_accel_calibration_ = true;
 }
 
-void ImuHardwareInterface::readAccelCalibration(const std::shared_ptr<bitbots_msgs::srv::AccelerometerCalibration::Request> req,
-                                                std::shared_ptr<bitbots_msgs::srv::AccelerometerCalibration::Response> resp) {
+void ImuHardwareInterface::readAccelCalibration(
+    const std::shared_ptr<bitbots_msgs::srv::AccelerometerCalibration::Request> req,
+    std::shared_ptr<bitbots_msgs::srv::AccelerometerCalibration::Response> resp) {
   resp->biases.resize(3);
   resp->scales.resize(3);
   if (driver_->readMultipleRegisters(id_, 118, 28, accel_calib_data_)) {
@@ -246,8 +248,9 @@ void ImuHardwareInterface::readAccelCalibration(const std::shared_ptr<bitbots_ms
   }
 }
 
-void ImuHardwareInterface::setAccelCalibrationThreshold(const std::shared_ptr<bitbots_msgs::srv::SetAccelerometerCalibrationThreshold::Request> req,
-                                                        std::shared_ptr<bitbots_msgs::srv::SetAccelerometerCalibrationThreshold::Response> resp) {
+void ImuHardwareInterface::setAccelCalibrationThreshold(
+    const std::shared_ptr<bitbots_msgs::srv::SetAccelerometerCalibrationThreshold::Request> req,
+    std::shared_ptr<bitbots_msgs::srv::SetAccelerometerCalibrationThreshold::Response> resp) {
   accel_calib_threshold_read_ = req->threshold;
   accel_calib_threshold_ = accel_calib_threshold_read_;
   set_accel_calib_threshold_ = true;
@@ -288,12 +291,12 @@ void ImuHardwareInterface::write(const rclcpp::Time &t, const rclcpp::Duration &
   }
   if (calibrate_accel_) {
     RCLCPP_ERROR(nh_->get_logger(), "Disabled for normal users for safety reasons, uncomment code to use");
-    //driver_->writeRegister(id_, "Calibrate_Accel", 1);
+    // driver_->writeRegister(id_, "Calibrate_Accel", 1);
     calibrate_accel_ = false;
   }
   if (reset_accel_calibration_) {
     RCLCPP_ERROR(nh_->get_logger(), "Disabled for normal users for safety reasons, uncomment code to use");
-    //driver_->writeRegister(id_, "Reset_Accel_Calibration", 1);
+    // driver_->writeRegister(id_, "Reset_Accel_Calibration", 1);
     reset_accel_calibration_ = false;
   }
   if (set_accel_calib_threshold_) {
@@ -303,6 +306,4 @@ void ImuHardwareInterface::write(const rclcpp::Time &t, const rclcpp::Duration &
     set_accel_calib_threshold_ = false;
   }
 }
-}
-
-
+}  // namespace bitbots_ros_control
diff --git a/bitbots_ros_control/src/leds_hardware_interface.cpp b/bitbots_ros_control/src/leds_hardware_interface.cpp
index 9041df62..faa516b2 100644
--- a/bitbots_ros_control/src/leds_hardware_interface.cpp
+++ b/bitbots_ros_control/src/leds_hardware_interface.cpp
@@ -1,5 +1,5 @@
-#include <bitbots_ros_control/leds_hardware_interface.h>
-#include <bitbots_ros_control/utils.h>
+#include <bitbots_ros_control/leds_hardware_interface.hpp>
+#include <bitbots_ros_control/utils.hpp>
 
 namespace bitbots_ros_control {
 using std::placeholders::_1;
@@ -23,22 +23,23 @@ LedsHardwareInterface::LedsHardwareInterface(rclcpp::Node::SharedPtr nh, std::sh
 }
 
 bool LedsHardwareInterface::init() {
-  if (start_number_==0) {
-    leds_service_ = nh_->create_service<bitbots_msgs::srv::Leds>("/set_leds", std::bind(&LedsHardwareInterface::setLeds, this, _1, _2));
+  if (start_number_ == 0) {
+    leds_service_ = nh_->create_service<bitbots_msgs::srv::Leds>(
+        "/set_leds", std::bind(&LedsHardwareInterface::setLeds, this, _1, _2));
   }
-  sub0_ = nh_->create_subscription<std_msgs::msg::ColorRGBA>("/led" + std::to_string(start_number_),
-                                                             1, std::bind(&LedsHardwareInterface::ledCb0, this, _1));
-  sub1_ = nh_->create_subscription<std_msgs::msg::ColorRGBA>("/led" + std::to_string(start_number_ + 1),
-                                                             1, std::bind(&LedsHardwareInterface::ledCb1, this, _1));
-  sub2_ = nh_->create_subscription<std_msgs::msg::ColorRGBA>("/led" + std::to_string(start_number_ + 2),
-                                                             1, std::bind(&LedsHardwareInterface::ledCb2, this, _1));
+  sub0_ = nh_->create_subscription<std_msgs::msg::ColorRGBA>("/led" + std::to_string(start_number_), 1,
+                                                             std::bind(&LedsHardwareInterface::ledCb0, this, _1));
+  sub1_ = nh_->create_subscription<std_msgs::msg::ColorRGBA>("/led" + std::to_string(start_number_ + 1), 1,
+                                                             std::bind(&LedsHardwareInterface::ledCb1, this, _1));
+  sub2_ = nh_->create_subscription<std_msgs::msg::ColorRGBA>("/led" + std::to_string(start_number_ + 2), 1,
+                                                             std::bind(&LedsHardwareInterface::ledCb2, this, _1));
   return true;
 }
 
 // todo this could be done more clever and for a general number of leds
 void LedsHardwareInterface::ledCb0(std_msgs::msg::ColorRGBA msg) {
   // only write to bus if there is actually a change
-  if (msg.r!=leds_[0].r || msg.g!=leds_[0].g || msg.b!=leds_[0].b) {
+  if (msg.r != leds_[0].r || msg.g != leds_[0].g || msg.b != leds_[0].b) {
     leds_[0] = msg;
     write_leds_ = true;
   }
@@ -46,7 +47,7 @@ void LedsHardwareInterface::ledCb0(std_msgs::msg::ColorRGBA msg) {
 
 void LedsHardwareInterface::ledCb1(std_msgs::msg::ColorRGBA msg) {
   // only write to bus if there is actually a change
-  if (msg.r!=leds_[1].r || msg.g!=leds_[1].g || msg.b!=leds_[1].b) {
+  if (msg.r != leds_[1].r || msg.g != leds_[1].g || msg.b != leds_[1].b) {
     leds_[1] = msg;
     write_leds_ = true;
   }
@@ -54,7 +55,7 @@ void LedsHardwareInterface::ledCb1(std_msgs::msg::ColorRGBA msg) {
 
 void LedsHardwareInterface::ledCb2(std_msgs::msg::ColorRGBA msg) {
   // only write to bus if there is actually a change
-  if (msg.r!=leds_[2].r || msg.g!=leds_[2].g || msg.b!=leds_[2].b) {
+  if (msg.r != leds_[2].r || msg.g != leds_[2].g || msg.b != leds_[2].b) {
     leds_[2] = msg;
     write_leds_ = true;
   }
@@ -65,7 +66,7 @@ void LedsHardwareInterface::read(const rclcpp::Time &t, const rclcpp::Duration &
 void LedsHardwareInterface::setLeds(const std::shared_ptr<bitbots_msgs::srv::Leds::Request> req,
                                     std::shared_ptr<bitbots_msgs::srv::Leds::Response> resp) {
   RCLCPP_WARN_STREAM(nh_->get_logger(), "service");
-  if (req->leds.size()!=leds_.size()) {
+  if (req->leds.size() != leds_.size()) {
     RCLCPP_WARN_STREAM(nh_->get_logger(), "You are trying to set " << req->leds.size() << " leds while the board has "
                                                                    << leds_.size() << " leds.");
   }
@@ -74,8 +75,7 @@ void LedsHardwareInterface::setLeds(const std::shared_ptr<bitbots_msgs::srv::Led
     // return current state of LEDs
     resp->previous_leds.push_back(leds_[i]);
 
-    if (req->leds[i].r > 1.0f || req->leds[i].r < 0.0f ||
-        req->leds[i].g > 1.0f || req->leds[i].g < 0.0f ||
+    if (req->leds[i].r > 1.0f || req->leds[i].r < 0.0f || req->leds[i].g > 1.0f || req->leds[i].g < 0.0f ||
         req->leds[i].b > 1.0f || req->leds[i].b < 0.0f) {
       RCLCPP_WARN_STREAM(nh_->get_logger(), "You tried to set LED_" << i << " to a value not between 0 and 1");
     }
@@ -85,10 +85,10 @@ void LedsHardwareInterface::setLeds(const std::shared_ptr<bitbots_msgs::srv::Led
 }
 
 uint32_t rgba_to_int32(std_msgs::msg::ColorRGBA rgba) {
-  uint32_t led = ((uint8_t) (rgba.r*255));
-  led |= ((uint8_t) (rgba.g*255)) << 8;
-  led |= ((uint8_t) (rgba.b*255)) << 16;
-  led |= ((uint8_t) (rgba.a*255)) << 24;
+  uint32_t led = ((uint8_t)(rgba.r * 255));
+  led |= ((uint8_t)(rgba.g * 255)) << 8;
+  led |= ((uint8_t)(rgba.b * 255)) << 16;
+  led |= ((uint8_t)(rgba.a * 255)) << 24;
   return led;
 }
 
@@ -102,4 +102,4 @@ void LedsHardwareInterface::write(const rclcpp::Time &t, const rclcpp::Duration
     write_leds_ = false;
   }
 }
-}
\ No newline at end of file
+}  // namespace bitbots_ros_control
\ No newline at end of file
diff --git a/bitbots_ros_control/src/node.cpp b/bitbots_ros_control/src/node.cpp
old mode 100755
new mode 100644
index b8d9eb7b..080ca4c8
--- a/bitbots_ros_control/src/node.cpp
+++ b/bitbots_ros_control/src/node.cpp
@@ -1,9 +1,10 @@
-#include <controller_manager/controller_manager.hpp>
-#include <bitbots_ros_control/wolfgang_hardware_interface.h>
 #include <signal.h>
-#include <thread>
-#include <rclcpp/rclcpp.hpp>
+
+#include <bitbots_ros_control/wolfgang_hardware_interface.hpp>
+#include <controller_manager/controller_manager.hpp>
 #include <rclcpp/experimental/executors/events_executor/events_executor.hpp>
+#include <rclcpp/rclcpp.hpp>
+#include <thread>
 
 sig_atomic_t volatile request_shutdown = 0;
 
@@ -18,7 +19,8 @@ int main(int argc, char *argv[]) {
 
   // initialize ros
   rclcpp::init(argc, argv);
-  rclcpp::NodeOptions options = rclcpp::NodeOptions().automatically_declare_parameters_from_overrides(true).allow_undeclared_parameters(true);
+  rclcpp::NodeOptions options =
+      rclcpp::NodeOptions().automatically_declare_parameters_from_overrides(true).allow_undeclared_parameters(true);
   rclcpp::Node::SharedPtr nh = rclcpp::Node::make_shared("wolfgang_hardware_interface", options);
 
   // create hardware interfaces
@@ -30,9 +32,9 @@ int main(int argc, char *argv[]) {
 
   // diagnostics
   int diag_counter = 0;
-  rclcpp::Publisher<diagnostic_msgs::msg::DiagnosticArray>::SharedPtr diagnostic_pub = nh->create_publisher<diagnostic_msgs::msg::DiagnosticArray>("/diagnostics", 10);
+  rclcpp::Publisher<diagnostic_msgs::msg::DiagnosticArray>::SharedPtr diagnostic_pub =
+      nh->create_publisher<diagnostic_msgs::msg::DiagnosticArray>("/diagnostics", 10);
   diagnostic_msgs::msg::DiagnosticArray array_msg = diagnostic_msgs::msg::DiagnosticArray();
-  std::vector<diagnostic_msgs::msg::DiagnosticStatus> array = std::vector<diagnostic_msgs::msg::DiagnosticStatus>();
   diagnostic_msgs::msg::DiagnosticStatus status = diagnostic_msgs::msg::DiagnosticStatus();
   // add prefix PS for pressure sensor to sort in diagnostic analyser
   status.name = "BUSBus";
@@ -50,7 +52,6 @@ int main(int argc, char *argv[]) {
   rclcpp::experimental::executors::EventsExecutor exec;
   exec.add_node(nh);
 
-
   while (!request_shutdown || nh->get_clock()->now().seconds() - stop_time.seconds() < 5) {
     //
     // read
@@ -64,7 +65,7 @@ int main(int argc, char *argv[]) {
     if (first_update) {
       first_update = false;
     } else {
-      //todo replaced controller part, if necessary
+      // todo replaced controller part, if necessary
     }
 
     //
@@ -81,7 +82,7 @@ int main(int argc, char *argv[]) {
     if (diag_counter % 100 == 0) {
       // check if we are staying the correct cycle time. warning if we only get half
       array_msg.header.stamp = nh->get_clock()->now();
-      if (rate.period() < std::chrono::nanoseconds (int(1e9 * (1 / control_loop_hz) * 2))) {
+      if (rate.period() < std::chrono::nanoseconds(int(1e9 * (1 / control_loop_hz) * 2))) {
         status.level = diagnostic_msgs::msg::DiagnosticStatus::OK;
         status.message = "";
       } else {
diff --git a/bitbots_ros_control/src/pressure_converter.cpp b/bitbots_ros_control/src/pressure_converter.cpp
index 8d72268c..1a929d88 100644
--- a/bitbots_ros_control/src/pressure_converter.cpp
+++ b/bitbots_ros_control/src/pressure_converter.cpp
@@ -1,274 +1,276 @@
-#include <bitbots_ros_control/pressure_converter.h>
+#include <bitbots_ros_control/pressure_converter.hpp>
 using std::placeholders::_1;
 using std::placeholders::_2;
 
-
-PressureConverter::PressureConverter(rclcpp::Node::SharedPtr nh, char side){
-    nh_ = nh;
-    std::string topic;
-    rclcpp::CallbackGroup::SharedPtr sub_cbg_ = nh_->create_callback_group(rclcpp::CallbackGroupType::MutuallyExclusive);
-
-    if (side == 'l') {
-        if (!nh_->has_parameter("left_topic"))
-            RCLCPP_ERROR_STREAM(nh_->get_logger(), nh_->get_name() << ": left_topic not specified");
-        topic = nh_->get_parameter("left_topic").as_string();
-    } else if (side == 'r') {
-        if (!nh_->has_parameter("right_topic"))
-            RCLCPP_ERROR_STREAM(nh_->get_logger(), nh_->get_name() << ": right_topic not specified");
-        topic = nh_->get_parameter("right_topic").as_string();
-    }
-
-    if (!nh_->has_parameter("average"))
-        RCLCPP_ERROR_STREAM(nh_->get_logger(), nh_->get_name() <<": average not specified");
-    average_ = nh_->get_parameter("average").as_int();
-
-    if (!nh_->has_parameter("scale_and_zero_average"))
-        RCLCPP_ERROR_STREAM(nh_->get_logger(), nh_->get_name() <<": scale_and_zero_average not specified");
-    scale_and_zero_average_ = nh_->get_parameter("scale_and_zero_average").as_int();
-
-    scale_lr_ = "scale_";
-    scale_lr_ += side;
-    zero_lr_ = "zero_";
-    zero_lr_ += side;
-    cop_lr_ = "cop_";
-    cop_lr_ += side;
-    sole_lr_ = side;
-    sole_lr_ += "_sole";
-
-    if (!nh_->has_parameter(zero_lr_)) {
-        RCLCPP_ERROR_STREAM(nh_->get_logger(), nh_->get_name() << ": " << zero_lr_ << " not specified");
-        zero_ = {0, 0, 0, 0};
-    }else{
-        zero_ = nh_->get_parameter(zero_lr_).as_double_array();
-    }
-
-    if (!nh_->has_parameter(scale_lr_)) {
-        RCLCPP_ERROR_STREAM(nh_->get_logger(), nh_->get_name()  << ": " << scale_lr_ << " not specified");
-        scale_ = {1, 1, 1, 1};
-    }else{
-        scale_ = nh_->get_parameter(scale_lr_).as_double_array();
-    }
-    if (!nh_->has_parameter("cop_threshold"))
-        RCLCPP_ERROR_STREAM(nh_->get_logger(), nh_->get_name() << ": cop_threshold not specified");
-    cop_threshold_ = nh_->get_parameter("cop_threshold").as_double();
-
-    side_ = side;
-
-    // initialize vector for previous values for average calculations
-    for (int i = 0; i < 4; i++) {
-        std::vector<double> x;
-        for (int j = 0; j < average_; j++) {
-            x.push_back(0.0);
-        }
-        previous_values_.push_back(x);
-    }
-    current_index_ = 0;
-
-    save_zero_and_scale_values_ = false;
-    resetZeroAndScaleValues();
-
-    filtered_pub_ = nh_->create_publisher<bitbots_msgs::msg::FootPressure>(topic + "/filtered", 1);
-    cop_pub_ = nh_->create_publisher<geometry_msgs::msg::PointStamped>("/" + cop_lr_, 1);
-    std::string wrench_topics[] = {"l_front", "l_back", "r_front", "r_back", "cop"};
-    for (int i = 0; i < 5; i++) {
-        std::stringstream single_wrench_topic;
-        single_wrench_topic << topic << "/wrench/" << wrench_topics[i];
-        wrench_pubs_.push_back(nh_->create_publisher<geometry_msgs::msg::WrenchStamped>(single_wrench_topic.str(), 1));
+PressureConverter::PressureConverter(rclcpp::Node::SharedPtr nh, char side) {
+  nh_ = nh;
+  std::string topic;
+  rclcpp::CallbackGroup::SharedPtr sub_cbg_ = nh_->create_callback_group(rclcpp::CallbackGroupType::MutuallyExclusive);
+
+  if (side == 'l') {
+    if (!nh_->has_parameter("left_topic"))
+      RCLCPP_ERROR_STREAM(nh_->get_logger(), nh_->get_name() << ": left_topic not specified");
+    topic = nh_->get_parameter("left_topic").as_string();
+  } else if (side == 'r') {
+    if (!nh_->has_parameter("right_topic"))
+      RCLCPP_ERROR_STREAM(nh_->get_logger(), nh_->get_name() << ": right_topic not specified");
+    topic = nh_->get_parameter("right_topic").as_string();
+  }
+
+  if (!nh_->has_parameter("average"))
+    RCLCPP_ERROR_STREAM(nh_->get_logger(), nh_->get_name() << ": average not specified");
+  average_ = nh_->get_parameter("average").as_int();
+
+  if (!nh_->has_parameter("scale_and_zero_average"))
+    RCLCPP_ERROR_STREAM(nh_->get_logger(), nh_->get_name() << ": scale_and_zero_average not specified");
+  scale_and_zero_average_ = nh_->get_parameter("scale_and_zero_average").as_int();
+
+  scale_lr_ = "scale_";
+  scale_lr_ += side;
+  zero_lr_ = "zero_";
+  zero_lr_ += side;
+  cop_lr_ = "cop_";
+  cop_lr_ += side;
+  sole_lr_ = side;
+  sole_lr_ += "_sole";
+
+  if (!nh_->has_parameter(zero_lr_)) {
+    RCLCPP_ERROR_STREAM(nh_->get_logger(), nh_->get_name() << ": " << zero_lr_ << " not specified");
+    zero_ = {0, 0, 0, 0};
+  } else {
+    zero_ = nh_->get_parameter(zero_lr_).as_double_array();
+  }
+
+  if (!nh_->has_parameter(scale_lr_)) {
+    RCLCPP_ERROR_STREAM(nh_->get_logger(), nh_->get_name() << ": " << scale_lr_ << " not specified");
+    scale_ = {1, 1, 1, 1};
+  } else {
+    scale_ = nh_->get_parameter(scale_lr_).as_double_array();
+  }
+  if (!nh_->has_parameter("cop_threshold"))
+    RCLCPP_ERROR_STREAM(nh_->get_logger(), nh_->get_name() << ": cop_threshold not specified");
+  cop_threshold_ = nh_->get_parameter("cop_threshold").as_double();
+
+  side_ = side;
+
+  // initialize vector for previous values for average calculations
+  for (int i = 0; i < 4; i++) {
+    std::vector<double> x;
+    for (int j = 0; j < average_; j++) {
+      x.push_back(0.0);
     }
-    for (int i = 0; i < 4; i++) {
-        std::stringstream single_wrench_frame;
-        single_wrench_frame << side << "_" << "cleat_" << wrench_topics[i];
-        wrench_frames_.push_back(single_wrench_frame.str());
-    }
-
-    scale_service_ = nh_->create_service<bitbots_msgs::srv::FootScale>(topic + "/set_foot_scale", std::bind(&PressureConverter::scaleCallback, this, _1, _2));
-    zero_service_ = nh_->create_service<std_srvs::srv::Empty>(topic + "/set_foot_zero", std::bind(&PressureConverter::zeroCallback, this, _1, _2));
-    rclcpp::SubscriptionOptions options;
-    rclcpp::QoS qos(0);
-    options.callback_group = sub_cbg_;
-    sub_ = nh_->create_subscription<bitbots_msgs::msg::FootPressure>(topic + "/raw", qos, std::bind(&PressureConverter::pressureCallback, this, _1), options);
-    tf_broadcaster_ = std::make_unique<tf2_ros::TransformBroadcaster>(*nh_);
-
-
-    sub_executor_.add_callback_group(sub_cbg_, nh_->get_node_base_interface());
-    sub_executor_thread_ = new std::thread(
-        [this]() {
-            sub_executor_.spin();
-        });
+    previous_values_.push_back(x);
+  }
+  current_index_ = 0;
+
+  save_zero_and_scale_values_ = false;
+  resetZeroAndScaleValues();
+
+  filtered_pub_ = nh_->create_publisher<bitbots_msgs::msg::FootPressure>(topic + "/filtered", 1);
+  cop_pub_ = nh_->create_publisher<geometry_msgs::msg::PointStamped>("/" + cop_lr_, 1);
+  std::string wrench_topics[] = {"l_front", "l_back", "r_front", "r_back", "cop"};
+  for (int i = 0; i < 5; i++) {
+    std::stringstream single_wrench_topic;
+    single_wrench_topic << topic << "/wrench/" << wrench_topics[i];
+    wrench_pubs_.push_back(nh_->create_publisher<geometry_msgs::msg::WrenchStamped>(single_wrench_topic.str(), 1));
+  }
+  for (int i = 0; i < 4; i++) {
+    std::stringstream single_wrench_frame;
+    single_wrench_frame << side << "_"
+                        << "cleat_" << wrench_topics[i];
+    wrench_frames_.push_back(single_wrench_frame.str());
+  }
+
+  scale_service_ = nh_->create_service<bitbots_msgs::srv::FootScale>(
+      topic + "/set_foot_scale", std::bind(&PressureConverter::scaleCallback, this, _1, _2));
+  zero_service_ = nh_->create_service<std_srvs::srv::Empty>(topic + "/set_foot_zero",
+                                                            std::bind(&PressureConverter::zeroCallback, this, _1, _2));
+  rclcpp::SubscriptionOptions options;
+  rclcpp::QoS qos(0);
+  options.callback_group = sub_cbg_;
+  sub_ = nh_->create_subscription<bitbots_msgs::msg::FootPressure>(
+      topic + "/raw", qos, std::bind(&PressureConverter::pressureCallback, this, _1), options);
+  tf_broadcaster_ = std::make_unique<tf2_ros::TransformBroadcaster>(*nh_);
+
+  sub_executor_.add_callback_group(sub_cbg_, nh_->get_node_base_interface());
+  sub_executor_thread_ = new std::thread([this]() { sub_executor_.spin(); });
 }
 
 void PressureConverter::pressureCallback(bitbots_msgs::msg::FootPressure pressure_raw) {
-    bitbots_msgs::msg::FootPressure filtered_msg;
-
-    filtered_msg.header = pressure_raw.header;
-
-    previous_values_[0][current_index_] = ((pressure_raw.left_front) - zero_[0]) * scale_[0];
-    previous_values_[1][current_index_] = ((pressure_raw.left_back) - zero_[1]) * scale_[1],
-    previous_values_[2][current_index_] = ((pressure_raw.right_front) - zero_[2]) * scale_[2],
-    previous_values_[3][current_index_] = ((pressure_raw.right_back) - zero_[3]) * scale_[3];
-
-    filtered_msg.left_front = std::accumulate(previous_values_[0].begin(), previous_values_[0].end(), 0.0) / average_;
-    filtered_msg.left_back = std::accumulate(previous_values_[1].begin(), previous_values_[1].end(), 0.0) / average_;
-    filtered_msg.right_front = std::accumulate(previous_values_[2].begin(), previous_values_[2].end(), 0.0) / average_;
-    filtered_msg.right_back = std::accumulate(previous_values_[3].begin(), previous_values_[3].end(), 0.0) / average_;
-    current_index_ = (current_index_ + 1) % average_;
-
-    filtered_msg.left_front = std::max(filtered_msg.left_front, 0.0);
-    filtered_msg.left_back = std::max(filtered_msg.left_back, 0.0);
-    filtered_msg.right_front = std::max(filtered_msg.right_front, 0.0);
-    filtered_msg.right_back = std::max(filtered_msg.right_back, 0.0);
-
-    std::vector<double> forces_list =
-        {filtered_msg.left_front, filtered_msg.left_back, filtered_msg.right_front, filtered_msg.right_back};
-    for (int i = 0; i < 4; i++) {
-        geometry_msgs::msg::WrenchStamped w;
-        w.header.frame_id = wrench_frames_[i];
-        w.header.stamp = pressure_raw.header.stamp;
-        w.wrench.force.z = forces_list[i];
-        wrench_pubs_[i]->publish(w);
-    }
-    filtered_pub_->publish(filtered_msg);
-    if (save_zero_and_scale_values_) {
-        zero_and_scale_values_[0].push_back(pressure_raw.left_front);
-        zero_and_scale_values_[1].push_back(pressure_raw.left_back);
-        zero_and_scale_values_[2].push_back(pressure_raw.right_front);
-        zero_and_scale_values_[3].push_back(pressure_raw.right_back);
-    }
-
-    // publish center of pressure
-    double pos_x = 0.085, pos_y = 0.045;
-    geometry_msgs::msg::PointStamped cop;
-    cop.header.frame_id = sole_lr_;
-    cop.header.stamp = pressure_raw.header.stamp;
-    double sum_of_forces =
-        filtered_msg.left_front + filtered_msg.left_back + filtered_msg.right_back + filtered_msg.right_front;
-    if (sum_of_forces > cop_threshold_) {
-        cop.point.x =
-            (filtered_msg.left_front + filtered_msg.right_front - filtered_msg.left_back - filtered_msg.right_back) * pos_x
-            / sum_of_forces;
-        cop.point.x = std::max(std::min(cop.point.x, pos_x), -pos_x);
-
-        cop.point.y =
-            (filtered_msg.left_front + filtered_msg.left_back - filtered_msg.right_front - filtered_msg.right_back) * pos_y
-            / sum_of_forces;
-        cop.point.y = std::max(std::min(cop.point.y, pos_y), -pos_y);
-    } else {
-        cop.point.x = 0;
-        cop.point.y = 0;
-    }
-    cop_pub_->publish(cop);
-
-    geometry_msgs::msg::TransformStamped cop_tf;
-    cop_tf.header = cop.header;
-    cop_tf.child_frame_id = cop_lr_;
-    cop_tf.transform.translation.x = cop.point.x;
-    cop_tf.transform.translation.y = cop.point.y;
-    cop_tf.transform.rotation.w = 1;
-    tf_broadcaster_->sendTransform(cop_tf);
-
-    geometry_msgs::msg::WrenchStamped w_cop;
-    w_cop.header.frame_id = cop_lr_;
-    w_cop.header.stamp = pressure_raw.header.stamp;
-    w_cop.wrench.force.z = sum_of_forces;
-    wrench_pubs_[4]->publish(w_cop);
-
+  bitbots_msgs::msg::FootPressure filtered_msg;
+
+  filtered_msg.header = pressure_raw.header;
+
+  previous_values_[0][current_index_] = ((pressure_raw.left_front) - zero_[0]) * scale_[0];
+  previous_values_[1][current_index_] = ((pressure_raw.left_back) - zero_[1]) * scale_[1],
+  previous_values_[2][current_index_] = ((pressure_raw.right_front) - zero_[2]) * scale_[2],
+  previous_values_[3][current_index_] = ((pressure_raw.right_back) - zero_[3]) * scale_[3];
+
+  filtered_msg.left_front = std::accumulate(previous_values_[0].begin(), previous_values_[0].end(), 0.0) / average_;
+  filtered_msg.left_back = std::accumulate(previous_values_[1].begin(), previous_values_[1].end(), 0.0) / average_;
+  filtered_msg.right_front = std::accumulate(previous_values_[2].begin(), previous_values_[2].end(), 0.0) / average_;
+  filtered_msg.right_back = std::accumulate(previous_values_[3].begin(), previous_values_[3].end(), 0.0) / average_;
+  current_index_ = (current_index_ + 1) % average_;
+
+  filtered_msg.left_front = std::max(filtered_msg.left_front, 0.0);
+  filtered_msg.left_back = std::max(filtered_msg.left_back, 0.0);
+  filtered_msg.right_front = std::max(filtered_msg.right_front, 0.0);
+  filtered_msg.right_back = std::max(filtered_msg.right_back, 0.0);
+
+  std::vector<double> forces_list = {filtered_msg.left_front, filtered_msg.left_back, filtered_msg.right_front,
+                                     filtered_msg.right_back};
+  for (int i = 0; i < 4; i++) {
+    geometry_msgs::msg::WrenchStamped w;
+    w.header.frame_id = wrench_frames_[i];
+    w.header.stamp = pressure_raw.header.stamp;
+    w.wrench.force.z = forces_list[i];
+    wrench_pubs_[i]->publish(w);
+  }
+  filtered_pub_->publish(filtered_msg);
+  if (save_zero_and_scale_values_) {
+    zero_and_scale_values_[0].push_back(pressure_raw.left_front);
+    zero_and_scale_values_[1].push_back(pressure_raw.left_back);
+    zero_and_scale_values_[2].push_back(pressure_raw.right_front);
+    zero_and_scale_values_[3].push_back(pressure_raw.right_back);
+  }
+
+  // publish center of pressure
+  double pos_x = 0.085, pos_y = 0.045;
+  geometry_msgs::msg::PointStamped cop;
+  cop.header.frame_id = sole_lr_;
+  cop.header.stamp = pressure_raw.header.stamp;
+  double sum_of_forces =
+      filtered_msg.left_front + filtered_msg.left_back + filtered_msg.right_back + filtered_msg.right_front;
+  if (sum_of_forces > cop_threshold_) {
+    cop.point.x =
+        (filtered_msg.left_front + filtered_msg.right_front - filtered_msg.left_back - filtered_msg.right_back) *
+        pos_x / sum_of_forces;
+    cop.point.x = std::max(std::min(cop.point.x, pos_x), -pos_x);
+
+    cop.point.y =
+        (filtered_msg.left_front + filtered_msg.left_back - filtered_msg.right_front - filtered_msg.right_back) *
+        pos_y / sum_of_forces;
+    cop.point.y = std::max(std::min(cop.point.y, pos_y), -pos_y);
+  } else {
+    cop.point.x = 0;
+    cop.point.y = 0;
+  }
+  cop_pub_->publish(cop);
+
+  geometry_msgs::msg::TransformStamped cop_tf;
+  cop_tf.header = cop.header;
+  cop_tf.child_frame_id = cop_lr_;
+  cop_tf.transform.translation.x = cop.point.x;
+  cop_tf.transform.translation.y = cop.point.y;
+  cop_tf.transform.rotation.w = 1;
+  tf_broadcaster_->sendTransform(cop_tf);
+
+  geometry_msgs::msg::WrenchStamped w_cop;
+  w_cop.header.frame_id = cop_lr_;
+  w_cop.header.stamp = pressure_raw.header.stamp;
+  w_cop.wrench.force.z = sum_of_forces;
+  wrench_pubs_[4]->publish(w_cop);
 }
 
 void PressureConverter::resetZeroAndScaleValues() {
-    zero_and_scale_values_.clear();
-    for (int i = 0; i < 4; i++) {
-        std::vector<double> x;
-        zero_and_scale_values_.push_back(x);
-    }
+  zero_and_scale_values_.clear();
+  for (int i = 0; i < 4; i++) {
+    std::vector<double> x;
+    zero_and_scale_values_.push_back(x);
+  }
 }
 
 void PressureConverter::collectMessages() {
-    save_zero_and_scale_values_ = true;
-        while (int(zero_and_scale_values_[0].size()) < scale_and_zero_average_) {
-            RCLCPP_WARN_THROTTLE(nh_->get_logger(), *nh_->get_clock(), 0.25, "%ld of %d msgs", zero_and_scale_values_[0].size(), scale_and_zero_average_);
-        }
-    save_zero_and_scale_values_ = false;
+  save_zero_and_scale_values_ = true;
+  while (int(zero_and_scale_values_[0].size()) < scale_and_zero_average_) {
+    RCLCPP_WARN_THROTTLE(nh_->get_logger(), *nh_->get_clock(), 0.25, "%ld of %d msgs", zero_and_scale_values_[0].size(),
+                         scale_and_zero_average_);
+  }
+  save_zero_and_scale_values_ = false;
 }
 
 void PressureConverter::saveYAML() {
-    std::string path_to_yaml, robot_name, command;
-    if (std::getenv("ROBOT_NAME")) {
-        robot_name = std::getenv("ROBOT_NAME");
-    } else {
-        robot_name = "nobot";
-    }
-    path_to_yaml = ament_index_cpp::get_package_share_directory("bitbots_ros_control") + "/config/pressure_" + robot_name + ".yaml";
-
-    std::vector<double> scale_r, scale_l, zero_r, zero_l;
-    zero_r = nh_->get_parameter("zero_r").as_double_array();
-    zero_l = nh_->get_parameter("zero_l").as_double_array();
-    scale_r = nh_->get_parameter("scale_r").as_double_array();
-    scale_l = nh_->get_parameter("scale_l").as_double_array();
-
-    YAML::Emitter e;
-    e << YAML::BeginMap;
-    e << YAML::Key << "pressure_converter";
-    e << YAML::BeginMap;
-    e<< YAML::Key << "ros__parameters";
-    e << YAML::BeginMap;
-    e << YAML::Key << "zero_r";
-    e << YAML::Value << zero_r;
-    e << YAML::Key << "zero_l";
-    e << YAML::Value << zero_l;
-    e << YAML::Key << "scale_r";
-    e << YAML::Value << scale_r;
-    e << YAML::Key << "scale_l";
-    e << YAML::Value << scale_r;
-    e << YAML::EndMap;
-    e << YAML::EndMap;
-
-    std::ofstream yaml_file;
-    yaml_file.open(path_to_yaml);
-    yaml_file << e.c_str();
-    yaml_file.close();
+  std::string path_to_yaml, robot_name;
+  if (std::getenv("ROBOT_NAME")) {
+    robot_name = std::getenv("ROBOT_NAME");
+  } else {
+    robot_name = "nobot";
+  }
+  path_to_yaml =
+      ament_index_cpp::get_package_share_directory("bitbots_ros_control") + "/config/pressure_" + robot_name + ".yaml";
+
+  std::vector<double> scale_r, scale_l, zero_r, zero_l;
+  zero_r = nh_->get_parameter("zero_r").as_double_array();
+  zero_l = nh_->get_parameter("zero_l").as_double_array();
+  scale_r = nh_->get_parameter("scale_r").as_double_array();
+  scale_l = nh_->get_parameter("scale_l").as_double_array();
+
+  YAML::Emitter e;
+  e << YAML::BeginMap;
+  e << YAML::Key << "pressure_converter";
+  e << YAML::BeginMap;
+  e << YAML::Key << "ros__parameters";
+  e << YAML::BeginMap;
+  e << YAML::Key << "zero_r";
+  e << YAML::Value << zero_r;
+  e << YAML::Key << "zero_l";
+  e << YAML::Value << zero_l;
+  e << YAML::Key << "scale_r";
+  e << YAML::Value << scale_r;
+  e << YAML::Key << "scale_l";
+  e << YAML::Value << scale_l;
+  e << YAML::EndMap;
+  e << YAML::EndMap;
+
+  std::ofstream yaml_file;
+  yaml_file.open(path_to_yaml);
+  yaml_file << e.c_str();
+  yaml_file.close();
 }
 
-bool PressureConverter::scaleCallback(const std::shared_ptr<bitbots_msgs::srv::FootScale::Request> req, std::shared_ptr<bitbots_msgs::srv::FootScale::Response> resp) {
-    collectMessages();
-    double average =
-        std::accumulate(zero_and_scale_values_[req->sensor].begin(), zero_and_scale_values_[req->sensor].end(), 0.0)
-        / zero_and_scale_values_[req->sensor].size();
-    RCLCPP_WARN_STREAM(nh_->get_logger(), "avg: " << average);
-    average -= zero_[req->sensor];
-    RCLCPP_WARN_STREAM(nh_->get_logger(), "avg_after: " << average);
-
-    scale_[req->sensor] = req->weight / average;
-    resetZeroAndScaleValues();
-    nh_->set_parameter(rclcpp::Parameter(scale_lr_, rclcpp::ParameterValue(scale_)));
-    saveYAML();
-    return true;
+bool PressureConverter::scaleCallback(const std::shared_ptr<bitbots_msgs::srv::FootScale::Request> req,
+                                      std::shared_ptr<bitbots_msgs::srv::FootScale::Response> resp) {
+  collectMessages();
+  double average =
+      std::accumulate(zero_and_scale_values_[req->sensor].begin(), zero_and_scale_values_[req->sensor].end(), 0.0) /
+      zero_and_scale_values_[req->sensor].size();
+  RCLCPP_WARN_STREAM(nh_->get_logger(), "avg: " << average);
+  average -= zero_[req->sensor];
+  RCLCPP_WARN_STREAM(nh_->get_logger(), "avg_after: " << average);
+
+  scale_[req->sensor] = req->weight / average;
+  resetZeroAndScaleValues();
+  nh_->set_parameter(rclcpp::Parameter(scale_lr_, rclcpp::ParameterValue(scale_)));
+  saveYAML();
+  return true;
 }
 
-bool PressureConverter::zeroCallback(const std::shared_ptr<std_srvs::srv::Empty::Request> req, std::shared_ptr<std_srvs::srv::Empty::Response> resp) {
-    collectMessages();
-    for (int i = 0; i < 4; i++) {
-        zero_[i] = std::accumulate(zero_and_scale_values_[i].begin(), zero_and_scale_values_[i].end(), 0.0)
-                   / zero_and_scale_values_[i].size();
-    }
-    resetZeroAndScaleValues();
-    nh_->set_parameter(rclcpp::Parameter(zero_lr_, rclcpp::ParameterValue(zero_)));
-    saveYAML();
-    return true;
+bool PressureConverter::zeroCallback(const std::shared_ptr<std_srvs::srv::Empty::Request> req,
+                                     std::shared_ptr<std_srvs::srv::Empty::Response> resp) {
+  collectMessages();
+  for (int i = 0; i < 4; i++) {
+    zero_[i] = std::accumulate(zero_and_scale_values_[i].begin(), zero_and_scale_values_[i].end(), 0.0) /
+               zero_and_scale_values_[i].size();
+  }
+  resetZeroAndScaleValues();
+  nh_->set_parameter(rclcpp::Parameter(zero_lr_, rclcpp::ParameterValue(zero_)));
+  saveYAML();
+  return true;
 }
 
 int main(int argc, char *argv[]) {
-    rclcpp::init(argc, argv);
-    // declare parameters automatically
-    rclcpp::NodeOptions options = rclcpp::NodeOptions().automatically_declare_parameters_from_overrides(true);
-    rclcpp::Node::SharedPtr nh = rclcpp::Node::make_shared("pressure_converter", options);
+  rclcpp::init(argc, argv);
+  // declare parameters automatically
+  rclcpp::NodeOptions options = rclcpp::NodeOptions().automatically_declare_parameters_from_overrides(true);
+  rclcpp::Node::SharedPtr nh = rclcpp::Node::make_shared("pressure_converter", options);
 
-    rclcpp::executors::StaticSingleThreadedExecutor executor;
-    executor.add_node(nh);
+  rclcpp::executors::StaticSingleThreadedExecutor executor;
+  executor.add_node(nh);
 
-    PressureConverter r(nh, 'r');
-    PressureConverter l(nh, 'l');
+  PressureConverter r(nh, 'r');
+  PressureConverter l(nh, 'l');
 
-    executor.spin();
+  executor.spin();
 
-    return 0;
+  return 0;
 }
diff --git a/bitbots_ros_control/src/servo_bus_interface.cpp b/bitbots_ros_control/src/servo_bus_interface.cpp
index da575121..8f5099d6 100644
--- a/bitbots_ros_control/src/servo_bus_interface.cpp
+++ b/bitbots_ros_control/src/servo_bus_interface.cpp
@@ -1,10 +1,9 @@
-#include <bitbots_ros_control/servo_bus_interface.h>
-#include <bitbots_ros_control/utils.h>
+#include <bitbots_ros_control/servo_bus_interface.hpp>
+#include <bitbots_ros_control/utils.hpp>
 
 namespace bitbots_ros_control {
 
-ServoBusInterface::ServoBusInterface(rclcpp::Node::SharedPtr nh,
-                                     std::shared_ptr<DynamixelDriver> &driver,
+ServoBusInterface::ServoBusInterface(rclcpp::Node::SharedPtr nh, std::shared_ptr<DynamixelDriver> &driver,
                                      std::vector<std::tuple<int, std::string, float, float, std::string>> servos)
     : first_cycle_(true), read_position_(true), read_velocity_(false), read_effort_(true) {
   nh_ = nh;
@@ -23,8 +22,8 @@ bool ServoBusInterface::init() {
   reading_errors_ = 0;
 
   torqueless_mode_ = nh_->get_parameter("torqueless_mode").as_bool();
-  read_volt_temp_  = nh_->get_parameter("servos.read_volt_temp").as_bool();
-  vt_update_rate_  = nh_->get_parameter("servos.VT_update_rate").as_int();
+  read_volt_temp_ = nh_->get_parameter("servos.read_volt_temp").as_bool();
+  vt_update_rate_ = nh_->get_parameter("servos.VT_update_rate").as_int();
   warn_volt_ = nh_->get_parameter("servos.warn_volt").as_double();
   warn_temp_ = nh_->get_parameter("servos.warn_temp").as_double();
 
@@ -81,7 +80,6 @@ bool ServoBusInterface::init() {
   sync_write_goal_current_ = new int32_t;
   sync_write_goal_pwm_ = new int32_t;
 
-
   // write ROM and RAM values if wanted
   if (nh_->get_parameter("servos.set_ROM_RAM").as_bool()) {
     if (!writeROMRAM(true)) {
@@ -134,7 +132,7 @@ bool ServoBusInterface::loadDynamixels() {
 
   // iterate over all servos and save the information
   // the wolfgang hardware interface already loaded them into the driver by pinging
-  for (std::tuple<int, std::string, float, float, std::string> &servo: servos_) {
+  for (std::tuple<int, std::string, float, float, std::string> &servo : servos_) {
     int motor_id = std::get<0>(servo);
     joint_ids_.push_back(uint8_t(motor_id));
     std::string joint_name = std::get<1>(servo);
@@ -175,14 +173,15 @@ bool ServoBusInterface::writeROMRAM(bool first_time) {
       // Get the parameters of the group
       static const std::string ROM_RAM_PARAMETER_NAMESPACE = "servos.ROM_RAM_";
       // Get a list of all parameters in this section
-      rcl_interfaces::msg::ListParametersResult rom_ram_parameter_list = nh_->list_parameters(
-        {ROM_RAM_PARAMETER_NAMESPACE + joint_group}, 0);
+      rcl_interfaces::msg::ListParametersResult rom_ram_parameter_list =
+          nh_->list_parameters({ROM_RAM_PARAMETER_NAMESPACE + joint_group}, 0);
       // Iterate over the parameters and set each one
-      for (std::string &register_parameter_name: rom_ram_parameter_list.names) {
+      for (std::string &register_parameter_name : rom_ram_parameter_list.names) {
         // Get the value of the parameter
         int value = nh_->get_parameter(register_parameter_name).as_int();
         // Get only the name of the register without the namespace
-        auto register_name = register_parameter_name.substr(ROM_RAM_PARAMETER_NAMESPACE.size() + joint_group.size() + 1);
+        auto register_name =
+            register_parameter_name.substr(ROM_RAM_PARAMETER_NAMESPACE.size() + joint_group.size() + 1);
         // Add the parameter to the cache
         group_param_cache[joint_group][register_name] = value;
       }
@@ -194,17 +193,17 @@ bool ServoBusInterface::writeROMRAM(bool first_time) {
   // Check that all groups have the same parameters not only the same number of parameters
   // Get all keys of the first group
   std::vector<std::string> parameter_names;
-  for (auto const& [key, value]: joint_register_value_map[0]) {
+  for (auto const &[key, value] : joint_register_value_map[0]) {
     parameter_names.push_back(key);
   }
-  for (auto const& [group_id, group_params]: group_param_cache) {
+  for (auto const &[group_id, group_params] : group_param_cache) {
     // Check that the size is the same
     if (group_params.size() != parameter_names.size()) {
       RCLCPP_ERROR(nh_->get_logger(), "Servo group %s has a different number of servo parameters", group_id.c_str());
       sucess = false;
     }
     // Check that all keys are the same
-    for (std::string &key: parameter_names) {
+    for (std::string &key : parameter_names) {
       if (group_params.find(key) == group_params.end()) {
         RCLCPP_ERROR(nh_->get_logger(), "Group %s does not have parameter %s", group_id.c_str(), key.c_str());
         sucess = false;
@@ -213,18 +212,19 @@ bool ServoBusInterface::writeROMRAM(bool first_time) {
   }
 
   // Allocate memory for the values in the driver
-  int *values = (int *) malloc(joint_names_.size()*sizeof(int));
+  int *values = (int *)malloc(joint_names_.size() * sizeof(int));
   // Iterate over parameter names
-  for (auto register_name: parameter_names) {
+  for (auto register_name : parameter_names) {
     // Get the value for each joint
     for (size_t num = 0; num < joint_names_.size(); num++) {
       // Get the value from the cache
       int register_value = joint_register_value_map[num][register_name];
-      RCLCPP_DEBUG(nh_->get_logger(), "Setting %s on servo %s to %d", register_name.c_str(), joint_names_[num].c_str(), register_value);
+      RCLCPP_DEBUG(nh_->get_logger(), "Setting %s on servo %s to %d", register_name.c_str(), joint_names_[num].c_str(),
+                   register_value);
       // Set the value in the driver
       values[num] = register_value;
     }
-    if (first_time){
+    if (first_time) {
       driver_->addSyncWrite(register_name.c_str());
     }
     sucess = sucess && driver_->syncWrite(register_name.c_str(), values);
@@ -286,12 +286,10 @@ void ServoBusInterface::read(const rclcpp::Time &t, const rclcpp::Duration &dt)
   }
 
   if (read_volt_temp_) {
-    if (read_vt_counter_ + 1==vt_update_rate_) {
+    if (read_vt_counter_ + 1 == vt_update_rate_) {
       bool success = true;
       if (!syncReadVoltageAndTemp()) {
-        RCLCPP_ERROR_THROTTLE(nh_->get_logger(),
-                              *nh_->get_clock(),
-                              1000,
+        RCLCPP_ERROR_THROTTLE(nh_->get_logger(), *nh_->get_clock(), 1000,
                               "Couldn't read current input volatage and temperature!");
         success = false;
       }
@@ -302,7 +300,7 @@ void ServoBusInterface::read(const rclcpp::Time &t, const rclcpp::Duration &dt)
       }
       processVte(success);
     }
-    read_vt_counter_ = (read_vt_counter_ + 1)%vt_update_rate_;
+    read_vt_counter_ = (read_vt_counter_ + 1) % vt_update_rate_;
   }
 
   if (first_cycle_) {
@@ -324,7 +322,7 @@ void ServoBusInterface::read(const rclcpp::Time &t, const rclcpp::Duration &dt)
   }
 
   if (reading_errors_ + reading_successes_ > 200 &&
-      (float) reading_errors_/(float) (reading_successes_ + reading_errors_) > 0.05f) {
+      (float)reading_errors_ / (float)(reading_successes_ + reading_errors_) > 0.05f) {
     speakError(speak_pub_, "Multiple servo reading errors!");
     reading_errors_ = 0;
     reading_successes_ = 0;
@@ -340,8 +338,8 @@ void ServoBusInterface::write(const rclcpp::Time &t, const rclcpp::Duration &dt)
   /**
    * This is part of the mainloop and handles all the writing to the connected devices
    */
-  //check if we have to switch the torque
-  if (current_torque_!=goal_torque_) {
+  // check if we have to switch the torque
+  if (current_torque_ != goal_torque_) {
     writeTorque(goal_torque_);
   }
   if (switch_individual_torque_) {
@@ -354,48 +352,48 @@ void ServoBusInterface::write(const rclcpp::Time &t, const rclcpp::Duration &dt)
     RCLCPP_INFO_THROTTLE(nh_->get_logger(), *nh_->get_clock(), 5, "resetting torque after lost connection");
     writeTorqueForServos(goal_torque_individual_);
   }
-  if (control_mode_==POSITION_CONTROL) {
-    if (goal_effort_!=last_goal_effort_ || lost_servo_connection_) {
+  if (control_mode_ == POSITION_CONTROL) {
+    if (goal_effort_ != last_goal_effort_ || lost_servo_connection_) {
       syncWritePWM();
       last_goal_effort_ = goal_effort_;
     }
 
-    if (goal_velocity_!=last_goal_velocity_ || lost_servo_connection_) {
+    if (goal_velocity_ != last_goal_velocity_ || lost_servo_connection_) {
       syncWriteProfileVelocity();
       last_goal_velocity_ = goal_velocity_;
     }
 
-    if (goal_acceleration_!=last_goal_acceleration_ || lost_servo_connection_) {
+    if (goal_acceleration_ != last_goal_acceleration_ || lost_servo_connection_) {
       syncWriteProfileAcceleration();
       last_goal_acceleration_ = goal_acceleration_;
     }
 
-    if (goal_position_!=last_goal_position_ || lost_servo_connection_) {
+    if (goal_position_ != last_goal_position_ || lost_servo_connection_) {
       syncWritePosition();
       last_goal_position_ = goal_position_;
     }
-  } else if (control_mode_==VELOCITY_CONTROL) {
+  } else if (control_mode_ == VELOCITY_CONTROL) {
     syncWriteVelocity();
-  } else if (control_mode_==EFFORT_CONTROL) {
+  } else if (control_mode_ == EFFORT_CONTROL) {
     syncWriteCurrent();
-  } else if (control_mode_==CURRENT_BASED_POSITION_CONTROL) {
+  } else if (control_mode_ == CURRENT_BASED_POSITION_CONTROL) {
     // only write things if it is necessary
-    if (goal_effort_!=last_goal_effort_ || lost_servo_connection_) {
+    if (goal_effort_ != last_goal_effort_ || lost_servo_connection_) {
       syncWriteCurrent();
       last_goal_effort_ = goal_effort_;
     }
 
-    if (goal_velocity_!=last_goal_velocity_ || lost_servo_connection_) {
+    if (goal_velocity_ != last_goal_velocity_ || lost_servo_connection_) {
       syncWriteProfileVelocity();
       last_goal_velocity_ = goal_velocity_;
     }
 
-    if (goal_acceleration_!=last_goal_acceleration_ || lost_servo_connection_) {
+    if (goal_acceleration_ != last_goal_acceleration_ || lost_servo_connection_) {
       syncWriteProfileAcceleration();
       last_goal_acceleration_ = goal_acceleration_;
     }
 
-    if (goal_position_!=last_goal_position_ || lost_servo_connection_) {
+    if (goal_position_ != last_goal_position_ || lost_servo_connection_) {
       syncWritePosition();
       last_goal_position_ = goal_position_;
     }
@@ -413,16 +411,16 @@ void ServoBusInterface::switchDynamixelControlMode() {
   bool torque_before_switch = current_torque_;
   writeTorque(false);
   // magic sleep to make sure that dynamixel have internally processed the request
-  nh_->get_clock()->sleep_for(rclcpp::Duration::from_nanoseconds(1e9*0.1));
+  nh_->get_clock()->sleep_for(rclcpp::Duration::from_nanoseconds(1e9 * 0.1));
 
   int32_t value = 3;
-  if (control_mode_==POSITION_CONTROL) {
+  if (control_mode_ == POSITION_CONTROL) {
     value = 3;
-  } else if (control_mode_==VELOCITY_CONTROL) {
+  } else if (control_mode_ == VELOCITY_CONTROL) {
     value = 1;
-  } else if (control_mode_==EFFORT_CONTROL) {
+  } else if (control_mode_ == EFFORT_CONTROL) {
     value = 0;
-  } else if (control_mode_==CURRENT_BASED_POSITION_CONTROL) {
+  } else if (control_mode_ == CURRENT_BASED_POSITION_CONTROL) {
     value = 5;
   } else {
     RCLCPP_WARN(nh_->get_logger(), "control_mode is wrong, will use position control");
@@ -432,14 +430,12 @@ void ServoBusInterface::switchDynamixelControlMode() {
   int32_t *o = &operating_mode[0];
   driver_->syncWrite("Operating_Mode", o);
 
-  nh_->get_clock()->sleep_for(rclcpp::Duration::from_nanoseconds(1e9*0.5));
-  //reenable torque if it was previously enabled
+  nh_->get_clock()->sleep_for(rclcpp::Duration::from_nanoseconds(1e9 * 0.5));
+  // reenable torque if it was previously enabled
   writeTorque(torque_before_switch);
 }
 
-diagnostic_msgs::msg::DiagnosticStatus ServoBusInterface::createServoDiagMsg(int id,
-                                                                             char level,
-                                                                             std::string message,
+diagnostic_msgs::msg::DiagnosticStatus ServoBusInterface::createServoDiagMsg(int id, char level, std::string message,
                                                                              std::map<std::string, std::string> map,
                                                                              std::string name) {
   /**
@@ -453,7 +449,7 @@ diagnostic_msgs::msg::DiagnosticStatus ServoBusInterface::createServoDiagMsg(int
   servo_status.hardware_id = std::to_string(id);
   std::vector<diagnostic_msgs::msg::KeyValue> keyValues = std::vector<diagnostic_msgs::msg::KeyValue>();
   // itarate through map and save it into values
-  for (auto const &ent1: map) {
+  for (auto const &ent1 : map) {
     diagnostic_msgs::msg::KeyValue key_value = diagnostic_msgs::msg::KeyValue();
     key_value.key = ent1.first;
     key_value.value = ent1.second;
@@ -495,7 +491,7 @@ void ServoBusInterface::processVte(bool success) {
       level = diagnostic_msgs::msg::DiagnosticStatus::WARN;
     }
     map.insert(std::make_pair("Error Byte", std::to_string(current_error_[i])));
-    if (current_error_[i]!=0) {
+    if (current_error_[i] != 0) {
       // some error is detected
       level = diagnostic_msgs::msg::DiagnosticStatus::ERROR;
       message = "Error(s): ";
@@ -505,25 +501,24 @@ void ServoBusInterface::processVte(bool success) {
       char encoder_error = 0x8;
       char shock_error = 0x10;
       char overload_error = 0x20;
-      if ((current_error_[i] & voltage_error)!=0) {
+      if ((current_error_[i] & voltage_error) != 0) {
         message = message + "Voltage ";
       }
-      if ((current_error_[i] & overheat_error)!=0) {
+      if ((current_error_[i] & overheat_error) != 0) {
         message = message + "Overheat ";
       }
-      if ((current_error_[i] & encoder_error)!=0) {
+      if ((current_error_[i] & encoder_error) != 0) {
         message = message + "Encoder ";
       }
-      if ((current_error_[i] & shock_error)!=0) {
+      if ((current_error_[i] & shock_error) != 0) {
         message = message + "Shock ";
       }
-      if ((current_error_[i] & overload_error)!=0) {
+      if ((current_error_[i] & overload_error) != 0) {
         message = message + "Overload";
         // turn off torque on all motors
         // todo should also turn off power, but is not possible yet
         goal_torque_ = false;
-        RCLCPP_ERROR(nh_->get_logger(),
-                     "OVERLOAD ERROR!!! OVERLOAD ERROR!!! OVERLOAD ERROR!!! In Motor %s",
+        RCLCPP_ERROR(nh_->get_logger(), "OVERLOAD ERROR!!! OVERLOAD ERROR!!! OVERLOAD ERROR!!! In Motor %s",
                      joint_names_[i].c_str());
         speakError(speak_pub_, "Overload Error!");
       }
@@ -539,7 +534,7 @@ void ServoBusInterface::writeTorque(bool enabled) {
   /**
    * This writes the torque for all servos to the same value
    */
-  //only set values if we're not in torqueless mode
+  // only set values if we're not in torqueless mode
   if (!torqueless_mode_) {
     std::vector<int32_t> torque(joint_names_.size(), enabled);
     int32_t *t = &torque[0];
@@ -554,7 +549,7 @@ void ServoBusInterface::writeTorqueForServos(std::vector<int32_t> torque) {
    * This writes the torque off all servos individually depended on the give vector.
    */
 
-  //only set values if we're not in torqueless mode
+  // only set values if we're not in torqueless mode
   if (!torqueless_mode_) {
     // this actually writes each value in the torque vector
     // but the dynamixel_toolbox requires a reference to the first element
@@ -567,12 +562,11 @@ bool ServoBusInterface::syncReadPositions() {
   /**
    * Reads all position information with a single sync read
    */
-  bool success;
-  success = driver_->syncRead("Present_Position", data_sync_read_positions_);
+  bool success = driver_->syncRead("Present_Position", data_sync_read_positions_);
   if (success) {
     for (int i = 0; i < joint_count_; i++) {
       // TODO test if this is required
-      if (data_sync_read_positions_[i]==0) {
+      if (data_sync_read_positions_[i] == 0) {
         // a value of 0 is often a reading error, therefore we discard it
         // this should not cause issues when a motor is actually close to 0
         // since 1 bit only corresponds to + or - 0.1 deg
@@ -580,7 +574,7 @@ bool ServoBusInterface::syncReadPositions() {
       }
       double current_pos = driver_->convertValue2Radian(joint_ids_[i], data_sync_read_positions_[i]);
       if (current_pos < 3.15 && current_pos > -3.15) {
-        //only write values which are possible
+        // only write values which are possible
         current_position_[i] = current_pos;
       }
     }
@@ -592,8 +586,7 @@ bool ServoBusInterface::syncReadVelocities() {
   /**
    * Reads all velocity information with a single sync read
    */
-  bool success;
-  success = driver_->syncRead("Present_Velocity", data_sync_read_velocities_);
+  bool success = driver_->syncRead("Present_Velocity", data_sync_read_velocities_);
   if (success) {
     for (int i = 0; i < joint_count_; i++) {
       current_velocity_[i] = driver_->convertValue2Velocity(joint_ids_[i], data_sync_read_velocities_[i]);
@@ -606,8 +599,7 @@ bool ServoBusInterface::syncReadEfforts() {
   /**
    * Reads all effort information with a single sync read
    */
-  bool success;
-  success = driver_->syncRead("Present_Current", data_sync_read_efforts_);
+  bool success = driver_->syncRead("Present_Current", data_sync_read_efforts_);
   if (success) {
     for (int i = 0; i < joint_count_; i++) {
       current_effort_[i] = driver_->convertValue2Torque(joint_ids_[i], data_sync_read_efforts_[i]);
@@ -620,14 +612,13 @@ bool ServoBusInterface::syncReadPWMs() {
   /**
    * Reads all PWM information with a single sync read
    */
-  bool success;
-  success = driver_->syncRead("Present_PWM", data_sync_read_pwms_);
+  bool success = driver_->syncRead("Present_PWM", data_sync_read_pwms_);
   if (success) {
     for (int i = 0; i < joint_count_; i++) {
       // the data is in int16
       // 100% is a value of 885
       // convert to range -1 to 1
-      current_pwm_[i] = ((int16_t) data_sync_read_pwms_[i])/885.0;
+      current_pwm_[i] = ((int16_t)data_sync_read_pwms_[i]) / 885.0;
     }
   }
   return success;
@@ -637,8 +628,7 @@ bool ServoBusInterface::syncReadError() {
   /**
    * Reads all error bytes with a single sync read
    */
-  bool success;
-  success = driver_->syncRead("Hardware_Error_Status", data_sync_read_error_);
+  bool success = driver_->syncRead("Hardware_Error_Status", data_sync_read_error_);
   if (success) {
     for (int i = 0; i < joint_count_; i++) {
       current_error_[i] = data_sync_read_error_[i];
@@ -653,13 +643,11 @@ bool ServoBusInterface::syncReadVoltageAndTemp() {
    */
   std::vector<uint8_t> data;
   if (driver_->syncReadMultipleRegisters(144, 3, &data)) {
-    uint16_t volt;
-    uint8_t temp;
     for (int i = 0; i < joint_count_; i++) {
-      volt = dxlMakeword(data[i*3], data[i*3 + 1]);
-      temp = data[i*3 + 2];
+      uint16_t volt = dxlMakeword(data[i * 3], data[i * 3 + 1]);
+      uint8_t temp = data[i * 3 + 2];
       // convert value to voltage
-      current_input_voltage_[i] = volt*0.1;
+      current_input_voltage_[i] = volt * 0.1;
       // is already in °C
       current_temperature_[i] = temp;
     }
@@ -674,20 +662,17 @@ bool ServoBusInterface::syncReadAll() {
    * Reads all positions, velocities and efforts with a single sync read
    */
   if (driver_->syncReadMultipleRegisters(126, 10, &sync_read_all_data_)) {
-    int16_t eff;
-    uint32_t vel;
-    uint32_t pos;
     for (int i = 0; i < joint_count_; i++) {
-      eff = dxlMakeword(sync_read_all_data_[i*10], sync_read_all_data_[i*10 + 1]);
-      vel = dxlMakedword(dxlMakeword(sync_read_all_data_[i*10 + 2], sync_read_all_data_[i*10 + 3]),
-                         dxlMakeword(sync_read_all_data_[i*10 + 4], sync_read_all_data_[i*10 + 5]));
-      pos = dxlMakedword(dxlMakeword(sync_read_all_data_[i*10 + 6], sync_read_all_data_[i*10 + 7]),
-                         dxlMakeword(sync_read_all_data_[i*10 + 8], sync_read_all_data_[i*10 + 9]));
+      int16_t eff = dxlMakeword(sync_read_all_data_[i * 10], sync_read_all_data_[i * 10 + 1]);
+      uint32_t vel = dxlMakedword(dxlMakeword(sync_read_all_data_[i * 10 + 2], sync_read_all_data_[i * 10 + 3]),
+                                  dxlMakeword(sync_read_all_data_[i * 10 + 4], sync_read_all_data_[i * 10 + 5]));
+      uint32_t pos = dxlMakedword(dxlMakeword(sync_read_all_data_[i * 10 + 6], sync_read_all_data_[i * 10 + 7]),
+                                  dxlMakeword(sync_read_all_data_[i * 10 + 8], sync_read_all_data_[i * 10 + 9]));
       current_effort_[i] = driver_->convertValue2Torque(joint_ids_[i], eff);
       current_velocity_[i] = driver_->convertValue2Velocity(joint_ids_[i], vel);
       double current_pos = driver_->convertValue2Radian(joint_ids_[i], pos);
       if (current_pos < 3.15 && current_pos > -3.15) {
-        //only write values which are possible
+        // only write values which are possible
         current_position_[i] = current_pos;
       }
     }
@@ -701,9 +686,8 @@ void ServoBusInterface::syncWritePosition() {
   /**
    * Writes all goal positions with a single sync write
    */
-  float radian;
   for (size_t num = 0; num < joint_names_.size(); num++) {
-    radian = goal_position_[num] - joint_mounting_offsets_[num] - joint_offsets_[num];
+    float radian = goal_position_[num] - joint_mounting_offsets_[num] - joint_offsets_[num];
     sync_write_goal_position_[num] = driver_->convertRadian2Value(joint_ids_[num], radian);
   }
   driver_->syncWrite("Goal_Position", sync_write_goal_position_);
@@ -745,9 +729,9 @@ void ServoBusInterface::syncWriteProfileAcceleration() {
       // we want to set to maximum, which is 0
       sync_write_profile_acceleration_[num] = 0;
     } else {
-      //572.9577952 for change of units, 214.577 rev/min^2 per LSB
+      // 572.9577952 for change of units, 214.577 rev/min^2 per LSB
       sync_write_profile_acceleration_[num] =
-          std::max(static_cast<int>(goal_acceleration_[num]*572.9577952/214.577), 1);
+          std::max(static_cast<int>(goal_acceleration_[num] * 572.9577952 / 214.577), 1);
     }
   }
   driver_->syncWrite("Profile_Acceleration", sync_write_profile_acceleration_);
@@ -760,9 +744,9 @@ void ServoBusInterface::syncWriteCurrent() {
   for (size_t num = 0; num < joint_names_.size(); num++) {
     if (goal_effort_[num] < 0) {
       // we want to set to maximum, which is different for MX-64 and MX-106
-      if (driver_->getModelNum(joint_ids_[num])==311) {
+      if (driver_->getModelNum(joint_ids_[num]) == 311) {
         sync_write_goal_current_[num] = 1941;
-      } else if (driver_->getModelNum(joint_ids_[num])==321) {
+      } else if (driver_->getModelNum(joint_ids_[num]) == 321) {
         sync_write_goal_current_[num] = 2047;
       } else {
         RCLCPP_WARN(nh_->get_logger(), "Maximal current for this dynamixel model is not defined");
@@ -780,10 +764,10 @@ void ServoBusInterface::syncWritePWM() {
       // we want to set to maximum
       sync_write_goal_pwm_[num] = 855;
     } else {
-      sync_write_goal_pwm_[num] = goal_effort_[num]/100.0*855.0;
+      sync_write_goal_pwm_[num] = goal_effort_[num] / 100.0 * 855.0;
     }
   }
   driver_->syncWrite("Goal_PWM", sync_write_goal_pwm_);
 }
 
-}
+}  // namespace bitbots_ros_control
diff --git a/bitbots_ros_control/src/utils.cpp b/bitbots_ros_control/src/utils.cpp
index d5f818b5..f67e750e 100644
--- a/bitbots_ros_control/src/utils.cpp
+++ b/bitbots_ros_control/src/utils.cpp
@@ -1,8 +1,8 @@
-#include <bitbots_ros_control/utils.h>
+#include <bitbots_ros_control/utils.hpp>
 
 namespace bitbots_ros_control {
 
-bool stringToControlMode(rclcpp::Node::SharedPtr nh, std::string _control_modestr, ControlMode &control_mode) {
+bool stringToControlMode(rclcpp::Node::SharedPtr nh, const std::string& _control_modestr, ControlMode& control_mode) {
   /**
    * Helper method to parse strings to corresponding control modes
    */
@@ -24,25 +24,24 @@ bool stringToControlMode(rclcpp::Node::SharedPtr nh, std::string _control_modest
   }
 }
 
-void speakError(const rclcpp::Publisher<humanoid_league_msgs::msg::Audio>::SharedPtr speak_pub, std::string text) {
+void speakError(const rclcpp::Publisher<humanoid_league_msgs::msg::Audio>::SharedPtr speak_pub,
+                const std::string& text) {
   /**
-    *  Helper method to send a message for text-to-speech output
-    */
+   *  Helper method to send a message for text-to-speech output
+   */
   humanoid_league_msgs::msg::Audio msg = humanoid_league_msgs::msg::Audio();
   msg.text = text;
   msg.priority = 100;
   speak_pub->publish(msg);
 }
 
-uint16_t dxlMakeword(uint64_t a, uint64_t b) {
-  return uint16_t(uint8_t(a & 0xff) | uint16_t(uint8_t(b & 0xff)) << 8);
-}
+uint16_t dxlMakeword(uint64_t a, uint64_t b) { return uint16_t(uint8_t(a & 0xff) | uint16_t(uint8_t(b & 0xff)) << 8); }
 
 uint32_t dxlMakedword(uint64_t a, uint64_t b) {
   return uint32_t(uint16_t(a & 0xffff) | uint32_t(uint16_t(b & 0xffff) << 16));
 }
 
-float dxlMakeFloat(uint8_t *data) {
+float dxlMakeFloat(uint8_t* data) {
   float f;
   uint32_t b = dxlMakedword(dxlMakeword(data[0], data[1]), dxlMakeword(data[2], data[3]));
   memcpy(&f, &b, sizeof(f));
@@ -51,22 +50,32 @@ float dxlMakeFloat(uint8_t *data) {
 
 std::string gyroRangeToString(uint8_t range) {
   switch (range) {
-    case 0:return "250 deg/s";
-    case 1:return "500 deg/s";
-    case 2:return "1000 deg/s";
-    case 3:return "2000 deg/s";
-    default:return "invalid range, defaulting to 2000 deg/s";
+    case 0:
+      return "250 deg/s";
+    case 1:
+      return "500 deg/s";
+    case 2:
+      return "1000 deg/s";
+    case 3:
+      return "2000 deg/s";
+    default:
+      return "invalid range, defaulting to 2000 deg/s";
   }
 }
 
 std::string accelRangeToString(uint8_t range) {
   switch (range) {
-    case 0:return "2G";
-    case 1:return "4G";
-    case 2:return "8G";
-    case 3:return "16G";
-    default:return "invalid range, defaulting to 16G";
+    case 0:
+      return "2G";
+    case 1:
+      return "4G";
+    case 2:
+      return "8G";
+    case 3:
+      return "16G";
+    default:
+      return "invalid range, defaulting to 16G";
   }
 }
 
-}
\ No newline at end of file
+}  // namespace bitbots_ros_control
\ No newline at end of file
diff --git a/bitbots_ros_control/src/wolfgang_hardware_interface.cpp b/bitbots_ros_control/src/wolfgang_hardware_interface.cpp
index c4d66190..f7556507 100644
--- a/bitbots_ros_control/src/wolfgang_hardware_interface.cpp
+++ b/bitbots_ros_control/src/wolfgang_hardware_interface.cpp
@@ -1,4 +1,4 @@
-#include <bitbots_ros_control/wolfgang_hardware_interface.h>
+#include <bitbots_ros_control/wolfgang_hardware_interface.hpp>
 
 namespace bitbots_ros_control {
 
@@ -31,9 +31,9 @@ WolfgangHardwareInterface::WolfgangHardwareInterface(rclcpp::Node::SharedPtr nh)
 
   // Convert dxls to native type: a vector of tuples with name and id for sorting purposes
   std::vector<std::pair<std::string, int>> dxl_devices;
-  for (const std::string &parameter_name: device_name_list.names) {
+  for (const std::string &parameter_name : device_name_list.names) {
     // we get directly the parameters and not the groups. use id parameter to identify them
-    if (parameter_name.find(".id")!=std::string::npos) {
+    if (parameter_name.find(".id") != std::string::npos) {
       int id = nh->get_parameter(parameter_name).as_int();
       // remove "device_info." and ".id"
       std::string device_name = parameter_name.substr(12, parameter_name.size() - 3 - 12);
@@ -41,9 +41,11 @@ WolfgangHardwareInterface::WolfgangHardwareInterface(rclcpp::Node::SharedPtr nh)
     }
   }
 
-  // sort the devices by id. This way the devices will always be read and written in ID order later, making debug easier.
-  std::sort(dxl_devices.begin(), dxl_devices.end(),
-            [](std::pair<std::string, int> &a, std::pair<std::string, int> &b) { return a.second < b.second; });
+  // sort the devices by id. This way the devices will always be read and written in ID order later, making debug
+  // easier.
+  std::sort(
+      dxl_devices.begin(), dxl_devices.end(),
+      [](const std::pair<std::string, int> &a, const std::pair<std::string, int> &b) { return a.second < b.second; });
 
   // create overall servo interface since we need a single interface for the controllers
   servo_interface_ = DynamixelServoHardwareInterface(nh);
@@ -59,13 +61,13 @@ WolfgangHardwareInterface::WolfgangHardwareInterface(rclcpp::Node::SharedPtr nh)
 }
 
 bool WolfgangHardwareInterface::create_interfaces(std::vector<std::pair<std::string, int>> dxl_devices) {
-  interfaces_ = std::vector<std::vector<bitbots_ros_control::HardwareInterface * >>();
+  interfaces_ = std::vector<std::vector<bitbots_ros_control::HardwareInterface *>>();
   // init bus drivers
   std::vector<std::string> pinged;
   rcl_interfaces::msg::ListParametersResult port_list = nh_->list_parameters({"port_info"}, 3);
-  for (const std::string &parameter_name: port_list.names) {
+  for (const std::string &parameter_name : port_list.names) {
     // we get directly the parameters and not the groups. use id parameter to identify them
-    if (parameter_name.find(".device_file")!=std::string::npos) {
+    if (parameter_name.find(".device_file") != std::string::npos) {
       std::string port_name = parameter_name.substr(10, parameter_name.size() - 11 - 11);
       // read bus driver specifications from config
       std::string device_file = nh_->get_parameter("port_info." + port_name + ".device_file").as_string();
@@ -85,11 +87,11 @@ bool WolfgangHardwareInterface::create_interfaces(std::vector<std::pair<std::str
       std::vector<bitbots_ros_control::HardwareInterface *> interfaces_on_port;
       // iterate over all devices and ping them to see what is connected to this bus
       std::vector<std::tuple<int, std::string, float, float, std::string>> servos_on_port;
-      for (std::pair<std::string, int> &device: dxl_devices) {
-        //RCLCPP_INFO_STREAM(nh_->get_logger(), device.first);
+      for (std::pair<std::string, int> &device : dxl_devices) {
+        // RCLCPP_INFO_STREAM(nh_->get_logger(), device.first);
         std::string name = device.first;
         int id = device.second;
-        if (std::find(pinged.begin(), pinged.end(), device.first)!=pinged.end()) {
+        if (std::find(pinged.begin(), pinged.end(), device.first) != pinged.end()) {
           // we already found this and don't have to search again
         } else {
           int model_number_specified;
@@ -101,20 +103,20 @@ bool WolfgangHardwareInterface::create_interfaces(std::vector<std::pair<std::str
           uint16_t *model_number_returned_16 = new uint16_t;
           if (driver->ping(uint8_t(id), model_number_returned_16)) {
             // check if the specified model number matches the actual model number of the device
-            if (model_number_specified_16!=*model_number_returned_16) {
+            if (model_number_specified_16 != *model_number_returned_16) {
               RCLCPP_WARN(nh_->get_logger(), "Model number of id %d does not match", id);
             }
             // ping was successful, add device correspondingly
             // only add them if the mode is set correspondingly
             // TODO maybe move more of the parameter stuff in the init of the modules instead of doing everything here
-            if (model_number_specified==0xABBA && interface_type=="CORE") {
+            if (model_number_specified == 0xABBA && interface_type == "CORE") {
               // CORE
               int read_rate;
               nh_->get_parameter("device_info." + name + ".read_rate", read_rate);
               driver->setTools(model_number_specified_16, id);
               core_interface_ = new CoreHardwareInterface(nh_, driver, id, read_rate);
               // turn on power, just to be sure
-              core_interface_->write(nh_->get_clock()->now(), rclcpp::Duration::from_nanoseconds(1e9*0));
+              core_interface_->write(nh_->get_clock()->now(), rclcpp::Duration::from_nanoseconds(1e9 * 0));
               interfaces_on_port.push_back(core_interface_);
               core_present_ = true;
             } else if (model_number_specified == 0 && !only_imu_) {  // model number is currently 0 on foot sensors
@@ -126,7 +128,7 @@ bool WolfgangHardwareInterface::create_interfaces(std::vector<std::pair<std::str
               BitFootHardwareInterface *interface = new BitFootHardwareInterface(nh_, driver, id, topic, name);
               interfaces_on_port.push_back(interface);
             } else if (model_number_specified == 0xBAFF && interface_type == "IMU" && !only_pressure_) {
-              //IMU
+              // IMU
               std::string topic;
               if (!nh_->get_parameter("device_info." + name + ".topic", topic)) {
                 RCLCPP_WARN(nh_->get_logger(), "IMU topic not specified");
@@ -191,9 +193,9 @@ bool WolfgangHardwareInterface::create_interfaces(std::vector<std::pair<std::str
     }
   }
 
-  if (pinged.size()!=dxl_devices.size()) {
+  if (pinged.size() != dxl_devices.size()) {
     // when we only have 1 or two devices it's only the core
-    if (pinged.empty() || pinged.size()==1 || pinged.size()==2) {
+    if (pinged.empty() || pinged.size() == 1 || pinged.size() == 2) {
       RCLCPP_ERROR_THROTTLE(nh_->get_logger(), *nh_->get_clock(), 5000, "Could not start ros control. Power is off!");
       speakError(speak_pub_, "Could not start ross control. Power is off!");
     } else {
@@ -203,8 +205,8 @@ bool WolfgangHardwareInterface::create_interfaces(std::vector<std::pair<std::str
         RCLCPP_ERROR(nh_->get_logger(), "Could not ping all devices!");
         speakError(speak_pub_, "error starting ross control");
         // check which devices were not pinged successful
-        for (std::pair<std::string, int> &device: dxl_devices) {
-          if (std::find(pinged.begin(), pinged.end(), device.first)!=pinged.end()) {
+        for (std::pair<std::string, int> &device : dxl_devices) {
+          if (std::find(pinged.begin(), pinged.end(), device.first) != pinged.end()) {
           } else {
             RCLCPP_ERROR(nh_->get_logger(), "%s with id %d missing", device.first.c_str(), device.second);
           }
@@ -218,11 +220,9 @@ bool WolfgangHardwareInterface::create_interfaces(std::vector<std::pair<std::str
   }
 }
 
-void threaded_init(std::vector<HardwareInterface *> &port_interfaces, rclcpp::Node::SharedPtr &nh, int &success) {
-  success = true;
-  for (HardwareInterface *interface: port_interfaces) {
-    success &= interface->init();
-  }
+void threaded_init(const std::vector<HardwareInterface *> &port_interfaces, rclcpp::Node::SharedPtr &nh, int &success) {
+  success = std::all_of(port_interfaces.begin(), port_interfaces.end(),
+                        [](HardwareInterface *interface) -> bool { return interface->init(); });
 }
 
 bool WolfgangHardwareInterface::init() {
@@ -230,7 +230,7 @@ bool WolfgangHardwareInterface::init() {
   std::vector<std::thread> threads;
   std::vector<int *> successes;
   int i = 0;
-  for (std::vector<HardwareInterface *> &port_interfaces: interfaces_) {
+  for (std::vector<HardwareInterface *> &port_interfaces : interfaces_) {
     // iterate through all interfaces on this port
     // we use an int instead of bool, since std::ref can't handle bool
     int suc = 0;
@@ -239,54 +239,50 @@ bool WolfgangHardwareInterface::init() {
     i++;
   }
   // wait for all inits to finish
-  for (std::thread &thread: threads) {
+  for (std::thread &thread : threads) {
     thread.join();
   }
   // see if all inits were successful
-  bool success = true;
-  for (bool s: successes) {
-    success &= s;
-  }
+  bool success = std::all_of(successes.begin(), successes.end(), [](int *s) { return *s; });
   // init servo interface last after all servo busses are there
   success &= servo_interface_.init();
   return success;
 }
 
-void threaded_read(std::vector<HardwareInterface *> &port_interfaces,
-                   const rclcpp::Time &t,
+void threaded_read(const std::vector<HardwareInterface *> &port_interfaces, const rclcpp::Time &t,
                    const rclcpp::Duration &dt) {
-  for (HardwareInterface *interface: port_interfaces) {
+  for (HardwareInterface *interface : port_interfaces) {
     interface->read(t, dt);
   }
 }
 
 void WolfgangHardwareInterface::read(const rclcpp::Time &t, const rclcpp::Duration &dt) {
   // give feedback to power changes
-  if (core_present_){
-    if(current_power_status_ && !last_power_status_){
+  if (core_present_) {
+    if (current_power_status_ && !last_power_status_) {
       speakError(speak_pub_, "Power switched on!");
-    }else if(!current_power_status_ && last_power_status_){
+    } else if (!current_power_status_ && last_power_status_) {
       speakError(speak_pub_, "Power switched off!");
     }
   }
-  if (!core_present_ || current_power_status_){
+  if (!core_present_ || current_power_status_) {
     // only read all hardware if power is on
     std::vector<std::thread> threads;
     // start all reads
-    for (std::vector<HardwareInterface *> &port_interfaces: interfaces_) {
+    for (std::vector<HardwareInterface *> &port_interfaces : interfaces_) {
       threads.push_back(std::thread(threaded_read, std::ref(port_interfaces), std::ref(t), std::ref(dt)));
     }
     // wait for all reads to finish
-    for (std::thread &thread: threads) {
+    for (std::thread &thread : threads) {
       thread.join();
     }
     // aggregate all servo values for controller
     servo_interface_.read(t, dt);
-    if (core_present_){
+    if (core_present_) {
       last_power_status_ = current_power_status_;
       current_power_status_ = core_interface_->get_power_status();
     }
-  }else{
+  } else {
     // read core to see if power is back on
     core_interface_->read(t, dt);
     last_power_status_ = current_power_status_;
@@ -295,10 +291,9 @@ void WolfgangHardwareInterface::read(const rclcpp::Time &t, const rclcpp::Durati
   }
 }
 
-void threaded_write(std::vector<HardwareInterface *> &port_interfaces,
-                    const rclcpp::Time &t,
+void threaded_write(const std::vector<HardwareInterface *> &port_interfaces, const rclcpp::Time &t,
                     const rclcpp::Duration &dt) {
-  for (HardwareInterface *interface: port_interfaces) {
+  for (HardwareInterface *interface : port_interfaces) {
     interface->write(t, dt);
   }
 }
@@ -308,7 +303,7 @@ void WolfgangHardwareInterface::write(const rclcpp::Time &t, const rclcpp::Durat
       nh_->get_parameter("servos.set_ROM_RAM").as_bool()) {
     // when we can read the power and see that it was just switched on, we write the ROM RAM again
     servo_interface_.writeROMRAM(false);
-  } else if(core_present_ && !current_power_status_){
+  } else if (core_present_ && !current_power_status_) {
     // if power is off only write CORE
     core_interface_->write(t, dt);
   } else {
@@ -316,14 +311,14 @@ void WolfgangHardwareInterface::write(const rclcpp::Time &t, const rclcpp::Durat
     servo_interface_.write(t, dt);
     std::vector<std::thread> threads;
     // start all writes
-    for (std::vector<HardwareInterface *> &port_interfaces: interfaces_) {
+    for (std::vector<HardwareInterface *> &port_interfaces : interfaces_) {
       threads.push_back(std::thread(threaded_write, std::ref(port_interfaces), std::ref(t), std::ref(dt)));
     }
 
     // wait for all writes to finish
-    for (std::thread &thread: threads) {
+    for (std::thread &thread : threads) {
       thread.join();
     }
   }
 }
-}
+}  // namespace bitbots_ros_control