From 00c259b7d5b3460e5fef944ff162dfc15b9bc9bb Mon Sep 17 00:00:00 2001 From: "Takagi, Isamu" Date: Thu, 27 Feb 2025 21:41:19 +0900 Subject: [PATCH 1/7] feat(autoware_utils_math): split package Signed-off-by: Takagi, Isamu --- .../autoware_utils/math/accumulator.hpp | 74 +-------------- .../include/autoware_utils/math/constants.hpp | 9 +- .../autoware_utils/math/normalization.hpp | 29 +----- .../include/autoware_utils/math/range.hpp | 57 +----------- .../include/autoware_utils/math/sin_table.hpp | 9 +- .../autoware_utils/math/trigonometry.hpp | 12 +-- .../autoware_utils/math/unit_conversion.hpp | 22 +---- autoware_utils/package.xml | 1 + autoware_utils_math/CMakeLists.txt | 15 +++ autoware_utils_math/README.md | 11 +++ .../autoware_utils_math/accumulator.hpp | 93 +++++++++++++++++++ .../include/autoware_utils_math/constants.hpp | 26 ++++++ .../autoware_utils_math/normalization.hpp | 50 ++++++++++ .../include/autoware_utils_math/range.hpp | 78 ++++++++++++++++ .../include/autoware_utils_math/sin_table.hpp | 30 ++++++ .../autoware_utils_math/trigonometry.hpp | 33 +++++++ .../autoware_utils_math/unit_conversion.hpp | 43 +++++++++ autoware_utils_math/package.xml | 23 +++++ .../src}/sin_table.cpp | 6 +- .../src}/trigonometry.cpp | 34 ++++--- 20 files changed, 450 insertions(+), 205 deletions(-) create mode 100644 autoware_utils_math/CMakeLists.txt create mode 100644 autoware_utils_math/README.md create mode 100644 autoware_utils_math/include/autoware_utils_math/accumulator.hpp create mode 100644 autoware_utils_math/include/autoware_utils_math/constants.hpp create mode 100644 autoware_utils_math/include/autoware_utils_math/normalization.hpp create mode 100644 autoware_utils_math/include/autoware_utils_math/range.hpp create mode 100644 autoware_utils_math/include/autoware_utils_math/sin_table.hpp create mode 100644 autoware_utils_math/include/autoware_utils_math/trigonometry.hpp create mode 100644 autoware_utils_math/include/autoware_utils_math/unit_conversion.hpp create mode 100644 autoware_utils_math/package.xml rename {autoware_utils/src/math => autoware_utils_math/src}/sin_table.cpp (99%) rename {autoware_utils/src/math => autoware_utils_math/src}/trigonometry.cpp (77%) diff --git a/autoware_utils/include/autoware_utils/math/accumulator.hpp b/autoware_utils/include/autoware_utils/math/accumulator.hpp index 4a2225e..7b39c5e 100644 --- a/autoware_utils/include/autoware_utils/math/accumulator.hpp +++ b/autoware_utils/include/autoware_utils/math/accumulator.hpp @@ -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. @@ -12,81 +12,15 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include -#include - #ifndef AUTOWARE_UTILS__MATH__ACCUMULATOR_HPP_ #define AUTOWARE_UTILS__MATH__ACCUMULATOR_HPP_ +#include + namespace autoware_utils { -/** - * @brief class to accumulate statistical data, supporting min, max and mean. - * @typedef T type of the values (default to double) - */ -template -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 - friend std::ostream & operator<<(std::ostream & os, const Accumulator & accumulator); - -private: - T min_ = std::numeric_limits::max(); - T max_ = std::numeric_limits::lowest(); - long double mean_ = 0.0; - unsigned int count_ = 0; -}; - -/** - * @brief overload << operator for easy print to output stream - */ -template -std::ostream & operator<<(std::ostream & os, const Accumulator & accumulator) -{ - if (accumulator.count() == 0) { - os << "None None None"; - } else { - os << accumulator.min() << " " << accumulator.max() << " " << accumulator.mean(); - } - return os; -} +using namespace autoware_utils_math; // NOLINT(build/namespaces) } // namespace autoware_utils diff --git a/autoware_utils/include/autoware_utils/math/constants.hpp b/autoware_utils/include/autoware_utils/math/constants.hpp index e86392a..e065b4c 100644 --- a/autoware_utils/include/autoware_utils/math/constants.hpp +++ b/autoware_utils/include/autoware_utils/math/constants.hpp @@ -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. @@ -15,10 +15,13 @@ #ifndef AUTOWARE_UTILS__MATH__CONSTANTS_HPP_ #define AUTOWARE_UTILS__MATH__CONSTANTS_HPP_ +#include + namespace autoware_utils { -constexpr double pi = 3.14159265358979323846; // To be replaced by std::numbers::pi in C++20 -constexpr double gravity = 9.80665; + +using namespace autoware_utils_math; // NOLINT(build/namespaces) + } // namespace autoware_utils #endif // AUTOWARE_UTILS__MATH__CONSTANTS_HPP_ diff --git a/autoware_utils/include/autoware_utils/math/normalization.hpp b/autoware_utils/include/autoware_utils/math/normalization.hpp index bb913aa..00e5c88 100644 --- a/autoware_utils/include/autoware_utils/math/normalization.hpp +++ b/autoware_utils/include/autoware_utils/math/normalization.hpp @@ -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. @@ -15,35 +15,12 @@ #ifndef AUTOWARE_UTILS__MATH__NORMALIZATION_HPP_ #define AUTOWARE_UTILS__MATH__NORMALIZATION_HPP_ -#include "autoware_utils/math/constants.hpp" - -#include +#include 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); -} +using namespace autoware_utils_math; // NOLINT(build/namespaces) } // namespace autoware_utils diff --git a/autoware_utils/include/autoware_utils/math/range.hpp b/autoware_utils/include/autoware_utils/math/range.hpp index da899a3..8184f1d 100644 --- a/autoware_utils/include/autoware_utils/math/range.hpp +++ b/autoware_utils/include/autoware_utils/math/range.hpp @@ -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. @@ -15,63 +15,12 @@ #ifndef AUTOWARE_UTILS__MATH__RANGE_HPP_ #define AUTOWARE_UTILS__MATH__RANGE_HPP_ -#include -#include -#include -#include +#include namespace autoware_utils { -template -std::vector arange(const T start, const T stop, const T step = 1) -{ - if (step == 0) { - throw std::invalid_argument("step must be non-zero value."); - } - - 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(stop - start) / step); - const auto max_i = static_cast(max_i_double); - - std::vector out; - out.reserve(max_i); - for (size_t i = 0; i < max_i; ++i) { - out.push_back(start + i * step); - } - - return out; -} - -template -std::vector linspace(const T start, const T stop, const size_t num) -{ - const auto start_double = static_cast(start); - const auto stop_double = static_cast(stop); - - if (num == 0) { - return {}; - } - - if (num == 1) { - return {start_double}; - } - - std::vector out; - out.reserve(num); - const double step = (stop_double - start_double) / static_cast(num - 1); - for (size_t i = 0; i < num; i++) { - out.push_back(start_double + static_cast(i) * step); - } - return out; -} +using namespace autoware_utils_math; // NOLINT(build/namespaces) } // namespace autoware_utils diff --git a/autoware_utils/include/autoware_utils/math/sin_table.hpp b/autoware_utils/include/autoware_utils/math/sin_table.hpp index bb8dac5..3605d5c 100644 --- a/autoware_utils/include/autoware_utils/math/sin_table.hpp +++ b/autoware_utils/include/autoware_utils/math/sin_table.hpp @@ -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. @@ -15,15 +15,12 @@ #ifndef AUTOWARE_UTILS__MATH__SIN_TABLE_HPP_ #define AUTOWARE_UTILS__MATH__SIN_TABLE_HPP_ -#include +#include namespace autoware_utils { -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]; +using namespace autoware_utils_math; // NOLINT(build/namespaces) } // namespace autoware_utils diff --git a/autoware_utils/include/autoware_utils/math/trigonometry.hpp b/autoware_utils/include/autoware_utils/math/trigonometry.hpp index 8ee5930..046a5f3 100644 --- a/autoware_utils/include/autoware_utils/math/trigonometry.hpp +++ b/autoware_utils/include/autoware_utils/math/trigonometry.hpp @@ -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. @@ -15,18 +15,12 @@ #ifndef AUTOWARE_UTILS__MATH__TRIGONOMETRY_HPP_ #define AUTOWARE_UTILS__MATH__TRIGONOMETRY_HPP_ -#include +#include namespace autoware_utils { -float sin(float radian); - -float cos(float radian); - -std::pair sin_and_cos(float radian); - -float opencv_fast_atan2(float dy, float dx); +using namespace autoware_utils_math; // NOLINT(build/namespaces) } // namespace autoware_utils diff --git a/autoware_utils/include/autoware_utils/math/unit_conversion.hpp b/autoware_utils/include/autoware_utils/math/unit_conversion.hpp index 97db93f..8d1647b 100644 --- a/autoware_utils/include/autoware_utils/math/unit_conversion.hpp +++ b/autoware_utils/include/autoware_utils/math/unit_conversion.hpp @@ -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. @@ -15,29 +15,13 @@ #ifndef AUTOWARE_UTILS__MATH__UNIT_CONVERSION_HPP_ #define AUTOWARE_UTILS__MATH__UNIT_CONVERSION_HPP_ -#include "autoware_utils/math/constants.hpp" +#include namespace autoware_utils { -constexpr double deg2rad(const double deg) -{ - return deg * pi / 180.0; -} -constexpr double rad2deg(const double rad) -{ - return rad * 180.0 / pi; -} +using namespace autoware_utils_math; // NOLINT(build/namespaces) -constexpr double kmph2mps(const double kmph) -{ - return kmph * 1000.0 / 3600.0; -} - -constexpr double mps2kmph(const double mps) -{ - return mps * 3600.0 / 1000.0; -} } // namespace autoware_utils #endif // AUTOWARE_UTILS__MATH__UNIT_CONVERSION_HPP_ diff --git a/autoware_utils/package.xml b/autoware_utils/package.xml index 7e9bc20..ea53333 100644 --- a/autoware_utils/package.xml +++ b/autoware_utils/package.xml @@ -20,6 +20,7 @@ autoware_perception_msgs autoware_planning_msgs autoware_utils_geometry + autoware_utils_math autoware_utils_pcl autoware_vehicle_msgs builtin_interfaces diff --git a/autoware_utils_math/CMakeLists.txt b/autoware_utils_math/CMakeLists.txt new file mode 100644 index 0000000..20462bf --- /dev/null +++ b/autoware_utils_math/CMakeLists.txt @@ -0,0 +1,15 @@ +cmake_minimum_required(VERSION 3.14) +project(autoware_utils_math) + +find_package(autoware_cmake REQUIRED) +autoware_package() + +ament_auto_add_library(${PROJECT_NAME} SHARED + "src/sin_table.cpp" + "src/trigonometry.cpp" +) + +if(BUILD_TESTING) +endif() + +ament_auto_package() diff --git a/autoware_utils_math/README.md b/autoware_utils_math/README.md new file mode 100644 index 0000000..908dd4c --- /dev/null +++ b/autoware_utils_math/README.md @@ -0,0 +1,11 @@ +# autoware_utils_math + +## Overview + +The **autoware_utils_math** library is a comprehensive toolkit designed to facilitate the development of autonomous driving applications. +This package provides essential utilities for geometry. +It is extensively used in the Autoware project to handle common tasks such as geometric calculations and message conversions. + +## Design + +- **`transform_listener.hpp`**: Manages transformation listeners. diff --git a/autoware_utils_math/include/autoware_utils_math/accumulator.hpp b/autoware_utils_math/include/autoware_utils_math/accumulator.hpp new file mode 100644 index 0000000..8eaef6e --- /dev/null +++ b/autoware_utils_math/include/autoware_utils_math/accumulator.hpp @@ -0,0 +1,93 @@ +// Copyright 2021-2024 Tier IV, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef AUTOWARE_UTILS_MATH__ACCUMULATOR_HPP_ +#define AUTOWARE_UTILS_MATH__ACCUMULATOR_HPP_ + +#include +#include + +namespace autoware_utils_math +{ +/** + * @brief class to accumulate statistical data, supporting min, max and mean. + * @typedef T type of the values (default to double) + */ +template +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 + friend std::ostream & operator<<(std::ostream & os, const Accumulator & accumulator); + +private: + T min_ = std::numeric_limits::max(); + T max_ = std::numeric_limits::lowest(); + long double mean_ = 0.0; + unsigned int count_ = 0; +}; + +/** + * @brief overload << operator for easy print to output stream + */ +template +std::ostream & operator<<(std::ostream & os, const Accumulator & accumulator) +{ + if (accumulator.count() == 0) { + os << "None None None"; + } else { + os << accumulator.min() << " " << accumulator.max() << " " << accumulator.mean(); + } + return os; +} + +} // namespace autoware_utils_math + +#endif // AUTOWARE_UTILS_MATH__ACCUMULATOR_HPP_ diff --git a/autoware_utils_math/include/autoware_utils_math/constants.hpp b/autoware_utils_math/include/autoware_utils_math/constants.hpp new file mode 100644 index 0000000..618f4a3 --- /dev/null +++ b/autoware_utils_math/include/autoware_utils_math/constants.hpp @@ -0,0 +1,26 @@ +// Copyright 2020 TIER IV, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef AUTOWARE_UTILS_MATH__CONSTANTS_HPP_ +#define AUTOWARE_UTILS_MATH__CONSTANTS_HPP_ + +namespace autoware_utils_math +{ + +constexpr double pi = 3.14159265358979323846; // To be replaced by std::numbers::pi in C++20 +constexpr double gravity = 9.80665; + +} // namespace autoware_utils_math + +#endif // AUTOWARE_UTILS_MATH__CONSTANTS_HPP_ diff --git a/autoware_utils_math/include/autoware_utils_math/normalization.hpp b/autoware_utils_math/include/autoware_utils_math/normalization.hpp new file mode 100644 index 0000000..ff1d462 --- /dev/null +++ b/autoware_utils_math/include/autoware_utils_math/normalization.hpp @@ -0,0 +1,50 @@ +// Copyright 2020 TIER IV, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef AUTOWARE_UTILS_MATH__NORMALIZATION_HPP_ +#define AUTOWARE_UTILS_MATH__NORMALIZATION_HPP_ + +#include "autoware_utils_math/constants.hpp" + +#include + +namespace autoware_utils_math +{ +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_math + +#endif // AUTOWARE_UTILS_MATH__NORMALIZATION_HPP_ diff --git a/autoware_utils_math/include/autoware_utils_math/range.hpp b/autoware_utils_math/include/autoware_utils_math/range.hpp new file mode 100644 index 0000000..9828435 --- /dev/null +++ b/autoware_utils_math/include/autoware_utils_math/range.hpp @@ -0,0 +1,78 @@ +// Copyright 2021 TIER IV, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef AUTOWARE_UTILS_MATH__RANGE_HPP_ +#define AUTOWARE_UTILS_MATH__RANGE_HPP_ + +#include +#include +#include +#include + +namespace autoware_utils_math +{ +template +std::vector arange(const T start, const T stop, const T step = 1) +{ + if (step == 0) { + throw std::invalid_argument("step must be non-zero value."); + } + + 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(stop - start) / step); + const auto max_i = static_cast(max_i_double); + + std::vector out; + out.reserve(max_i); + for (size_t i = 0; i < max_i; ++i) { + out.push_back(start + i * step); + } + + return out; +} + +template +std::vector linspace(const T start, const T stop, const size_t num) +{ + const auto start_double = static_cast(start); + const auto stop_double = static_cast(stop); + + if (num == 0) { + return {}; + } + + if (num == 1) { + return {start_double}; + } + + std::vector out; + out.reserve(num); + const double step = (stop_double - start_double) / static_cast(num - 1); + for (size_t i = 0; i < num; i++) { + out.push_back(start_double + static_cast(i) * step); + } + + return out; +} + +} // namespace autoware_utils_math + +#endif // AUTOWARE_UTILS_MATH__RANGE_HPP_ diff --git a/autoware_utils_math/include/autoware_utils_math/sin_table.hpp b/autoware_utils_math/include/autoware_utils_math/sin_table.hpp new file mode 100644 index 0000000..e85dd02 --- /dev/null +++ b/autoware_utils_math/include/autoware_utils_math/sin_table.hpp @@ -0,0 +1,30 @@ +// Copyright 2023 TIER IV, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef AUTOWARE_UTILS_MATH__SIN_TABLE_HPP_ +#define AUTOWARE_UTILS_MATH__SIN_TABLE_HPP_ + +#include + +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_math + +#endif // AUTOWARE_UTILS_MATH__SIN_TABLE_HPP_ diff --git a/autoware_utils_math/include/autoware_utils_math/trigonometry.hpp b/autoware_utils_math/include/autoware_utils_math/trigonometry.hpp new file mode 100644 index 0000000..a8c271a --- /dev/null +++ b/autoware_utils_math/include/autoware_utils_math/trigonometry.hpp @@ -0,0 +1,33 @@ +// Copyright 2023 TIER IV, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef AUTOWARE_UTILS_MATH__TRIGONOMETRY_HPP_ +#define AUTOWARE_UTILS_MATH__TRIGONOMETRY_HPP_ + +#include + +namespace autoware_utils_math +{ + +float sin(float radian); + +float cos(float radian); + +std::pair sin_and_cos(float radian); + +float opencv_fast_atan2(float dy, float dx); + +} // namespace autoware_utils_math + +#endif // AUTOWARE_UTILS_MATH__TRIGONOMETRY_HPP_ diff --git a/autoware_utils_math/include/autoware_utils_math/unit_conversion.hpp b/autoware_utils_math/include/autoware_utils_math/unit_conversion.hpp new file mode 100644 index 0000000..1173f4d --- /dev/null +++ b/autoware_utils_math/include/autoware_utils_math/unit_conversion.hpp @@ -0,0 +1,43 @@ +// Copyright 2020 TIER IV, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef AUTOWARE_UTILS_MATH__UNIT_CONVERSION_HPP_ +#define AUTOWARE_UTILS_MATH__UNIT_CONVERSION_HPP_ + +#include "autoware_utils_math/constants.hpp" + +namespace autoware_utils_math +{ +constexpr double deg2rad(const double deg) +{ + return deg * pi / 180.0; +} + +constexpr double rad2deg(const double rad) +{ + return rad * 180.0 / pi; +} + +constexpr double kmph2mps(const double kmph) +{ + return kmph * 1000.0 / 3600.0; +} + +constexpr double mps2kmph(const double mps) +{ + return mps * 3600.0 / 1000.0; +} +} // namespace autoware_utils_math + +#endif // AUTOWARE_UTILS_MATH__UNIT_CONVERSION_HPP_ diff --git a/autoware_utils_math/package.xml b/autoware_utils_math/package.xml new file mode 100644 index 0000000..396134f --- /dev/null +++ b/autoware_utils_math/package.xml @@ -0,0 +1,23 @@ + + + + autoware_utils_math + 1.1.0 + The autoware_utils_math package + Jian Kang + Ryohsuke Mitsudome + Esteve Fernandez + Yutaka Kondo + Takagi, Isamu + Apache License 2.0 + + ament_cmake_auto + autoware_cmake + + ament_lint_auto + autoware_lint_common + + + ament_cmake + + diff --git a/autoware_utils/src/math/sin_table.cpp b/autoware_utils_math/src/sin_table.cpp similarity index 99% rename from autoware_utils/src/math/sin_table.cpp rename to autoware_utils_math/src/sin_table.cpp index 7b733f2..12edf82 100644 --- a/autoware_utils/src/math/sin_table.cpp +++ b/autoware_utils_math/src/sin_table.cpp @@ -12,9 +12,9 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "autoware_utils/math/sin_table.hpp" +#include "autoware_utils_math/sin_table.hpp" -namespace autoware_utils +namespace autoware_utils_math { const float g_sin_table[sin_table_size] = { @@ -8212,4 +8212,4 @@ const float g_sin_table[sin_table_size] = { 0.9999999816164293f, 0.9999999896592414f, 0.9999999954041073f, 0.9999999988510269f, 1.0000000000000000f}; -} // namespace autoware_utils +} // namespace autoware_utils_math diff --git a/autoware_utils/src/math/trigonometry.cpp b/autoware_utils_math/src/trigonometry.cpp similarity index 77% rename from autoware_utils/src/math/trigonometry.cpp rename to autoware_utils_math/src/trigonometry.cpp index f7e5a9f..16c97c6 100644 --- a/autoware_utils/src/math/trigonometry.cpp +++ b/autoware_utils_math/src/trigonometry.cpp @@ -12,21 +12,21 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "autoware_utils/math/trigonometry.hpp" +#include "autoware_utils_math/trigonometry.hpp" -#include "autoware_utils/math/constants.hpp" -#include "autoware_utils/math/sin_table.hpp" +#include "autoware_utils_math/constants.hpp" +#include "autoware_utils_math/sin_table.hpp" #include #include -namespace autoware_utils +namespace autoware_utils_math { float sin(float radian) { - float degree = - radian * (180.f / static_cast(autoware_utils::pi)) * (discrete_arcs_num_360 / 360.f); + float degree = radian * (180.f / static_cast(autoware_utils_math::pi)) * + (discrete_arcs_num_360 / 360.f); size_t idx = (static_cast(std::round(degree)) % discrete_arcs_num_360 + discrete_arcs_num_360) % discrete_arcs_num_360; @@ -47,13 +47,13 @@ float sin(float radian) float cos(float radian) { - return sin(radian + static_cast(autoware_utils::pi) / 2.f); + return sin(radian + static_cast(autoware_utils_math::pi) / 2.f); } std::pair sin_and_cos(float radian) { constexpr float tmp = - (180.f / static_cast(autoware_utils::pi)) * (discrete_arcs_num_360 / 360.f); + (180.f / static_cast(autoware_utils_math::pi)) * (discrete_arcs_num_360 / 360.f); const float degree = radian * tmp; size_t idx = (static_cast(std::round(degree)) % discrete_arcs_num_360 + discrete_arcs_num_360) % @@ -78,7 +78,7 @@ std::pair sin_and_cos(float radian) // subject to the license terms in the LICENSE file found in the top-level directory of this // distribution and at http://opencv.org/license.html. // The license can be found in -// autoware_utils/third_party_licenses/opencv-license.md +// autoware_utils_math/third_party_licenses/opencv-license.md // and https://github.com/opencv/opencv/blob/master/LICENSE // Modification: @@ -86,10 +86,14 @@ std::pair sin_and_cos(float radian) // 2. output of the function is changed from degrees to radians. namespace detail_fast_atan2 { -static const float atan2_p1 = 0.9997878412794807f * static_cast(180) / autoware_utils::pi; -static const float atan2_p3 = -0.3258083974640975f * static_cast(180) / autoware_utils::pi; -static const float atan2_p5 = 0.1555786518463281f * static_cast(180) / autoware_utils::pi; -static const float atan2_p7 = -0.04432655554792128f * static_cast(180) / autoware_utils::pi; +static const float atan2_p1 = + 0.9997878412794807f * static_cast(180) / autoware_utils_math::pi; +static const float atan2_p3 = + -0.3258083974640975f * static_cast(180) / autoware_utils_math::pi; +static const float atan2_p5 = + 0.1555786518463281f * static_cast(180) / autoware_utils_math::pi; +static const float atan2_p7 = + -0.04432655554792128f * static_cast(180) / autoware_utils_math::pi; static const float atan2_DBL_EPSILON = 2.2204460492503131e-016f; } // namespace detail_fast_atan2 @@ -118,8 +122,8 @@ float opencv_fast_atan2(float dy, float dx) if (dx < 0) a = 180.f - a; if (dy < 0) a = 360.f - a; - a = a * autoware_utils::pi / 180.f; + a = a * autoware_utils_math::pi / 180.f; return a; } -} // namespace autoware_utils +} // namespace autoware_utils_math From e7676ba176d8699685f1cfc1b919106a38a69b9d Mon Sep 17 00:00:00 2001 From: "Takagi, Isamu" Date: Fri, 28 Feb 2025 00:42:36 +0900 Subject: [PATCH 2/7] add math test Signed-off-by: Takagi, Isamu --- autoware_utils_math/CMakeLists.txt | 2 + .../test/cases/accumulator.cpp | 41 +++++++++ .../test/cases/normalization.cpp | 54 +++++++++++ .../test/cases/range_arange.cpp | 61 +++++++++++++ .../test/cases/range_linspace.cpp | 49 ++++++++++ .../test/cases/trigonometry.cpp | 91 +++++++++++++++++++ .../test/cases/unit_conversion.cpp | 86 ++++++++++++++++++ autoware_utils_math/test/main.cpp | 21 +++++ 8 files changed, 405 insertions(+) create mode 100644 autoware_utils_math/test/cases/accumulator.cpp create mode 100644 autoware_utils_math/test/cases/normalization.cpp create mode 100644 autoware_utils_math/test/cases/range_arange.cpp create mode 100644 autoware_utils_math/test/cases/range_linspace.cpp create mode 100644 autoware_utils_math/test/cases/trigonometry.cpp create mode 100644 autoware_utils_math/test/cases/unit_conversion.cpp create mode 100644 autoware_utils_math/test/main.cpp diff --git a/autoware_utils_math/CMakeLists.txt b/autoware_utils_math/CMakeLists.txt index 20462bf..14ed6ad 100644 --- a/autoware_utils_math/CMakeLists.txt +++ b/autoware_utils_math/CMakeLists.txt @@ -10,6 +10,8 @@ ament_auto_add_library(${PROJECT_NAME} SHARED ) if(BUILD_TESTING) + file(GLOB_RECURSE test_files test/**/*.cpp) + ament_auto_add_gtest(test_${PROJECT_NAME} ${test_files}) endif() ament_auto_package() diff --git a/autoware_utils_math/test/cases/accumulator.cpp b/autoware_utils_math/test/cases/accumulator.cpp new file mode 100644 index 0000000..f28a255 --- /dev/null +++ b/autoware_utils_math/test/cases/accumulator.cpp @@ -0,0 +1,41 @@ +// 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. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include "autoware_utils_math/accumulator.hpp" + +#include + +TEST(TestAccumulator, Case1) +{ + autoware_utils_math::Accumulator a; + for (int i = 0; i < 10; ++i) { + a.add(i); + } + EXPECT_DOUBLE_EQ(a.count(), 10); + EXPECT_DOUBLE_EQ(a.min(), 0.0); + EXPECT_DOUBLE_EQ(a.max(), 9.0); + EXPECT_DOUBLE_EQ(a.mean(), 4.5); +} + +TEST(TestAccumulator, Case2) +{ + autoware_utils_math::Accumulator a; + for (int i = 0; i < 10; ++i) { + a.add(i + 1); + } + EXPECT_DOUBLE_EQ(a.count(), 10); + EXPECT_DOUBLE_EQ(a.min(), 1.0); + EXPECT_DOUBLE_EQ(a.max(), 10.0); + EXPECT_DOUBLE_EQ(a.mean(), 5.5); +} diff --git a/autoware_utils_math/test/cases/normalization.cpp b/autoware_utils_math/test/cases/normalization.cpp new file mode 100644 index 0000000..448e382 --- /dev/null +++ b/autoware_utils_math/test/cases/normalization.cpp @@ -0,0 +1,54 @@ +// 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. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include "autoware_utils_math/normalization.hpp" + +#include + +#include + +// The arguments a and b must be normalized. +double diff_radian(const double a, const double b) +{ + const auto diff = std::abs(a - b); + return std::min(diff, 2 * autoware_utils_math::pi - diff); +} + +TEST(TestNormalization, Degree) +{ + const double range_min = -180.0; + const double range_max = +180.0; + const double step = 30.0; + for (double a = range_min; a < range_max; a += step) { + for (int i = -3; i <= 3; ++i) { + const auto r = autoware_utils_math::normalize_degree(i * 360.0 + a); + EXPECT_DOUBLE_EQ(r, a); + } + } +} + +TEST(TestNormalization, Radian) +{ + using autoware_utils_math::pi; + const double range_min = -pi; + const double range_max = +pi; + const double step = pi / 6.0; + for (double a = range_min; a < range_max; a += step) { + for (int c = -3; c <= 3; ++c) { + const auto x = a + 2 * pi * c; + const auto r = autoware_utils_math::normalize_radian(x); + EXPECT_NEAR(0.0, diff_radian(r, a), 2e-15); + } + } +} diff --git a/autoware_utils_math/test/cases/range_arange.cpp b/autoware_utils_math/test/cases/range_arange.cpp new file mode 100644 index 0000000..3f04d3c --- /dev/null +++ b/autoware_utils_math/test/cases/range_arange.cpp @@ -0,0 +1,61 @@ +// 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. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include "autoware_utils_math/range.hpp" + +#include + +#include + +struct ParamArange +{ + double start; + double stop; + double step; + std::vector result; +}; + +class TestArange : public testing::TestWithParam +{ +}; + +TEST_P(TestArange, Normal) +{ + const auto p = GetParam(); + const auto v = autoware_utils_math::arange(p.start, p.stop, p.step); + EXPECT_EQ(v.size(), p.result.size()); + for (size_t i = 0; i < v.size(); ++i) { + EXPECT_DOUBLE_EQ(v.at(i), p.result.at(i)); + } +} + +TEST_P(TestArange, StepZero) +{ + const auto p = GetParam(); + EXPECT_THROW(autoware_utils_math::arange(p.start, p.stop, 0.0), std::invalid_argument); +} + +TEST_P(TestArange, StepInverse) +{ + const auto p = GetParam(); + EXPECT_THROW(autoware_utils_math::arange(p.start, p.stop, -p.step), std::invalid_argument); +} + +INSTANTIATE_TEST_CASE_P( + TestRange, TestArange, + testing::Values( + ParamArange{0.0, 1.0, +0.1, {0.0, 0.1, 0.2, 0.3, 0.4, 0.5, 0.6, 0.7, 0.8, 0.9}}, + ParamArange{0.0, 1.0, +0.2, {0.0, 0.2, 0.4, 0.6, 0.8}}, + ParamArange{1.0, 0.0, -0.1, {1.0, 0.9, 0.8, 0.7, 0.6, 0.5, 0.4, 0.3, 0.2, 0.1}}, + ParamArange{1.0, 0.0, -0.2, {1.0, 0.8, 0.6, 0.4, 0.2}})); diff --git a/autoware_utils_math/test/cases/range_linspace.cpp b/autoware_utils_math/test/cases/range_linspace.cpp new file mode 100644 index 0000000..44df8b5 --- /dev/null +++ b/autoware_utils_math/test/cases/range_linspace.cpp @@ -0,0 +1,49 @@ +// 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. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include "autoware_utils_math/range.hpp" + +#include + +#include + +struct ParamLinspace +{ + double start; + double stop; + size_t num; + std::vector result; +}; + +class TestLinspace : public testing::TestWithParam +{ +}; + +TEST_P(TestLinspace, Normal) +{ + const auto p = GetParam(); + const auto v = autoware_utils_math::linspace(p.start, p.stop, p.num); + EXPECT_EQ(v.size(), p.result.size()); + for (size_t i = 0; i < v.size(); ++i) { + EXPECT_DOUBLE_EQ(v.at(i), p.result.at(i)); + } +} + +INSTANTIATE_TEST_CASE_P( + TestRange, TestLinspace, + testing::Values( + ParamLinspace{0.0, 1.0, 11, {0.0, 0.1, 0.2, 0.3, 0.4, 0.5, 0.6, 0.7, 0.8, 0.9, 1.0}}, + ParamLinspace{1.0, 0.0, 11, {1.0, 0.9, 0.8, 0.7, 0.6, 0.5, 0.4, 0.3, 0.2, 0.1, 0.0}}, + ParamLinspace{0.0, 1.0, 6, {0.0, 0.2, 0.4, 0.6, 0.8, 1.0}}, + ParamLinspace{1.0, 0.0, 6, {1.0, 0.8, 0.6, 0.4, 0.2, 0.0}})); diff --git a/autoware_utils_math/test/cases/trigonometry.cpp b/autoware_utils_math/test/cases/trigonometry.cpp new file mode 100644 index 0000000..d9fbed0 --- /dev/null +++ b/autoware_utils_math/test/cases/trigonometry.cpp @@ -0,0 +1,91 @@ +// 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. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include "autoware_utils_math/trigonometry.hpp" + +#include "autoware_utils_math/constants.hpp" + +#include + +#include +#include + +struct ParamSinCos +{ + float radian; + float sin; + float cos; +}; + +std::vector make_test_cases(double min, double max, size_t size) +{ + const double step = (max - min) / size; + std::vector params; + for (size_t i = 0; i < size; ++i) { + ParamSinCos p; + p.radian = min + i * step; + p.sin = std::sin(p.radian); + p.cos = std::cos(p.radian); + params.push_back(p); + } + return params; +} + +std::vector make_normalized_cases() +{ + using autoware_utils_math::pi; + return make_test_cases(-pi * 1, +pi * 1, 1000); +} + +std::vector make_periodic_cases() +{ + using autoware_utils_math::pi; + return make_test_cases(-pi * 3, +pi * 3, 100); +} + +TEST(TestTrigonometry, SinCos) +{ + std::vector params; + { + const auto cases1 = make_normalized_cases(); + const auto cases2 = make_periodic_cases(); + params.insert(params.end(), cases1.begin(), cases1.end()); + params.insert(params.end(), cases2.begin(), cases2.end()); + } + + for (const auto & p : params) { + const auto sin1 = autoware_utils_math::sin(p.radian); + const auto cos1 = autoware_utils_math::cos(p.radian); + const auto [sin2, cos2] = autoware_utils_math::sin_and_cos(p.radian); + constexpr double eps = 3e-5; + EXPECT_NEAR(sin1, p.sin, eps); + EXPECT_NEAR(cos1, p.cos, eps); + EXPECT_NEAR(sin2, p.sin, eps); + EXPECT_NEAR(cos2, p.cos, eps); + } +} + +TEST(TestTrigonometry, Atan2) +{ + for (const auto & p : make_normalized_cases()) { + const auto r0 = 0 <= p.radian ? p.radian : p.radian + 2 * autoware_utils_math::pi; + const auto r1 = autoware_utils_math::opencv_fast_atan2(p.sin * 0.5, p.cos * 0.5); + const auto r2 = autoware_utils_math::opencv_fast_atan2(p.sin * 1.0, p.cos * 1.0); + const auto r3 = autoware_utils_math::opencv_fast_atan2(p.sin * 1.5, p.cos * 1.5); + constexpr double eps = 2e-4; + EXPECT_NEAR(r1, r0, eps); + EXPECT_NEAR(r2, r0, eps); + EXPECT_NEAR(r3, r0, eps); + } +} diff --git a/autoware_utils_math/test/cases/unit_conversion.cpp b/autoware_utils_math/test/cases/unit_conversion.cpp new file mode 100644 index 0000000..fe40d41 --- /dev/null +++ b/autoware_utils_math/test/cases/unit_conversion.cpp @@ -0,0 +1,86 @@ +// 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. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include "autoware_utils_math/unit_conversion.hpp" + +#include + +#include + +struct ParamAngle +{ + double radian; + double degree; +}; + +struct ParamSpeed +{ + double mps; + double kmph; +}; + +std::vector make_angle_cases() +{ + using autoware_utils_math::pi; + const double range_min = -360 * 3; + const double range_max = +360 * 3; + const double step = 10; + + std::vector params; + for (double i = range_min; i <= range_max; i += step) { + params.push_back({i * pi / 180.0, i}); + } + return params; +} + +std::vector make_speed_cases() +{ + const double range_min = -70; + const double range_max = +70; + const double step = 7; + + std::vector params; + for (double i = range_min; i <= range_max; i += step) { + params.push_back({i / 3.6, i}); + } + return params; +} + +TEST(TestUnitConversion, DegToRad) +{ + for (const auto & p : make_angle_cases()) { + EXPECT_DOUBLE_EQ(p.radian, autoware_utils_math::deg2rad(p.degree)); + } +} + +TEST(TestUnitConversion, RadToDeg) +{ + for (const auto & p : make_angle_cases()) { + EXPECT_DOUBLE_EQ(p.degree, autoware_utils_math::rad2deg(p.radian)); + } +} + +TEST(TestUnitConversion, KmphToMps) +{ + for (const auto & p : make_speed_cases()) { + EXPECT_DOUBLE_EQ(p.mps, autoware_utils_math::kmph2mps(p.kmph)); + } +} + +TEST(TestUnitConversion, MpsToKmph) +{ + for (const auto & p : make_speed_cases()) { + EXPECT_DOUBLE_EQ(p.kmph, autoware_utils_math::mps2kmph(p.mps)); + } +} diff --git a/autoware_utils_math/test/main.cpp b/autoware_utils_math/test/main.cpp new file mode 100644 index 0000000..7c283b2 --- /dev/null +++ b/autoware_utils_math/test/main.cpp @@ -0,0 +1,21 @@ +// 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. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include + +int main(int argc, char ** argv) +{ + testing::InitGoogleTest(&argc, argv); + return RUN_ALL_TESTS(); +} From 1de1415272a93699e3c6f006fb919fe0b92e3bc2 Mon Sep 17 00:00:00 2001 From: "Takagi, Isamu" Date: Fri, 28 Feb 2025 07:47:57 +0900 Subject: [PATCH 3/7] update readme Signed-off-by: Takagi, Isamu --- autoware_utils/README.md | 11 ----------- autoware_utils_math/README.md | 11 ++++++++--- 2 files changed, 8 insertions(+), 14 deletions(-) diff --git a/autoware_utils/README.md b/autoware_utils/README.md index a5597e9..9b62acd 100644 --- a/autoware_utils/README.md +++ b/autoware_utils/README.md @@ -29,17 +29,6 @@ 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: diff --git a/autoware_utils_math/README.md b/autoware_utils_math/README.md index 908dd4c..cdf8d96 100644 --- a/autoware_utils_math/README.md +++ b/autoware_utils_math/README.md @@ -3,9 +3,14 @@ ## Overview The **autoware_utils_math** library is a comprehensive toolkit designed to facilitate the development of autonomous driving applications. -This package provides essential utilities for geometry. -It is extensively used in the Autoware project to handle common tasks such as geometric calculations and message conversions. +This package provides essential utilities for mathematics. +It is extensively used in the Autoware project to handle common tasks such as value normalization and unit conversion. ## Design -- **`transform_listener.hpp`**: Manages transformation listeners. +- **`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). From c6a5425edfd0c3f6e6f11398adac6650f752e32c Mon Sep 17 00:00:00 2001 From: "Takagi, Isamu" Date: Fri, 28 Feb 2025 16:54:06 +0900 Subject: [PATCH 4/7] fix readme Signed-off-by: Takagi, Isamu --- autoware_utils_math/README.md | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/autoware_utils_math/README.md b/autoware_utils_math/README.md index cdf8d96..a767029 100644 --- a/autoware_utils_math/README.md +++ b/autoware_utils_math/README.md @@ -2,7 +2,7 @@ ## Overview -The **autoware_utils_math** library is a comprehensive toolkit designed to facilitate the development of autonomous driving applications. +The **autoware_utils** library is a comprehensive toolkit designed to facilitate the development of autonomous driving applications. This package provides essential utilities for mathematics. It is extensively used in the Autoware project to handle common tasks such as value normalization and unit conversion. From e18aa01d92aed71fe60464ceaf230d4f3c62443e Mon Sep 17 00:00:00 2001 From: "Takagi, Isamu" Date: Fri, 28 Feb 2025 17:53:28 +0900 Subject: [PATCH 5/7] move readme code snippets Signed-off-by: Takagi, Isamu --- autoware_utils/README.md | 21 --------------------- autoware_utils_math/README.md | 24 ++++++++++++++++++++++++ 2 files changed, 24 insertions(+), 21 deletions(-) diff --git a/autoware_utils/README.md b/autoware_utils/README.md index 9b62acd..62b55a4 100644 --- a/autoware_utils/README.md +++ b/autoware_utils/README.md @@ -99,27 +99,6 @@ int main() { } ``` -#### Using Accumulator from accumulator.hpp - -```cpp -#include "autoware_utils/math/accumulator.hpp" - -int main() { - autoware_utils::Accumulator 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 diff --git a/autoware_utils_math/README.md b/autoware_utils_math/README.md index a767029..f36fc94 100644 --- a/autoware_utils_math/README.md +++ b/autoware_utils_math/README.md @@ -14,3 +14,27 @@ It is extensively used in the Autoware project to handle common tasks such as va - **`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). + +## Example Code Snippets + +### Using Accumulator from accumulator.hpp + +```cpp +#include + +int main() +{ + autoware_utils_math::Accumulator 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; +} +``` From fba6738f9acbf8289150c2a9d4d45405eebd3594 Mon Sep 17 00:00:00 2001 From: "Takagi, Isamu" Date: Mon, 3 Mar 2025 14:58:54 +0900 Subject: [PATCH 6/7] add deprecated message Signed-off-by: Takagi, Isamu --- .../include/autoware_utils/math/accumulator.hpp | 13 +++++++------ .../include/autoware_utils/math/constants.hpp | 13 +++++++------ .../include/autoware_utils/math/normalization.hpp | 13 +++++++------ .../include/autoware_utils/math/range.hpp | 13 +++++++------ .../include/autoware_utils/math/sin_table.hpp | 13 +++++++------ .../include/autoware_utils/math/trigonometry.hpp | 13 +++++++------ .../include/autoware_utils/math/unit_conversion.hpp | 13 +++++++------ 7 files changed, 49 insertions(+), 42 deletions(-) diff --git a/autoware_utils/include/autoware_utils/math/accumulator.hpp b/autoware_utils/include/autoware_utils/math/accumulator.hpp index 7b39c5e..ba8cb90 100644 --- a/autoware_utils/include/autoware_utils/math/accumulator.hpp +++ b/autoware_utils/include/autoware_utils/math/accumulator.hpp @@ -15,13 +15,14 @@ #ifndef AUTOWARE_UTILS__MATH__ACCUMULATOR_HPP_ #define AUTOWARE_UTILS__MATH__ACCUMULATOR_HPP_ -#include - -namespace autoware_utils -{ +// NOLINTBEGIN(build/namespaces, whitespace/line_length) +// clang-format off -using namespace autoware_utils_math; // NOLINT(build/namespaces) +#pragma message("#include is deprecated. Use #include instead.") +#include +namespace autoware_utils { using namespace autoware_utils_math; } -} // namespace autoware_utils +// clang-format on +// NOLINTEND #endif // AUTOWARE_UTILS__MATH__ACCUMULATOR_HPP_ diff --git a/autoware_utils/include/autoware_utils/math/constants.hpp b/autoware_utils/include/autoware_utils/math/constants.hpp index e065b4c..220aeac 100644 --- a/autoware_utils/include/autoware_utils/math/constants.hpp +++ b/autoware_utils/include/autoware_utils/math/constants.hpp @@ -15,13 +15,14 @@ #ifndef AUTOWARE_UTILS__MATH__CONSTANTS_HPP_ #define AUTOWARE_UTILS__MATH__CONSTANTS_HPP_ -#include - -namespace autoware_utils -{ +// NOLINTBEGIN(build/namespaces, whitespace/line_length) +// clang-format off -using namespace autoware_utils_math; // NOLINT(build/namespaces) +#pragma message("#include is deprecated. Use #include instead.") +#include +namespace autoware_utils { using namespace autoware_utils_math; } -} // namespace autoware_utils +// clang-format on +// NOLINTEND #endif // AUTOWARE_UTILS__MATH__CONSTANTS_HPP_ diff --git a/autoware_utils/include/autoware_utils/math/normalization.hpp b/autoware_utils/include/autoware_utils/math/normalization.hpp index 00e5c88..d209905 100644 --- a/autoware_utils/include/autoware_utils/math/normalization.hpp +++ b/autoware_utils/include/autoware_utils/math/normalization.hpp @@ -15,13 +15,14 @@ #ifndef AUTOWARE_UTILS__MATH__NORMALIZATION_HPP_ #define AUTOWARE_UTILS__MATH__NORMALIZATION_HPP_ -#include - -namespace autoware_utils -{ +// NOLINTBEGIN(build/namespaces, whitespace/line_length) +// clang-format off -using namespace autoware_utils_math; // NOLINT(build/namespaces) +#pragma message("#include is deprecated. Use #include instead.") +#include +namespace autoware_utils { using namespace autoware_utils_math; } -} // namespace autoware_utils +// clang-format on +// NOLINTEND #endif // AUTOWARE_UTILS__MATH__NORMALIZATION_HPP_ diff --git a/autoware_utils/include/autoware_utils/math/range.hpp b/autoware_utils/include/autoware_utils/math/range.hpp index 8184f1d..4bef194 100644 --- a/autoware_utils/include/autoware_utils/math/range.hpp +++ b/autoware_utils/include/autoware_utils/math/range.hpp @@ -15,13 +15,14 @@ #ifndef AUTOWARE_UTILS__MATH__RANGE_HPP_ #define AUTOWARE_UTILS__MATH__RANGE_HPP_ -#include - -namespace autoware_utils -{ +// NOLINTBEGIN(build/namespaces, whitespace/line_length) +// clang-format off -using namespace autoware_utils_math; // NOLINT(build/namespaces) +#pragma message("#include is deprecated. Use #include instead.") +#include +namespace autoware_utils { using namespace autoware_utils_math; } -} // namespace autoware_utils +// clang-format on +// NOLINTEND #endif // AUTOWARE_UTILS__MATH__RANGE_HPP_ diff --git a/autoware_utils/include/autoware_utils/math/sin_table.hpp b/autoware_utils/include/autoware_utils/math/sin_table.hpp index 3605d5c..71d9ba9 100644 --- a/autoware_utils/include/autoware_utils/math/sin_table.hpp +++ b/autoware_utils/include/autoware_utils/math/sin_table.hpp @@ -15,13 +15,14 @@ #ifndef AUTOWARE_UTILS__MATH__SIN_TABLE_HPP_ #define AUTOWARE_UTILS__MATH__SIN_TABLE_HPP_ -#include - -namespace autoware_utils -{ +// NOLINTBEGIN(build/namespaces, whitespace/line_length) +// clang-format off -using namespace autoware_utils_math; // NOLINT(build/namespaces) +#pragma message("#include is deprecated. Use #include instead.") +#include +namespace autoware_utils { using namespace autoware_utils_math; } -} // namespace autoware_utils +// clang-format on +// NOLINTEND #endif // AUTOWARE_UTILS__MATH__SIN_TABLE_HPP_ diff --git a/autoware_utils/include/autoware_utils/math/trigonometry.hpp b/autoware_utils/include/autoware_utils/math/trigonometry.hpp index 046a5f3..ff04c43 100644 --- a/autoware_utils/include/autoware_utils/math/trigonometry.hpp +++ b/autoware_utils/include/autoware_utils/math/trigonometry.hpp @@ -15,13 +15,14 @@ #ifndef AUTOWARE_UTILS__MATH__TRIGONOMETRY_HPP_ #define AUTOWARE_UTILS__MATH__TRIGONOMETRY_HPP_ -#include - -namespace autoware_utils -{ +// NOLINTBEGIN(build/namespaces, whitespace/line_length) +// clang-format off -using namespace autoware_utils_math; // NOLINT(build/namespaces) +#pragma message("#include is deprecated. Use #include instead.") +#include +namespace autoware_utils { using namespace autoware_utils_math; } -} // namespace autoware_utils +// clang-format on +// NOLINTEND #endif // AUTOWARE_UTILS__MATH__TRIGONOMETRY_HPP_ diff --git a/autoware_utils/include/autoware_utils/math/unit_conversion.hpp b/autoware_utils/include/autoware_utils/math/unit_conversion.hpp index 8d1647b..5cb15f6 100644 --- a/autoware_utils/include/autoware_utils/math/unit_conversion.hpp +++ b/autoware_utils/include/autoware_utils/math/unit_conversion.hpp @@ -15,13 +15,14 @@ #ifndef AUTOWARE_UTILS__MATH__UNIT_CONVERSION_HPP_ #define AUTOWARE_UTILS__MATH__UNIT_CONVERSION_HPP_ -#include - -namespace autoware_utils -{ +// NOLINTBEGIN(build/namespaces, whitespace/line_length) +// clang-format off -using namespace autoware_utils_math; // NOLINT(build/namespaces) +#pragma message("#include is deprecated. Use #include instead.") +#include +namespace autoware_utils { using namespace autoware_utils_math; } -} // namespace autoware_utils +// clang-format on +// NOLINTEND #endif // AUTOWARE_UTILS__MATH__UNIT_CONVERSION_HPP_ From 6cd871b9391271201a8279a2649e75c156f6d975 Mon Sep 17 00:00:00 2001 From: "Takagi, Isamu" Date: Mon, 3 Mar 2025 15:46:37 +0900 Subject: [PATCH 7/7] fix test file glob Signed-off-by: Takagi, Isamu --- autoware_utils_math/CMakeLists.txt | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/autoware_utils_math/CMakeLists.txt b/autoware_utils_math/CMakeLists.txt index 14ed6ad..b833a3e 100644 --- a/autoware_utils_math/CMakeLists.txt +++ b/autoware_utils_math/CMakeLists.txt @@ -10,7 +10,7 @@ ament_auto_add_library(${PROJECT_NAME} SHARED ) if(BUILD_TESTING) - file(GLOB_RECURSE test_files test/**/*.cpp) + file(GLOB_RECURSE test_files test/*.cpp) ament_auto_add_gtest(test_${PROJECT_NAME} ${test_files}) endif()