Add style checks (#157)

This commit adds style checks to the code base

.github/workflows/check_style.yml

@@ -0,0 +1,29 @@
+name: Style Checks
+
+on:
+  push:
+    branches:
+      - master
+  pull_request:
+    branches:
+      - '*'
+
+jobs:
+  build:
+    runs-on: ubuntu-latest
+    strategy:
+      fail-fast: false
+      matrix:
+        container:
+        - 'px4io/px4-dev-simulation-focal:2020-09-14'
+    container: ${{ matrix.container }}
+    steps:
+    - uses: actions/checkout@v1
+    - name: submodule update
+      run: git submodule update --init --recursive
+    - name: Install clang-format
+      run: apt update && apt install -y clang-format-6.0
+    - name: Check Code format
+      working-directory: Tools
+      run: |
+        ./check_code_format.sh ..

Tools/check_code_format.sh

@@ -0,0 +1,10 @@
+#!/bin/bash
+
+# Fix style recursively in all the repository
+sh fix_code_style.sh ..
+
+# Print the diff with the remote branch (empty if no diff)
+git --no-pager diff -U0 --color
+
+# Check if there are changes, and failed
+if ! git diff-index --quiet HEAD --; then echo "Code style check failed, please run clang-format (e.g. with scripts/fix_code_style.sh)"; exit 1; fi

Tools/fix_style.sh → Tools/fix_code_style.sh

@@ -13,9 +13,9 @@ fi
 for arg in "$@"
 do
     if [ -f $arg ]; then
-        clang-format-4.0 -i -style='{BasedOnStyle: google, ColumnLimit: 120}' $arg
+        clang-format-6.0 -i -style='{BasedOnStyle: google, ColumnLimit: 120}' $arg
     elif [ -d $arg ]; then
-        find $arg -iname '*.h' -o -iname '*.cpp' -o -iname '*.hpp' | xargs clang-format-4.0 -i -style='{BasedOnStyle: google, ColumnLimit: 120}'
+        find $arg -iname '*.h' -o -iname '*.cpp' -o -iname '*.hpp' | xargs clang-format-6.0 -i -style='{BasedOnStyle: google, ColumnLimit: 120}'
         find $arg -iname '*.h' -o -iname '*.cpp' -o -iname '*.hpp' | xargs chmod 644
     fi
 done

geometric_controller/include/geometric_controller/geometric_controller.h

@@ -9,32 +9,31 @@
 
 #include <stdio.h>
 #include <cstdlib>
-#include <string>
 #include <sstream>
+#include <string>
 
-#include <Eigen/Dense>
-#include <std_msgs/Float32.h>
 #include <geometry_msgs/PoseStamped.h>
 #include <geometry_msgs/Twist.h>
 #include <geometry_msgs/TwistStamped.h>
-#include <geometry_msgs/PoseStamped.h>
-#include <nav_msgs/Odometry.h>
-#include <nav_msgs/Path.h>
-#include <mavros_msgs/SetMode.h>
-#include <mavros_msgs/State.h>
-#include <mavros_msgs/CommandBool.h>
 #include <mavros_msgs/AttitudeTarget.h>
+#include <mavros_msgs/CommandBool.h>
 #include <mavros_msgs/CompanionProcessStatus.h>
+#include <mavros_msgs/SetMode.h>
+#include <mavros_msgs/State.h>
+#include <nav_msgs/Odometry.h>
+#include <nav_msgs/Path.h>
+#include <std_msgs/Float32.h>
+#include <Eigen/Dense>
 
 #include <controller_msgs/FlatTarget.h>
-#include <std_srvs/SetBool.h>
-#include <trajectory_msgs/MultiDOFJointTrajectoryPoint.h>
-#include <trajectory_msgs/MultiDOFJointTrajectory.h>
 #include <dynamic_reconfigure/server.h>
 #include <geometric_controller/GeometricControllerConfig.h>
+#include <std_srvs/SetBool.h>
+#include <trajectory_msgs/MultiDOFJointTrajectory.h>
+#include <trajectory_msgs/MultiDOFJointTrajectoryPoint.h>
 
-#define ERROR_QUATERNION  1
-#define ERROR_GEOMETRIC   2
+#define ERROR_QUATERNION 1
+#define ERROR_GEOMETRIC 2
 
 using namespace std;
 using namespace Eigen;
@@ -51,129 +50,127 @@ enum class MAV_STATE {
   MAV_STATE_FLIGHT_TERMINATION,
 };
 
-class geometricCtrl
-{
-  private:
-    ros::NodeHandle nh_;
-    ros::NodeHandle nh_private_;
-    ros::Subscriber referenceSub_;
-    ros::Subscriber flatreferenceSub_;
-    ros::Subscriber multiDOFJointSub_;
-    ros::Subscriber mavstateSub_;
-    ros::Subscriber mavposeSub_, gzmavposeSub_;
-    ros::Subscriber mavtwistSub_;
-    ros::Subscriber yawreferenceSub_;
-    ros::Publisher rotorVelPub_, angularVelPub_, target_pose_pub_;
-    ros::Publisher referencePosePub_;
-    ros::Publisher posehistoryPub_;
-    ros::Publisher systemstatusPub_;
-    ros::ServiceClient arming_client_;
-    ros::ServiceClient set_mode_client_;
-    ros::ServiceServer ctrltriggerServ_;
-    ros::ServiceServer land_service_;
-    ros::Timer cmdloop_timer_, statusloop_timer_;
-    ros::Time last_request_, reference_request_now_, reference_request_last_;
-
-    string mav_name_;
-    bool fail_detec_, ctrl_enable_, feedthrough_enable_;
-    int ctrl_mode_;
-    bool landing_commanded_;
-    bool sim_enable_;
-    bool velocity_yaw_;
-    double kp_rot_, kd_rot_;
-    double reference_request_dt_;
-    double attctrl_tau_;
-    double norm_thrust_const_, norm_thrust_offset_;
-    double max_fb_acc_;
-    double dx_, dy_, dz_;
-
-    mavros_msgs::State current_state_;
-    mavros_msgs::SetMode offb_set_mode_;
-    mavros_msgs::CommandBool arm_cmd_;
-    std::vector<geometry_msgs::PoseStamped> posehistory_vector_;
-    MAV_STATE companion_state_ = MAV_STATE::MAV_STATE_ACTIVE;
-
-    double initTargetPos_x_, initTargetPos_y_, initTargetPos_z_;
-    Eigen::Vector3d targetPos_, targetVel_, targetAcc_, targetJerk_, targetSnap_, targetPos_prev_, targetVel_prev_;
-    Eigen::Vector3d mavPos_, mavVel_, mavRate_;
-    double mavYaw_;
-    Eigen::Vector3d g_;
-    Eigen::Vector4d mavAtt_, q_des;
-    Eigen::Vector4d cmdBodyRate_; //{wx, wy, wz, Thrust}
-    Eigen::Vector3d Kpos_, Kvel_, D_;
-    Eigen::Vector3d a0, a1, tau;
-    double tau_x, tau_y, tau_z;
-    double Kpos_x_, Kpos_y_, Kpos_z_, Kvel_x_, Kvel_y_, Kvel_z_;
-    int posehistory_window_;
-
-    void pubMotorCommands();
-    void pubRateCommands(const Eigen::Vector4d &cmd);
-    void pubReferencePose(const Eigen::Vector3d &target_position, const Eigen::Vector4d &target_attitude);
-    void pubPoseHistory();
-    void pubSystemStatus();
-    void appendPoseHistory();
-    void odomCallback(const nav_msgs::OdometryConstPtr& odomMsg);
-    void targetCallback(const geometry_msgs::TwistStamped& msg);
-    void flattargetCallback(const controller_msgs::FlatTarget& msg);
-    void yawtargetCallback(const std_msgs::Float32& msg);
-    void multiDOFJointCallback(const trajectory_msgs::MultiDOFJointTrajectory& msg);
-    void keyboardCallback(const geometry_msgs::Twist& msg);
-    void cmdloopCallback(const ros::TimerEvent& event);
-    void mavstateCallback(const mavros_msgs::State::ConstPtr& msg);
-    void mavposeCallback(const geometry_msgs::PoseStamped& msg);
-    void mavtwistCallback(const geometry_msgs::TwistStamped& msg);
-    void statusloopCallback(const ros::TimerEvent& event);
-    bool ctrltriggerCallback(std_srvs::SetBool::Request &req, std_srvs::SetBool::Response &res);
-    bool landCallback(std_srvs::SetBool::Request& request, std_srvs::SetBool::Response& response);
-    Eigen::Matrix3d quat2RotMatrix(const Eigen::Vector4d &q);
-    geometry_msgs::PoseStamped vector3d2PoseStampedMsg(Eigen::Vector3d &position, Eigen::Vector4d &orientation);
-    void computeBodyRateCmd(Eigen::Vector4d &bodyrate_cmd, const Eigen::Vector3d &target_pos, const Eigen::Vector3d &target_vel, const Eigen::Vector3d &target_acc);
-    Eigen::Vector4d quatMultiplication(const Eigen::Vector4d &q, const Eigen::Vector4d &p);
-    Eigen::Vector4d attcontroller(const Eigen::Vector4d &ref_att, const Eigen::Vector3d &ref_acc, Eigen::Vector4d &curr_att);
-    Eigen::Vector4d geometric_attcontroller(const Eigen::Vector4d &ref_att, const Eigen::Vector3d &ref_acc, Eigen::Vector4d &curr_att);
-
-    inline Eigen::Vector3d toEigen(const geometry_msgs::Point& p) {
-      Eigen::Vector3d ev3(p.x, p.y, p.z);
-      return ev3;
-    }
-
-    inline Eigen::Vector3d toEigen(const geometry_msgs::Vector3& v3) {
-      Eigen::Vector3d ev3(v3.x, v3.y, v3.z);
-      return ev3;
+class geometricCtrl {
+ private:
+  ros::NodeHandle nh_;
+  ros::NodeHandle nh_private_;
+  ros::Subscriber referenceSub_;
+  ros::Subscriber flatreferenceSub_;
+  ros::Subscriber multiDOFJointSub_;
+  ros::Subscriber mavstateSub_;
+  ros::Subscriber mavposeSub_, gzmavposeSub_;
+  ros::Subscriber mavtwistSub_;
+  ros::Subscriber yawreferenceSub_;
+  ros::Publisher rotorVelPub_, angularVelPub_, target_pose_pub_;
+  ros::Publisher referencePosePub_;
+  ros::Publisher posehistoryPub_;
+  ros::Publisher systemstatusPub_;
+  ros::ServiceClient arming_client_;
+  ros::ServiceClient set_mode_client_;
+  ros::ServiceServer ctrltriggerServ_;
+  ros::ServiceServer land_service_;
+  ros::Timer cmdloop_timer_, statusloop_timer_;
+  ros::Time last_request_, reference_request_now_, reference_request_last_;
+
+  string mav_name_;
+  bool fail_detec_, ctrl_enable_, feedthrough_enable_;
+  int ctrl_mode_;
+  bool landing_commanded_;
+  bool sim_enable_;
+  bool velocity_yaw_;
+  double kp_rot_, kd_rot_;
+  double reference_request_dt_;
+  double attctrl_tau_;
+  double norm_thrust_const_, norm_thrust_offset_;
+  double max_fb_acc_;
+  double dx_, dy_, dz_;
+
+  mavros_msgs::State current_state_;
+  mavros_msgs::SetMode offb_set_mode_;
+  mavros_msgs::CommandBool arm_cmd_;
+  std::vector<geometry_msgs::PoseStamped> posehistory_vector_;
+  MAV_STATE companion_state_ = MAV_STATE::MAV_STATE_ACTIVE;
+
+  double initTargetPos_x_, initTargetPos_y_, initTargetPos_z_;
+  Eigen::Vector3d targetPos_, targetVel_, targetAcc_, targetJerk_, targetSnap_, targetPos_prev_, targetVel_prev_;
+  Eigen::Vector3d mavPos_, mavVel_, mavRate_;
+  double mavYaw_;
+  Eigen::Vector3d g_;
+  Eigen::Vector4d mavAtt_, q_des;
+  Eigen::Vector4d cmdBodyRate_;  //{wx, wy, wz, Thrust}
+  Eigen::Vector3d Kpos_, Kvel_, D_;
+  Eigen::Vector3d a0, a1, tau;
+  double tau_x, tau_y, tau_z;
+  double Kpos_x_, Kpos_y_, Kpos_z_, Kvel_x_, Kvel_y_, Kvel_z_;
+  int posehistory_window_;
+
+  void pubMotorCommands();
+  void pubRateCommands(const Eigen::Vector4d &cmd);
+  void pubReferencePose(const Eigen::Vector3d &target_position, const Eigen::Vector4d &target_attitude);
+  void pubPoseHistory();
+  void pubSystemStatus();
+  void appendPoseHistory();
+  void odomCallback(const nav_msgs::OdometryConstPtr &odomMsg);
+  void targetCallback(const geometry_msgs::TwistStamped &msg);
+  void flattargetCallback(const controller_msgs::FlatTarget &msg);
+  void yawtargetCallback(const std_msgs::Float32 &msg);
+  void multiDOFJointCallback(const trajectory_msgs::MultiDOFJointTrajectory &msg);
+  void keyboardCallback(const geometry_msgs::Twist &msg);
+  void cmdloopCallback(const ros::TimerEvent &event);
+  void mavstateCallback(const mavros_msgs::State::ConstPtr &msg);
+  void mavposeCallback(const geometry_msgs::PoseStamped &msg);
+  void mavtwistCallback(const geometry_msgs::TwistStamped &msg);
+  void statusloopCallback(const ros::TimerEvent &event);
+  bool ctrltriggerCallback(std_srvs::SetBool::Request &req, std_srvs::SetBool::Response &res);
+  bool landCallback(std_srvs::SetBool::Request &request, std_srvs::SetBool::Response &response);
+  Eigen::Matrix3d quat2RotMatrix(const Eigen::Vector4d &q);
+  geometry_msgs::PoseStamped vector3d2PoseStampedMsg(Eigen::Vector3d &position, Eigen::Vector4d &orientation);
+  void computeBodyRateCmd(Eigen::Vector4d &bodyrate_cmd, const Eigen::Vector3d &target_pos,
+                          const Eigen::Vector3d &target_vel, const Eigen::Vector3d &target_acc);
+  Eigen::Vector4d quatMultiplication(const Eigen::Vector4d &q, const Eigen::Vector4d &p);
+  Eigen::Vector4d attcontroller(const Eigen::Vector4d &ref_att, const Eigen::Vector3d &ref_acc,
+                                Eigen::Vector4d &curr_att);
+  Eigen::Vector4d geometric_attcontroller(const Eigen::Vector4d &ref_att, const Eigen::Vector3d &ref_acc,
+                                          Eigen::Vector4d &curr_att);
+
+  inline Eigen::Vector3d toEigen(const geometry_msgs::Point &p) {
+    Eigen::Vector3d ev3(p.x, p.y, p.z);
+    return ev3;
+  }
+
+  inline Eigen::Vector3d toEigen(const geometry_msgs::Vector3 &v3) {
+    Eigen::Vector3d ev3(v3.x, v3.y, v3.z);
+    return ev3;
+  }
+
+  enum FlightState { WAITING_FOR_HOME_POSE, MISSION_EXECUTION, LANDING, LANDED } node_state;
+
+  template <class T>
+  void waitForPredicate(const T *pred, const std::string &msg, double hz = 2.0) {
+    ros::Rate pause(hz);
+    ROS_INFO_STREAM(msg);
+    while (ros::ok() && !(*pred)) {
+      ros::spinOnce();
+      pause.sleep();
     }
-
-    enum FlightState {
-      WAITING_FOR_HOME_POSE, MISSION_EXECUTION, LANDING, LANDED
-    } node_state;
-
-    template <class T>
-    void waitForPredicate(const T* pred, const std::string& msg, double hz = 2.0){
-        ros::Rate pause(hz);
-        ROS_INFO_STREAM(msg);
-        while(ros::ok() && !(*pred)){
-            ros::spinOnce();
-            pause.sleep();
-        }
-    };
-    geometry_msgs::Pose home_pose_;
-    bool received_home_pose;
-
-  public:
-    void dynamicReconfigureCallback(geometric_controller::GeometricControllerConfig &config,uint32_t level);
-    geometricCtrl(const ros::NodeHandle& nh, const ros::NodeHandle& nh_private);
-    void getStates(Eigen::Vector3d &pos, Eigen::Vector4d &att, Eigen::Vector3d &vel, Eigen::Vector3d &angvel);
-    void getErrors(Eigen::Vector3d &pos, Eigen::Vector3d &vel);
-    void setBodyRateCommand(Eigen::Vector4d bodyrate_command);
-    void setFeedthrough(bool feed_through);
-    virtual ~ geometricCtrl();
-
-    static double getVelocityYaw(const Eigen::Vector3d velocity);
-    static Eigen::Vector4d acc2quaternion(const Eigen::Vector3d &vector_acc, const double &yaw);
-    static Eigen::Vector4d rot2Quaternion(const Eigen::Matrix3d &R);
-    static Eigen::Matrix3d matrix_hat(const Eigen::Vector3d &v);
-    static Eigen::Vector3d matrix_hat_inv(const Eigen::Matrix3d &m);
-
+  };
+  geometry_msgs::Pose home_pose_;
+  bool received_home_pose;
+
+ public:
+  void dynamicReconfigureCallback(geometric_controller::GeometricControllerConfig &config, uint32_t level);
+  geometricCtrl(const ros::NodeHandle &nh, const ros::NodeHandle &nh_private);
+  void getStates(Eigen::Vector3d &pos, Eigen::Vector4d &att, Eigen::Vector3d &vel, Eigen::Vector3d &angvel);
+  void getErrors(Eigen::Vector3d &pos, Eigen::Vector3d &vel);
+  void setBodyRateCommand(Eigen::Vector4d bodyrate_command);
+  void setFeedthrough(bool feed_through);
+  virtual ~geometricCtrl();
+
+  static double getVelocityYaw(const Eigen::Vector3d velocity);
+  static Eigen::Vector4d acc2quaternion(const Eigen::Vector3d &vector_acc, const double &yaw);
+  static Eigen::Vector4d rot2Quaternion(const Eigen::Matrix3d &R);
+  static Eigen::Matrix3d matrix_hat(const Eigen::Vector3d &v);
+  static Eigen::Vector3d matrix_hat_inv(const Eigen::Matrix3d &m);
 };
 
-
 #endif

geometric_controller/test/geometric_controller-test.cpp

@@ -6,33 +6,32 @@
 TEST(GeometricControllerTest, acc2quaternion) {
   Eigen::Vector3d acceleration;
   double yaw;
-  //Expected result;
+  // Expected result;
   Eigen::Vector4d ref_attitude;
   Eigen::Vector4d attitude;
 
-  //Condition
+  // Condition
   acceleration << 0.0, 0.0, 1.0;
   yaw = 0.0;
   // Expected outcome
   ref_attitude << 1.0, 0.0, 0.0, 0.0;
 
-  attitude = geometricCtrl::acc2quaternion(acceleration, yaw);  
-  
+  attitude = geometricCtrl::acc2quaternion(acceleration, yaw);
+
   ASSERT_TRUE(attitude.isApprox(ref_attitude));
 
-  //Condition
+  // Condition
   acceleration << 0.0, 0.0, 1.0;
   yaw = 1.5714;
   // Expected outcome
   ref_attitude << std::cos(0.5 * yaw), 0.0, 0.0, std::sin(0.5 * yaw);
 
   attitude = geometricCtrl::acc2quaternion(acceleration, yaw);
-  
+
   ASSERT_TRUE(attitude.isApprox(ref_attitude));
 }
 
 TEST(GeometricControllerTest, velocityyaw) {
-
   Eigen::Vector3d vel_x(1.0, 0.0, 0.0);
   double vel_x_yaw = geometricCtrl::getVelocityYaw(vel_x);
   EXPECT_NEAR(0.0, vel_x_yaw, 0.0001);

geometric_controller/test/test_example.cpp

@@ -48,7 +48,7 @@ void reverseInPlace(string &toReverse) {
     toReverse[j] = tmp;
   }
 }
-}
+}  // namespace myNormalCode
 
 TEST_F(StringTest, StrEqual) {
   // GIVEN: two strings that are the same