[JavaScript知识库]ROS angles包说明

在看move_base源码时，有几处用到了 angles::shortest_angular_distance以及angles::normalize_angle，就查了下angle这个类，发现是ros中的一个名称为angles的软件包，就读了下源码。

一.函数功能说明

功能说明：该软件包提供了一组简单的数学实用程序来处理角度。
该实用程序涵盖了简单的事情，例如标准化角度以及度和弧度之间的转换
角度包含以下方法：
角度转换：angles :: from_degrees，angles :: to_degrees 　标准化角度以及度和弧度之间的转换
角度操作：angles :: normalize_angle_positive，angles :: normalize_angle
角距：angles :: shortest_angular_distance，angles :: shortest_angular_distance_with_limits
角度工具：angles :: find_min_max_delta，angles :: two_pi_complement

程序说明，

1.from_degrees-角度转弧度

// 角度转弧度  Convert degrees to radians
static inline double from_degrees(double degrees) {
  return degrees * M_PI / 180.0;
}

2.to_degrees-弧度转角度

// 弧度转角度
static inline double to_degrees(double radians) {
  return radians * 180.0 / M_PI;
}

3.normalize_angle_positive-转换为[0,2PI]

// 转换为[0,2PI] 　单位:弧度制
static inline double normalize_angle_positive(double angle) {
  // fmod(x,y)为C 标准库 - <math.h>函数,返回Ｘ/Y的余数
  const double result = fmod(angle, 2.0 * M_PI);
  if (result < 0)
    return result + 2.0 * M_PI;
  return result;
}

3.normalize_angle-转换为[-PI,PI]

// 转换为[-PI,PI] 　单位:弧度制
static inline double normalize_angle(double angle) {
  const double result = fmod(angle + M_PI, 2.0 * M_PI);
  if (result <= 0.0)
    return result + M_PI;
  return result - M_PI;
}

4.shortest_angular_distance-两角最小距离

/*!
 * \function
 * \brief shortest_angular_distance
 *
 * Given 2 angles, this returns the shortest angular
 * difference.  The inputs and ouputs are of course radians.
 *
 * The result
 * would always be -pi <= result <= pi.  Adding the result
 * to "from" will always get you an equivelent angle to "to".
 */
/// 两角最小距离
static inline double shortest_angular_distance(double from, double to) {
  return normalize_angle(to - from);
}

5.two_pi_complement-返回沿单位圆反向的角度

/*!
 * \function
 *
 * \brief returns the angle in [-2*M_PI, 2*M_PI]  going the other way along the
 * unit circle. \param angle The angle to which you want to turn in the range
 * [-2*M_PI, 2*M_PI] E.g. two_pi_complement(-M_PI/4) returns 7_M_PI/4
 * two_pi_complement(M_PI/4) returns -7*M_PI/4
 *
 */
// 返回沿单位圆反向的角度[-2*M PI, 2*M PI]
static inline double two_pi_complement(double angle) {
  // check input conditions
  if (angle > 2 * M_PI || angle < -2.0 * M_PI)
    angle = fmod(angle, 2.0 * M_PI);
  if (angle < 0)
    return (2 * M_PI + angle);
  else if (angle > 0)
    return (-2 * M_PI + angle);

  return (2 * M_PI);
}

6.shortest_angular_distance_with_large_limits与shortest_angular_distance_with_limits

这两个主要是用来返回限定角度范围内的最小角间距
不太常用

#ifndef GEOMETRY_ANGLES_UTILS_H
#define GEOMETRY_ANGLES_UTILS_H

#include <algorithm>
#include <cmath>

