add formatting and CI support

testing ci

fix header ordering

.ci/check_format.sh

@@ -0,0 +1,7 @@
+#!/bin/bash
+
+set -ex
+
+cd /workspace/build/gen3_compliant_controllers/
+#Check whether files match format
+make check-format

.ci/generate_build.sh

@@ -0,0 +1,25 @@
+#!/bin/bash
+set -ex
+
+# setup ros environment in Docker Container
+source "/opt/ros/$ROS_DISTRO/setup.bash" --
+cd /
+# create workspace directory where package will be built
+mkdir workspace
+cd workspace
+cp -r ${GITHUB_WORKSPACE}/. src/
+# Update and install packages
+apt-get update
+apt-get -y install python3-pip
+# Install catkin build
+pip3 install -U catkin_tools
+# Install gen3_compliant_controllers package deps
+apt-get install python3-rosdep
+rosdep init
+rosdep update
+rosdep install --from-paths src --ignore-src -y
+# Install format checker
+apt-get install clang-format-10 -y
+apt-get update
+#build
+catkin build 

.clang-format

@@ -0,0 +1,90 @@
+---
+BasedOnStyle:  Google
+AccessModifierOffset: -2
+AlignAfterOpenBracket: AlwaysBreak
+AlignEscapedNewlinesLeft: false
+AlignOperands:   true
+AlignTrailingComments: true
+AllowAllParametersOfDeclarationOnNextLine: true
+AllowShortBlocksOnASingleLine: false
+AllowShortCaseLabelsOnASingleLine: false
+AllowShortFunctionsOnASingleLine: None
+AllowShortIfStatementsOnASingleLine: false
+AllowShortLoopsOnASingleLine: false
+AlwaysBreakAfterDefinitionReturnType: false
+AlwaysBreakBeforeMultilineStrings: true
+AlwaysBreakTemplateDeclarations: true
+BinPackArguments: false
+BinPackParameters: false
+BraceWrapping:
+  AfterClass:      true
+  AfterControlStatement: true
+  AfterEnum:       true
+  AfterFunction:   true
+  AfterNamespace:  false
+  AfterObjCDeclaration: true
+  AfterStruct:     true
+  AfterUnion:      true
+  BeforeCatch:     true
+  BeforeElse:      true
+  IndentBraces:    false
+BreakBeforeBinaryOperators: All
+BreakBeforeBraces: Custom
+BreakBeforeInheritanceComma: true
+BreakBeforeTernaryOperators: true
+BreakConstructorInitializersBeforeComma: true
+ColumnLimit:     80
+CommentPragmas:  ''
+ConstructorInitializerAllOnOneLineOrOnePerLine: true
+ConstructorInitializerIndentWidth: 2
+ContinuationIndentWidth: 4
+Cpp11BracedListStyle: true
+DerivePointerAlignment: false
+DisableFormat:   false
+ExperimentalAutoDetectBinPacking: false
+ForEachMacros:   [ foreach, Q_FOREACH, BOOST_FOREACH ]
+# sort headers by main include file (implicit priority 0),
+# then project and private includes, then system headers
+IncludeBlocks: Regroup
+IncludeCategories:
+  - Regex:     '^<([a-z|_]+)>' # standard library headers
+    Priority:  1
+  - Regex:     '^(<|")aikido/' # Aikido headers
+    Priority:  3
+  - Regex:     '^(<[A-z]+)'    # dependency headers
+    Priority:  2
+  - Regex:     '^".*'          # headers relative to this project
+    Priority:  4
+IndentCaseLabels: true
+IndentFunctionDeclarationAfterType: false
+IndentWidth:     2
+IndentWrappedFunctionNames: false
+KeepEmptyLinesAtTheStartOfBlocks: true
+MacroBlockBegin: ''
+MacroBlockEnd: ''
+Language:        Cpp
+MaxEmptyLinesToKeep: 1
+NamespaceIndentation: None
+ObjCBlockIndentWidth: 2
+ObjCSpaceAfterProperty: false
+PenaltyBreakBeforeFirstCallParameter: 1
+PenaltyBreakComment: 300
+PenaltyBreakFirstLessLess: 120
+PenaltyBreakString: 1000
+PenaltyExcessCharacter: 1000000
+PenaltyReturnTypeOnItsOwnLine: 200
+PointerAlignment: Left
+SpaceAfterCStyleCast: false
+SpaceBeforeAssignmentOperators: true
+SpaceBeforeParens: ControlStatements
+SpaceInEmptyParentheses: false
+SpacesBeforeTrailingComments: 1
+SpacesInAngles: false
+SpacesInContainerLiterals: true
+SpacesInCStyleCastParentheses: false
+SpacesInParentheses: false
+SpacesInSquareBrackets: false
+Standard:        Auto
+TabWidth:        2
+UseTab:          Never
+...

