Add unit tests for gimbal controller plugin

Author: LukasWoodtli

.travis.yml

@@ -2,6 +2,7 @@ env:
   global:
     - CCACHE_DIR=$HOME/.ccache
     - CMAKE_BUILD="mkdir Build; cd Build; cmake ..; make -j$(nproc) -l$(nproc)"
+    - CMAKE_UNIT_TEST_BUILD="mkdir Build; cd Build; cmake -DENABLE_UNIT_TESTS=On ..; make -j$(nproc) -l$(nproc); make -j$(nproc) test"
     - CATKIN_BUILD="mkdir -p ~/catkin_ws/src; cd ~/catkin_ws; catkin init; ln -s ${TRAVIS_BUILD_DIR} src; catkin build -j$(nproc) -l$(nproc) -DBUILD_ROS_INTERFACE=ON"
     - KINETIC="source /opt/ros/kinetic/setup.bash; ${CATKIN_BUILD}"
     - MELODIC="source /opt/ros/melodic/setup.bash; ${CATKIN_BUILD}"
@@ -18,6 +19,16 @@ matrix:
           env:
             - PX4_DOCKER_REPO=px4io/px4-dev-simulation:2019-02-09
             - BUILD=${CMAKE_BUILD}
+        - name: CMake unit tests build and run (Gazebo 9)
+          os: linux
+          language: cpp
+          services:
+            - docker
+          cache:
+            ccache: true
+          env:
+            - PX4_DOCKER_REPO=px4io/px4-dev-simulation:2019-02-09
+            - BUILD=${CMAKE_UNIT_TEST_BUILD}
         - name: Catkin build on Ubuntu 16.04 with ROS Kinetic (Gazebo 7)
           os: linux
           language: cpp

CMakeLists.txt

@@ -387,6 +387,15 @@ if (catkin_FOUND)
     SHELLS sh)
 endif()
 
+
+################
+## Unit Tests ##
+################
+include(UnitTests)
+
+add_subdirectory(unit_tests)
+
+
 #############
 ## Install ##
 #############

README.md

@@ -120,6 +120,36 @@ gazebo worlds/iris.world
 
 Please refer to the documentation of the particular flight stack how to run it against this framework, e.g. [PX4](http://dev.px4.io/simulation-gazebo.html)
 
+
+## Unit Tests
+
+For building and running test an installation of 'googletest' is needed. On Ubuntu it can be installed with:
+
+```bash
+sudo apt-get install libgtest-dev
+cd /usr/src/googletest
+sudo cmake . && cd googletest
+sudo make
+sudo cp *.a /usr/lib
+```
+
+Building the tests on an other platform than Linux is not yet supported.
+
+For building the tests, the flag `ENABLE_UNIT_TESTS` needs to be provided to cmake.
+
+```bash
+mkdir build && cd build
+cmake -DENABLE_UNIT_TESTS=On ..
+```
+
+Then build and run the tests:
+
+```bash
+make && make test
+```
+
+When writing test it’s important to be careful which API functions of Gazebo are called. As no Gazebo server is running during the tests some functions can produce undefined behaviour (e.g. segfaults).
+
 ## Packaging
 
 ### Deb

cmake/UnitTests.cmake

@@ -0,0 +1,11 @@
+
+option(ENABLE_UNIT_TESTS "Build unit tests for the plugins" OFF)
+
+if(ENABLE_UNIT_TESTS)
+
+    enable_testing()
+
+    find_package(GTest REQUIRED)
+    find_package(Threads REQUIRED)
+
+endif(ENABLE_UNIT_TESTS)

include/gazebo_gimbal_controller_plugin.hh

@@ -92,21 +92,6 @@ namespace gazebo
     private: void OnYawStringMsg(ConstGzStringPtr &_msg);
 #endif
 
-    /// \TODO something to move into Angle class
-    /// \brief returns _angle1 normalized about
-    /// (_reference - M_PI, _reference + M_PI]
-    /// \param[in] _angle1 input angle
-    /// \param[in] _reference reference input angle for normalization
-    /// \return normalized _angle1 about _reference
-    private: double NormalizeAbout(double _angle, double _reference);
-
-    /// \TODO something to move into Angle class
-    /// \brief returns shortest angular distance from _from to _to
-    /// \param[in] _from starting anglular position
-    /// \param[in] _to end angular position
-    /// \return distance traveled from starting to end angular positions
-    private: double ShortestAngularDistance(double _from, double _to);
-
     private: sdf::ElementPtr sdf;
 
     private: std::vector<event::ConnectionPtr> connections;
@@ -150,11 +135,6 @@ namespace gazebo
     private: common::PID rollPid;
     private: common::PID yawPid;
     private: common::Time lastUpdateTime;
-
-    private: ignition::math::Vector3d ThreeAxisRot(
-      double r11, double r12, double r21, double r31, double r32);
-    private: ignition::math::Vector3d QtoZXY(
-      const ignition::math::Quaterniond &_q);
   };
 }
 #endif