namespace angles {

/*!
 * \function
 *
 * \brief This function is only intended for internal use and not intended for
 * external use. If you do use it, read the documentation very carefully.
 * Returns the min and max amount (in radians) that can be moved from "from"
 * angle to "left_limit" and "right_limit".
 *  \return returns false if "from"
 * angle does not lie in the interval [left_limit,right_limit] 
 * \param from -"from" angle - must lie in [-M_PI, M_PI) \
 * param left_limit - left limit of
 * valid interval for angular position - must lie in [-M_PI, M_PI], left and
 * right limits are specified on the unit circle w.r.t to a reference pointing
 * inwards 
 * \param right_limit - right limit of valid interval for angular
 * position - must lie in [-M_PI, M_PI], left and right limits are specified on
 * the unit circle w.r.t to a reference pointing inwards 
 * \param result_min_delta
 * - minimum (delta) angle (in radians) that can be moved from "from" position
 * before hitting the joint stop 
 * \param result_max_delta - maximum (delta) angle
 * (in radians) that can be movedd from "from" position before hitting the joint
 * stop
 */
static bool find_min_max_delta(double from, double left_limit,
                               double right_limit, double &result_min_delta,
                               double &result_max_delta) {
  double delta[4];

  delta[0] = shortest_angular_distance(from, left_limit);
  delta[1] = shortest_angular_distance(from, right_limit);

  delta[2] = two_pi_complement(delta[0]);
  delta[3] = two_pi_complement(delta[1]);

  if (delta[0] == 0) {
    result_min_delta = delta[0];
    result_max_delta = std::max<double>(delta[1], delta[3]);
    return true;
  }

  if (delta[1] == 0) {
    result_max_delta = delta[1];
    result_min_delta = std::min<double>(delta[0], delta[2]);
    return true;
  }

  double delta_min = delta[0];
  double delta_min_2pi = delta[2];
  if (delta[2] < delta_min) {
    delta_min = delta[2];
    delta_min_2pi = delta[0];
  }

  double delta_max = delta[1];
  double delta_max_2pi = delta[3];
  if (delta[3] > delta_max) {
    delta_max = delta[3];
    delta_max_2pi = delta[1];
  }

  //    printf("%f %f %f %f\n",delta_min,delta_min_2pi,delta_max,delta_max_2pi);
  if ((delta_min <= delta_max_2pi) || (delta_max >= delta_min_2pi)) {
    result_min_delta = delta_max_2pi;
    result_max_delta = delta_min_2pi;
    if (left_limit == -M_PI && right_limit == M_PI)
      return true;
    else
      return false;
  }
  result_min_delta = delta_min;
  result_max_delta = delta_max;
  return true;
}

/*!
 * \function
 *
 * \brief Returns the delta from `from_angle` to `to_angle`, making sure it does
 * not violate limits specified by `left_limit` and `right_limit`. This function
 * is similar to `shortest_angular_distance_with_limits()`, with the main
 * difference that it accepts limits outside the `[-M_PI, M_PI]` range. Even if
 * this is quite uncommon, one could indeed consider revolute joints with large
 * rotation limits, e.g., in the range `[-2*M_PI, 2*M_PI]`.
 *
 * In this case, a strict requirement is to have `left_limit` smaller than
 * `right_limit`. Note also that `from` must lie inside the valid range, while
 * `to` does not need to. In fact, this function will evaluate the shortest
 * (valid) angle `shortest_angle` so that `from+shortest_angle` equals `to` up
 * to an integer multiple of `2*M_PI`. As an example, a call to
 * `shortest_angular_distance_with_large_limits(0, 10.5*M_PI, -2*M_PI, 2*M_PI,
 * shortest_angle)` will return `true`, with `shortest_angle=0.5*M_PI`. This is
 * because `from` and `from+shortest_angle` are both inside the limits, and
 * `fmod(to+shortest_angle, 2*M_PI)` equals `fmod(to, 2*M_PI)`. On the other
 * hand, `shortest_angular_distance_with_large_limits(10.5*M_PI, 0, -2*M_PI,
 * 2*M_PI, shortest_angle)` will return false, since `from` is not in the valid
 * range. Finally, note that the call
 * `shortest_angular_distance_with_large_limits(0, 10.5*M_PI, -2*M_PI, 0.1*M_PI,
 * shortest_angle)` will also return `true`. However, `shortest_angle` in this
 * case will be `-1.5*M_PI`.
 *
 * \return true if `left_limit < right_limit` and if "from" and
 * "from+shortest_angle" positions are within the valid interval, false otherwise. 
 * \param from - "from" angle. 
 * \param to - "to" angle. 
 * \param left_limit - left limit of valid interval, must be smaller than right_limit.
 * \param right_limit - right limit of valid interval, must be greater than left_limit. 
 * \param shortest_angle - result of the shortest angle calculation.
 */
static inline bool shortest_angular_distance_with_large_limits(
    double from, double to, double left_limit, double right_limit,
    double &shortest_angle) {
  // Shortest steps in the two directions
  double delta = shortest_angular_distance(from, to);
  double delta_2pi = two_pi_complement(delta);

  // "sort" distances so that delta is shorter than delta_2pi
  if (std::fabs(delta) > std::fabs(delta_2pi))
    std::swap(delta, delta_2pi);

  if (left_limit > right_limit) {
    // If limits are something like [PI/2 , -PI/2] it actually means that we
    // want rotations to be in the interval [-PI,PI/2] U [PI/2,PI], ie, the
    // half unit circle not containing the 0. This is already gracefully
    // handled by shortest_angular_distance_with_limits, and therefore this
    // function should not be called at all. However, if one has limits that
    // are larger than PI, the same rationale behind
    // shortest_angular_distance_with_limits does not hold, ie, M_PI+x should
    // not be directly equal to -M_PI+x. In this case, the correct way of
    // getting the shortest solution is to properly set the limits, eg, by
    // saying that the interval is either [PI/2, 3*PI/2] or [-3*M_PI/2,
    // -M_PI/2]. For this reason, here we return false by default.
    shortest_angle = delta;
    return false;
  }

  // Check in which direction we should turn (clockwise or counter-clockwise).

  // start by trying with the shortest angle (delta).
  double to2 = from + delta;
  if (left_limit <= to2 && to2 <= right_limit) {
    // we can move in this direction: return success if the "from" angle is
    // inside limits
    shortest_angle = delta;
    return left_limit <= from && from <= right_limit;
  }

  // delta is not ok, try to move in the other direction (using its complement)
  to2 = from + delta_2pi;
  if (left_limit <= to2 && to2 <= right_limit) {
    // we can move in this direction: return success if the "from" angle is
    // inside limits
    shortest_angle = delta_2pi;
    return left_limit <= from && from <= right_limit;
  }

  // nothing works: we always go outside limits
  shortest_angle = delta; // at least give some "coherent" result
  return false;
}

/*!
 * \function
 *
 * \brief Returns the delta from "from_angle" to "to_angle" making sure it does
 * not violate limits specified by left_limit and right_limit. The valid
 * interval of angular positions is [left_limit,right_limit]. E.g., [-0.25,0.25]
 * is a 0.5 radians wide interval that contains 0. But [0.25,-0.25] is a
 * 2*M_PI-0.5 wide interval that contains M_PI (but not 0). The value of
 * shortest_angle is the angular difference between "from" and "to" that lies
 * within the defined valid interval. E.g.
 * shortest_angular_distance_with_limits(-0.5,0.5,0.25,-0.25,ss) evaluates ss to
 * 2*M_PI-1.0 and returns true while
 * shortest_angular_distance_with_limits(-0.5,0.5,-0.25,0.25,ss) returns false
 * since -0.5 and 0.5 do not lie in the interval [-0.25,0.25]
 *
 * \return true if "from" and "to" positions are within the limit interval,
 * false otherwise \param from - "from" angle \param to - "to" angle \param
 * left_limit - left limit of valid interval for angular position, left and
 * right limits are specified on the unit circle w.r.t to a reference pointing
 * inwards \param right_limit - right limit of valid interval for angular
 * position, left and right limits are specified on the unit circle w.r.t to a
 * reference pointing inwards \param shortest_angle - result of the shortest
 * angle calculation
 */
//? 没太懂　　作用
static inline bool
shortest_angular_distance_with_limits(double from, double to, double left_limit,
                                      double right_limit,
                                      double &shortest_angle) {

  double min_delta = -2 * M_PI;
  double max_delta = 2 * M_PI;
  double min_delta_to = -2 * M_PI;
  double max_delta_to = 2 * M_PI;
  bool flag =
      find_min_max_delta(from, left_limit, right_limit, min_delta, max_delta);
  double delta = shortest_angular_distance(from, to);
  double delta_mod_2pi = two_pi_complement(delta);

  if (flag) // from position is within the limits
  {
    if (delta >= min_delta && delta <= max_delta) {
      shortest_angle = delta;
      return true;
    } else if (delta_mod_2pi >= min_delta && delta_mod_2pi <= max_delta) {
      shortest_angle = delta_mod_2pi;
      return true;
    } else // to position is outside the limits
    {
      find_min_max_delta(to, left_limit, right_limit, min_delta_to,
                         max_delta_to);
      if (fabs(min_delta_to) < fabs(max_delta_to))
        shortest_angle = std::max<double>(delta, delta_mod_2pi);
      else if (fabs(min_delta_to) > fabs(max_delta_to))
        shortest_angle = std::min<double>(delta, delta_mod_2pi);
      else {
        if (fabs(delta) < fabs(delta_mod_2pi))
          shortest_angle = delta;
        else
          shortest_angle = delta_mod_2pi;
      }
      return false;
    }
  } else // from position is outside the limits
  {
    find_min_max_delta(to, left_limit, right_limit, min_delta_to, max_delta_to);

    if (fabs(min_delta) < fabs(max_delta))
      shortest_angle = std::min<double>(delta, delta_mod_2pi);
    else if (fabs(min_delta) > fabs(max_delta))
      shortest_angle = std::max<double>(delta, delta_mod_2pi);
    else {
      if (fabs(delta) < fabs(delta_mod_2pi))
        shortest_angle = delta;
      else
        shortest_angle = delta_mod_2pi;
    }
    return false;
  }

  shortest_angle = delta;
  return false;
}
} // namespace angles

#endif

二.gtest 测试

功能包中提供了gtest测试实例
包括了C++版和python版

#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();
}

测试结果

三.用法说明

该包在安装ROS的时候已经默认安装了，所以在CMakelist中　添加包依赖后可直接包含头文件

在CMakelist中　添加包依赖

find_package(catkin REQUIRED
  COMPONENTS
    angles
)

包含头文件

#include <angles/angles.h>

使用其中的函数
如　shortest_angular_distance

double dist_left = std::fabs(angles::shortest_angular_distance(current_angle, start_angle))