fzi-forschungszentrum-informatik · stefanscherzinger · Jan 9, 2024 · Jan 9, 2024 · Jan 9, 2024 · Jan 9, 2024
diff --git a/.clang-format b/.clang-format
@@ -0,0 +1,20 @@
+---
+Language: Cpp
+BasedOnStyle: Google
+
+AccessModifierOffset: -2
+AlignAfterOpenBracket: Align
+BraceWrapping:
+  AfterClass: true
+  AfterFunction: true
+  AfterNamespace: true
+  AfterStruct: true
+  AfterEnum: true
+BreakBeforeBraces: Allman
+ColumnLimit: 100
+ConstructorInitializerIndentWidth: 0
+ContinuationIndentWidth: 2
+DerivePointerAlignment: false
+PointerAlignment: Middle
+ReflowComments: false
+...
diff --git a/.clang-tidy b/.clang-tidy
@@ -0,0 +1,10 @@
+Checks: '-*,readability-identifier-naming'
+CheckOptions:
+  - { key: readability-identifier-naming.NamespaceCase,        value: lower_case }
+  - { key: readability-identifier-naming.ClassCase,            value: CamelCase  }
+  - { key: readability-identifier-naming.PrivateMemberPrefix,  value: m_         }
+  - { key: readability-identifier-naming.StructCase,           value: CamelCase  }
+  - { key: readability-identifier-naming.FunctionCase,         value: camelBack }
+  - { key: readability-identifier-naming.VariableCase,         value: lower_case }
+  - { key: readability-identifier-naming.GlobalVariablePrefix, value: g_ }
+  - { key: readability-identifier-naming.GlobalConstantCase,   value: UPPER_CASE }
diff --git a/.flake8 b/.flake8
@@ -0,0 +1,4 @@
+
+[flake8]
+# Use black's line length (default 88) instead of the flake8 default of 79:
+max-line-length = 88
diff --git a/.github/workflows/ci_format.yml b/.github/workflows/ci_format.yml
@@ -0,0 +1,18 @@
+# Pre-commit as a Github action.
+# See: https://github.com/pre-commit/action
+# This will run all hooks from the .pre-commit-config.yaml against all files.
+
+name: Format
+
+on:
+  pull_request:
+  push:
+    branches: [ros2]
+
+jobs:
+  pre-commit:
+    runs-on: ubuntu-22.04
+    steps:
+    - uses: actions/checkout@v3
+    - uses: actions/setup-python@v3
+    - uses: pre-commit/[email protected]
diff --git a/.pre-commit-config.yaml b/.pre-commit-config.yaml
@@ -0,0 +1,40 @@
+repos:
+  # Standard hooks
+  - repo: https://github.com/pre-commit/pre-commit-hooks
+    rev: v3.4.0
+    hooks:
+      - id: check-added-large-files
+      - id: check-ast
+      - id: check-case-conflict
+      - id: check-docstring-first
+      - id: check-merge-conflict
+      - id: check-symlinks
+      - id: check-xml
+      - id: check-yaml
+      - id: debug-statements
+      - id: end-of-file-fixer
+      - id: mixed-line-ending
+      - id: trailing-whitespace
+      - id: check-byte-order-marker  # Forbid UTF-8 byte-order markers
+
+  # Python
+  - repo: https://github.com/pycqa/flake8
+    rev: 6.0.0
+    hooks:
+    -   id: flake8
+
+  - repo: https://github.com/psf/black.git
+    rev: 23.7.0
+    hooks:
+    -   id: black
+
+  # C++
+  - repo: local
+    hooks:
+      - id: clang-format
+        name: clang-format
+        description: Format files with ClangFormat.
+        entry: clang-format
+        language: system
+        files: \.(c|cc|cxx|cpp|frag|glsl|h|hpp|hxx|ih|ispc|ipp|java|js|m|proto|vert)$
+        args: ['-fallback-style=none', '-i']
diff --git a/README.md b/README.md
@@ -29,7 +29,7 @@ slides](https://roscon.ros.org/2019/talks/roscon2019_cartesiancontrollers.pdf)
 to get an overview.
 
 ## Why this package?
-Users may refer to `MoveIt` for end-effector motion planning, but 
+Users may refer to `MoveIt` for end-effector motion planning, but
 integrating a full planning stack is often unnecessary for simple applications.
 Additionally, there are a lot of use cases where direct control in task space is mandatory:
 dynamic following of target poses, such as **visual servoing**, **teleoperation**, **Cartesian teaching,** or
@@ -88,4 +88,3 @@ If you are interested in more details, have a look at
 - *Virtual Forward Dynamics Models for Cartesian Robot Control* ([Paper](https://arxiv.org/pdf/2009.11888.pdf))
 - *Contact Skill Imitation Learning for Robot-Independent Assembly Programming* ([Paper](https://arxiv.org/pdf/1908.06272.pdf))
 - *Human-Inspired Compliant Controllers for Robotic Assembly* ([PhD Thesis](https://publikationen.bibliothek.kit.edu/1000139834), especially Chapter 4)
-
diff --git a/...ance_controller/include/cartesian_compliance_controller/cartesian_compliance_controller.h b/...ance_controller/include/cartesian_compliance_controller/cartesian_compliance_controller.h
@@ -44,11 +44,11 @@
 #include <cartesian_controller_base/cartesian_controller_base.h>
 #include <cartesian_force_controller/cartesian_force_controller.h>
 #include <cartesian_motion_controller/cartesian_motion_controller.h>
+
 #include <controller_interface/controller_interface.hpp>
 
 namespace cartesian_compliance_controller
 {
-
 /**
  * @brief A ROS2-control controller for Cartesian compliance control
  *
@@ -69,51 +69,52 @@ namespace cartesian_compliance_controller
  * where the additional forces are applied.
  *
  */
-class CartesianComplianceController
-: public cartesian_motion_controller::CartesianMotionController
-, public cartesian_force_controller::CartesianForceController
+class CartesianComplianceController : public cartesian_motion_controller::CartesianMotionController,
+                                      public cartesian_force_controller::CartesianForceController
 {
-  public:
-    CartesianComplianceController();
+public:
+  CartesianComplianceController();
 
-#if defined CARTESIAN_CONTROLLERS_GALACTIC || defined CARTESIAN_CONTROLLERS_HUMBLE || defined CARTESIAN_CONTROLLERS_IRON
-    virtual LifecycleNodeInterface::CallbackReturn on_init() override;
+#if defined CARTESIAN_CONTROLLERS_GALACTIC || defined CARTESIAN_CONTROLLERS_HUMBLE || \
+  defined CARTESIAN_CONTROLLERS_IRON
+  virtual LifecycleNodeInterface::CallbackReturn on_init() override;
 #elif defined CARTESIAN_CONTROLLERS_FOXY
-    virtual controller_interface::return_type init(const std::string & controller_name) override;
+  virtual controller_interface::return_type init(const std::string & controller_name) override;
 #endif
 
-    rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn on_configure(
-        const rclcpp_lifecycle::State & previous_state) override;
+  rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn on_configure(
+    const rclcpp_lifecycle::State & previous_state) override;
 
-    rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn on_activate(
-        const rclcpp_lifecycle::State & previous_state) override;
+  rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn on_activate(
+    const rclcpp_lifecycle::State & previous_state) override;
 
-    rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn on_deactivate(
-        const rclcpp_lifecycle::State & previous_state) override;
+  rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn on_deactivate(
+    const rclcpp_lifecycle::State & previous_state) override;
 
-#if defined CARTESIAN_CONTROLLERS_GALACTIC || defined CARTESIAN_CONTROLLERS_HUMBLE || defined CARTESIAN_CONTROLLERS_IRON
-    controller_interface::return_type update(const rclcpp::Time & time, const rclcpp::Duration & period) override;
+#if defined CARTESIAN_CONTROLLERS_GALACTIC || defined CARTESIAN_CONTROLLERS_HUMBLE || \
+  defined CARTESIAN_CONTROLLERS_IRON
+  controller_interface::return_type update(const rclcpp::Time & time,
+                                           const rclcpp::Duration & period) override;
 #elif defined CARTESIAN_CONTROLLERS_FOXY
-    controller_interface::return_type update() override;
+  controller_interface::return_type update() override;
 #endif
 
-    using Base = cartesian_controller_base::CartesianControllerBase;
-    using MotionBase = cartesian_motion_controller::CartesianMotionController;
-    using ForceBase = cartesian_force_controller::CartesianForceController;
+  using Base = cartesian_controller_base::CartesianControllerBase;
+  using MotionBase = cartesian_motion_controller::CartesianMotionController;
+  using ForceBase = cartesian_force_controller::CartesianForceController;
 
-  private:
-    /**
+private:
+  /**
      * @brief Compute the net force of target wrench and stiffness-related pose offset
      *
      * @return The remaining error wrench, given in robot base frame
      */
-    ctrl::Vector6D        computeComplianceError();
-
-    ctrl::Matrix6D        m_stiffness;
-    std::string           m_compliance_ref_link;
+  ctrl::Vector6D computeComplianceError();
 
+  ctrl::Matrix6D m_stiffness;
+  std::string m_compliance_ref_link;
 };
 
-}
+}  // namespace cartesian_compliance_controller
 
 #endif
diff --git a/cartesian_compliance_controller/package.xml b/cartesian_compliance_controller/package.xml
@@ -6,8 +6,8 @@
   <description>Control your robot through Cartesian target poses and target wrenches</description>
   <maintainer email="[email protected]">scherzin</maintainer>
   <license>BSD-3-Clause</license>
-  <url type="repository">https://github.com/fzi-forschungszentrum-informatik/cartesian_controllers</url> 
-  <author email="[email protected]">Stefan Scherzinger</author> 
+  <url type="repository">https://github.com/fzi-forschungszentrum-informatik/cartesian_controllers</url>
+  <author email="[email protected]">Stefan Scherzinger</author>
 
   <buildtool_depend>ament_cmake</buildtool_depend>
 

diff --git a/cartesian_compliance_controller/src/cartesian_compliance_controller.cpp b/cartesian_compliance_controller/src/cartesian_compliance_controller.cpp
@@ -37,13 +37,13 @@
  */
 //-----------------------------------------------------------------------------
 
+#include <cartesian_compliance_controller/cartesian_compliance_controller.h>
+
 #include "cartesian_controller_base/Utility.h"
 #include "controller_interface/controller_interface.hpp"
-#include <cartesian_compliance_controller/cartesian_compliance_controller.h>
 
 namespace cartesian_compliance_controller
 {
-
 CartesianComplianceController::CartesianComplianceController()
 // Base constructor won't be called in diamond inheritance, so call that
 // explicitly
@@ -53,8 +53,10 @@ CartesianComplianceController::CartesianComplianceController()
 {
 }
 
-#if defined CARTESIAN_CONTROLLERS_GALACTIC || defined CARTESIAN_CONTROLLERS_HUMBLE || defined CARTESIAN_CONTROLLERS_IRON
-rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn CartesianComplianceController::on_init()
+#if defined CARTESIAN_CONTROLLERS_GALACTIC || defined CARTESIAN_CONTROLLERS_HUMBLE || \
+  defined CARTESIAN_CONTROLLERS_IRON
+rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn
+CartesianComplianceController::on_init()
 {
   using TYPE = rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn;
   if (MotionBase::on_init() != TYPE::SUCCESS || ForceBase::on_init() != TYPE::SUCCESS)
@@ -76,7 +78,8 @@ rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn Cartes
   return TYPE::SUCCESS;
 }
 #elif defined CARTESIAN_CONTROLLERS_FOXY
-controller_interface::return_type CartesianComplianceController::init(const std::string & controller_name)
+controller_interface::return_type CartesianComplianceController::init(
+  const std::string & controller_name)
 {
   using TYPE = controller_interface::return_type;
   if (MotionBase::init(controller_name) != TYPE::OK || ForceBase::init(controller_name) != TYPE::OK)
@@ -90,23 +93,24 @@ controller_interface::return_type CartesianComplianceController::init(const std:
 }
 #endif
 
-rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn CartesianComplianceController::on_configure(
-    const rclcpp_lifecycle::State & previous_state)
+rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn
+CartesianComplianceController::on_configure(const rclcpp_lifecycle::State & previous_state)
 {
   using TYPE = rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn;
-  if (MotionBase::on_configure(previous_state) != TYPE::SUCCESS || ForceBase::on_configure(previous_state) != TYPE::SUCCESS)
+  if (MotionBase::on_configure(previous_state) != TYPE::SUCCESS ||
+      ForceBase::on_configure(previous_state) != TYPE::SUCCESS)
   {
     return TYPE::ERROR;
   }
 
   // Make sure compliance link is part of the robot chain
   m_compliance_ref_link = get_node()->get_parameter("compliance_ref_link").as_string();
-  if(!Base::robotChainContains(m_compliance_ref_link))
+  if (!Base::robotChainContains(m_compliance_ref_link))
   {
-    RCLCPP_ERROR_STREAM(get_node()->get_logger(),
-                        m_compliance_ref_link << " is not part of the kinematic chain from "
-                                              << Base::m_robot_base_link << " to "
-                                              << Base::m_end_effector_link);
+    RCLCPP_ERROR_STREAM(get_node()->get_logger(), m_compliance_ref_link
+                                                    << " is not part of the kinematic chain from "
+                                                    << Base::m_robot_base_link << " to "
+                                                    << Base::m_end_effector_link);
     return TYPE::ERROR;
   }
 
@@ -116,33 +120,36 @@ rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn Cartes
   return TYPE::SUCCESS;
 }
 
-rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn CartesianComplianceController::on_activate(
-    const rclcpp_lifecycle::State & previous_state)
+rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn
+CartesianComplianceController::on_activate(const rclcpp_lifecycle::State & previous_state)
 {
   // Base::on_activation(..) will get called twice,
   // but that's fine.
   using TYPE = rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn;
-  if (MotionBase::on_activate(previous_state) != TYPE::SUCCESS || ForceBase::on_activate(previous_state) != TYPE::SUCCESS)
+  if (MotionBase::on_activate(previous_state) != TYPE::SUCCESS ||
+      ForceBase::on_activate(previous_state) != TYPE::SUCCESS)
   {
     return TYPE::ERROR;
   }
   return TYPE::SUCCESS;
 }
 
-rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn CartesianComplianceController::on_deactivate(
-    const rclcpp_lifecycle::State & previous_state)
+rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn
+CartesianComplianceController::on_deactivate(const rclcpp_lifecycle::State & previous_state)
 {
   using TYPE = rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn;
-  if (MotionBase::on_deactivate(previous_state) != TYPE::SUCCESS || ForceBase::on_deactivate(previous_state) != TYPE::SUCCESS)
+  if (MotionBase::on_deactivate(previous_state) != TYPE::SUCCESS ||
+      ForceBase::on_deactivate(previous_state) != TYPE::SUCCESS)
   {
     return TYPE::ERROR;
   }
   return TYPE::SUCCESS;
 }
 
-#if defined CARTESIAN_CONTROLLERS_GALACTIC || defined CARTESIAN_CONTROLLERS_HUMBLE || defined CARTESIAN_CONTROLLERS_IRON
-controller_interface::return_type CartesianComplianceController::update(const rclcpp::Time& time,
-                                                                   const rclcpp::Duration& period)
+#if defined CARTESIAN_CONTROLLERS_GALACTIC || defined CARTESIAN_CONTROLLERS_HUMBLE || \
+  defined CARTESIAN_CONTROLLERS_IRON
+controller_interface::return_type CartesianComplianceController::update(
+  const rclcpp::Time & time, const rclcpp::Duration & period)
 #elif defined CARTESIAN_CONTROLLERS_FOXY
 controller_interface::return_type CartesianComplianceController::update()
 #endif
@@ -162,7 +169,7 @@ controller_interface::return_type CartesianComplianceController::update()
     ctrl::Vector6D error = computeComplianceError();
 
     // Turn Cartesian error into joint motion
-    Base::computeJointControlCmds(error,internal_period);
+    Base::computeJointControlCmds(error, internal_period);
   }
 
   // Write final commands to the hardware interface
@@ -186,18 +193,18 @@ ctrl::Vector6D CartesianComplianceController::computeComplianceError()
   ctrl::Vector6D net_force =
 
     // Spring force in base orientation
-    Base::displayInBaseLink(m_stiffness,m_compliance_ref_link) * MotionBase::computeMotionError()
+    Base::displayInBaseLink(m_stiffness, m_compliance_ref_link) * MotionBase::computeMotionError()
 
     // Sensor and target force in base orientation
     + ForceBase::computeForceError();
 
   return net_force;
 }
 
-} // namespace
-
+}  // namespace cartesian_compliance_controller
 
 // Pluginlib
 #include <pluginlib/class_list_macros.hpp>
 
-PLUGINLIB_EXPORT_CLASS(cartesian_compliance_controller::CartesianComplianceController, controller_interface::ControllerInterface)
+PLUGINLIB_EXPORT_CLASS(cartesian_compliance_controller::CartesianComplianceController,
+                       controller_interface::ControllerInterface)
diff --git a/cartesian_controller_base/CMakeLists.txt b/cartesian_controller_base/CMakeLists.txt
@@ -90,6 +90,7 @@ target_include_directories(ik_solvers
   PUBLIC
     $<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>
     $<INSTALL_INTERFACE:include>
+    ${CMAKE_BINARY_DIR}  # ROS2VersionConfig.h
 )
 
 # Prevent pluginlib from using boost