Skip to content

Commit 5099343

Browse files
authored
feat(autoware_utils_math): split package (#33)
* feat(autoware_utils_math): split package Signed-off-by: Takagi, Isamu <isamu.takagi@tier4.jp> * add math test Signed-off-by: Takagi, Isamu <isamu.takagi@tier4.jp> * update readme Signed-off-by: Takagi, Isamu <isamu.takagi@tier4.jp> * fix readme Signed-off-by: Takagi, Isamu <isamu.takagi@tier4.jp> * move readme code snippets Signed-off-by: Takagi, Isamu <isamu.takagi@tier4.jp> * add deprecated message Signed-off-by: Takagi, Isamu <isamu.takagi@tier4.jp> * fix test file glob Signed-off-by: Takagi, Isamu <isamu.takagi@tier4.jp> --------- Signed-off-by: Takagi, Isamu <isamu.takagi@tier4.jp>
1 parent 62b5953 commit 5099343

28 files changed

+917
-263
lines changed

autoware_utils/README.md

-32
Original file line numberDiff line numberDiff line change
@@ -29,17 +29,6 @@ The geometry module provides classes and functions for handling 2D and 3D points
2929
- Intersection checks for convex polygons using GJK.
3030
- Conversion between different coordinate systems.
3131

32-
#### Math Module
33-
34-
The math module offers a variety of mathematical utilities:
35-
36-
- **`accumulator.hpp`**: A class for accumulating statistical data, supporting min, max, and mean calculations.
37-
- **`constants.hpp`**: Defines commonly used mathematical constants like π and gravity.
38-
- **`normalization.hpp`**: Functions for normalizing angles and degrees.
39-
- **`range.hpp`**: Functions for generating sequences of numbers (arange, linspace).
40-
- **`trigonometry.hpp`**: Optimized trigonometric functions for faster computation.
41-
- **`unit_conversion.hpp`**: Functions for converting between different units (e.g., degrees to radians, km/h to m/s).
42-
4332
#### ROS Module
4433

4534
The ROS module provides utilities for working with ROS messages and nodes:
@@ -110,27 +99,6 @@ int main() {
11099
}
111100
```
112101
113-
#### Using Accumulator from accumulator.hpp
114-
115-
```cpp
116-
#include "autoware_utils/math/accumulator.hpp"
117-
118-
int main() {
119-
autoware_utils::Accumulator<double> acc;
120-
121-
acc.add(1.0);
122-
acc.add(2.0);
123-
acc.add(3.0);
124-
125-
std::cout << "Mean: " << acc.mean() << "\n";
126-
std::cout << "Min: " << acc.min() << "\n";
127-
std::cout << "Max: " << acc.max() << "\n";
128-
std::cout << "Count: " << acc.count() << "\n";
129-
130-
return 0;
131-
}
132-
```
133-
134102
### Detailed Usage Examples
135103
136104
#### Update Parameters Dynamically with update_param.hpp
Original file line numberDiff line numberDiff line change
@@ -1,4 +1,4 @@
1-
// Copyright 2021-2024 Tier IV, Inc.
1+
// Copyright 2025 The Autoware Contributors
22
//
33
// Licensed under the Apache License, Version 2.0 (the "License");
44
// you may not use this file except in compliance with the License.
@@ -12,82 +12,17 @@
1212
// See the License for the specific language governing permissions and
1313
// limitations under the License.
1414

15-
#include <iostream>
16-
#include <limits>
17-
1815
#ifndef AUTOWARE_UTILS__MATH__ACCUMULATOR_HPP_
1916
#define AUTOWARE_UTILS__MATH__ACCUMULATOR_HPP_
2017

21-
namespace autoware_utils
22-
{
23-
/**
24-
* @brief class to accumulate statistical data, supporting min, max and mean.
25-
* @typedef T type of the values (default to double)
26-
*/
27-
template <typename T = double>
28-
class Accumulator
29-
{
30-
public:
31-
/**
32-
* @brief add a value
33-
* @param value value to add
34-
*/
35-
void add(const T & value)
36-
{
37-
if (value < min_) {
38-
min_ = value;
39-
}
40-
if (value > max_) {
41-
max_ = value;
42-
}
43-
++count_;
44-
mean_ = mean_ + (value - mean_) / count_;
45-
}
46-
47-
/**
48-
* @brief get the mean value
49-
*/
50-
long double mean() const { return mean_; }
51-
52-
/**
53-
* @brief get the minimum value
54-
*/
55-
T min() const { return min_; }
56-
57-
/**
58-
* @brief get the maximum value
59-
*/
60-
T max() const { return max_; }
61-
62-
/**
63-
* @brief get the number of values used to build this statistic
64-
*/
65-
unsigned int count() const { return count_; }
66-
67-
template <typename U>
68-
friend std::ostream & operator<<(std::ostream & os, const Accumulator<U> & accumulator);
69-
70-
private:
71-
T min_ = std::numeric_limits<T>::max();
72-
T max_ = std::numeric_limits<T>::lowest();
73-
long double mean_ = 0.0;
74-
unsigned int count_ = 0;
75-
};
18+
// NOLINTBEGIN(build/namespaces, whitespace/line_length)
19+
// clang-format off
7620

77-
/**
78-
* @brief overload << operator for easy print to output stream
79-
*/
80-
template <typename T>
81-
std::ostream & operator<<(std::ostream & os, const Accumulator<T> & accumulator)
82-
{
83-
if (accumulator.count() == 0) {
84-
os << "None None None";
85-
} else {
86-
os << accumulator.min() << " " << accumulator.max() << " " << accumulator.mean();
87-
}
88-
return os;
89-
}
21+
#pragma message("#include <autoware_utils/math/accumulator.hpp> is deprecated. Use #include <autoware_utils_math/accumulator.hpp> instead.")
22+
#include <autoware_utils_math/accumulator.hpp>
23+
namespace autoware_utils { using namespace autoware_utils_math; }
9024

91-
} // namespace autoware_utils
25+
// clang-format on
26+
// NOLINTEND
9227

9328
#endif // AUTOWARE_UTILS__MATH__ACCUMULATOR_HPP_

autoware_utils/include/autoware_utils/math/constants.hpp

+10-6
Original file line numberDiff line numberDiff line change
@@ -1,4 +1,4 @@
1-
// Copyright 2020 TIER IV, Inc.
1+
// Copyright 2025 The Autoware Contributors
22
//
33
// Licensed under the Apache License, Version 2.0 (the "License");
44
// you may not use this file except in compliance with the License.
@@ -15,10 +15,14 @@
1515
#ifndef AUTOWARE_UTILS__MATH__CONSTANTS_HPP_
1616
#define AUTOWARE_UTILS__MATH__CONSTANTS_HPP_
1717

18-
namespace autoware_utils
19-
{
20-
constexpr double pi = 3.14159265358979323846; // To be replaced by std::numbers::pi in C++20
21-
constexpr double gravity = 9.80665;
22-
} // namespace autoware_utils
18+
// NOLINTBEGIN(build/namespaces, whitespace/line_length)
19+
// clang-format off
20+
21+
#pragma message("#include <autoware_utils/math/constants.hpp> is deprecated. Use #include <autoware_utils_math/constants.hpp> instead.")
22+
#include <autoware_utils_math/constants.hpp>
23+
namespace autoware_utils { using namespace autoware_utils_math; }
24+
25+
// clang-format on
26+
// NOLINTEND
2327

2428
#endif // AUTOWARE_UTILS__MATH__CONSTANTS_HPP_
Original file line numberDiff line numberDiff line change
@@ -1,4 +1,4 @@
1-
// Copyright 2020 TIER IV, Inc.
1+
// Copyright 2025 The Autoware Contributors
22
//
33
// Licensed under the Apache License, Version 2.0 (the "License");
44
// you may not use this file except in compliance with the License.
@@ -15,36 +15,14 @@
1515
#ifndef AUTOWARE_UTILS__MATH__NORMALIZATION_HPP_
1616
#define AUTOWARE_UTILS__MATH__NORMALIZATION_HPP_
1717

18-
#include "autoware_utils/math/constants.hpp"
18+
// NOLINTBEGIN(build/namespaces, whitespace/line_length)
19+
// clang-format off
1920

20-
#include <cmath>
21+
#pragma message("#include <autoware_utils/math/normalization.hpp> is deprecated. Use #include <autoware_utils_math/normalization.hpp> instead.")
22+
#include <autoware_utils_math/normalization.hpp>
23+
namespace autoware_utils { using namespace autoware_utils_math; }
2124

22-
namespace autoware_utils
23-
{
24-
inline double normalize_degree(const double deg, const double min_deg = -180)
25-
{
26-
const auto max_deg = min_deg + 360.0;
27-
28-
const auto value = std::fmod(deg, 360.0);
29-
if (min_deg <= value && value < max_deg) {
30-
return value;
31-
}
32-
33-
return value - std::copysign(360.0, value);
34-
}
35-
36-
inline double normalize_radian(const double rad, const double min_rad = -pi)
37-
{
38-
const auto max_rad = min_rad + 2 * pi;
39-
40-
const auto value = std::fmod(rad, 2 * pi);
41-
if (min_rad <= value && value < max_rad) {
42-
return value;
43-
}
44-
45-
return value - std::copysign(2 * pi, value);
46-
}
47-
48-
} // namespace autoware_utils
25+
// clang-format on
26+
// NOLINTEND
4927

5028
#endif // AUTOWARE_UTILS__MATH__NORMALIZATION_HPP_
Original file line numberDiff line numberDiff line change
@@ -1,4 +1,4 @@
1-
// Copyright 2021 TIER IV, Inc.
1+
// Copyright 2025 The Autoware Contributors
22
//
33
// Licensed under the Apache License, Version 2.0 (the "License");
44
// you may not use this file except in compliance with the License.
@@ -15,64 +15,14 @@
1515
#ifndef AUTOWARE_UTILS__MATH__RANGE_HPP_
1616
#define AUTOWARE_UTILS__MATH__RANGE_HPP_
1717

18-
#include <cmath>
19-
#include <limits>
20-
#include <stdexcept>
21-
#include <vector>
18+
// NOLINTBEGIN(build/namespaces, whitespace/line_length)
19+
// clang-format off
2220

23-
namespace autoware_utils
24-
{
25-
template <class T>
26-
std::vector<T> arange(const T start, const T stop, const T step = 1)
27-
{
28-
if (step == 0) {
29-
throw std::invalid_argument("step must be non-zero value.");
30-
}
21+
#pragma message("#include <autoware_utils/math/range.hpp> is deprecated. Use #include <autoware_utils_math/range.hpp> instead.")
22+
#include <autoware_utils_math/range.hpp>
23+
namespace autoware_utils { using namespace autoware_utils_math; }
3124

32-
if (step > 0 && stop < start) {
33-
throw std::invalid_argument("must be stop >= start for positive step.");
34-
}
35-
36-
if (step < 0 && stop > start) {
37-
throw std::invalid_argument("must be stop <= start for negative step.");
38-
}
39-
40-
const double max_i_double = std::ceil(static_cast<double>(stop - start) / step);
41-
const auto max_i = static_cast<size_t>(max_i_double);
42-
43-
std::vector<T> out;
44-
out.reserve(max_i);
45-
for (size_t i = 0; i < max_i; ++i) {
46-
out.push_back(start + i * step);
47-
}
48-
49-
return out;
50-
}
51-
52-
template <class T>
53-
std::vector<double> linspace(const T start, const T stop, const size_t num)
54-
{
55-
const auto start_double = static_cast<double>(start);
56-
const auto stop_double = static_cast<double>(stop);
57-
58-
if (num == 0) {
59-
return {};
60-
}
61-
62-
if (num == 1) {
63-
return {start_double};
64-
}
65-
66-
std::vector<double> out;
67-
out.reserve(num);
68-
const double step = (stop_double - start_double) / static_cast<double>(num - 1);
69-
for (size_t i = 0; i < num; i++) {
70-
out.push_back(start_double + static_cast<double>(i) * step);
71-
}
72-
73-
return out;
74-
}
75-
76-
} // namespace autoware_utils
25+
// clang-format on
26+
// NOLINTEND
7727

7828
#endif // AUTOWARE_UTILS__MATH__RANGE_HPP_
Original file line numberDiff line numberDiff line change
@@ -1,4 +1,4 @@
1-
// Copyright 2023 TIER IV, Inc.
1+
// Copyright 2025 The Autoware Contributors
22
//
33
// Licensed under the Apache License, Version 2.0 (the "License");
44
// you may not use this file except in compliance with the License.
@@ -15,16 +15,14 @@
1515
#ifndef AUTOWARE_UTILS__MATH__SIN_TABLE_HPP_
1616
#define AUTOWARE_UTILS__MATH__SIN_TABLE_HPP_
1717

18-
#include <cstddef>
18+
// NOLINTBEGIN(build/namespaces, whitespace/line_length)
19+
// clang-format off
1920

20-
namespace autoware_utils
21-
{
21+
#pragma message("#include <autoware_utils/math/sin_table.hpp> is deprecated. Use #include <autoware_utils_math/sin_table.hpp> instead.")
22+
#include <autoware_utils_math/sin_table.hpp>
23+
namespace autoware_utils { using namespace autoware_utils_math; }
2224

23-
constexpr size_t sin_table_size = 32769;
24-
constexpr size_t discrete_arcs_num_90 = 32768;
25-
constexpr size_t discrete_arcs_num_360 = 131072;
26-
extern const float g_sin_table[sin_table_size];
27-
28-
} // namespace autoware_utils
25+
// clang-format on
26+
// NOLINTEND
2927

3028
#endif // AUTOWARE_UTILS__MATH__SIN_TABLE_HPP_
Original file line numberDiff line numberDiff line change
@@ -1,4 +1,4 @@
1-
// Copyright 2023 TIER IV, Inc.
1+
// Copyright 2025 The Autoware Contributors
22
//
33
// Licensed under the Apache License, Version 2.0 (the "License");
44
// you may not use this file except in compliance with the License.
@@ -15,19 +15,14 @@
1515
#ifndef AUTOWARE_UTILS__MATH__TRIGONOMETRY_HPP_
1616
#define AUTOWARE_UTILS__MATH__TRIGONOMETRY_HPP_
1717

18-
#include <utility>
18+
// NOLINTBEGIN(build/namespaces, whitespace/line_length)
19+
// clang-format off
1920

20-
namespace autoware_utils
21-
{
21+
#pragma message("#include <autoware_utils/math/trigonometry.hpp> is deprecated. Use #include <autoware_utils_math/trigonometry.hpp> instead.")
22+
#include <autoware_utils_math/trigonometry.hpp>
23+
namespace autoware_utils { using namespace autoware_utils_math; }
2224

23-
float sin(float radian);
24-
25-
float cos(float radian);
26-
27-
std::pair<float, float> sin_and_cos(float radian);
28-
29-
float opencv_fast_atan2(float dy, float dx);
30-
31-
} // namespace autoware_utils
25+
// clang-format on
26+
// NOLINTEND
3227

3328
#endif // AUTOWARE_UTILS__MATH__TRIGONOMETRY_HPP_

0 commit comments

Comments
 (0)