src/gazebo_gimbal_controller_plugin.cpp

@@ -24,6 +24,70 @@ using namespace std;
 
 GZ_REGISTER_MODEL_PLUGIN(GimbalControllerPlugin)
 
+/* Keep these functions in the 'detail' namespace so that they
+ * can be called from unit tests. */
+namespace detail {
+/////////////////////////////////////////////////
+ignition::math::Vector3d ThreeAxisRot(
+  double r11, double r12, double r21, double r31, double r32)
+{
+  return ignition::math::Vector3d(
+    atan2( r31, r32 ),
+    asin ( r21 ),
+    atan2( r11, r12 ));
+}
+
+/////////////////////////////////////////////////
+/// \TODO something to move into Angle class
+/// \brief returns _angle1 normalized about
+/// (_reference - M_PI, _reference + M_PI]
+/// \param[in] _angle1 input angle
+/// \param[in] _reference reference input angle for normalization
+/// \return normalized _angle1 about _reference
+double NormalizeAbout(double _angle, double reference)
+{
+  double diff = _angle - reference;
+  // normalize diff about (-pi, pi], then add reference
+  while (diff <= -M_PI)
+  {
+    diff += 2.0*M_PI;
+  }
+  while (diff > M_PI)
+  {
+    diff -= 2.0*M_PI;
+  }
+  return diff + reference;
+}
+
+/////////////////////////////////////////////////
+/// \TODO something to move into Angle class
+/// \brief returns shortest angular distance from _from to _to
+/// \param[in] _from starting anglular position
+/// \param[in] _to end angular position
+/// \return distance traveled from starting to end angular positions
+double ShortestAngularDistance(double _from, double _to)
+{
+  return NormalizeAbout(_to, _from) - _from;
+}
+
+/////////////////////////////////////////////////
+ignition::math::Vector3d QtoZXY(
+  const ignition::math::Quaterniond &_q)
+{
+  // taken from
+  // http://bediyap.com/programming/convert-quaternion-to-euler-rotations/
+  // case zxy:
+  ignition::math::Vector3d result = detail::ThreeAxisRot(
+    -2*(_q.X()*_q.Y() - _q.W()*_q.Z()),
+    _q.W()*_q.W() - _q.X()*_q.X() + _q.Y()*_q.Y() - _q.Z()*_q.Z(),
+    2*(_q.Y()*_q.Z() + _q.W()*_q.X()),
+    -2*(_q.X()*_q.Z() - _q.W()*_q.Y()),
+    _q.W()*_q.W() - _q.X()*_q.X() - _q.Y()*_q.Y() + _q.Z()*_q.Z());
+  return result;
+}
+}
+
+
 /////////////////////////////////////////////////
 GimbalControllerPlugin::GimbalControllerPlugin()
   :status("closed")
@@ -388,32 +452,6 @@ void GimbalControllerPlugin::OnYawStringMsg(ConstGzStringPtr &_msg)
 }
 #endif
 
