Skip to content

Commit

Permalink
Merge branch 'main' into feat/split-autoware-utils-system
Browse files Browse the repository at this point in the history
  • Loading branch information
youtalk authored Mar 6, 2025
2 parents f839e32 + 417b3cc commit f152c02
Show file tree
Hide file tree
Showing 36 changed files with 1,202 additions and 329 deletions.
33 changes: 0 additions & 33 deletions autoware_utils/README.md
Original file line number Diff line number Diff line change
Expand Up @@ -29,25 +29,13 @@ The geometry module provides classes and functions for handling 2D and 3D points
- Intersection checks for convex polygons using GJK.
- Conversion between different coordinate systems.

#### Math Module

The math module offers a variety of mathematical utilities:

- **`accumulator.hpp`**: A class for accumulating statistical data, supporting min, max, and mean calculations.
- **`constants.hpp`**: Defines commonly used mathematical constants like π and gravity.
- **`normalization.hpp`**: Functions for normalizing angles and degrees.
- **`range.hpp`**: Functions for generating sequences of numbers (arange, linspace).
- **`trigonometry.hpp`**: Optimized trigonometric functions for faster computation.
- **`unit_conversion.hpp`**: Functions for converting between different units (e.g., degrees to radians, km/h to m/s).

#### ROS Module

The ROS module provides utilities for working with ROS messages and nodes:

