From 9357e884d5cd93ca12578e244fc218b0745253b3 Mon Sep 17 00:00:00 2001 From: duongtd Date: Mon, 3 Aug 2026 22:38:42 +0700 Subject: [PATCH] fix file cmake --- CMakeLists.txt | 42 ++-- cmake/anglesConfig.cmake.in | 4 +- doc.dox | 8 +- include/robot_angles_prv/angles.h | 21 ++ package.xml | 6 +- setup.py | 2 +- src/robot_angles_prv/__init__.py | 1 + test/CMakeLists.txt | 4 - test/utest.cpp | 323 ------------------------------ test/utest.py | 308 ---------------------------- 10 files changed, 51 insertions(+), 668 deletions(-) create mode 100644 include/robot_angles_prv/angles.h create mode 100644 src/robot_angles_prv/__init__.py delete mode 100644 test/CMakeLists.txt delete mode 100644 test/utest.cpp delete mode 100644 test/utest.py diff --git a/CMakeLists.txt b/CMakeLists.txt index a3217b2..db21901 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -1,5 +1,5 @@ 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) @@ -8,7 +8,7 @@ option(BUILD_WITH_CATKIN "Build with ROS catkin" ON) # ========================= 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) @@ -21,15 +21,11 @@ if(BUILD_WITH_CATKIN AND DEFINED CATKIN_DEVEL_PREFIX) ${catkin_INCLUDE_DIRS} ) - install(DIRECTORY include/${PROJECT_NAME}/ + install(DIRECTORY include/ DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION} FILES_MATCHING PATTERN "*.h" ) - if(CATKIN_ENABLE_TESTING) - add_subdirectory(test) - endif() - catkin_python_setup() # ========================= @@ -37,48 +33,48 @@ if(BUILD_WITH_CATKIN AND DEFINED CATKIN_DEVEL_PREFIX) # ========================= 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 $ $ ) include(CMakePackageConfigHelpers) - install(TARGETS robot_angles - EXPORT anglesTargets + install(TARGETS robot_angles_prv + EXPORT robot_angles_prvTargets ) - install(DIRECTORY include/${PROJECT_NAME}/ + install(DIRECTORY include/ DESTINATION include FILES_MATCHING PATTERN "*.h" ) - install(EXPORT anglesTargets - FILE anglesTargets.cmake - NAMESPACE robot_angles:: - DESTINATION lib/cmake/robot_angles + install(EXPORT robot_angles_prvTargets + FILE robot_angles_prvTargets.cmake + NAMESPACE robot_angles_prv:: + DESTINATION lib/cmake/robot_angles_prv ) write_basic_package_version_file( - anglesConfigVersion.cmake + robot_angles_prvConfigVersion.cmake VERSION ${PROJECT_VERSION} COMPATIBILITY AnyNewerVersion ) configure_package_config_file( cmake/anglesConfig.cmake.in - ${CMAKE_CURRENT_BINARY_DIR}/anglesConfig.cmake - INSTALL_DESTINATION lib/cmake/robot_angles + ${CMAKE_CURRENT_BINARY_DIR}/robot_angles_prvConfig.cmake + INSTALL_DESTINATION lib/cmake/robot_angles_prv ) install(FILES - ${CMAKE_CURRENT_BINARY_DIR}/anglesConfig.cmake - ${CMAKE_CURRENT_BINARY_DIR}/anglesConfigVersion.cmake - DESTINATION lib/cmake/robot_angles + ${CMAKE_CURRENT_BINARY_DIR}/robot_angles_prvConfig.cmake + ${CMAKE_CURRENT_BINARY_DIR}/robot_angles_prvConfigVersion.cmake + DESTINATION lib/cmake/robot_angles_prv ) endif() diff --git a/cmake/anglesConfig.cmake.in b/cmake/anglesConfig.cmake.in index 4ad3089..7d376ab 100644 --- a/cmake/anglesConfig.cmake.in +++ b/cmake/anglesConfig.cmake.in @@ -1,5 +1,5 @@ @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) diff --git a/doc.dox b/doc.dox index c5c043b..30395fd 100644 --- a/doc.dox +++ b/doc.dox @@ -4,8 +4,8 @@ @htmlinclude manifest.html The Angles contains the following methods: - \li Angular conversions: angles::from_degrees, angles::to_degrees - \li Angular manipulations: angles::normalize_angle_positive, angles::normalize_angle - \li Angular distance: angles::shortest_angular_distance, angles::shortest_angular_distance_with_limits - \li Angular tools: angles::find_min_max_delta, angles::two_pi_complement + \li Angular conversions: robot_angles_prv::from_degrees, robot_angles_prv::to_degrees + \li Angular manipulations: robot_angles_prv::normalize_angle_positive, robot_angles_prv::normalize_angle + \li Angular distance: robot_angles_prv::shortest_angular_distance, robot_angles_prv::shortest_angular_distance_with_limits + \li Angular tools: robot_angles_prv::find_min_max_delta, robot_angles_prv::two_pi_complement **/ diff --git a/include/robot_angles_prv/angles.h b/include/robot_angles_prv/angles.h new file mode 100644 index 0000000..3c68cf2 --- /dev/null +++ b/include/robot_angles_prv/angles.h @@ -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 + +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 diff --git a/package.xml b/package.xml index c768257..c556a85 100644 --- a/package.xml +++ b/package.xml @@ -3,10 +3,10 @@ href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?> - robot_angles + robot_angles_prv 1.9.14 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 radians. But even if you're trying to calculate things like the shortest angular distance between two joint space @@ -19,7 +19,7 @@ Geoffrey Biggs BSD - http://wiki.ros.org/robot_angles + http://wiki.ros.org/robot_angles_prv catkin python-setuptools diff --git a/setup.py b/setup.py index 7e42a3a..cc58022 100644 --- a/setup.py +++ b/setup.py @@ -4,7 +4,7 @@ from setuptools import setup from catkin_pkg.python_setup import generate_distutils_setup package_info = generate_distutils_setup( - packages=['angles'], + packages=['robot_angles_prv'], package_dir={'': 'src'} ) diff --git a/src/robot_angles_prv/__init__.py b/src/robot_angles_prv/__init__.py new file mode 100644 index 0000000..72e9aef --- /dev/null +++ b/src/robot_angles_prv/__init__.py @@ -0,0 +1 @@ +"""Private T800 angular utility package.""" diff --git a/test/CMakeLists.txt b/test/CMakeLists.txt deleted file mode 100644 index 0aabf54..0000000 --- a/test/CMakeLists.txt +++ /dev/null @@ -1,4 +0,0 @@ -if (CATKIN_ENABLE_TESTING) - catkin_add_gtest(utest utest.cpp) - catkin_add_nosetests(utest.py) -endif (CATKIN_ENABLE_TESTING) diff --git a/test/utest.cpp b/test/utest.cpp deleted file mode 100644 index 454fd85..0000000 --- a/test/utest.cpp +++ /dev/null @@ -1,323 +0,0 @@ -#include "angles/angles.h" -#include - -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(); -} diff --git a/test/utest.py b/test/utest.py deleted file mode 100644 index 13b6a41..0000000 --- a/test/utest.py +++ /dev/null @@ -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)