Menu
▾
▴
[Personalrobots-commit] SF.net SVN: personalrobots:[2522] pkg/trunk
|
From: <is...@us...> - 2008-08-04 23:34:04
|
Revision: 2522
http://personalrobots.svn.sourceforge.net/personalrobots/?rev=2522&view=rev
Author: isucan
Date: 2008-08-04 23:34:10 +0000 (Mon, 04 Aug 2008)
Log Message:
-----------
removed MIN, MAX (defined in <algorithm>), renamed CLAMP to clamp and made it a template (with Tully's help)
Modified Paths:
--------------
pkg/trunk/controllers/pr2Controllers/src/BaseController.cpp
pkg/trunk/util/laser_scan_utils/src/laser_scan.cc
pkg/trunk/util/math_utils/include/math_utils/angles.h
pkg/trunk/util/math_utils/include/math_utils/math_utils.h
pkg/trunk/util/math_utils/test/math_utils_unittest.cpp
Modified: pkg/trunk/controllers/pr2Controllers/src/BaseController.cpp
===================================================================
--- pkg/trunk/controllers/pr2Controllers/src/BaseController.cpp 2008-08-04 23:06:06 UTC (rev 2521)
+++ pkg/trunk/controllers/pr2Controllers/src/BaseController.cpp 2008-08-04 23:34:10 UTC (rev 2522)
@@ -129,9 +129,9 @@
void BaseController::receiveBaseCommandMessage(){
maxXDot = maxYDot = maxYawDot = 1; //Until we start reading the xml file for parameters
- double vx = CLAMP(baseCommandMessage.axes[1], -maxXDot, maxXDot);
- double vy = CLAMP(baseCommandMessage.axes[0], -maxYDot, maxYDot);
- double vyaw = CLAMP(baseCommandMessage.axes[2], -maxYawDot, maxYawDot);
+ double vx = math_utils::clamp<double>(baseCommandMessage.axes[1], -maxXDot, maxXDot);
+ double vy = math_utils::clamp<double>(baseCommandMessage.axes[0], -maxYDot, maxYDot);
+ double vyaw = math_utils::clamp<double>(baseCommandMessage.axes[2], -maxYawDot, maxYawDot);
setVelocity(vx, vy, vyaw);
}
Modified: pkg/trunk/util/laser_scan_utils/src/laser_scan.cc
===================================================================
--- pkg/trunk/util/laser_scan_utils/src/laser_scan.cc 2008-08-04 23:06:06 UTC (rev 2521)
+++ pkg/trunk/util/laser_scan_utils/src/laser_scan.cc 2008-08-04 23:34:10 UTC (rev 2522)
@@ -28,8 +28,8 @@
*/
#include "laser_scan_utils/laser_scan.h"
+#include <algorithm>
-
namespace laser_scan{
@@ -133,7 +133,7 @@
temp_scan_ = scan_in; //HACK to store all metadata
/** \todo check for length of intensities too */
- unsigned int iterations = MIN(scan_in.ranges_size, num_ranges_);
+ unsigned int iterations = std::min(scan_in.ranges_size, num_ranges_);
for (unsigned int index = 0; index < iterations; index ++)
{
range_data_(current_packet_num_+1, index+1)= (double) scan_in.ranges[index];
@@ -158,7 +158,7 @@
NEWMAT::ColumnVector iColumn;
- unsigned int iterations = MIN(scan_result.ranges_size, num_ranges_);
+ unsigned int iterations = std::min(scan_result.ranges_size, num_ranges_);
/** \todo Resize output cloud/check length */
for (unsigned int index = 0; index < iterations; index ++)
{
Modified: pkg/trunk/util/math_utils/include/math_utils/angles.h
===================================================================
--- pkg/trunk/util/math_utils/include/math_utils/angles.h 2008-08-04 23:06:06 UTC (rev 2521)
+++ pkg/trunk/util/math_utils/include/math_utils/angles.h 2008-08-04 23:34:10 UTC (rev 2522)
@@ -1,3 +1,37 @@
+/*********************************************************************
+* Software License Agreement (BSD License)
+*
+* Copyright (c) 2008, Willow Garage, 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 Willow Garage 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.
+*********************************************************************/
+
#ifndef MATH_UTILS_ANGLES
#define MATH_UTILS_ANGLES
@@ -3,68 +37,73 @@
#include <cmath>
-/** Convert degrees to radians */
-static inline double from_degrees(double degrees)
+namespace math_utils
{
- return degrees * M_PI / 180.0;
-}
-
-/** Convert radians to degrees */
-static inline double to_degrees(double radians)
-{
- return radians * 180.0 / M_PI;
-}
-
-/*
- * normalize_angle_positive
- *
- * Normalizes the angle to be 0 circle to 1 circle
- * It takes and returns native units.
- */
-
-static inline double normalize_angle_positive(double angle)
-{
- return fmod(fmod(angle, 2.0*M_PI) + 2.0*M_PI, 2.0*M_PI);
-}
-/*
- * normalize
- *
- * Normalizes the angle to be -1/2 circle to +1/2 circle
- * It takes and returns native units.
- *
- */
-
-static inline double normalize_angle(double angle)
-{
- double a = normalize_angle_positive(angle);
- if (a > M_PI)
- a -= 2.0 *M_PI;
- return a;
-}
-
-/*
- * shortest_angular_distance
- *
- * Given 2 angles, this returns the shortest angular
- * difference. The inputs and ouputs are of course native
- * units.
- *
- * As an example, if native units are degrees, the result
- * would always be -180 <= result <= 180. Adding the result
- * to "from" will always get you an equivelent angle to "to".
- */
-
-static inline double shortest_angular_distance(double from, double to)
-{
- double result = normalize_angle_positive(normalize_angle_positive(to) - normalize_angle_positive(from));
-
- if (result > M_PI)
- // If the result > 180,
- // It's shorter the other way.
- result = -(2.0*M_PI - result);
- return normalize_angle(result);
+ /** Convert degrees to radians */
+
+ static inline double from_degrees(double degrees)
+ {
+ return degrees * M_PI / 180.0;
+ }
+
+ /** Convert radians to degrees */
+ static inline double to_degrees(double radians)
+ {
+ return radians * 180.0 / M_PI;
+ }
+
+ /*
+ * normalize_angle_positive
+ *
+ * Normalizes the angle to be 0 circle to 1 circle
+ * It takes and returns native units.
+ */
+
+ static inline double normalize_angle_positive(double angle)
+ {
+ return fmod(fmod(angle, 2.0*M_PI) + 2.0*M_PI, 2.0*M_PI);
+ }
+ /*
+ * normalize
+ *
+ * Normalizes the angle to be -1/2 circle to +1/2 circle
+ * It takes and returns native units.
+ *
+ */
+
+ static inline double normalize_angle(double angle)
+ {
+ double a = normalize_angle_positive(angle);
+ if (a > M_PI)
+ a -= 2.0 *M_PI;
+ return a;
+ }
+
+ /*
+ * shortest_angular_distance
+ *
+ * Given 2 angles, this returns the shortest angular
+ * difference. The inputs and ouputs are of course native
+ * units.
+ *
+ * As an example, if native units are degrees, the result
+ * would always be -180 <= result <= 180. Adding the result
+ * to "from" will always get you an equivelent angle to "to".
+ */
+
+ static inline double shortest_angular_distance(double from, double to)
+ {
+ double result = normalize_angle_positive(normalize_angle_positive(to) - normalize_angle_positive(from));
+
+ if (result > M_PI)
+ // If the result > 180,
+ // It's shorter the other way.
+ result = -(2.0*M_PI - result);
+
+ return normalize_angle(result);
+ }
+
}
-
#endif
Modified: pkg/trunk/util/math_utils/include/math_utils/math_utils.h
===================================================================
--- pkg/trunk/util/math_utils/include/math_utils/math_utils.h 2008-08-04 23:06:06 UTC (rev 2521)
+++ pkg/trunk/util/math_utils/include/math_utils/math_utils.h 2008-08-04 23:34:10 UTC (rev 2522)
@@ -1,22 +1,51 @@
+/*********************************************************************
+* Software License Agreement (BSD License)
+*
+* Copyright (c) 2008, Willow Garage, 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 Willow Garage 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.
+*********************************************************************/
+
#ifndef MATH_UTIL_H
#define MATH_UTIL_H
-/** Minimum between two doubles */
-static inline double MIN(double a, double b)
-{
- return a < b ? a : b;
-}
+#include <algorithm>
-/** Maximum between two doubles */
-static inline double MAX(double a, double b)
+namespace math_utils
{
- return a > b ? a : b;
+
+ /** Clamp value a between b and c */
+ template<typename T>
+ static inline const T& clamp(const T &a, const T &b, const T &c)
+ {
+ return std::min<T>(std::max<T>(b, a), c);
+ }
}
-/** Clamp value a between b and c */
-static inline double CLAMP(double a, double b, double c)
-{
- return MIN(MAX(b, a), c);
-}
-
#endif
Modified: pkg/trunk/util/math_utils/test/math_utils_unittest.cpp
===================================================================
--- pkg/trunk/util/math_utils/test/math_utils_unittest.cpp 2008-08-04 23:06:06 UTC (rev 2521)
+++ pkg/trunk/util/math_utils/test/math_utils_unittest.cpp 2008-08-04 23:34:10 UTC (rev 2522)
@@ -37,17 +37,11 @@
}
TEST(math_utils, BasicOperations){
- EXPECT_EQ(MIN(10, 20), 10);
- EXPECT_EQ(MAX(10, 20), 20);
- EXPECT_EQ(CLAMP(-10, 10, 20), 10);
- EXPECT_EQ(CLAMP(15, 10, 20), 15);
- EXPECT_EQ(CLAMP(25, 10, 20), 20);
+ EXPECT_EQ(math_utils::clamp<int>(-10, 10, 20), 10);
+ EXPECT_EQ(math_utils::clamp<int>(15, 10, 20), 15);
+ EXPECT_EQ(math_utils::clamp<int>(25, 10, 20), 20);
}
-TEST(math_utils, BoundaryCases){
- EXPECT_EQ(MAX(10, 10), 10);
-}
-
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.
|