- **`debug_publisher.hpp`**: A helper class for publishing debug messages with timestamps.
- **`diagnostics_interface.hpp`**: An interface for publishing diagnostic messages.
- **`logger_level_configure.hpp`**: Utility for configuring logger levels dynamically.
- **`marker_helper.hpp`**: Helper functions for creating and manipulating visualization markers.
- **`msg_covariance.hpp`**: Indices for accessing covariance matrices in ROS messages.
- **`msg_operation.hpp`**: Overloaded operators for quaternion messages.
- **`parameter.hpp`**: Simplifies parameter retrieval and declaration.
Expand Down Expand Up @@ -107,27 +95,6 @@ int main() {
}
```
#### Using Accumulator from accumulator.hpp
```cpp
#include "autoware_utils/math/accumulator.hpp"
int main() {
autoware_utils::Accumulator<double> acc;
acc.add(1.0);
acc.add(2.0);
acc.add(3.0);
std::cout << "Mean: " << acc.mean() << "\n";
std::cout << "Min: " << acc.min() << "\n";
std::cout << "Max: " << acc.max() << "\n";
std::cout << "Count: " << acc.count() << "\n";
return 0;
}
```

### Detailed Usage Examples
#### Update Parameters Dynamically with update_param.hpp
Expand Down
81 changes: 8 additions & 73 deletions autoware_utils/include/autoware_utils/math/accumulator.hpp
Original file line number Diff line number Diff line change
@@ -1,4 +1,4 @@
// Copyright 2021-2024 Tier IV, Inc.
// Copyright 2025 The Autoware Contributors
//
// Licensed under the Apache License, Version 2.0 (the "License");
// you may not use this file except in compliance with the License.
Expand All @@ -12,82 +12,17 @@
// See the License for the specific language governing permissions and
// limitations under the License.

#include <iostream>
#include <limits>

#ifndef AUTOWARE_UTILS__MATH__ACCUMULATOR_HPP_
#define AUTOWARE_UTILS__MATH__ACCUMULATOR_HPP_

namespace autoware_utils
{
/**
* @brief class to accumulate statistical data, supporting min, max and mean.
* @typedef T type of the values (default to double)
*/
template <typename T = double>
class Accumulator
{
public:
/**
* @brief add a value
* @param value value to add
*/
void add(const T & value)
{
if (value < min_) {
min_ = value;
}
if (value > max_) {
max_ = value;
}
++count_;
mean_ = mean_ + (value - mean_) / count_;
}

/**
* @brief get the mean value
*/
long double mean() const { return mean_; }

/**
* @brief get the minimum value
*/
T min() const { return min_; }

/**
* @brief get the maximum value
*/
T max() const { return max_; }

/**
* @brief get the number of values used to build this statistic
*/
unsigned int count() const { return count_; }

template <typename U>
friend std::ostream & operator<<(std::ostream & os, const Accumulator<U> & accumulator);

private:
T min_ = std::numeric_limits<T>::max();
T max_ = std::numeric_limits<T>::lowest();
long double mean_ = 0.0;
unsigned int count_ = 0;
};
// NOLINTBEGIN(build/namespaces, whitespace/line_length)
// clang-format off

/**
* @brief overload << operator for easy print to output stream
*/
template <typename T>
std::ostream & operator<<(std::ostream & os, const Accumulator<T> & accumulator)
{
if (accumulator.count() == 0) {
os << "None None None";
} else {
os << accumulator.min() << " " << accumulator.max() << " " << accumulator.mean();
}
return os;
}
#pragma message("#include <autoware_utils/math/accumulator.hpp> is deprecated. Use #include <autoware_utils_math/accumulator.hpp> instead.")
#include <autoware_utils_math/accumulator.hpp>
namespace autoware_utils { using namespace autoware_utils_math; }

} // namespace autoware_utils
// clang-format on
// NOLINTEND

#endif // AUTOWARE_UTILS__MATH__ACCUMULATOR_HPP_
16 changes: 10 additions & 6 deletions autoware_utils/include/autoware_utils/math/constants.hpp
Original file line number Diff line number Diff line change
@@ -1,4 +1,4 @@
// Copyright 2020 TIER IV, Inc.
// Copyright 2025 The Autoware Contributors
//
// Licensed under the Apache License, Version 2.0 (the "License");
// you may not use this file except in compliance with the License.
Expand All @@ -15,10 +15,14 @@
#ifndef AUTOWARE_UTILS__MATH__CONSTANTS_HPP_
#define AUTOWARE_UTILS__MATH__CONSTANTS_HPP_

namespace autoware_utils
{
constexpr double pi = 3.14159265358979323846; // To be replaced by std::numbers::pi in C++20
constexpr double gravity = 9.80665;
} // namespace autoware_utils
// NOLINTBEGIN(build/namespaces, whitespace/line_length)
// clang-format off

#pragma message("#include <autoware_utils/math/constants.hpp> is deprecated. Use #include <autoware_utils_math/constants.hpp> instead.")
#include <autoware_utils_math/constants.hpp>
namespace autoware_utils { using namespace autoware_utils_math; }

// clang-format on
// NOLINTEND

#endif // AUTOWARE_UTILS__MATH__CONSTANTS_HPP_
38 changes: 8 additions & 30 deletions autoware_utils/include/autoware_utils/math/normalization.hpp
Original file line number Diff line number Diff line change
@@ -1,4 +1,4 @@
// Copyright 2020 TIER IV, Inc.
// Copyright 2025 The Autoware Contributors
//
// Licensed under the Apache License, Version 2.0 (the "License");
// you may not use this file except in compliance with the License.
Expand All @@ -15,36 +15,14 @@
#ifndef AUTOWARE_UTILS__MATH__NORMALIZATION_HPP_
#define AUTOWARE_UTILS__MATH__NORMALIZATION_HPP_

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

#include <cmath>
#pragma message("#include <autoware_utils/math/normalization.hpp> is deprecated. Use #include <autoware_utils_math/normalization.hpp> instead.")
#include <autoware_utils_math/normalization.hpp>
namespace autoware_utils { using namespace autoware_utils_math; }

namespace autoware_utils
{
inline double normalize_degree(const double deg, const double min_deg = -180)
{
const auto max_deg = min_deg + 360.0;

const auto value = std::fmod(deg, 360.0);
if (min_deg <= value && value < max_deg) {
return value;
}

return value - std::copysign(360.0, value);
}

inline double normalize_radian(const double rad, const double min_rad = -pi)
{
const auto max_rad = min_rad + 2 * pi;

const auto value = std::fmod(rad, 2 * pi);
if (min_rad <= value && value < max_rad) {
return value;
}

return value - std::copysign(2 * pi, value);
}

} // namespace autoware_utils
// clang-format on
// NOLINTEND

#endif // AUTOWARE_UTILS__MATH__NORMALIZATION_HPP_
66 changes: 8 additions & 58 deletions autoware_utils/include/autoware_utils/math/range.hpp
Original file line number Diff line number Diff line change
@@ -1,4 +1,4 @@
// Copyright 2021 TIER IV, Inc.
// Copyright 2025 The Autoware Contributors
//
// Licensed under the Apache License, Version 2.0 (the "License");
// you may not use this file except in compliance with the License.
Expand All @@ -15,64 +15,14 @@
#ifndef AUTOWARE_UTILS__MATH__RANGE_HPP_
#define AUTOWARE_UTILS__MATH__RANGE_HPP_

#include <cmath>
#include <limits>
#include <stdexcept>
#include <vector>
// NOLINTBEGIN(build/namespaces, whitespace/line_length)
// clang-format off

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

if (step > 0 && stop < start) {
throw std::invalid_argument("must be stop >= start for positive step.");
}

if (step < 0 && stop > start) {
throw std::invalid_argument("must be stop <= start for negative step.");
}

const double max_i_double = std::ceil(static_cast<double>(stop - start) / step);
const auto max_i = static_cast<size_t>(max_i_double);

std::vector<T> out;
out.reserve(max_i);
for (size_t i = 0; i < max_i; ++i) {
out.push_back(start + i * step);
}

return out;
}

template <class T>
std::vector<double> linspace(const T start, const T stop, const size_t num)
{
const auto start_double = static_cast<double>(start);
const auto stop_double = static_cast<double>(stop);

if (num == 0) {
return {};
}

if (num == 1) {
return {start_double};
}

std::vector<double> out;
out.reserve(num);
const double step = (stop_double - start_double) / static_cast<double>(num - 1);
for (size_t i = 0; i < num; i++) {
out.push_back(start_double + static_cast<double>(i) * step);
}

return out;
}

} // namespace autoware_utils
// clang-format on
// NOLINTEND

#endif // AUTOWARE_UTILS__MATH__RANGE_HPP_
18 changes: 8 additions & 10 deletions autoware_utils/include/autoware_utils/math/sin_table.hpp
Original file line number Diff line number Diff line change
@@ -1,4 +1,4 @@
// Copyright 2023 TIER IV, Inc.
// Copyright 2025 The Autoware Contributors
//
// Licensed under the Apache License, Version 2.0 (the "License");
// you may not use this file except in compliance with the License.
Expand All @@ -15,16 +15,14 @@
#ifndef AUTOWARE_UTILS__MATH__SIN_TABLE_HPP_
#define AUTOWARE_UTILS__MATH__SIN_TABLE_HPP_

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

namespace autoware_utils
{
#pragma message("#include <autoware_utils/math/sin_table.hpp> is deprecated. Use #include <autoware_utils_math/sin_table.hpp> instead.")
#include <autoware_utils_math/sin_table.hpp>
namespace autoware_utils { using namespace autoware_utils_math; }

constexpr size_t sin_table_size = 32769;
constexpr size_t discrete_arcs_num_90 = 32768;
constexpr size_t discrete_arcs_num_360 = 131072;
extern const float g_sin_table[sin_table_size];

} // namespace autoware_utils
// clang-format on
// NOLINTEND

#endif // AUTOWARE_UTILS__MATH__SIN_TABLE_HPP_
21 changes: 8 additions & 13 deletions autoware_utils/include/autoware_utils/math/trigonometry.hpp
Original file line number Diff line number Diff line change
@@ -1,4 +1,4 @@
// Copyright 2023 TIER IV, Inc.
// Copyright 2025 The Autoware Contributors
//
// Licensed under the Apache License, Version 2.0 (the "License");
// you may not use this file except in compliance with the License.
Expand All @@ -15,19 +15,14 @@
#ifndef AUTOWARE_UTILS__MATH__TRIGONOMETRY_HPP_
#define AUTOWARE_UTILS__MATH__TRIGONOMETRY_HPP_

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

namespace autoware_utils
{
#pragma message("#include <autoware_utils/math/trigonometry.hpp> is deprecated. Use #include <autoware_utils_math/trigonometry.hpp> instead.")
#include <autoware_utils_math/trigonometry.hpp>
namespace autoware_utils { using namespace autoware_utils_math; }

float sin(float radian);

float cos(float radian);

std::pair<float, float> sin_and_cos(float radian);

float opencv_fast_atan2(float dy, float dx);

} // namespace autoware_utils
// clang-format on
// NOLINTEND

#endif // AUTOWARE_UTILS__MATH__TRIGONOMETRY_HPP_
Loading

0 comments on commit f152c02

Please sign in to comment.