fix file cmake
This commit is contained in:
@@ -1,5 +1,5 @@
|
|||||||
cmake_minimum_required(VERSION 3.10)
|
cmake_minimum_required(VERSION 3.10)
|
||||||
project(robot_angles VERSION 1.0.0 LANGUAGES CXX)
|
project(robot_angles_prv VERSION 1.0.0 LANGUAGES CXX)
|
||||||
|
|
||||||
option(BUILD_WITH_CATKIN "Build with ROS catkin" ON)
|
option(BUILD_WITH_CATKIN "Build with ROS catkin" ON)
|
||||||
|
|
||||||
@@ -8,7 +8,7 @@ option(BUILD_WITH_CATKIN "Build with ROS catkin" ON)
|
|||||||
# =========================
|
# =========================
|
||||||
if(BUILD_WITH_CATKIN AND DEFINED CATKIN_DEVEL_PREFIX)
|
if(BUILD_WITH_CATKIN AND DEFINED CATKIN_DEVEL_PREFIX)
|
||||||
|
|
||||||
message(STATUS "Building robot_angles in CATKIN mode")
|
message(STATUS "Building robot_angles_prv in CATKIN mode")
|
||||||
|
|
||||||
find_package(catkin REQUIRED)
|
find_package(catkin REQUIRED)
|
||||||
|
|
||||||
@@ -21,15 +21,11 @@ if(BUILD_WITH_CATKIN AND DEFINED CATKIN_DEVEL_PREFIX)
|
|||||||
${catkin_INCLUDE_DIRS}
|
${catkin_INCLUDE_DIRS}
|
||||||
)
|
)
|
||||||
|
|
||||||
install(DIRECTORY include/${PROJECT_NAME}/
|
install(DIRECTORY include/
|
||||||
DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION}
|
DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION}
|
||||||
FILES_MATCHING PATTERN "*.h"
|
FILES_MATCHING PATTERN "*.h"
|
||||||
)
|
)
|
||||||
|
|
||||||
if(CATKIN_ENABLE_TESTING)
|
|
||||||
add_subdirectory(test)
|
|
||||||
endif()
|
|
||||||
|
|
||||||
catkin_python_setup()
|
catkin_python_setup()
|
||||||
|
|
||||||
# =========================
|
# =========================
|
||||||
@@ -37,48 +33,48 @@ if(BUILD_WITH_CATKIN AND DEFINED CATKIN_DEVEL_PREFIX)
|
|||||||
# =========================
|
# =========================
|
||||||
else()
|
else()
|
||||||
|
|
||||||
message(STATUS "Building robot_angles in PURE CMAKE mode")
|
message(STATUS "Building robot_angles_prv in PURE CMAKE mode")
|
||||||
|
|
||||||
add_library(robot_angles INTERFACE)
|
add_library(robot_angles_prv INTERFACE)
|
||||||
|
|
||||||
target_include_directories(robot_angles INTERFACE
|
target_include_directories(robot_angles_prv INTERFACE
|
||||||
$<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>
|
$<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>
|
||||||
$<INSTALL_INTERFACE:include>
|
$<INSTALL_INTERFACE:include>
|
||||||
)
|
)
|
||||||
|
|
||||||
include(CMakePackageConfigHelpers)
|
include(CMakePackageConfigHelpers)
|
||||||
|
|
||||||
install(TARGETS robot_angles
|
install(TARGETS robot_angles_prv
|
||||||
EXPORT anglesTargets
|
EXPORT robot_angles_prvTargets
|
||||||
)
|
)
|
||||||
|
|
||||||
install(DIRECTORY include/${PROJECT_NAME}/
|
install(DIRECTORY include/
|
||||||
DESTINATION include
|
DESTINATION include
|
||||||
FILES_MATCHING PATTERN "*.h"
|
FILES_MATCHING PATTERN "*.h"
|
||||||
)
|
)
|
||||||
|
|
||||||
install(EXPORT anglesTargets
|
install(EXPORT robot_angles_prvTargets
|
||||||
FILE anglesTargets.cmake
|
FILE robot_angles_prvTargets.cmake
|
||||||
NAMESPACE robot_angles::
|
NAMESPACE robot_angles_prv::
|
||||||
DESTINATION lib/cmake/robot_angles
|
DESTINATION lib/cmake/robot_angles_prv
|
||||||
)
|
)
|
||||||
|
|
||||||
write_basic_package_version_file(
|
write_basic_package_version_file(
|
||||||
anglesConfigVersion.cmake
|
robot_angles_prvConfigVersion.cmake
|
||||||
VERSION ${PROJECT_VERSION}
|
VERSION ${PROJECT_VERSION}
|
||||||
COMPATIBILITY AnyNewerVersion
|
COMPATIBILITY AnyNewerVersion
|
||||||
)
|
)
|
||||||
|
|
||||||
configure_package_config_file(
|
configure_package_config_file(
|
||||||
cmake/anglesConfig.cmake.in
|
cmake/anglesConfig.cmake.in
|
||||||
${CMAKE_CURRENT_BINARY_DIR}/anglesConfig.cmake
|
${CMAKE_CURRENT_BINARY_DIR}/robot_angles_prvConfig.cmake
|
||||||
INSTALL_DESTINATION lib/cmake/robot_angles
|
INSTALL_DESTINATION lib/cmake/robot_angles_prv
|
||||||
)
|
)
|
||||||
|
|
||||||
install(FILES
|
install(FILES
|
||||||
${CMAKE_CURRENT_BINARY_DIR}/anglesConfig.cmake
|
${CMAKE_CURRENT_BINARY_DIR}/robot_angles_prvConfig.cmake
|
||||||
${CMAKE_CURRENT_BINARY_DIR}/anglesConfigVersion.cmake
|
${CMAKE_CURRENT_BINARY_DIR}/robot_angles_prvConfigVersion.cmake
|
||||||
DESTINATION lib/cmake/robot_angles
|
DESTINATION lib/cmake/robot_angles_prv
|
||||||
)
|
)
|
||||||
|
|
||||||
endif()
|
endif()
|
||||||
|
|||||||
@@ -1,5 +1,5 @@
|
|||||||
@PACKAGE_INIT@
|
@PACKAGE_INIT@
|
||||||
|
|
||||||
include("${CMAKE_CURRENT_LIST_DIR}/anglesTargets.cmake")
|
include("${CMAKE_CURRENT_LIST_DIR}/robot_angles_prvTargets.cmake")
|
||||||
|
|
||||||
check_required_components(robot_angles)
|
check_required_components(robot_angles_prv)
|
||||||
|
|||||||
8
doc.dox
8
doc.dox
@@ -4,8 +4,8 @@
|
|||||||
@htmlinclude manifest.html
|
@htmlinclude manifest.html
|
||||||
|
|
||||||
The Angles contains the following methods:
|
The Angles contains the following methods:
|
||||||
\li Angular conversions: angles::from_degrees, angles::to_degrees
|
\li Angular conversions: robot_angles_prv::from_degrees, robot_angles_prv::to_degrees
|
||||||
\li Angular manipulations: angles::normalize_angle_positive, angles::normalize_angle
|
\li Angular manipulations: robot_angles_prv::normalize_angle_positive, robot_angles_prv::normalize_angle
|
||||||
\li Angular distance: angles::shortest_angular_distance, angles::shortest_angular_distance_with_limits
|
\li Angular distance: robot_angles_prv::shortest_angular_distance, robot_angles_prv::shortest_angular_distance_with_limits
|
||||||
\li Angular tools: angles::find_min_max_delta, angles::two_pi_complement
|
\li Angular tools: robot_angles_prv::find_min_max_delta, robot_angles_prv::two_pi_complement
|
||||||
**/
|
**/
|
||||||
|
|||||||
21
include/robot_angles_prv/angles.h
Normal file
21
include/robot_angles_prv/angles.h
Normal file
@@ -0,0 +1,21 @@
|
|||||||
|
#ifndef ROBOT_ANGLES_PRV_ANGLES_H
|
||||||
|
#define ROBOT_ANGLES_PRV_ANGLES_H
|
||||||
|
|
||||||
|
// Keep the tested implementation in one place while exposing the private
|
||||||
|
// package name used by T800 planners.
|
||||||
|
#include <robot_angles/angles.h>
|
||||||
|
|
||||||
|
namespace robot_angles_prv
|
||||||
|
{
|
||||||
|
using robot_angles::find_min_max_delta;
|
||||||
|
using robot_angles::from_degrees;
|
||||||
|
using robot_angles::normalize_angle;
|
||||||
|
using robot_angles::normalize_angle_positive;
|
||||||
|
using robot_angles::shortest_angular_distance;
|
||||||
|
using robot_angles::shortest_angular_distance_with_large_limits;
|
||||||
|
using robot_angles::shortest_angular_distance_with_limits;
|
||||||
|
using robot_angles::to_degrees;
|
||||||
|
using robot_angles::two_pi_complement;
|
||||||
|
} // namespace robot_angles_prv
|
||||||
|
|
||||||
|
#endif // ROBOT_ANGLES_PRV_ANGLES_H
|
||||||
@@ -3,10 +3,10 @@
|
|||||||
href="http://download.ros.org/schema/package_format3.xsd"
|
href="http://download.ros.org/schema/package_format3.xsd"
|
||||||
schematypens="http://www.w3.org/2001/XMLSchema"?>
|
schematypens="http://www.w3.org/2001/XMLSchema"?>
|
||||||
<package format="3">
|
<package format="3">
|
||||||
<name>robot_angles</name>
|
<name>robot_angles_prv</name>
|
||||||
<version>1.9.14</version>
|
<version>1.9.14</version>
|
||||||
<description>This package provides a set of simple math utilities to work
|
<description>This package provides a set of simple math utilities to work
|
||||||
with robot_angles. The utilities cover simple things like
|
with robot_angles_prv. The utilities cover simple things like
|
||||||
normalizing an angle and conversion between degrees and
|
normalizing an angle and conversion between degrees and
|
||||||
radians. But even if you're trying to calculate things like
|
radians. But even if you're trying to calculate things like
|
||||||
the shortest angular distance between two joint space
|
the shortest angular distance between two joint space
|
||||||
@@ -19,7 +19,7 @@
|
|||||||
<maintainer email="geoff@openrobotics.org">Geoffrey Biggs</maintainer>
|
<maintainer email="geoff@openrobotics.org">Geoffrey Biggs</maintainer>
|
||||||
<license>BSD</license>
|
<license>BSD</license>
|
||||||
|
|
||||||
<url>http://wiki.ros.org/robot_angles</url>
|
<url>http://wiki.ros.org/robot_angles_prv</url>
|
||||||
|
|
||||||
<buildtool_depend version_gte="0.5.68">catkin</buildtool_depend>
|
<buildtool_depend version_gte="0.5.68">catkin</buildtool_depend>
|
||||||
<buildtool_depend condition="$ROS_PYTHON_VERSION == 2">python-setuptools</buildtool_depend>
|
<buildtool_depend condition="$ROS_PYTHON_VERSION == 2">python-setuptools</buildtool_depend>
|
||||||
|
|||||||
2
setup.py
2
setup.py
@@ -4,7 +4,7 @@ from setuptools import setup
|
|||||||
from catkin_pkg.python_setup import generate_distutils_setup
|
from catkin_pkg.python_setup import generate_distutils_setup
|
||||||
|
|
||||||
package_info = generate_distutils_setup(
|
package_info = generate_distutils_setup(
|
||||||
packages=['angles'],
|
packages=['robot_angles_prv'],
|
||||||
package_dir={'': 'src'}
|
package_dir={'': 'src'}
|
||||||
)
|
)
|
||||||
|
|
||||||
|
|||||||
1
src/robot_angles_prv/__init__.py
Normal file
1
src/robot_angles_prv/__init__.py
Normal file
@@ -0,0 +1 @@
|
|||||||
|
"""Private T800 angular utility package."""
|
||||||
@@ -1,4 +0,0 @@
|
|||||||
if (CATKIN_ENABLE_TESTING)
|
|
||||||
catkin_add_gtest(utest utest.cpp)
|
|
||||||
catkin_add_nosetests(utest.py)
|
|
||||||
endif (CATKIN_ENABLE_TESTING)
|
|
||||||
323
test/utest.cpp
323
test/utest.cpp
@@ -1,323 +0,0 @@
|
|||||||
#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);
|
|
||||||
EXPECT_FALSE(result);
|
|
||||||
|
|
||||||
result = angles::shortest_angular_distance_with_limits(-0.5, 0.5,0.25,0.25,shortest_angle);
|
|
||||||
EXPECT_FALSE(result);
|
|
||||||
|
|
||||||
result = angles::shortest_angular_distance_with_limits(-0.5, 0.5,0.25,-0.25,shortest_angle);
|
|
||||||
EXPECT_TRUE(result);
|
|
||||||
EXPECT_NEAR(shortest_angle, -2*M_PI+1.0,1e-6);
|
|
||||||
|
|
||||||
result = angles::shortest_angular_distance_with_limits(0.5, 0.5,0.25,-0.25,shortest_angle);
|
|
||||||
EXPECT_TRUE(result);
|
|
||||||
EXPECT_NEAR(shortest_angle, 0,1e-6);
|
|
||||||
|
|
||||||
result = angles::shortest_angular_distance_with_limits(0.5, 0,0.25,-0.25,shortest_angle);
|
|
||||||
EXPECT_FALSE(result);
|
|
||||||
EXPECT_NEAR(shortest_angle, -0.5,1e-6);
|
|
||||||
|
|
||||||
result = angles::shortest_angular_distance_with_limits(-0.5, 0,0.25,-0.25,shortest_angle);
|
|
||||||
EXPECT_FALSE(result);
|
|
||||||
EXPECT_NEAR(shortest_angle, 0.5,1e-6);
|
|
||||||
|
|
||||||
result = angles::shortest_angular_distance_with_limits(-0.2,0.2,0.25,-0.25,shortest_angle);
|
|
||||||
EXPECT_FALSE(result);
|
|
||||||
EXPECT_NEAR(shortest_angle, -2*M_PI+0.4,1e-6);
|
|
||||||
|
|
||||||
result = angles::shortest_angular_distance_with_limits(0.2,-0.2,0.25,-0.25,shortest_angle);
|
|
||||||
EXPECT_FALSE(result);
|
|
||||||
EXPECT_NEAR(shortest_angle,2*M_PI-0.4,1e-6);
|
|
||||||
|
|
||||||
result = angles::shortest_angular_distance_with_limits(0.2,0,0.25,-0.25,shortest_angle);
|
|
||||||
EXPECT_FALSE(result);
|
|
||||||
EXPECT_NEAR(shortest_angle,2*M_PI-0.2,1e-6);
|
|
||||||
|
|
||||||
result = angles::shortest_angular_distance_with_limits(-0.2,0,0.25,-0.25,shortest_angle);
|
|
||||||
EXPECT_FALSE(result);
|
|
||||||
EXPECT_NEAR(shortest_angle,-2*M_PI+0.2,1e-6);
|
|
||||||
|
|
||||||
result = angles::shortest_angular_distance_with_limits(-0.25,-0.5,0.25,-0.25,shortest_angle);
|
|
||||||
EXPECT_TRUE(result);
|
|
||||||
EXPECT_NEAR(shortest_angle,-0.25,1e-6);
|
|
||||||
|
|
||||||
result = angles::shortest_angular_distance_with_limits(-0.25,0.5,0.25,-0.25,shortest_angle);
|
|
||||||
EXPECT_TRUE(result);
|
|
||||||
EXPECT_NEAR(shortest_angle,-2*M_PI+0.75,1e-6);
|
|
||||||
|
|
||||||
result = angles::shortest_angular_distance_with_limits(-0.2500001,0.5,0.25,-0.25,shortest_angle);
|
|
||||||
EXPECT_TRUE(result);
|
|
||||||
EXPECT_NEAR(shortest_angle,-2*M_PI+0.5+0.2500001,1e-6);
|
|
||||||
|
|
||||||
result = angles::shortest_angular_distance_with_limits(-0.6, 0.5,-0.25,0.25,shortest_angle);
|
|
||||||
EXPECT_FALSE(result);
|
|
||||||
|
|
||||||
result = angles::shortest_angular_distance_with_limits(-0.5, 0.6,-0.25,0.25,shortest_angle);
|
|
||||||
EXPECT_FALSE(result);
|
|
||||||
|
|
||||||
result = angles::shortest_angular_distance_with_limits(-0.6, 0.75,-0.25,0.3,shortest_angle);
|
|
||||||
EXPECT_FALSE(result);
|
|
||||||
|
|
||||||
result = angles::shortest_angular_distance_with_limits(-0.6, M_PI*3.0/4.0,-0.25,0.3,shortest_angle);
|
|
||||||
EXPECT_FALSE(result);
|
|
||||||
|
|
||||||
result = angles::shortest_angular_distance_with_limits(-M_PI, M_PI,-M_PI,M_PI,shortest_angle);
|
|
||||||
EXPECT_TRUE(result);
|
|
||||||
EXPECT_NEAR(shortest_angle,0.0,1e-6);
|
|
||||||
|
|
||||||
}
|
|
||||||
|
|
||||||
|
|
||||||
TEST(Angles, shortestDistanceWithLargeLimits)
|
|
||||||
{
|
|
||||||
double shortest_angle;
|
|
||||||
bool result;
|
|
||||||
|
|
||||||
// 'delta' is valid
|
|
||||||
result = angles::shortest_angular_distance_with_large_limits(0, 10.5*M_PI, -2*M_PI, 2*M_PI, shortest_angle);
|
|
||||||
EXPECT_TRUE(result);
|
|
||||||
EXPECT_NEAR(shortest_angle, 0.5*M_PI, 1e-6);
|
|
||||||
|
|
||||||
// 'delta' is not valid, but 'delta_2pi' is
|
|
||||||
result = angles::shortest_angular_distance_with_large_limits(0, 10.5*M_PI, -2*M_PI, 0.1*M_PI, shortest_angle);
|
|
||||||
EXPECT_TRUE(result);
|
|
||||||
EXPECT_NEAR(shortest_angle, -1.5*M_PI, 1e-6);
|
|
||||||
|
|
||||||
// neither 'delta' nor 'delta_2pi' are valid
|
|
||||||
result = angles::shortest_angular_distance_with_large_limits(2*M_PI, M_PI, 2*M_PI-0.1, 2*M_PI+0.1, shortest_angle);
|
|
||||||
EXPECT_FALSE(result);
|
|
||||||
|
|
||||||
// start position outside limits
|
|
||||||
result = angles::shortest_angular_distance_with_large_limits(10.5*M_PI, 0, -2*M_PI, 2*M_PI, shortest_angle);
|
|
||||||
EXPECT_FALSE(result);
|
|
||||||
|
|
||||||
// invalid limits (lower > upper)
|
|
||||||
result = angles::shortest_angular_distance_with_large_limits(0, 0.1, 2*M_PI, -2*M_PI, shortest_angle);
|
|
||||||
EXPECT_FALSE(result);
|
|
||||||
|
|
||||||
// specific test case
|
|
||||||
result = angles::shortest_angular_distance_with_large_limits(0.999507, 1.0, -20*M_PI, 20*M_PI, shortest_angle);
|
|
||||||
EXPECT_TRUE(result);
|
|
||||||
EXPECT_NEAR(shortest_angle, 0.000493, 1e-6);
|
|
||||||
}
|
|
||||||
|
|
||||||
|
|
||||||
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);
|
|
||||||
|
|
||||||
// Backside wrapping
|
|
||||||
EXPECT_NEAR(-M_PI/2, shortest_angular_distance(-3*M_PI/4, 3*M_PI/4), epsilon);
|
|
||||||
EXPECT_NEAR(M_PI/2, shortest_angular_distance(3*M_PI/4, -3*M_PI/4), epsilon);
|
|
||||||
}
|
|
||||||
|
|
||||||
TEST(Angles, two_pi_complement)
|
|
||||||
{
|
|
||||||
double epsilon = 1e-9;
|
|
||||||
EXPECT_NEAR(two_pi_complement(0), 2*M_PI, epsilon);
|
|
||||||
EXPECT_NEAR(two_pi_complement(2*M_PI), 0, epsilon);
|
|
||||||
EXPECT_NEAR(two_pi_complement(-2*M_PI), 0, epsilon);
|
|
||||||
EXPECT_NEAR(two_pi_complement(2*M_PI-epsilon), -epsilon, epsilon);
|
|
||||||
EXPECT_NEAR(two_pi_complement(-2*M_PI+epsilon), epsilon, epsilon);
|
|
||||||
EXPECT_NEAR(two_pi_complement(M_PI/2), -3*M_PI/2, epsilon);
|
|
||||||
EXPECT_NEAR(two_pi_complement(M_PI), -M_PI, epsilon);
|
|
||||||
EXPECT_NEAR(two_pi_complement(-M_PI), M_PI, epsilon);
|
|
||||||
EXPECT_NEAR(two_pi_complement(-M_PI/2), 3*M_PI/2, epsilon);
|
|
||||||
|
|
||||||
EXPECT_NEAR(two_pi_complement(3*M_PI), -M_PI, epsilon);
|
|
||||||
EXPECT_NEAR(two_pi_complement(-3.0*M_PI), M_PI, epsilon);
|
|
||||||
EXPECT_NEAR(two_pi_complement(-5.0*M_PI/2.0), 3*M_PI/2, epsilon);
|
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
}
|
|
||||||
|
|
||||||
TEST(Angles, find_min_max_delta)
|
|
||||||
{
|
|
||||||
double epsilon = 1e-9;
|
|
||||||
double min_delta, max_delta;
|
|
||||||
// Straight forward full range
|
|
||||||
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);
|
|
||||||
|
|
||||||
// M_PI/2 Full Range
|
|
||||||
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);
|
|
||||||
|
|
||||||
// -M_PI/2 Full range
|
|
||||||
EXPECT_TRUE(find_min_max_delta( -M_PI/2, -M_PI, M_PI, min_delta, max_delta));
|
|
||||||
EXPECT_NEAR(min_delta, -M_PI/2, epsilon);
|
|
||||||
EXPECT_NEAR(max_delta, 3*M_PI/2, epsilon);
|
|
||||||
|
|
||||||
// Straight forward partial range
|
|
||||||
EXPECT_TRUE(find_min_max_delta( 0, -M_PI/2, M_PI/2, min_delta, max_delta));
|
|
||||||
EXPECT_NEAR(min_delta, -M_PI/2, epsilon);
|
|
||||||
EXPECT_NEAR(max_delta, M_PI/2, epsilon);
|
|
||||||
|
|
||||||
// M_PI/4 Partial Range
|
|
||||||
EXPECT_TRUE(find_min_max_delta( M_PI/4, -M_PI/2, M_PI/2, min_delta, max_delta));
|
|
||||||
EXPECT_NEAR(min_delta, -3*M_PI/4, epsilon);
|
|
||||||
EXPECT_NEAR(max_delta, M_PI/4, epsilon);
|
|
||||||
|
|
||||||
// -M_PI/4 Partial Range
|
|
||||||
EXPECT_TRUE(find_min_max_delta( -M_PI/4, -M_PI/2, M_PI/2, min_delta, max_delta));
|
|
||||||
EXPECT_NEAR(min_delta, -M_PI/4, epsilon);
|
|
||||||
EXPECT_NEAR(max_delta, 3*M_PI/4, epsilon);
|
|
||||||
|
|
||||||
// bump stop negative full range
|
|
||||||
EXPECT_TRUE(find_min_max_delta( -M_PI, -M_PI, M_PI, min_delta, max_delta));
|
|
||||||
EXPECT_TRUE((fabs(min_delta) <= epsilon && fabs(max_delta - 2*M_PI) <= epsilon) || (fabs(min_delta+2*M_PI) <= epsilon && fabs(max_delta) <= epsilon));
|
|
||||||
EXPECT_NEAR(min_delta, 0.0, epsilon);
|
|
||||||
EXPECT_NEAR(max_delta, 2*M_PI, epsilon);
|
|
||||||
|
|
||||||
EXPECT_TRUE(find_min_max_delta(-0.25,0.25,-0.25,min_delta, max_delta));
|
|
||||||
EXPECT_NEAR(min_delta, -2*M_PI+0.5, epsilon);
|
|
||||||
EXPECT_NEAR(max_delta, 0.0, epsilon);
|
|
||||||
|
|
||||||
// bump stop positive full range
|
|
||||||
EXPECT_TRUE(find_min_max_delta( M_PI-epsilon, -M_PI, M_PI, min_delta, max_delta));
|
|
||||||
//EXPECT_TRUE((fabs(min_delta) <= epsilon && fabs(max_delta - 2*M_PI) <= epsilon) || (fabs(min_delta+2*M_PI) <= epsilon && fabs(max_delta) <= epsilon));
|
|
||||||
EXPECT_NEAR(min_delta, -2*M_PI+epsilon, epsilon);
|
|
||||||
EXPECT_NEAR(max_delta, epsilon, epsilon);
|
|
||||||
|
|
||||||
// bump stop negative partial range
|
|
||||||
EXPECT_TRUE(find_min_max_delta( -M_PI, -M_PI, M_PI, min_delta, max_delta));
|
|
||||||
EXPECT_NEAR(min_delta, 0, epsilon);
|
|
||||||
EXPECT_NEAR(max_delta, 2*M_PI, epsilon);
|
|
||||||
|
|
||||||
// bump stop positive partial range
|
|
||||||
EXPECT_TRUE(find_min_max_delta( -M_PI/2, -M_PI/2, M_PI/2, min_delta, max_delta));
|
|
||||||
EXPECT_NEAR(min_delta, 0.0, epsilon);
|
|
||||||
EXPECT_NEAR(max_delta, M_PI, epsilon);
|
|
||||||
|
|
||||||
|
|
||||||
//Test out of range negative
|
|
||||||
EXPECT_FALSE(find_min_max_delta( -M_PI, -M_PI/2, M_PI/2, min_delta, max_delta));
|
|
||||||
//Test out of range postive
|
|
||||||
EXPECT_FALSE(find_min_max_delta( M_PI, -M_PI/2, M_PI/2, min_delta, max_delta));
|
|
||||||
|
|
||||||
|
|
||||||
// M_PI/4 Partial Range
|
|
||||||
EXPECT_TRUE(find_min_max_delta( 3*M_PI/4, M_PI/2, -M_PI/2, min_delta, max_delta));
|
|
||||||
EXPECT_NEAR(min_delta, -M_PI/4, epsilon);
|
|
||||||
EXPECT_NEAR(max_delta, 3*M_PI/4, epsilon);
|
|
||||||
|
|
||||||
|
|
||||||
}
|
|
||||||
|
|
||||||
int main(int argc, char **argv){
|
|
||||||
testing::InitGoogleTest(&argc, argv);
|
|
||||||
return RUN_ALL_TESTS();
|
|
||||||
}
|
|
||||||
308
test/utest.py
308
test/utest.py
@@ -1,308 +0,0 @@
|
|||||||
#!/usr/bin/env python
|
|
||||||
#*********************************************************************
|
|
||||||
# Software License Agreement (BSD License)
|
|
||||||
#
|
|
||||||
# Copyright (c) 2015, Bossa Nova Robotics
|
|
||||||
# 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 Bossa Nova 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.
|
|
||||||
#********************************************************************/
|
|
||||||
from angles import normalize_angle_positive, normalize_angle, shortest_angular_distance, two_pi_complement, shortest_angular_distance_with_limits, shortest_angular_distance_with_large_limits
|
|
||||||
from angles import _find_min_max_delta
|
|
||||||
import sys
|
|
||||||
import unittest
|
|
||||||
from math import pi, fabs
|
|
||||||
|
|
||||||
## A sample python unit test
|
|
||||||
class TestAngles(unittest.TestCase):
|
|
||||||
def test_shortestDistanceWithLimits(self):
|
|
||||||
result, shortest_angle = shortest_angular_distance_with_limits(-0.5, 0.5,-0.25,0.25)
|
|
||||||
self.assertFalse(result)
|
|
||||||
|
|
||||||
result, shortest_angle = shortest_angular_distance_with_limits(-0.5, 0.5,0.25,0.25)
|
|
||||||
self.assertFalse(result)
|
|
||||||
|
|
||||||
result, shortest_angle = shortest_angular_distance_with_limits(-0.5, 0.5,0.25,-0.25)
|
|
||||||
self.assertTrue(result)
|
|
||||||
self.assertAlmostEqual(shortest_angle, -2*pi+1.0)
|
|
||||||
|
|
||||||
result, shortest_angle = shortest_angular_distance_with_limits(0.5, 0.5,0.25,-0.25)
|
|
||||||
self.assertTrue(result)
|
|
||||||
self.assertAlmostEqual(shortest_angle, 0)
|
|
||||||
|
|
||||||
result, shortest_angle = shortest_angular_distance_with_limits(0.5, 0,0.25,-0.25)
|
|
||||||
self.assertFalse(result)
|
|
||||||
self.assertAlmostEqual(shortest_angle, -0.5)
|
|
||||||
|
|
||||||
result, shortest_angle = shortest_angular_distance_with_limits(-0.5, 0,0.25,-0.25)
|
|
||||||
self.assertFalse(result)
|
|
||||||
self.assertAlmostEqual(shortest_angle, 0.5)
|
|
||||||
|
|
||||||
result, shortest_angle = shortest_angular_distance_with_limits(-0.2,0.2,0.25,-0.25)
|
|
||||||
self.assertFalse(result)
|
|
||||||
self.assertAlmostEqual(shortest_angle, -2*pi+0.4)
|
|
||||||
|
|
||||||
result, shortest_angle = shortest_angular_distance_with_limits(0.2,-0.2,0.25,-0.25)
|
|
||||||
self.assertFalse(result)
|
|
||||||
self.assertAlmostEqual(shortest_angle,2*pi-0.4)
|
|
||||||
|
|
||||||
result, shortest_angle = shortest_angular_distance_with_limits(0.2,0,0.25,-0.25)
|
|
||||||
self.assertFalse(result)
|
|
||||||
self.assertAlmostEqual(shortest_angle,2*pi-0.2)
|
|
||||||
|
|
||||||
result, shortest_angle = shortest_angular_distance_with_limits(-0.2,0,0.25,-0.25)
|
|
||||||
self.assertFalse(result)
|
|
||||||
self.assertAlmostEqual(shortest_angle,-2*pi+0.2)
|
|
||||||
|
|
||||||
result, shortest_angle = shortest_angular_distance_with_limits(-0.25,-0.5,0.25,-0.25)
|
|
||||||
self.assertTrue(result)
|
|
||||||
self.assertAlmostEqual(shortest_angle,-0.25)
|
|
||||||
|
|
||||||
result, shortest_angle = shortest_angular_distance_with_limits(-0.25,0.5,0.25,-0.25)
|
|
||||||
self.assertTrue(result)
|
|
||||||
self.assertAlmostEqual(shortest_angle,-2*pi+0.75)
|
|
||||||
|
|
||||||
result, shortest_angle = shortest_angular_distance_with_limits(-0.2500001,0.5,0.25,-0.25)
|
|
||||||
self.assertTrue(result)
|
|
||||||
self.assertAlmostEqual(shortest_angle,-2*pi+0.5+0.2500001)
|
|
||||||
|
|
||||||
result, shortest_angle = shortest_angular_distance_with_limits(-0.6, 0.5,-0.25,0.25)
|
|
||||||
self.assertFalse(result)
|
|
||||||
|
|
||||||
result, shortest_angle = shortest_angular_distance_with_limits(-0.5, 0.6,-0.25,0.25)
|
|
||||||
self.assertFalse(result)
|
|
||||||
|
|
||||||
result, shortest_angle = shortest_angular_distance_with_limits(-0.6, 0.75,-0.25,0.3)
|
|
||||||
self.assertFalse(result)
|
|
||||||
|
|
||||||
result, shortest_angle = shortest_angular_distance_with_limits(-0.6, pi*3.0/4.0,-0.25,0.3)
|
|
||||||
self.assertFalse(result)
|
|
||||||
|
|
||||||
result, shortest_angle = shortest_angular_distance_with_limits(-pi, pi,-pi,pi)
|
|
||||||
self.assertTrue(result)
|
|
||||||
self.assertAlmostEqual(shortest_angle,0.0)
|
|
||||||
|
|
||||||
def test_shortestDistanceWithLargeLimits(self):
|
|
||||||
# 'delta' is valid
|
|
||||||
result, shortest_angle = shortest_angular_distance_with_large_limits(0, 10.5*pi, -2*pi, 2*pi)
|
|
||||||
self.assertTrue(result)
|
|
||||||
self.assertAlmostEqual(shortest_angle, 0.5*pi)
|
|
||||||
|
|
||||||
# 'delta' is not valid, but 'delta_2pi' is
|
|
||||||
result, shortest_angle = shortest_angular_distance_with_large_limits(0, 10.5*pi, -2*pi, 0.1*pi)
|
|
||||||
self.assertTrue(result)
|
|
||||||
self.assertAlmostEqual(shortest_angle, -1.5*pi)
|
|
||||||
|
|
||||||
# neither 'delta' nor 'delta_2pi' are valid
|
|
||||||
result, shortest_angle = shortest_angular_distance_with_large_limits(2*pi, pi, 2*pi-0.1, 2*pi+0.1)
|
|
||||||
self.assertFalse(result)
|
|
||||||
|
|
||||||
# start position outside limits
|
|
||||||
result, shortest_angle = shortest_angular_distance_with_large_limits(10.5*pi, 0, -2*pi, 2*pi)
|
|
||||||
self.assertFalse(result)
|
|
||||||
|
|
||||||
# invalid limits (lower > upper)
|
|
||||||
result, shortest_angle = shortest_angular_distance_with_large_limits(0, 0.1, 2*pi, -2*pi)
|
|
||||||
self.assertFalse(result)
|
|
||||||
|
|
||||||
# specific test case
|
|
||||||
result, shortest_angle = shortest_angular_distance_with_large_limits(0.999507, 1.0, -20*pi, 20*pi)
|
|
||||||
self.assertTrue(result)
|
|
||||||
self.assertAlmostEqual(shortest_angle, 0.000493)
|
|
||||||
|
|
||||||
def test_normalize_angle_positive(self):
|
|
||||||
self.assertAlmostEqual(0, normalize_angle_positive(0))
|
|
||||||
self.assertAlmostEqual(pi, normalize_angle_positive(pi))
|
|
||||||
self.assertAlmostEqual(0, normalize_angle_positive(2*pi))
|
|
||||||
self.assertAlmostEqual(pi, normalize_angle_positive(3*pi))
|
|
||||||
self.assertAlmostEqual(0, normalize_angle_positive(4*pi))
|
|
||||||
|
|
||||||
self.assertAlmostEqual(0, normalize_angle_positive(-0))
|
|
||||||
self.assertAlmostEqual(pi, normalize_angle_positive(-pi))
|
|
||||||
self.assertAlmostEqual(0, normalize_angle_positive(-2*pi))
|
|
||||||
self.assertAlmostEqual(pi, normalize_angle_positive(-3*pi))
|
|
||||||
self.assertAlmostEqual(0, normalize_angle_positive(-4*pi))
|
|
||||||
|
|
||||||
self.assertAlmostEqual(0, normalize_angle_positive(-0))
|
|
||||||
self.assertAlmostEqual(3*pi/2, normalize_angle_positive(-pi/2))
|
|
||||||
self.assertAlmostEqual(pi, normalize_angle_positive(-pi))
|
|
||||||
self.assertAlmostEqual(pi/2, normalize_angle_positive(-3*pi/2))
|
|
||||||
self.assertAlmostEqual(0, normalize_angle_positive(-4*pi/2))
|
|
||||||
|
|
||||||
self.assertAlmostEqual(0, normalize_angle_positive(0))
|
|
||||||
self.assertAlmostEqual(pi/2, normalize_angle_positive(pi/2))
|
|
||||||
self.assertAlmostEqual(pi/2, normalize_angle_positive(5*pi/2))
|
|
||||||
self.assertAlmostEqual(pi/2, normalize_angle_positive(9*pi/2))
|
|
||||||
self.assertAlmostEqual(pi/2, normalize_angle_positive(-3*pi/2))
|
|
||||||
|
|
||||||
def test_normalize_angle(self):
|
|
||||||
self.assertAlmostEqual(0, normalize_angle(0))
|
|
||||||
self.assertAlmostEqual(pi, normalize_angle(pi))
|
|
||||||
self.assertAlmostEqual(0, normalize_angle(2*pi))
|
|
||||||
self.assertAlmostEqual(pi, normalize_angle(3*pi))
|
|
||||||
self.assertAlmostEqual(0, normalize_angle(4*pi))
|
|
||||||
|
|
||||||
self.assertAlmostEqual(0, normalize_angle(-0))
|
|
||||||
self.assertAlmostEqual(pi, normalize_angle(-pi))
|
|
||||||
self.assertAlmostEqual(0, normalize_angle(-2*pi))
|
|
||||||
self.assertAlmostEqual(pi, normalize_angle(-3*pi))
|
|
||||||
self.assertAlmostEqual(0, normalize_angle(-4*pi))
|
|
||||||
|
|
||||||
self.assertAlmostEqual(0, normalize_angle(-0))
|
|
||||||
self.assertAlmostEqual(-pi/2, normalize_angle(-pi/2))
|
|
||||||
self.assertAlmostEqual(pi, normalize_angle(-pi))
|
|
||||||
self.assertAlmostEqual(pi/2, normalize_angle(-3*pi/2))
|
|
||||||
self.assertAlmostEqual(0, normalize_angle(-4*pi/2))
|
|
||||||
|
|
||||||
self.assertAlmostEqual(0, normalize_angle(0))
|
|
||||||
self.assertAlmostEqual(pi/2, normalize_angle(pi/2))
|
|
||||||
self.assertAlmostEqual(pi/2, normalize_angle(5*pi/2))
|
|
||||||
self.assertAlmostEqual(pi/2, normalize_angle(9*pi/2))
|
|
||||||
self.assertAlmostEqual(pi/2, normalize_angle(-3*pi/2))
|
|
||||||
|
|
||||||
def test_shortest_angular_distance(self):
|
|
||||||
self.assertAlmostEqual(pi/2, shortest_angular_distance(0, pi/2))
|
|
||||||
self.assertAlmostEqual(-pi/2, shortest_angular_distance(0, -pi/2))
|
|
||||||
self.assertAlmostEqual(-pi/2, shortest_angular_distance(pi/2, 0))
|
|
||||||
self.assertAlmostEqual(pi/2, shortest_angular_distance(-pi/2, 0))
|
|
||||||
|
|
||||||
self.assertAlmostEqual(-pi/2, shortest_angular_distance(pi, pi/2))
|
|
||||||
self.assertAlmostEqual(pi/2, shortest_angular_distance(pi, -pi/2))
|
|
||||||
self.assertAlmostEqual(pi/2, shortest_angular_distance(pi/2, pi))
|
|
||||||
self.assertAlmostEqual(-pi/2, shortest_angular_distance(-pi/2, pi))
|
|
||||||
|
|
||||||
self.assertAlmostEqual(-pi/2, shortest_angular_distance(5*pi, pi/2))
|
|
||||||
self.assertAlmostEqual(pi/2, shortest_angular_distance(7*pi, -pi/2))
|
|
||||||
self.assertAlmostEqual(pi/2, shortest_angular_distance(9*pi/2, pi))
|
|
||||||
self.assertAlmostEqual(pi/2, shortest_angular_distance(-3*pi/2, pi))
|
|
||||||
|
|
||||||
# Backside wrapping
|
|
||||||
self.assertAlmostEqual(-pi/2, shortest_angular_distance(-3*pi/4, 3*pi/4))
|
|
||||||
self.assertAlmostEqual(pi/2, shortest_angular_distance(3*pi/4, -3*pi/4))
|
|
||||||
|
|
||||||
def test_two_pi_complement(self):
|
|
||||||
epsilon = 1e-9
|
|
||||||
self.assertAlmostEqual(two_pi_complement(0), 2*pi)
|
|
||||||
self.assertAlmostEqual(two_pi_complement(2*pi), 0)
|
|
||||||
self.assertAlmostEqual(two_pi_complement(-2*pi), 0)
|
|
||||||
self.assertAlmostEqual(two_pi_complement(2*pi-epsilon), -epsilon)
|
|
||||||
self.assertAlmostEqual(two_pi_complement(-2*pi+epsilon), epsilon)
|
|
||||||
self.assertAlmostEqual(two_pi_complement(pi/2), -3*pi/2)
|
|
||||||
self.assertAlmostEqual(two_pi_complement(pi), -pi)
|
|
||||||
self.assertAlmostEqual(two_pi_complement(-pi), pi)
|
|
||||||
self.assertAlmostEqual(two_pi_complement(-pi/2), 3*pi/2)
|
|
||||||
|
|
||||||
self.assertAlmostEqual(two_pi_complement(3*pi), -pi)
|
|
||||||
self.assertAlmostEqual(two_pi_complement(-3.0*pi), pi)
|
|
||||||
self.assertAlmostEqual(two_pi_complement(-5.0*pi/2.0), 3*pi/2)
|
|
||||||
|
|
||||||
def test_find_min_max_delta(self):
|
|
||||||
epsilon = 1e-9
|
|
||||||
# Straight forward full range
|
|
||||||
flag, min_delta, max_delta = _find_min_max_delta( 0, -pi, pi)
|
|
||||||
self.assertTrue(flag)
|
|
||||||
self.assertAlmostEqual(min_delta, -pi)
|
|
||||||
self.assertAlmostEqual(max_delta, pi)
|
|
||||||
|
|
||||||
# pi/2 Full Range
|
|
||||||
flag, min_delta, max_delta = _find_min_max_delta( pi/2, -pi, pi)
|
|
||||||
self.assertTrue(flag)
|
|
||||||
self.assertAlmostEqual(min_delta, -3*pi/2)
|
|
||||||
self.assertAlmostEqual(max_delta, pi/2)
|
|
||||||
|
|
||||||
# -pi/2 Full range
|
|
||||||
flag, min_delta, max_delta = _find_min_max_delta( -pi/2, -pi, pi)
|
|
||||||
self.assertTrue(flag)
|
|
||||||
self.assertAlmostEqual(min_delta, -pi/2)
|
|
||||||
self.assertAlmostEqual(max_delta, 3*pi/2)
|
|
||||||
|
|
||||||
# Straight forward partial range
|
|
||||||
flag, min_delta, max_delta = _find_min_max_delta( 0, -pi/2, pi/2)
|
|
||||||
self.assertTrue(flag)
|
|
||||||
self.assertAlmostEqual(min_delta, -pi/2)
|
|
||||||
self.assertAlmostEqual(max_delta, pi/2)
|
|
||||||
|
|
||||||
# pi/4 Partial Range
|
|
||||||
flag, min_delta, max_delta = _find_min_max_delta( pi/4, -pi/2, pi/2)
|
|
||||||
self.assertTrue(flag)
|
|
||||||
self.assertAlmostEqual(min_delta, -3*pi/4)
|
|
||||||
self.assertAlmostEqual(max_delta, pi/4)
|
|
||||||
|
|
||||||
# -pi/4 Partial Range
|
|
||||||
flag, min_delta, max_delta = _find_min_max_delta( -pi/4, -pi/2, pi/2)
|
|
||||||
self.assertTrue(flag)
|
|
||||||
self.assertAlmostEqual(min_delta, -pi/4)
|
|
||||||
self.assertAlmostEqual(max_delta, 3*pi/4)
|
|
||||||
|
|
||||||
# bump stop negative full range
|
|
||||||
flag, min_delta, max_delta = _find_min_max_delta( -pi, -pi, pi)
|
|
||||||
self.assertTrue(flag)
|
|
||||||
self.assertTrue((fabs(min_delta) <= epsilon and fabs(max_delta - 2*pi) <= epsilon) or (fabs(min_delta+2*pi) <= epsilon and fabs(max_delta) <= epsilon))
|
|
||||||
self.assertAlmostEqual(min_delta, 0.0)
|
|
||||||
self.assertAlmostEqual(max_delta, 2*pi)
|
|
||||||
|
|
||||||
flag, min_delta, max_delta = _find_min_max_delta(-0.25,0.25,-0.25)
|
|
||||||
self.assertTrue(flag)
|
|
||||||
self.assertAlmostEqual(min_delta, -2*pi+0.5)
|
|
||||||
self.assertAlmostEqual(max_delta, 0.0)
|
|
||||||
|
|
||||||
# bump stop positive full range
|
|
||||||
flag, min_delta, max_delta = _find_min_max_delta( pi-epsilon, -pi, pi)
|
|
||||||
self.assertTrue(flag)
|
|
||||||
#self.assertTrue((fabs(min_delta) <= epsilon and fabs(max_delta - 2*pi) <= epsilon) or (fabs(min_delta+2*pi) <= epsilon and fabs(max_delta) <= epsilon))
|
|
||||||
self.assertAlmostEqual(min_delta, -2*pi+epsilon)
|
|
||||||
self.assertAlmostEqual(max_delta, epsilon)
|
|
||||||
|
|
||||||
# bump stop negative partial range
|
|
||||||
flag, min_delta, max_delta = _find_min_max_delta( -pi, -pi, pi)
|
|
||||||
self.assertTrue(flag)
|
|
||||||
self.assertAlmostEqual(min_delta, 0)
|
|
||||||
self.assertAlmostEqual(max_delta, 2*pi)
|
|
||||||
|
|
||||||
# bump stop positive partial range
|
|
||||||
flag, min_delta, max_delta = _find_min_max_delta( -pi/2, -pi/2, pi/2)
|
|
||||||
self.assertTrue(flag)
|
|
||||||
self.assertAlmostEqual(min_delta, 0.0)
|
|
||||||
self.assertAlmostEqual(max_delta, pi)
|
|
||||||
|
|
||||||
#Test out of range negative
|
|
||||||
flag, min_delta, max_delta = _find_min_max_delta( -pi, -pi/2, pi/2)
|
|
||||||
self.assertFalse(flag)
|
|
||||||
#Test out of range postive
|
|
||||||
flag, min_delta, max_delta = _find_min_max_delta( pi, -pi/2, pi/2)
|
|
||||||
self.assertFalse(flag)
|
|
||||||
|
|
||||||
# pi/4 Partial Range
|
|
||||||
flag, min_delta, max_delta = _find_min_max_delta( 3*pi/4, pi/2, -pi/2)
|
|
||||||
self.assertTrue(flag)
|
|
||||||
self.assertAlmostEqual(min_delta, -pi/4)
|
|
||||||
self.assertAlmostEqual(max_delta, 3*pi/4)
|
|
||||||
|
|
||||||
if __name__ == '__main__':
|
|
||||||
import rosunit
|
|
||||||
rosunit.unitrun('angles', 'test_python_angles', TestAngles)
|
|
||||||
Reference in New Issue
Block a user