From 5099343d742822759b762128cf7adb933f5d3db3 Mon Sep 17 00:00:00 2001 From: "Takagi, Isamu" <43976882+isamu-takagi@users.noreply.github.com> Date: Thu, 6 Mar 2025 15:06:00 +0900 Subject: [PATCH 1/2] feat(autoware_utils_math): split package (#33) * feat(autoware_utils_math): split package Signed-off-by: Takagi, Isamu * add math test Signed-off-by: Takagi, Isamu * update readme Signed-off-by: Takagi, Isamu * fix readme Signed-off-by: Takagi, Isamu * move readme code snippets Signed-off-by: Takagi, Isamu * add deprecated message Signed-off-by: Takagi, Isamu * fix test file glob Signed-off-by: Takagi, Isamu --------- Signed-off-by: Takagi, Isamu --- autoware_utils/README.md | 32 ------- .../autoware_utils/math/accumulator.hpp | 81 ++-------------- .../include/autoware_utils/math/constants.hpp | 16 ++-- .../autoware_utils/math/normalization.hpp | 38 ++------ .../include/autoware_utils/math/range.hpp | 66 ++----------- .../include/autoware_utils/math/sin_table.hpp | 18 ++-- .../autoware_utils/math/trigonometry.hpp | 21 ++--- .../autoware_utils/math/unit_conversion.hpp | 31 ++----- autoware_utils/package.xml | 1 + autoware_utils_math/CMakeLists.txt | 17 ++++ autoware_utils_math/README.md | 40 ++++++++ .../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 ++++--- .../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 +++++ 28 files changed, 917 insertions(+), 263 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%) 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/README.md b/autoware_utils/README.md index a5597e9..62b55a4 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: @@ -110,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/include/autoware_utils/math/accumulator.hpp b/autoware_utils/include/autoware_utils/math/accumulator.hpp index 4a2225e..ba8cb90 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,82 +12,17 @@ // 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_ -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; -}; +// NOLINTBEGIN(build/namespaces, whitespace/line_length) +// clang-format off -/** - * @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; -} +#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 e86392a..220aeac 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,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 is deprecated. Use #include instead.") +#include +namespace autoware_utils { using namespace autoware_utils_math; } + +// 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 bb913aa..d209905 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,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 +#pragma message("#include is deprecated. Use #include instead.") +#include +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_ diff --git a/autoware_utils/include/autoware_utils/math/range.hpp b/autoware_utils/include/autoware_utils/math/range.hpp index da899a3..4bef194 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,64 +15,14 @@ #ifndef AUTOWARE_UTILS__MATH__RANGE_HPP_ #define AUTOWARE_UTILS__MATH__RANGE_HPP_ -#include -#include -#include -#include +// NOLINTBEGIN(build/namespaces, whitespace/line_length) +// clang-format off -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."); - } +#pragma message("#include is deprecated. Use #include instead.") +#include +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(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 +// 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 bb8dac5..71d9ba9 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,16 +15,14 @@ #ifndef AUTOWARE_UTILS__MATH__SIN_TABLE_HPP_ #define AUTOWARE_UTILS__MATH__SIN_TABLE_HPP_ -#include +// NOLINTBEGIN(build/namespaces, whitespace/line_length) +// clang-format off -namespace autoware_utils -{ +#pragma message("#include is deprecated. Use #include instead.") +#include +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_ diff --git a/autoware_utils/include/autoware_utils/math/trigonometry.hpp b/autoware_utils/include/autoware_utils/math/trigonometry.hpp index 8ee5930..ff04c43 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,19 +15,14 @@ #ifndef AUTOWARE_UTILS__MATH__TRIGONOMETRY_HPP_ #define AUTOWARE_UTILS__MATH__TRIGONOMETRY_HPP_ -#include +// NOLINTBEGIN(build/namespaces, whitespace/line_length) +// clang-format off -namespace autoware_utils -{ +#pragma message("#include is deprecated. Use #include instead.") +#include +namespace autoware_utils { using 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 +// 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 97db93f..5cb15f6 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,14 @@ #ifndef AUTOWARE_UTILS__MATH__UNIT_CONVERSION_HPP_ #define AUTOWARE_UTILS__MATH__UNIT_CONVERSION_HPP_ -#include "autoware_utils/math/constants.hpp" +// NOLINTBEGIN(build/namespaces, whitespace/line_length) +// clang-format off -namespace autoware_utils -{ -constexpr double deg2rad(const double deg) -{ - return deg * pi / 180.0; -} +#pragma message("#include is deprecated. Use #include instead.") +#include +namespace autoware_utils { using namespace autoware_utils_math; } -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 +// clang-format on +// NOLINTEND #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..b833a3e --- /dev/null +++ b/autoware_utils_math/CMakeLists.txt @@ -0,0 +1,17 @@ +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) + 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/README.md b/autoware_utils_math/README.md new file mode 100644 index 0000000..f36fc94 --- /dev/null +++ b/autoware_utils_math/README.md @@ -0,0 +1,40 @@ +# autoware_utils_math + +## Overview + +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. + +## Design + +- **`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). + +## 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; +} +``` 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 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 417b3ccca652480c204603bf70db48db12b68adf Mon Sep 17 00:00:00 2001 From: "Takagi, Isamu" <43976882+isamu-takagi@users.noreply.github.com> Date: Thu, 6 Mar 2025 15:06:27 +0900 Subject: [PATCH 2/2] feat(autoware_utils_visualization): split package (#36) * feat(autoware_utils_visualization): split package Signed-off-by: Takagi, Isamu * add deprecated message Signed-off-by: Takagi, Isamu * fix test file glob Signed-off-by: Takagi, Isamu --------- Signed-off-by: Takagi, Isamu --- autoware_utils/README.md | 1 - .../autoware_utils/ros/marker_helper.hpp | 69 ++-------- autoware_utils/package.xml | 2 +- autoware_utils_visualization/CMakeLists.txt | 16 +++ autoware_utils_visualization/README.md | 11 ++ .../marker_helper.hpp | 81 ++++++++++++ autoware_utils_visualization/package.xml | 26 ++++ .../src}/marker_helper.cpp | 6 +- .../test/cases/marker_helper.cpp | 118 ++++++++++++++++++ autoware_utils_visualization/test/main.cpp | 21 ++++ 10 files changed, 285 insertions(+), 66 deletions(-) create mode 100644 autoware_utils_visualization/CMakeLists.txt create mode 100644 autoware_utils_visualization/README.md create mode 100644 autoware_utils_visualization/include/autoware_utils_visualization/marker_helper.hpp create mode 100644 autoware_utils_visualization/package.xml rename {autoware_utils/src/ros => autoware_utils_visualization/src}/marker_helper.cpp (93%) create mode 100644 autoware_utils_visualization/test/cases/marker_helper.cpp create mode 100644 autoware_utils_visualization/test/main.cpp diff --git a/autoware_utils/README.md b/autoware_utils/README.md index 62b55a4..ce66b83 100644 --- a/autoware_utils/README.md +++ b/autoware_utils/README.md @@ -36,7 +36,6 @@ 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. diff --git a/autoware_utils/include/autoware_utils/ros/marker_helper.hpp b/autoware_utils/include/autoware_utils/ros/marker_helper.hpp index bc2c015..5224947 100644 --- a/autoware_utils/include/autoware_utils/ros/marker_helper.hpp +++ b/autoware_utils/include/autoware_utils/ros/marker_helper.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,67 +15,14 @@ #ifndef AUTOWARE_UTILS__ROS__MARKER_HELPER_HPP_ #define AUTOWARE_UTILS__ROS__MARKER_HELPER_HPP_ -#include +// NOLINTBEGIN(build/namespaces, whitespace/line_length) +// clang-format off -#include +#pragma message("#include is deprecated. Use #include instead.") +#include +namespace autoware_utils { using namespace autoware_utils_visualization; } -#include -#include - -namespace autoware_utils -{ -inline geometry_msgs::msg::Point create_marker_position(double x, double y, double z) -{ - geometry_msgs::msg::Point point; - point.x = x; - point.y = y; - point.z = z; - return point; -} - -inline geometry_msgs::msg::Quaternion create_marker_orientation( - double x, double y, double z, double w) -{ - geometry_msgs::msg::Quaternion quaternion; - quaternion.x = x; - quaternion.y = y; - quaternion.z = z; - quaternion.w = w; - return quaternion; -} - -inline geometry_msgs::msg::Vector3 create_marker_scale(double x, double y, double z) -{ - geometry_msgs::msg::Vector3 scale; - scale.x = x; - scale.y = y; - scale.z = z; - return scale; -} - -inline std_msgs::msg::ColorRGBA create_marker_color(float r, float g, float b, float a) -{ - std_msgs::msg::ColorRGBA color; - color.r = r; - color.g = g; - color.b = b; - color.a = a; - return color; -} - -visualization_msgs::msg::Marker create_default_marker( - const std::string & frame_id, const rclcpp::Time & now, const std::string & ns, const int32_t id, - const int32_t type, const geometry_msgs::msg::Vector3 & scale, - const std_msgs::msg::ColorRGBA & color); - -visualization_msgs::msg::Marker create_deleted_default_marker( - const rclcpp::Time & now, const std::string & ns, const int32_t id); - -void append_marker_array( - const visualization_msgs::msg::MarkerArray & additional_marker_array, - visualization_msgs::msg::MarkerArray * marker_array, - const std::optional & current_time = {}); - -} // namespace autoware_utils +// clang-format on +// NOLINTEND #endif // AUTOWARE_UTILS__ROS__MARKER_HELPER_HPP_ diff --git a/autoware_utils/package.xml b/autoware_utils/package.xml index ea53333..a8ae066 100644 --- a/autoware_utils/package.xml +++ b/autoware_utils/package.xml @@ -22,6 +22,7 @@ autoware_utils_geometry autoware_utils_math autoware_utils_pcl + autoware_utils_visualization autoware_vehicle_msgs builtin_interfaces diagnostic_msgs @@ -33,7 +34,6 @@ tf2_eigen tf2_geometry_msgs unique_identifier_msgs - visualization_msgs ament_cmake_ros ament_lint_auto diff --git a/autoware_utils_visualization/CMakeLists.txt b/autoware_utils_visualization/CMakeLists.txt new file mode 100644 index 0000000..f76e9c8 --- /dev/null +++ b/autoware_utils_visualization/CMakeLists.txt @@ -0,0 +1,16 @@ +cmake_minimum_required(VERSION 3.14) +project(autoware_utils_visualization) + +find_package(autoware_cmake REQUIRED) +autoware_package() + +ament_auto_add_library(${PROJECT_NAME} SHARED + "src/marker_helper.cpp" +) + +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_visualization/README.md b/autoware_utils_visualization/README.md new file mode 100644 index 0000000..7097932 --- /dev/null +++ b/autoware_utils_visualization/README.md @@ -0,0 +1,11 @@ +# autoware_utils_visualization + +## Overview + +The **autoware_utils** library is a comprehensive toolkit designed to facilitate the development of autonomous driving applications. +This package provides essential utilities for visualization. +It is extensively used in the Autoware project to handle common tasks such as creating markers for RViz. + +## Design + +- **`marker_helper.hpp`**: Helper functions for creating and manipulating visualization markers. diff --git a/autoware_utils_visualization/include/autoware_utils_visualization/marker_helper.hpp b/autoware_utils_visualization/include/autoware_utils_visualization/marker_helper.hpp new file mode 100644 index 0000000..6448f49 --- /dev/null +++ b/autoware_utils_visualization/include/autoware_utils_visualization/marker_helper.hpp @@ -0,0 +1,81 @@ +// 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_VISUALIZATION__MARKER_HELPER_HPP_ +#define AUTOWARE_UTILS_VISUALIZATION__MARKER_HELPER_HPP_ + +#include + +#include + +#include +#include + +namespace autoware_utils_visualization +{ +inline geometry_msgs::msg::Point create_marker_position(double x, double y, double z) +{ + geometry_msgs::msg::Point point; + point.x = x; + point.y = y; + point.z = z; + return point; +} + +inline geometry_msgs::msg::Quaternion create_marker_orientation( + double x, double y, double z, double w) +{ + geometry_msgs::msg::Quaternion quaternion; + quaternion.x = x; + quaternion.y = y; + quaternion.z = z; + quaternion.w = w; + return quaternion; +} + +inline geometry_msgs::msg::Vector3 create_marker_scale(double x, double y, double z) +{ + geometry_msgs::msg::Vector3 scale; + scale.x = x; + scale.y = y; + scale.z = z; + return scale; +} + +inline std_msgs::msg::ColorRGBA create_marker_color(float r, float g, float b, float a) +{ + std_msgs::msg::ColorRGBA color; + color.r = r; + color.g = g; + color.b = b; + color.a = a; + return color; +} + +visualization_msgs::msg::Marker create_default_marker( + const std::string & frame_id, const rclcpp::Time & now, const std::string & ns, const int32_t id, + const int32_t type, const geometry_msgs::msg::Vector3 & scale, + const std_msgs::msg::ColorRGBA & color); + +visualization_msgs::msg::Marker create_deleted_default_marker( + const rclcpp::Time & now, const std::string & ns, const int32_t id); + +void append_marker_array( + const visualization_msgs::msg::MarkerArray & additional_marker_array, + visualization_msgs::msg::MarkerArray * marker_array, + const std::optional & current_time = {}); + +} // namespace autoware_utils_visualization + +#endif // AUTOWARE_UTILS_VISUALIZATION__MARKER_HELPER_HPP_ diff --git a/autoware_utils_visualization/package.xml b/autoware_utils_visualization/package.xml new file mode 100644 index 0000000..1fb33fb --- /dev/null +++ b/autoware_utils_visualization/package.xml @@ -0,0 +1,26 @@ + + + + autoware_utils_visualization + 1.1.0 + The autoware_utils_visualization package + Jian Kang + Ryohsuke Mitsudome + Esteve Fernandez + Yutaka Kondo + Takagi, Isamu + Apache License 2.0 + + ament_cmake_auto + autoware_cmake + + rclcpp + visualization_msgs + + ament_lint_auto + autoware_lint_common + + + ament_cmake + + diff --git a/autoware_utils/src/ros/marker_helper.cpp b/autoware_utils_visualization/src/marker_helper.cpp similarity index 93% rename from autoware_utils/src/ros/marker_helper.cpp rename to autoware_utils_visualization/src/marker_helper.cpp index a6c2fa6..ddd9385 100644 --- a/autoware_utils/src/ros/marker_helper.cpp +++ b/autoware_utils_visualization/src/marker_helper.cpp @@ -12,11 +12,11 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "autoware_utils/ros/marker_helper.hpp" +#include "autoware_utils_visualization/marker_helper.hpp" #include -namespace autoware_utils +namespace autoware_utils_visualization { visualization_msgs::msg::Marker create_default_marker( const std::string & frame_id, const rclcpp::Time & now, const std::string & ns, const int32_t id, @@ -69,4 +69,4 @@ void append_marker_array( } } -} // namespace autoware_utils +} // namespace autoware_utils_visualization diff --git a/autoware_utils_visualization/test/cases/marker_helper.cpp b/autoware_utils_visualization/test/cases/marker_helper.cpp new file mode 100644 index 0000000..67afdf9 --- /dev/null +++ b/autoware_utils_visualization/test/cases/marker_helper.cpp @@ -0,0 +1,118 @@ +// 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_visualization/marker_helper.hpp" + +#include + +TEST(TestMarkerHelper, CreatePosition) +{ + const auto r = autoware_utils_visualization::create_marker_position(0.1, 0.2, 0.3); + EXPECT_DOUBLE_EQ(r.x, 0.1); + EXPECT_DOUBLE_EQ(r.y, 0.2); + EXPECT_DOUBLE_EQ(r.z, 0.3); +} + +TEST(TestMarkerHelper, CreateOrientation) +{ + const auto r = autoware_utils_visualization::create_marker_orientation(0.1, 0.2, 0.3, 0.4); + EXPECT_DOUBLE_EQ(r.x, 0.1); + EXPECT_DOUBLE_EQ(r.y, 0.2); + EXPECT_DOUBLE_EQ(r.z, 0.3); + EXPECT_DOUBLE_EQ(r.w, 0.4); +} + +TEST(TestMarkerHelper, CreateScale) +{ + const auto r = autoware_utils_visualization::create_marker_scale(0.1, 0.2, 0.3); + EXPECT_DOUBLE_EQ(r.x, 0.1); + EXPECT_DOUBLE_EQ(r.y, 0.2); + EXPECT_DOUBLE_EQ(r.z, 0.3); +} + +TEST(TestMarkerHelper, CreateColor) +{ + const auto r = autoware_utils_visualization::create_marker_color(0.1, 0.2, 0.3, 0.4); + EXPECT_FLOAT_EQ(r.r, 0.1); + EXPECT_FLOAT_EQ(r.g, 0.2); + EXPECT_FLOAT_EQ(r.b, 0.3); + EXPECT_FLOAT_EQ(r.a, 0.4); +} + +TEST(TestMarkerHelper, CreateDefaultMarker) +{ + using visualization_msgs::msg::Marker; + const auto stamp = rclcpp::Time(12345, 67890); + const auto scale = autoware_utils_visualization::create_marker_scale(0.1, 0.2, 0.3); + const auto color = autoware_utils_visualization::create_marker_color(0.1, 0.2, 0.3, 0.4); + + const auto m = autoware_utils_visualization::create_default_marker( + "frame", stamp, "ns", 99, Marker::CUBE, scale, color); + + EXPECT_EQ(m.header.stamp.sec, 12345); + EXPECT_EQ(m.header.stamp.nanosec, 67890); + EXPECT_EQ(m.header.frame_id, "frame"); + EXPECT_EQ(m.ns, "ns"); + EXPECT_EQ(m.id, 99); + EXPECT_EQ(m.action, Marker::ADD); + EXPECT_EQ(m.type, Marker::CUBE); + EXPECT_DOUBLE_EQ(m.pose.position.x, 0.0); + EXPECT_DOUBLE_EQ(m.pose.position.y, 0.0); + EXPECT_DOUBLE_EQ(m.pose.position.z, 0.0); + EXPECT_DOUBLE_EQ(m.pose.orientation.x, 0.0); + EXPECT_DOUBLE_EQ(m.pose.orientation.y, 0.0); + EXPECT_DOUBLE_EQ(m.pose.orientation.z, 0.0); + EXPECT_DOUBLE_EQ(m.pose.orientation.w, 1.0); + EXPECT_DOUBLE_EQ(m.scale.x, 0.1); + EXPECT_DOUBLE_EQ(m.scale.y, 0.2); + EXPECT_DOUBLE_EQ(m.scale.z, 0.3); + EXPECT_FLOAT_EQ(m.color.r, 0.1); + EXPECT_FLOAT_EQ(m.color.g, 0.2); + EXPECT_FLOAT_EQ(m.color.b, 0.3); + EXPECT_FLOAT_EQ(m.color.a, 0.4); +} + +TEST(TestMarkerHelper, CreateDeleteMarker) +{ + using visualization_msgs::msg::Marker; + const auto stamp = rclcpp::Time(12345, 67890); + + const auto m = autoware_utils_visualization::create_deleted_default_marker(stamp, "ns", 99); + EXPECT_EQ(m.header.stamp.sec, 12345); + EXPECT_EQ(m.header.stamp.nanosec, 67890); + EXPECT_EQ(m.ns, "ns"); + EXPECT_EQ(m.id, 99); + EXPECT_EQ(m.action, Marker::DELETE); +} + +TEST(TestMarkerHelper, CreateAppendMarkerArray) +{ + visualization_msgs::msg::MarkerArray array1; + visualization_msgs::msg::MarkerArray array2; + array1.markers.resize(2); + array1.markers[0].id = 10; + array1.markers[1].id = 11; + array2.markers.resize(3); + array2.markers[0].id = 20; + array2.markers[1].id = 21; + array2.markers[2].id = 22; + + autoware_utils_visualization::append_marker_array(array2, &array1, std::nullopt); + EXPECT_EQ(array1.markers.size(), 5); + EXPECT_EQ(array1.markers[0].id, 10); + EXPECT_EQ(array1.markers[1].id, 11); + EXPECT_EQ(array1.markers[2].id, 20); + EXPECT_EQ(array1.markers[3].id, 21); + EXPECT_EQ(array1.markers[4].id, 22); +} diff --git a/autoware_utils_visualization/test/main.cpp b/autoware_utils_visualization/test/main.cpp new file mode 100644 index 0000000..7c283b2 --- /dev/null +++ b/autoware_utils_visualization/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(); +}