[Personalrobots-commit] SF.net SVN: personalrobots:[11356] pkg/trunk

[<tf...@us...>]

Revision: 11356
          http://personalrobots.svn.sourceforge.net/personalrobots/?rev=11356&view=rev
Author:   tfoote
Date:     2009-02-18 23:35:50 +0000 (Wed, 18 Feb 2009)

Log Message:
-----------
cleaning up angles package trying to code review.  lots of tests added, and exposed some problems

Modified Paths:
--------------
    pkg/trunk/deprecated/libTF/test/pose3d_unittest.cpp
    pkg/trunk/prcore/angles/CMakeLists.txt
    pkg/trunk/prcore/angles/include/angles/angles.h
    pkg/trunk/prcore/angles/test/utest.cpp

Modified: pkg/trunk/deprecated/libTF/test/pose3d_unittest.cpp
===================================================================
--- pkg/trunk/deprecated/libTF/test/pose3d_unittest.cpp	2009-02-18 23:10:06 UTC (rev 11355)
+++ pkg/trunk/deprecated/libTF/test/pose3d_unittest.cpp	2009-02-18 23:35:50 UTC (rev 11356)
@@ -4,6 +4,21 @@
 #include <sys/time.h>
 #include <cstdlib>
 
+  /*!
+   * \brief modNPiBy2
+   *
+   * Returns the angle between -M_PI/2 to M_PI/2
+   */
+static inline double angles::modNPiBy2(double angle)
+  {
+    if (angle < -M_PI/2) 
+      angle += M_PI;
+    if(angle > M_PI/2)
+      angle -= M_PI;
+    return angle;
+  }
+
+
   //Seed random number generator with current microseond count
 void seed_rand(){
   timeval temp_time_struct;

Modified: pkg/trunk/prcore/angles/CMakeLists.txt
===================================================================
--- pkg/trunk/prcore/angles/CMakeLists.txt	2009-02-18 23:10:06 UTC (rev 11355)
+++ pkg/trunk/prcore/angles/CMakeLists.txt	2009-02-18 23:35:50 UTC (rev 11356)
@@ -4,6 +4,4 @@
 
 # Unit tests
 rospack_add_gtest(utest test/utest.cpp)
-set_target_properties(utest PROPERTIES
-                      RUNTIME_OUTPUT_DIRECTORY ${PROJECT_SOURCE_DIR}/test)
 target_link_libraries(utest)

Modified: pkg/trunk/prcore/angles/include/angles/angles.h
===================================================================
--- pkg/trunk/prcore/angles/include/angles/angles.h	2009-02-18 23:10:06 UTC (rev 11355)
+++ pkg/trunk/prcore/angles/include/angles/angles.h	2009-02-18 23:35:50 UTC (rev 11356)
@@ -263,19 +263,7 @@
 
     
 
-  /*!
-   * \brief modNPiBy2
-   *
-   * Returns the angle between -M_PI/2 to M_PI/2
-   */
-  static inline double modNPiBy2(double angle)
-  {
-    if (angle < -M_PI/2) 
-      angle += M_PI;
-    if(angle > M_PI/2)
-      angle -= M_PI;
-    return angle;
-  }
+
 }
 
 #endif

Modified: pkg/trunk/prcore/angles/test/utest.cpp
===================================================================
--- pkg/trunk/prcore/angles/test/utest.cpp	2009-02-18 23:10:06 UTC (rev 11355)
+++ pkg/trunk/prcore/angles/test/utest.cpp	2009-02-18 23:35:50 UTC (rev 11356)
@@ -1,6 +1,8 @@
 #include "angles/angles.h"
 #include <gtest/gtest.h>
 
+using namespace angles;
+
 TEST(Angles, shortestDistanceWithLimits){
   double shortest_angle;
   bool result = angles::shortest_angular_distance_with_limits(-0.5, 0.5,-0.25,0.25,shortest_angle);
@@ -55,6 +57,158 @@
 
 }
 
