Add odometry unit test suite

Author: Enrique Fernandez

diff_drive_controller/CMakeLists.txt

@@ -83,6 +83,9 @@ if (CATKIN_ENABLE_TESTING)
     ${catkin_INCLUDE_DIRS}
     ${EIGEN_INCLUDE_DIRS})
 
+  catkin_add_gtest(odometry_test test/odometry_test.cpp)
+  target_link_libraries(odometry_test ${PROJECT_NAME})
+
   add_executable(diffbot test/diffbot.cpp)
   target_link_libraries(diffbot ${catkin_LIBRARIES})
 

diff_drive_controller/include/diff_drive_controller/rigid_body_motion.h

@@ -0,0 +1,105 @@
+/*********************************************************************
+ * Software License Agreement (BSD License)
+ *
+ *  Copyright (c) 2016, Clearpath Robotics, Inc.
+ *  All rights reserved.
+ *
+ *  Redistribution and use in source and binary forms, with or without
+ *  modification, are permitted provided that the following conditions
+ *  are met:
+ *
+ *   * Redistributions of source code must retain the above copyright
+ *     notice, this list of conditions and the following disclaimer.
+ *   * Redistributions in binary form must reproduce the above
+ *     copyright notice, this list of conditions and the following
+ *     disclaimer in the documentation and/or other materials provided
+ *     with the distribution.
+ *   * Neither the name of the PAL Robotics nor the names of its
+ *     contributors may be used to endorse or promote products derived
+ *     from this software without specific prior written permission.
+ *
+ *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ *  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+ *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+ *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ *  POSSIBILITY OF SUCH DAMAGE.
+ *********************************************************************/
+
+/*
+ * Author: Enrique Fernández
+ */
+
+#ifndef RIGID_BODY_MOTION_H_
+#define RIGID_BODY_MOTION_H_
+
+#include <Eigen/Dense>
+
+#include <limits>
+
+namespace diff_drive_controller
+{
+
+/**
+ * \brief Compute rigid body motion in SE(2) using element-wise equations.
+ * \param[in, out] x   Pose x component
+ * \param[in, out] y   Pose y component
+ * \param[in, out] yaw Pose yaw component
+ * \param[in] v_x   Velocity/Twist x   component
+ * \param[in] v_y   Velocity/Twist y   component
+ * \param[in] v_yaw Velocity/Twist yaw component
+ * \param[in] dt Time step
+ */
+static void integrate_motion(double& x, double &y, double &yaw,
+    const double v_x, const double v_y, const double v_yaw,
+    const double dt)
+{
+  const double cos_yaw = std::cos(yaw);
+  const double sin_yaw = std::sin(yaw);
+
+  x   += (v_x * cos_yaw - v_y * sin_yaw) * dt;
+  y   += (v_x * sin_yaw + v_y * cos_yaw) * dt;
+  yaw += v_yaw * dt;
+}
+
+/**
+ * \brief Compute rigid body motion in SE(2) using element-wise equations.
+ * Also computes the Jacobians wrt the pose and the velocity/twist.
+ * \param[in, out] x   Pose x component
+ * \param[in, out] y   Pose y component
+ * \param[in, out] yaw Pose yaw component
+ * \param[in] v_x   Velocity/Twist x   component
+ * \param[in] v_y   Velocity/Twist y   component
+ * \param[in] v_yaw Velocity/Twist yaw component
+ * \param[in] dt Time step
+ */
+static void integrate_motion(double& x, double &y, double &yaw,
+    const double v_x, const double v_y, const double v_yaw,
+    const double dt,
+    Eigen::Matrix3d& J_pose, Eigen::Matrix3d& J_twist)
+{
+  const double cos_yaw = std::cos(yaw);
+  const double sin_yaw = std::sin(yaw);
+
+  x   += (v_x * cos_yaw - v_y * sin_yaw) * dt;
+  y   += (v_x * sin_yaw + v_y * cos_yaw) * dt;
+  yaw += v_yaw * dt;
+
+  J_pose << 1.0, 0.0, (-v_x * sin_yaw - v_y * cos_yaw) * dt,
+            0.0, 1.0, ( v_x * cos_yaw - v_y * sin_yaw) * dt,
+            0.0, 0.0,                                   1.0;
+
+  J_twist << cos_yaw * dt, -sin_yaw * dt, 0.0,
+             sin_yaw * dt,  cos_yaw * dt, 0.0,
+                      0.0,           0.0, 1.0;
+}
+
+}
+
+#endif /* RIGID_BODY_MOTION_H_ */

diff_drive_controller/package.xml

@@ -46,6 +46,7 @@
   <!--Tests-->
   <build_depend>rostest</build_depend>
 
+  <test_depend>gtest</test_depend>
   <test_depend>std_srvs</test_depend>
   <test_depend>controller_manager</test_depend>
   <test_depend>control_toolbox</test_depend>

diff_drive_controller/test/diff_drive_test.cpp

@@ -34,8 +34,8 @@
 
 #include <tf/transform_listener.h>
 
-#define POSE_COVARIANCE_MAX_CONDITION_NUMBER 1e8
-#define TWIST_COVARIANCE_MAX_CONDITION_NUMBER 1e8
+const double POSE_COVARIANCE_MAX_CONDITION_NUMBER  = 1e8;
+const double TWIST_COVARIANCE_MAX_CONDITION_NUMBER = POSE_COVARIANCE_MAX_CONDITION_NUMBER;
 
 // TEST CASES
 TEST_F(DiffDriveControllerTest, testNoMove)
@@ -147,7 +147,7 @@ TEST_F(DiffDriveControllerTest, testNoMove)
   // where Odometry::DEFAULT_MINIMUM_TWIST_COVARIANCE == 1e-9
   TwistCovariance minimum_twist_covariance = 1e-9 * TwistCovariance::Identity();
   EXPECT_TRUE(((twist_covariance - minimum_twist_covariance).array() == 0).all())
-    << "Twist covariance=\n" << twist_covariance.format(HeavyFmt);
+    << "Twist covariance =\n" << twist_covariance.format(HeavyFmt);
 }
 
 TEST_F(DiffDriveControllerTest, testForward)