Add unit tests (#18)

* Add unit tests for environmentxyztheta

* Add boost::filesystem library (must be after ugv_nav4d)

* Add unit tests

* Cleanup headers

* Use set locale to C for grid resolution parameter from cli for gui

* Throw error if translation or angular velocity is 0.0

* Add unit tests for computed motions

* Update readme for unit tests

* Test costs calculation for only one motion

* Check if final pose of full and sampled splines

* Check validity of underlying values in generated trajectories

* Use the ugv_nav4d target for tests

* Use rock executable macro for tests

* Add filesystem dependency to all ugv_nav4d dependent tests

---------

Co-authored-by: Mulo01 <[email protected]>
Co-authored-by: Christoph Hertzberg <[email protected]>

README.md

@@ -106,17 +106,8 @@ cd build
 cmake -DCMAKE_INSTALL_PREFIX=./install -DTESTS_ENABLED=ON -DENABLE_DEBUG_DRAWINGS=OFF -DCMAKE_BUILD_TYPE=RELEASE ..
 make install
 ```
-Run the unit tests using the executable
-```
-test_ugv_nav4d ../test_data/Plane1Mio.ply
-```
 
-At the end you should see the output
-```
-[----------] Global test environment tear-down
-[==========] 6 tests from 2 test suites ran. (10297 ms total)
-[  PASSED  ] 6 tests.
-```
+The test executables are in the folder: `build/src/test/`.
 
 ---
 # ROS 2 Humble Test Environment with Gazebo Fortress

src/Mobility.hpp

@@ -43,19 +43,19 @@ struct Mobility {
     Mobility() :
            translationSpeed(1.0),
            rotationSpeed(1.0),
-           minTurningRadius(0.0),
-           spline_sampling_resolution(0.01),
-           remove_goal_offset(true),
+           minTurningRadius(1.0),
+           spline_sampling_resolution(0.05),
+           remove_goal_offset(false),
            multiplierForward(1),
-           multiplierBackward(1),
-           multiplierLateral(1),
-           multiplierForwardTurn(1),
-           multiplierBackwardTurn(1),
-           multiplierPointTurn(1),
-           multiplierLateralCurve(1),
-           searchRadius(2.0),
+           multiplierBackward(2),
+           multiplierLateral(4),
+           multiplierForwardTurn(2),
+           multiplierBackwardTurn(3),
+           multiplierPointTurn(3),
+           multiplierLateralCurve(4),
+           searchRadius(1.0),
            searchProgressSteps(0.1),
-           maxMotionCurveLength(1.2)
+           maxMotionCurveLength(100)
     {
     }
 

src/PreComputedMotions.cpp

@@ -296,6 +296,11 @@ void PreComputedMotions::computeSplinePrimCost(const SplinePrimitive& prim,
 int Motion::calculateCost(double translationalDist, double angularDist, double translationVelocity,
                           double angularVelocity, double costMultiplier)
 {
+    if (translationVelocity == 0.0 || angularVelocity == 0.0) {
+        LOG_ERROR_S << "ERROR calculateCost: Division by zero translation or angular velocity.";
+        throw std::runtime_error("ERROR calculateCost: Division by zero translation or angular velocity.");
+
+    }
     const double translationTime = translationalDist / translationVelocity;
     const double angularTime = angularDist / angularVelocity;
 
@@ -304,7 +309,7 @@ int Motion::calculateCost(double translationalDist, double angularDist, double t
 
     if(cost > std::numeric_limits<int>::max())
     {
-        std::cerr << "WARNING: primitive cost too large for int. Clipping to int_max." << std::endl;
+        LOG_ERROR_S << "WARNING: primitive cost too large for int. Clipping to int_max.";
         return std::numeric_limits<int>::max();
     }
     else

src/test/CMakeLists.txt

@@ -1,13 +1,24 @@
-find_package(Boost REQUIRED COMPONENTS filesystem)
+find_package(Boost REQUIRED COMPONENTS filesystem serialization)
 
-
-rock_executable(test_ugv_nav4d
-    SOURCES test_ugv_nav4d.cpp
+rock_executable(test_Planner
+    SOURCES test_Planner.cpp
+	DEPS Boost::filesystem
     DEPS_PKGCONFIG ugv_nav4d
 )
 
 rock_executable(test_EnvironmentXYZTheta
     SOURCES test_EnvironmentXYZTheta.cpp
-    DEPS Boost::filesystem
+	DEPS Boost::filesystem Boost::serialization
+    DEPS_PKGCONFIG ugv_nav4d
+)
+
+rock_executable(test_DiscreteTheta
+    SOURCES test_DiscreteTheta.cpp
+    DEPS_PKGCONFIG ugv_nav4d
+)
+
+rock_executable(test_PreComputedMotions
+    SOURCES test_PreComputedMotions.cpp
+	DEPS Boost::filesystem
     DEPS_PKGCONFIG ugv_nav4d
 )

src/test/test_DiscreteTheta.cpp

@@ -0,0 +1,38 @@
+#define BOOST_TEST_MODULE DiscreteThetaModule
+#include <boost/test/included/unit_test.hpp>
+
+#include "ugv_nav4d/DiscreteTheta.hpp"
+
+BOOST_AUTO_TEST_CASE(check_discrete_theta_init) {
+    DiscreteTheta theta = DiscreteTheta(0, 16);
+    BOOST_CHECK_CLOSE_FRACTION(theta.getRadian(), 0, 0.01);
+
+    theta = DiscreteTheta(1, 16);
+    BOOST_CHECK_EQUAL(theta.getNumAngles(), 16);
+    BOOST_CHECK_CLOSE_FRACTION(theta.getRadian(), 0.3926, 0.01);
+
+    theta = DiscreteTheta(-1, 16);
+    BOOST_CHECK_EQUAL(theta.getTheta(), 15);
+    BOOST_CHECK_CLOSE_FRACTION(theta.getRadian(), 5.8904, 0.01);
+
+    theta = DiscreteTheta(18, 16);
+    BOOST_CHECK_EQUAL(theta.getTheta(), 2);
+
+    theta = DiscreteTheta(16, 16);
+    BOOST_CHECK_EQUAL(theta.getTheta(), 0);
+
+    theta = DiscreteTheta(5.90, 16);
+    BOOST_CHECK_EQUAL(theta.getTheta(), 15);
+
+    theta = DiscreteTheta(5.45, 16);
+    BOOST_CHECK_CLOSE_FRACTION(theta.getRadian(), 5.45, 0.01);
+    BOOST_CHECK_EQUAL(theta.getTheta(), 14);
+
+    DiscreteTheta thetaA = DiscreteTheta(3, 16);
+    DiscreteTheta thetaB = DiscreteTheta(5, 16);
+    BOOST_CHECK_EQUAL(thetaA.shortestDist(thetaB).getTheta(), 2);
+
+    thetaA = DiscreteTheta(2, 16);
+    thetaB = DiscreteTheta(14, 16);
+    BOOST_CHECK_EQUAL(thetaA.shortestDist(thetaB).getTheta(), 4);
+}