+TEST(Angles, from_degrees)
+{
+  double epsilon = 1e-9;
+  EXPECT_NEAR(0, from_degrees(0), epsilon);
+  EXPECT_NEAR(M_PI/2, from_degrees(90), epsilon);
+  EXPECT_NEAR(M_PI, from_degrees(180), epsilon);
+  EXPECT_NEAR(M_PI*3/2, from_degrees(270), epsilon);
+  EXPECT_NEAR(2*M_PI, from_degrees(360), epsilon);
+  EXPECT_NEAR(M_PI/3, from_degrees(60), epsilon);
+  EXPECT_NEAR(M_PI*2/3, from_degrees(120), epsilon);
+  EXPECT_NEAR(M_PI/4, from_degrees(45), epsilon);
+  EXPECT_NEAR(M_PI*3/4, from_degrees(135), epsilon);
+  EXPECT_NEAR(M_PI/6, from_degrees(30), epsilon);
+  
+}
+
+TEST(Angles, to_degrees)
+{
+  double epsilon = 1e-9;
+  EXPECT_NEAR(to_degrees(0), 0, epsilon);
+  EXPECT_NEAR(to_degrees(M_PI/2), 90, epsilon);
+  EXPECT_NEAR(to_degrees(M_PI), 180, epsilon);
+  EXPECT_NEAR(to_degrees(M_PI*3/2), 270, epsilon);
+  EXPECT_NEAR(to_degrees(2*M_PI), 360, epsilon);
+  EXPECT_NEAR(to_degrees(M_PI/3), 60, epsilon);
+  EXPECT_NEAR(to_degrees(M_PI*2/3), 120, epsilon);
+  EXPECT_NEAR(to_degrees(M_PI/4), 45, epsilon);
+  EXPECT_NEAR(to_degrees(M_PI*3/4), 135, epsilon);
+  EXPECT_NEAR(to_degrees(M_PI/6), 30, epsilon);
+}
+
+TEST(Angles, normalize_angle_positive)
+{
+ double epsilon = 1e-9;
+ EXPECT_NEAR(0, normalize_angle_positive(0), epsilon);
+ EXPECT_NEAR(M_PI, normalize_angle_positive(M_PI), epsilon);
+ EXPECT_NEAR(0, normalize_angle_positive(2*M_PI), epsilon);
+ EXPECT_NEAR(M_PI, normalize_angle_positive(3*M_PI), epsilon);
+ EXPECT_NEAR(0, normalize_angle_positive(4*M_PI), epsilon);
+
+ EXPECT_NEAR(0, normalize_angle_positive(-0), epsilon);
+ EXPECT_NEAR(M_PI, normalize_angle_positive(-M_PI), epsilon);
+ EXPECT_NEAR(0, normalize_angle_positive(-2*M_PI), epsilon);
+ EXPECT_NEAR(M_PI, normalize_angle_positive(-3*M_PI), epsilon);
+ EXPECT_NEAR(0, normalize_angle_positive(-4*M_PI), epsilon);
+
+ EXPECT_NEAR(0, normalize_angle_positive(-0), epsilon);
+ EXPECT_NEAR(3*M_PI/2, normalize_angle_positive(-M_PI/2), epsilon);
+ EXPECT_NEAR(M_PI, normalize_angle_positive(-M_PI), epsilon);
+ EXPECT_NEAR(M_PI/2, normalize_angle_positive(-3*M_PI/2), epsilon);
+ EXPECT_NEAR(0, normalize_angle_positive(-4*M_PI/2), epsilon);
+
+ EXPECT_NEAR(0, normalize_angle_positive(0), epsilon);
+ EXPECT_NEAR(M_PI/2, normalize_angle_positive(M_PI/2), epsilon);
+ EXPECT_NEAR(M_PI/2, normalize_angle_positive(5*M_PI/2), epsilon);
+ EXPECT_NEAR(M_PI/2, normalize_angle_positive(9*M_PI/2), epsilon);
+ EXPECT_NEAR(M_PI/2, normalize_angle_positive(-3*M_PI/2), epsilon);
+
+}
+
+
+TEST(Angles, normalize_angle)
+{
+ double epsilon = 1e-9;
+ EXPECT_NEAR(0, normalize_angle(0), epsilon);
+ EXPECT_NEAR(M_PI, normalize_angle(M_PI), epsilon);
+ EXPECT_NEAR(0, normalize_angle(2*M_PI), epsilon);
+ EXPECT_NEAR(M_PI, normalize_angle(3*M_PI), epsilon);
+ EXPECT_NEAR(0, normalize_angle(4*M_PI), epsilon);
+
+ EXPECT_NEAR(0, normalize_angle(-0), epsilon);
+ EXPECT_NEAR(M_PI, normalize_angle(-M_PI), epsilon);
+ EXPECT_NEAR(0, normalize_angle(-2*M_PI), epsilon);
+ EXPECT_NEAR(M_PI, normalize_angle(-3*M_PI), epsilon);
+ EXPECT_NEAR(0, normalize_angle(-4*M_PI), epsilon);
+
+ EXPECT_NEAR(0, normalize_angle(-0), epsilon);
+ EXPECT_NEAR(-M_PI/2, normalize_angle(-M_PI/2), epsilon);
+ EXPECT_NEAR(M_PI, normalize_angle(-M_PI), epsilon);
+ EXPECT_NEAR(M_PI/2, normalize_angle(-3*M_PI/2), epsilon);
+ EXPECT_NEAR(0, normalize_angle(-4*M_PI/2), epsilon);
+
+ EXPECT_NEAR(0, normalize_angle(0), epsilon);
+ EXPECT_NEAR(M_PI/2, normalize_angle(M_PI/2), epsilon);
+ EXPECT_NEAR(M_PI/2, normalize_angle(5*M_PI/2), epsilon);
+ EXPECT_NEAR(M_PI/2, normalize_angle(9*M_PI/2), epsilon);
+ EXPECT_NEAR(M_PI/2, normalize_angle(-3*M_PI/2), epsilon);
+
+}
+
+TEST(Angles, shortest_angular_distance)
+{
+  double epsilon = 1e-9;
+  EXPECT_NEAR(M_PI/2, shortest_angular_distance(0, M_PI/2), epsilon);
+  EXPECT_NEAR(-M_PI/2, shortest_angular_distance(0, -M_PI/2), epsilon);
+  EXPECT_NEAR(-M_PI/2, shortest_angular_distance(M_PI/2, 0), epsilon);
+  EXPECT_NEAR(M_PI/2, shortest_angular_distance(-M_PI/2, 0), epsilon);
+
+  EXPECT_NEAR(-M_PI/2, shortest_angular_distance(M_PI, M_PI/2), epsilon);
+  EXPECT_NEAR(M_PI/2, shortest_angular_distance(M_PI, -M_PI/2), epsilon);
+  EXPECT_NEAR(M_PI/2, shortest_angular_distance(M_PI/2, M_PI), epsilon);
+  EXPECT_NEAR(-M_PI/2, shortest_angular_distance(-M_PI/2, M_PI), epsilon);
+
+  EXPECT_NEAR(-M_PI/2, shortest_angular_distance(5*M_PI, M_PI/2), epsilon);
+  EXPECT_NEAR(M_PI/2, shortest_angular_distance(7*M_PI, -M_PI/2), epsilon);
+  EXPECT_NEAR(M_PI/2, shortest_angular_distance(9*M_PI/2, M_PI), epsilon);
+  EXPECT_NEAR(M_PI/2, shortest_angular_distance(-3*M_PI/2, M_PI), epsilon);
+
+}
+
+TEST(Angles, add_mod_2Pi)
+{
+  double epsilon = 1e-9;
+  EXPECT_NEAR(add_mod_2Pi(0), -2*M_PI, epsilon); //Should this be?
+  EXPECT_NEAR(add_mod_2Pi(2*M_PI), 0, epsilon); 
+  EXPECT_NEAR(add_mod_2Pi(-2*M_PI), 0, epsilon); 
+  EXPECT_NEAR(add_mod_2Pi(M_PI/2), -3*M_PI/2, epsilon);
+  EXPECT_NEAR(add_mod_2Pi(M_PI), -M_PI, epsilon);
+  EXPECT_NEAR(add_mod_2Pi(-M_PI), M_PI, epsilon);
+  EXPECT_NEAR(add_mod_2Pi(-M_PI/2), 3*M_PI/2, epsilon);
+
+  
+
+}
+
+/** \todo find_min_max_delta is always returning false commented until fixed 
+TEST(Angles, find_min_max_delta)
+{
+  double epsilon = 1e-9;
+  double min_delta, max_delta;
+  EXPECT_TRUE(find_min_max_delta( 0, -M_PI, M_PI, min_delta, max_delta));
+  EXPECT_NEAR(min_delta, -M_PI, epsilon);
+  EXPECT_NEAR(max_delta, M_PI, epsilon);
+
+  EXPECT_TRUE(find_min_max_delta( M_PI/2, -M_PI, M_PI, min_delta, max_delta));
+  EXPECT_NEAR(min_delta, -3*M_PI/2, epsilon);
+  EXPECT_NEAR(max_delta, M_PI/2, epsilon);
+
+  EXPECT_TRUE(find_min_max_delta( M_PI/2, -M_PI, 3*M_PI, min_delta, max_delta));
+  EXPECT_NEAR(min_delta, -3*M_PI/2, epsilon);
+  EXPECT_NEAR(max_delta, M_PI/2, epsilon);
+
+  EXPECT_TRUE(find_min_max_delta( -M_PI/2, -M_PI, 3*M_PI, min_delta, max_delta));
+  EXPECT_NEAR(min_delta, -M_PI/2, epsilon);
+  EXPECT_NEAR(max_delta, 3*M_PI/2, epsilon);
+
+  
+  EXPECT_FALSE(find_min_max_delta( 2*M_PI, -M_PI, M_PI, min_delta, max_delta));
+  EXPECT_FALSE(find_min_max_delta( -M_PI, -M_PI/2, M_PI/2, min_delta, max_delta));
+
+}
+*/
 int main(int argc, char **argv){
   testing::InitGoogleTest(&argc, argv);
   return RUN_ALL_TESTS();


This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.