-/////////////////////////////////////////////////
-ignition::math::Vector3d GimbalControllerPlugin::ThreeAxisRot(
-  double r11, double r12, double r21, double r31, double r32)
-{
-  return ignition::math::Vector3d(
-    atan2( r31, r32 ),
-    asin ( r21 ),
-    atan2( r11, r12 ));
-}
-
-/////////////////////////////////////////////////
-ignition::math::Vector3d GimbalControllerPlugin::QtoZXY(
-  const ignition::math::Quaterniond &_q)
-{
-  // taken from
-  // http://bediyap.com/programming/convert-quaternion-to-euler-rotations/
-  // case zxy:
-  ignition::math::Vector3d result = this->ThreeAxisRot(
-    -2*(_q.X()*_q.Y() - _q.W()*_q.Z()),
-    _q.W()*_q.W() - _q.X()*_q.X() + _q.Y()*_q.Y() - _q.Z()*_q.Z(),
-    2*(_q.Y()*_q.Z() + _q.W()*_q.X()),
-    -2*(_q.X()*_q.Z() - _q.W()*_q.Y()),
-    _q.W()*_q.W() - _q.X()*_q.X() - _q.Y()*_q.Y() + _q.Z()*_q.Z());
-  return result;
-}
-
 /////////////////////////////////////////////////
 void GimbalControllerPlugin::OnUpdate()
 {
@@ -468,10 +506,10 @@ void GimbalControllerPlugin::OnUpdate()
 
 #if GAZEBO_MAJOR_VERSION >= 8
     ignition::math::Vector3d currentAnglePRYVariable(
-      this->QtoZXY(ignition::math::Quaterniond(currentAngleYPRVariable)));
+      detail::QtoZXY(ignition::math::Quaterniond(currentAngleYPRVariable)));
 #else
     ignition::math::Vector3d currentAnglePRYVariable(
-      this->QtoZXY(currentAngleYPRVariable));
+      detail::QtoZXY(currentAngleYPRVariable));
 #endif
 
     /// get joint limits (in sensor frame)
@@ -497,11 +535,11 @@ void GimbalControllerPlugin::OnUpdate()
 #endif
 
     // normalize errors
-    double pitchError = this->ShortestAngularDistance(
+    double pitchError = detail::ShortestAngularDistance(
       pitchLimited, currentAnglePRYVariable.X());
-    double rollError = this->ShortestAngularDistance(
+    double rollError = detail::ShortestAngularDistance(
       rollLimited, currentAnglePRYVariable.Y());
-    double yawError = this->ShortestAngularDistance(
+    double yawError = detail::ShortestAngularDistance(
       yawLimited, currentAnglePRYVariable.Z());
 
     // Clamp errors based on current angle and estimated errors from rotations:
@@ -609,24 +647,3 @@ void GimbalControllerPlugin::OnUpdate()
   }
 }
 
-/////////////////////////////////////////////////
-double GimbalControllerPlugin::NormalizeAbout(double _angle, double reference)
-{
-  double diff = _angle - reference;
-  // normalize diff about (-pi, pi], then add reference
-  while (diff <= -M_PI)
-  {
-    diff += 2.0*M_PI;
-  }
-  while (diff > M_PI)
-  {
-    diff -= 2.0*M_PI;
-  }
-  return diff + reference;
-}
-
-/////////////////////////////////////////////////
-double GimbalControllerPlugin::ShortestAngularDistance(double _from, double _to)
-{
-  return this->NormalizeAbout(_to, _from) - _from;
-}

unit_tests/CMakeLists.txt

@@ -0,0 +1,13 @@
+
+if(ENABLE_UNIT_TESTS)
+
+    add_executable(gazebo_gimbal_controller_plugin_test gazebo_gimbal_controller_plugin_test.cpp)
+
+    target_link_libraries(gazebo_gimbal_controller_plugin_test
+        PRIVATE gazebo_gimbal_controller_plugin
+                ${GTEST_BOTH_LIBRARIES}
+                ${CMAKE_THREAD_LIBS_INIT})
+
+    add_test(gazebo_gimbal_controller_plugin_test gazebo_gimbal_controller_plugin_test)
+
+endif(ENABLE_UNIT_TESTS)