.github/workflows/build-test.yml

@@ -0,0 +1,23 @@
+on: 
+  push:
+    branches:
+      - main
+  pull_request:
+    branches:
+      - main
+
+jobs:
+  build:
+    runs-on: ubuntu-20.04
+    container: 
+      image: ros:noetic-ros-core-focal
+    name: rosbuild
+    steps:
+      - name: Checkout
+        uses: actions/checkout@v3
+
+      - name: gen_build
+        run: ./.ci/generate_build.sh
+
+      - name: check format
+        run: ./.ci/check_format.sh

CMakeLists.txt

@@ -14,6 +14,7 @@ find_package(catkin REQUIRED COMPONENTS
   control_toolbox
   controller_interface
   hardware_interface
+  eigen_conversions
   realtime_tools
   eigen_conversions
   dynamic_reconfigure
@@ -47,6 +48,7 @@ catkin_package(
     control_msgs
     control_toolbox
     controller_interface
+    eigen_conversions
     hardware_interface
     realtime_tools
     dynamic_reconfigure
@@ -75,21 +77,20 @@ add_dependencies(${PROJECT_NAME} ${PROJECT_NAME}_gencfg)
 # Format
 #
 
-# list(INSERT CMAKE_MODULE_PATH 0 "${PROJECT_SOURCE_DIR}/cmake")
-# include(ClangFormat)
-
-# clang_format_setup(VERSION 6.0)
+include(ClangFormat.cmake)
+file(GLOB_RECURSE FILES
+    ${PROJECT_SOURCE_DIR}/src/*.[ch]pp
+)
+set(FILES ${FILES} ${FILES_INCLUDE})
 
-# if (CLANG_FORMAT_EXECUTABLE)
-#   file(GLOB_RECURSE ALL_SOURCE_FILES
-#     LIST_DIRECTORIES false
-#     include/*.h include/*.hpp src/*.c src/*.cpp tests/*.cpp tests/*.hpp)
 
-#   clang_format_add_sources(${ALL_SOURCE_FILES})
+clang_format_setup(VERSION 10)
 
-#   clang_format_add_targets()
-# endif()
+if (CLANG_FORMAT_EXECUTABLE)
+  clang_format_add_sources(${FILES})
 
+  clang_format_add_targets()
+endif()
 #==============================================================================
 # Installation
 #

cmake/ClangFormat.cmake → ClangFormat.cmake

@@ -74,7 +74,7 @@ function(clang_format_add_targets)
     )
     add_custom_target(check-format
       COMMAND ${CMAKE_COMMAND} -E echo "Checking ${formatting_files_length} files..."
-      COMMAND ${CMAKE_SOURCE_DIR}/tools/check_format.sh ${CLANG_FORMAT_EXECUTABLE} ${formatting_files}
+      COMMAND ${CLANG_FORMAT_EXECUTABLE} -style=file --dry-run --Werror ${formatting_files} >/dev/null
       COMMAND ${CMAKE_COMMAND} -E echo "Done."
       DEPENDS ${CLANG_FORMAT_EXECUTABLE}
       WORKING_DIRECTORY ${CMAKE_CURRENT_SOURCE_DIR}

include/gen3_compliant_controllers/JointSpaceCompliantController.hpp

@@ -5,42 +5,42 @@
 #include <pinocchio/algorithm/joint-configuration.hpp>
 #include <pinocchio/algorithm/rnea.hpp>
 #include <pinocchio/parsers/sample-models.hpp>
-#include "pinocchio/parsers/urdf.hpp"
-#include "pinocchio/algorithm/frames.hpp"
-#include "pinocchio/algorithm/model.hpp"
-#include "pinocchio/multibody/data.hpp"
 
-#include <vector>
 #include <string>
+#include <vector>
 
-#include <ros/node_handle.h>
 #include <realtime_tools/realtime_buffer.h>
+#include <ros/node_handle.h>
+
+#include "pinocchio/algorithm/frames.hpp"
+#include "pinocchio/algorithm/model.hpp"
+#include "pinocchio/multibody/data.hpp"
+#include "pinocchio/parsers/urdf.hpp"
 
 // ROS messages
-#include <trajectory_msgs/JointTrajectoryPoint.h>
 #include <eigen_conversions/eigen_msg.h>
+#include <trajectory_msgs/JointTrajectoryPoint.h>
 
 // ros_controls
+#include <chrono>
+
+#include <controller_interface/multi_interface_controller.h>
 #include <hardware_interface/joint_command_interface.h>
 #include <hardware_interface/joint_state_interface.h>
 #include <hardware_interface/robot_hw.h>
-#include <controller_interface/multi_interface_controller.h>
 #include <realtime_tools/realtime_buffer.h>
-
-#include <chrono>
 using namespace std::chrono;
 
-#include <gen3_compliant_controllers/helpers.hpp>
-
 #include <dynamic_reconfigure/server.h>
 #include <gen3_compliant_controllers/JointSpaceCompliantControllerConfig.h>
-
+#include <gen3_compliant_controllers/helpers.hpp>
 
 namespace gen3_compliant_controllers {
 class JointSpaceCompliantController
-    : public controller_interface::MultiInterfaceController<
-          hardware_interface::EffortJointInterface,
-          hardware_interface::JointStateInterface> {
+  : public controller_interface::MultiInterfaceController<
+        hardware_interface::EffortJointInterface,
+        hardware_interface::JointStateInterface>
+{
 public:
   JointSpaceCompliantController();
   ~JointSpaceCompliantController();
@@ -58,31 +58,32 @@ class JointSpaceCompliantController
    * \returns True if initialization was successful and the controller
    * is ready to be started.
    */
-  bool init(hardware_interface::RobotHW *robot, ros::NodeHandle &n) override;
+  bool init(hardware_interface::RobotHW* robot, ros::NodeHandle& n) override;
 
   /** \brief This is called from within the realtime thread just before the
    * first call to \ref update
    *
    * \param time The current time
    */
-  void starting(const ros::Time &time) override;
-  void stopping(const ros::Time &time) override;
+  void starting(const ros::Time& time) override;
+  void stopping(const ros::Time& time) override;
 
   /*!
    * \brief Issues commands to the joint. Should be called at regular intervals
    */
-  void update(const ros::Time &time, const ros::Duration &period) override;
+  void update(const ros::Time& time, const ros::Duration& period) override;
 
 private:
   std::unique_ptr<JointStateUpdater> mJointStateUpdater;
   bool shouldAcceptRequests();
-  bool shouldStopExecution(std::string &message);
+  bool shouldStopExecution(std::string& message);
 
-  realtime_tools::RealtimeBuffer<trajectory_msgs::JointTrajectoryPoint> mCommandsBuffer;
+  realtime_tools::RealtimeBuffer<trajectory_msgs::JointTrajectoryPoint>
+      mCommandsBuffer;
   std::atomic_bool mExecuteDefaultCommand;
 
-  std::string                                    mName;               ///< Controller name.
-  std::vector< std::string >                     mJointNames;         ///< Controlled joint names
+  std::string mName;                    ///< Controller name.
+  std::vector<std::string> mJointNames; ///< Controlled joint names
 
   // pinocchio objects
   std::shared_ptr<pinocchio::Model> mModel;
@@ -94,7 +95,7 @@ class JointSpaceCompliantController
 
   std::vector<hardware_interface::JointHandle> mControlledJointHandles;
 
-  //DYNAMIC PARAMETER OF KINOVA GEN3
+  // DYNAMIC PARAMETER OF KINOVA GEN3
   Eigen::MatrixXd mJointStiffnessMatrix;
   Eigen::MatrixXd mRotorInertiaMatrix;
 
@@ -139,16 +140,23 @@ class JointSpaceCompliantController
   ExtendedJointPosition* mExtendedJoints;
 
   ros::Subscriber mSubCommand;
-  void commandCallback(const trajectory_msgs::JointTrajectoryPointConstPtr& msg);
+  void commandCallback(
+      const trajectory_msgs::JointTrajectoryPointConstPtr& msg);
 
   // for debugging
   std::chrono::time_point<std::chrono::high_resolution_clock> mLastTimePoint;
 
   // dynamic reconfigure
-  dynamic_reconfigure::Server<gen3_compliant_controllers::JointSpaceCompliantControllerConfig> server;
-  dynamic_reconfigure::Server<gen3_compliant_controllers::JointSpaceCompliantControllerConfig>::CallbackType f;
-
-  void dynamicReconfigureCallback(gen3_compliant_controllers::JointSpaceCompliantControllerConfig &config, uint32_t level);
+  dynamic_reconfigure::Server<
+      gen3_compliant_controllers::JointSpaceCompliantControllerConfig>
+      server;
+  dynamic_reconfigure::Server<
+      gen3_compliant_controllers::JointSpaceCompliantControllerConfig>::
+      CallbackType f;
+
+  void dynamicReconfigureCallback(
+      gen3_compliant_controllers::JointSpaceCompliantControllerConfig& config,
+      uint32_t level);
 };
 
 } // namespace gen3_compliant_controllers