Add tests around Openplanner utilities (autowarefoundation#2262)

* Add initial tests for op_utility package Signed-off-by: Sander van Dijk <sander@parkopedia.com> * Fix angle range checks to get tests to pass Signed-off-by: Sander van Dijk <sander@parkopedia.com> * Add test for tsCompare Signed-off-by: Sander van Dijk <sander@parkopedia.com>
NuronLabs · May 24, 2019 · 4b82282 · 4b82282
1 parent 85c235c
commit 4b82282
Show file tree

Hide file tree

Showing 4 changed files with 184 additions and 51 deletions.
diff --git a/ros/src/computing/planning/common/lib/openplanner/op_utility/CMakeLists.txt b/ros/src/computing/planning/common/lib/openplanner/op_utility/CMakeLists.txt
@@ -58,3 +58,13 @@ install(TARGETS op_utility
         LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
         RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
         )
+
+if (CATKIN_ENABLE_TESTING)
+    find_package(rostest REQUIRED)
+    add_rostest_gtest(test-op_utility
+      test/test_op_utility.test
+      test/src/test_op_utility.cpp
+    )
+    add_dependencies(test-op_utility ${catkin_EXPORTED_TARGETS})
+    target_link_libraries(test-op_utility ${catkin_LIBRARIES} ${PROJECT_NAME})
+endif ()
diff --git a/ros/src/computing/planning/common/lib/openplanner/op_utility/src/UtilityH.cpp b/ros/src/computing/planning/common/lib/openplanner/op_utility/src/UtilityH.cpp
@@ -59,53 +59,47 @@ UtilityH::UtilityH()
 
  double UtilityH::FixNegativeAngle(const double& a)
 {
-   double angle = 0;
-   if (a < -2.0*M_PI || a > 2.0*M_PI)
-	{
-	   angle = fmod(a, 2.0*M_PI);
-	}
-   else
-	   angle = a;
-
-
-   if(angle < 0)
-   {
-	   angle = 2.0*M_PI + angle;
-   }
-
-   return angle;
+  double angle = 0;
+  if (a < -2.0 * M_PI || a >= 2.0 * M_PI) {
+    angle = fmod(a, 2.0 * M_PI);
+  } else {
+    angle = a;
+  }
+
+  if(angle < 0) {
+    angle = 2.0 * M_PI + angle;
+  }
+
+  return angle;
 }
 
  double UtilityH::SplitPositiveAngle(const double& a)
 {
-	 double angle = a;
-
-	if (a < -2.0*M_PI || a > 2.0*M_PI)
-	{
-		angle = fmod(a, 2.0*M_PI);
-	}
-
-	if (angle > M_PI)
-	{
-		angle -= 2.0*M_PI;
-	}
-	else if (angle < -M_PI)
-	{
-		angle += 2.0*M_PI;
-	}
-
-	return angle;
+  double angle = a;
+
+  if (a < -2.0 * M_PI || a >= 2.0 * M_PI) {
+    angle = fmod(a, 2.0 * M_PI);
+  }
+
+  if (angle >= M_PI) {
+    angle -= 2.0 * M_PI;
+  } else if (angle < -M_PI) {
+    angle += 2.0 * M_PI;
+  }
+
+  return angle;
 }
 
 double UtilityH::InverseAngle(const double& a)
 {
 
    double angle = 0;
-   if(a <= M_PI)
-		angle =  a + M_PI;
-	else
-		angle = a - M_PI;
-
+   if(a < M_PI) {
+     angle =  a + M_PI;
+   } else {
+     angle = a - M_PI;
+   }
+
    return angle;
 }
 
@@ -201,20 +195,17 @@ string UtilityH::GetDateTimeStr()
 
 int  UtilityH::tsCompare (struct  timespec  time1,   struct  timespec  time2, int micro_tolerance)
 {
-
-    if (time1.tv_sec < time2.tv_sec)
-        return (-1) ;				/* Less than. */
-    else if (time1.tv_sec > time2.tv_sec)
-        return (1) ;				/* Greater than. */
-
-    long diff = time1.tv_nsec - time2.tv_nsec;
-    if (diff < -micro_tolerance)
-        return (-1) ;				/* Less than. */
-    else if (diff > micro_tolerance)
-        return (1) ;				/* Greater than. */
-    else
-        return (0) ;				/* Equal. */
-
+  long nanoDiff =
+    (time1.tv_sec * 1000000000 + time1.tv_nsec) - 
+    (time2.tv_sec * 1000000000 + time2.tv_nsec);
+
+  if (nanoDiff < -micro_tolerance) {
+    return -1;  // time1 is less than time2
+  } else if (nanoDiff > micro_tolerance) {
+    return 1;  // time1 is greater than time2
+  } else {
+    return 0;  // time1 is equal to time2
+  }
 }
 
 timespec UtilityH::GetTimeSpec(const time_t& srcT)

diff --git a/ros/src/computing/planning/common/lib/openplanner/op_utility/test/src/test_op_utility.cpp b/ros/src/computing/planning/common/lib/openplanner/op_utility/test/src/test_op_utility.cpp
@@ -0,0 +1,129 @@
+#include <ros/ros.h>
+#include <gtest/gtest.h>
+
+#include "op_utility/UtilityH.h"
+
+class TestSuite : public ::testing::Test
+{
+public:
+  TestSuite() {}
+  ~TestSuite() {}
+};
+
+TEST(TestSuite, UtilityH_fixNegativeAngle) {
+  ASSERT_EQ(0.0, UtilityHNS::UtilityH::FixNegativeAngle(0));
+  ASSERT_EQ(0.0, UtilityHNS::UtilityH::FixNegativeAngle(2 * M_PI));
+  ASSERT_EQ(0.0, UtilityHNS::UtilityH::FixNegativeAngle(4 * M_PI));
+  ASSERT_EQ(0.0, UtilityHNS::UtilityH::FixNegativeAngle(-2 * M_PI));
+  ASSERT_EQ(0.0, UtilityHNS::UtilityH::FixNegativeAngle(-4 * M_PI));
+
+  ASSERT_EQ(M_PI, UtilityHNS::UtilityH::FixNegativeAngle(M_PI));
+  ASSERT_EQ(M_PI, UtilityHNS::UtilityH::FixNegativeAngle(3 * M_PI));
+  ASSERT_EQ(M_PI, UtilityHNS::UtilityH::FixNegativeAngle(-M_PI));
+  ASSERT_EQ(M_PI, UtilityHNS::UtilityH::FixNegativeAngle(-3 * M_PI));
+
+  ASSERT_EQ(1.0, UtilityHNS::UtilityH::FixNegativeAngle(1.0));
+  ASSERT_EQ(1.0, UtilityHNS::UtilityH::FixNegativeAngle(1.0 + 2 * M_PI));
+  ASSERT_EQ(1.0, UtilityHNS::UtilityH::FixNegativeAngle(1.0 - 2 * M_PI));
+  ASSERT_EQ(1.0 + M_PI, UtilityHNS::UtilityH::FixNegativeAngle(1.0 - M_PI));
+}
+
+TEST(TestSuite, UtilityH_splitPositiveAngle) {
+  ASSERT_EQ(0.0, UtilityHNS::UtilityH::SplitPositiveAngle(0));
+  ASSERT_EQ(0.0, UtilityHNS::UtilityH::SplitPositiveAngle(2.0 * M_PI));
+  ASSERT_EQ(0.0, UtilityHNS::UtilityH::SplitPositiveAngle(4.0 * M_PI));
+  ASSERT_EQ(0.0, UtilityHNS::UtilityH::SplitPositiveAngle(-2.0 * M_PI));
+  ASSERT_EQ(0.0, UtilityHNS::UtilityH::SplitPositiveAngle(-4.0 * M_PI));
+
+  ASSERT_EQ(-M_PI, UtilityHNS::UtilityH::SplitPositiveAngle(-M_PI));
+  ASSERT_EQ(-M_PI, UtilityHNS::UtilityH::SplitPositiveAngle(3.0 * M_PI));
+  ASSERT_EQ(-M_PI, UtilityHNS::UtilityH::SplitPositiveAngle(M_PI));
+  ASSERT_EQ(-M_PI, UtilityHNS::UtilityH::SplitPositiveAngle(-3.0 * M_PI));
+
+  ASSERT_EQ(1.0, UtilityHNS::UtilityH::SplitPositiveAngle(1.0));
+  ASSERT_EQ(1.0, UtilityHNS::UtilityH::SplitPositiveAngle(1.0 + 2.0 * M_PI));
+  ASSERT_EQ(1.0, UtilityHNS::UtilityH::SplitPositiveAngle(1.0 - 2.0 * M_PI));
+
+  ASSERT_EQ(-1.0, UtilityHNS::UtilityH::SplitPositiveAngle(-1.0));
+  ASSERT_EQ(-1.0, UtilityHNS::UtilityH::SplitPositiveAngle(-1.0 + 2.0 * M_PI));
+  ASSERT_EQ(-1.0, UtilityHNS::UtilityH::SplitPositiveAngle(-1.0 - 2.0 * M_PI));
+
+  ASSERT_EQ(4.0 - 2.0 * M_PI, UtilityHNS::UtilityH::SplitPositiveAngle(4.0));
+  ASSERT_EQ(-4.0 + 2.0 * M_PI, UtilityHNS::UtilityH::SplitPositiveAngle(-4.0));
+}
+
+TEST(TestSuite, UtilityH_inverseAngle) {
+  ASSERT_EQ(M_PI, UtilityHNS::UtilityH::InverseAngle(0.0));
+  ASSERT_EQ(0.0, UtilityHNS::UtilityH::InverseAngle(M_PI));
+  ASSERT_EQ(1.0 + M_PI, UtilityHNS::UtilityH::InverseAngle(1.0));
+  ASSERT_EQ(1.0, UtilityHNS::UtilityH::InverseAngle(1.0 + M_PI));
+}
+
+TEST(TestSuite, UtilityH_angleBetweenTwoAnglesPositive) {
+  ASSERT_EQ(0.0, UtilityHNS::UtilityH::AngleBetweenTwoAnglesPositive(0.0, 0.0));
+  ASSERT_EQ(0.0, UtilityHNS::UtilityH::AngleBetweenTwoAnglesPositive(0.0, 2.0 * M_PI));
+  ASSERT_EQ(0.0, UtilityHNS::UtilityH::AngleBetweenTwoAnglesPositive(M_PI, M_PI));
+  ASSERT_EQ(0.0, UtilityHNS::UtilityH::AngleBetweenTwoAnglesPositive(-M_PI, -M_PI));
+  ASSERT_EQ(0.0, UtilityHNS::UtilityH::AngleBetweenTwoAnglesPositive(-M_PI, M_PI));
+  ASSERT_EQ(0.0, UtilityHNS::UtilityH::AngleBetweenTwoAnglesPositive(M_PI, -M_PI));
+  ASSERT_EQ(0.0, UtilityHNS::UtilityH::AngleBetweenTwoAnglesPositive(M_PI, 3 * M_PI));
+
+  ASSERT_EQ(M_PI, UtilityHNS::UtilityH::AngleBetweenTwoAnglesPositive(0.0, M_PI));
+  ASSERT_EQ(M_PI, UtilityHNS::UtilityH::AngleBetweenTwoAnglesPositive(M_PI, 0.0));
+  ASSERT_EQ(M_PI, UtilityHNS::UtilityH::AngleBetweenTwoAnglesPositive(-M_PI, 0.0));
+  ASSERT_EQ(M_PI, UtilityHNS::UtilityH::AngleBetweenTwoAnglesPositive(0.0, -M_PI));
+
+  ASSERT_EQ(M_PI_2, UtilityHNS::UtilityH::AngleBetweenTwoAnglesPositive(0.0, M_PI_2));
+  ASSERT_EQ(M_PI_2, UtilityHNS::UtilityH::AngleBetweenTwoAnglesPositive(M_PI_2, 0.0));
+  ASSERT_EQ(M_PI_2, UtilityHNS::UtilityH::AngleBetweenTwoAnglesPositive(-M_PI_2, 0.0));
+  ASSERT_EQ(M_PI_2, UtilityHNS::UtilityH::AngleBetweenTwoAnglesPositive(0.0, -M_PI_2));
+
+  ASSERT_EQ(M_PI, UtilityHNS::UtilityH::AngleBetweenTwoAnglesPositive(-M_PI_2, M_PI_2));
+  ASSERT_EQ(M_PI, UtilityHNS::UtilityH::AngleBetweenTwoAnglesPositive(M_PI_2, -M_PI_2));
+}
+
+TEST(TestSuite, UtilityH_getSign) {
+  ASSERT_EQ(1, UtilityHNS::UtilityH::GetSign(0.0));
+  ASSERT_EQ(1, UtilityHNS::UtilityH::GetSign(1.0));
+  ASSERT_EQ(1, UtilityHNS::UtilityH::GetSign(1e20));
+  ASSERT_EQ(1, UtilityHNS::UtilityH::GetSign(1e-20));
+
+  ASSERT_EQ(-1, UtilityHNS::UtilityH::GetSign(-1.0));
+  ASSERT_EQ(-1, UtilityHNS::UtilityH::GetSign(-1e20));
+  ASSERT_EQ(-1, UtilityHNS::UtilityH::GetSign(-1e-20));
+}
+
+TEST(TestSuite, UtilityH_getTimeDiff) {
+  ASSERT_EQ(0.0, UtilityHNS::UtilityH::GetTimeDiff(timespec{0, 0}, timespec{0, 0}));
+  ASSERT_EQ(1.0, UtilityHNS::UtilityH::GetTimeDiff(timespec{0, 0}, timespec{1, 0}));
+  ASSERT_EQ(-1.0, UtilityHNS::UtilityH::GetTimeDiff(timespec{1, 0}, timespec{0, 0}));
+  ASSERT_EQ(1.0e-9, UtilityHNS::UtilityH::GetTimeDiff(timespec{0, 0}, timespec{0, 1}));
+  ASSERT_EQ(1.0e-6, UtilityHNS::UtilityH::GetTimeDiff(timespec{0, 0}, timespec{0, 1000}));
+  ASSERT_EQ(1.0e-3, UtilityHNS::UtilityH::GetTimeDiff(timespec{0, 0}, timespec{0, 1000000}));
+}
+
+TEST(TestSuite, UtilityH_tsCompare) {
+  // Clear cases
+  ASSERT_EQ(0, UtilityHNS::UtilityH::tsCompare(timespec{0, 0}, timespec{0, 0}));
+  ASSERT_EQ(1, UtilityHNS::UtilityH::tsCompare(timespec{1, 0}, timespec{0, 0}));
+  ASSERT_EQ(-1, UtilityHNS::UtilityH::tsCompare(timespec{0, 0}, timespec{1, 0}));
+
+  // Just within tolerance
+  ASSERT_EQ(0, UtilityHNS::UtilityH::tsCompare(timespec{0, 0}, timespec{0, 10}, 10));
+  ASSERT_EQ(0, UtilityHNS::UtilityH::tsCompare(timespec{0, 10}, timespec{0, 0}, 10));
+  ASSERT_EQ(0, UtilityHNS::UtilityH::tsCompare(timespec{0, 999999990}, timespec{1, 0}, 10));
+  ASSERT_EQ(0, UtilityHNS::UtilityH::tsCompare(timespec{1, 0}, timespec{0, 999999990}, 10));
+
+  // Just outside of tolerance
+  ASSERT_EQ(-1, UtilityHNS::UtilityH::tsCompare(timespec{0, 0}, timespec{0, 11}, 10));
+  ASSERT_EQ(1, UtilityHNS::UtilityH::tsCompare(timespec{0, 11}, timespec{0, 0}, 10));
+  ASSERT_EQ(-1, UtilityHNS::UtilityH::tsCompare(timespec{0, 999999989}, timespec{1, 0}, 10));
+  ASSERT_EQ(1, UtilityHNS::UtilityH::tsCompare(timespec{1, 0}, timespec{0, 999999989}, 10));
+}
+
+int main(int argc, char ** argv)
+{
+  testing::InitGoogleTest(&argc, argv);
+  ros::init(argc, argv, "TestNode");
+  return RUN_ALL_TESTS();
+}
diff --git a/ros/src/computing/planning/common/lib/openplanner/op_utility/test/test_op_utility.test b/ros/src/computing/planning/common/lib/openplanner/op_utility/test/test_op_utility.test
@@ -0,0 +1,3 @@
+<launch>
+  <test test-name="test-op_utility" pkg="op_utility" type="test-op_utility" name="test"/>
+</launch>