Initial import of FaRui Autoware Dual-Orin Stack

master/src/universe/common/autoware_auto_common/CMakeLists.txt

@@ -0,0 +1,33 @@
+cmake_minimum_required(VERSION 3.14)
+project(autoware_auto_common)
+
+find_package(autoware_cmake REQUIRED)
+autoware_package()
+
+find_package(Eigen3 REQUIRED)
+include_directories(SYSTEM ${EIGEN3_INCLUDE_DIR})
+
+if(BUILD_TESTING)
+  set(TEST_COMMON test_common_gtest)
+  ament_add_ros_isolated_gtest(${TEST_COMMON}
+    test/gtest_main.cpp
+    test/test_bool_comparisons.cpp
+    test/test_byte_reader.cpp
+    test/test_float_comparisons.cpp
+    test/test_mahalanobis_distance.cpp
+    test/test_message_field_adapters.cpp
+    test/test_template_utils.cpp
+    test/test_angle_utils.cpp
+    test/test_type_name.cpp
+    test/test_type_traits.cpp
+  )
+  target_compile_options(${TEST_COMMON} PRIVATE -Wno-sign-conversion)
+  target_include_directories(${TEST_COMMON} PRIVATE include)
+  ament_target_dependencies(${TEST_COMMON}
+    builtin_interfaces
+    Eigen3
+    geometry_msgs
+  )
+endif()
+
+ament_auto_package()

master/src/universe/common/autoware_auto_common/design/comparisons.md

@@ -0,0 +1,65 @@
+# Comparisons
+
+The `float_comparisons.hpp` library is a simple set of functions for performing approximate numerical comparisons.
+There are separate functions for performing comparisons using absolute bounds and relative bounds. Absolute comparison checks are prefixed with `abs_` and relative checks are prefixed with `rel_`.
+
+The `bool_comparisons.hpp` library additionally contains an XOR operator.
+
+The intent of the library is to improve readability of code and reduce likelihood of typographical errors when using numerical and boolean comparisons.
+
+## Target use cases
+
+The approximate comparisons are intended to be used to check whether two numbers lie within some absolute or relative interval.
+The `exclusive_or` function will test whether two values cast to different boolean values.
+
+## Assumptions
+
+- The approximate comparisons all take an `epsilon` parameter.
+  The value of this parameter must be >= 0.
+- The library is only intended to be used with floating point types.
+  A static assertion will be thrown if the library is used with a non-floating point type.
+
+## Example Usage
+
+```c++
+#include "autoware_auto_common/common/bool_comparisons.hpp"
+#include "autoware_auto_common/common/float_comparisons.hpp"
+
+#include <iostream>
+
+// using-directive is just for illustration; don't do this in practice
+using namespace autoware::common::helper_functions::comparisons;
+
+static constexpr auto epsilon = 0.2;
+static constexpr auto relative_epsilon = 0.01;
+
+std::cout << exclusive_or(true, false) << "\n";
+// Prints: true
+
+std::cout << rel_eq(1.0, 1.1, relative_epsilon)) << "\n";
+// Prints: false
+
+std::cout << approx_eq(10000.0, 10010.0, epsilon, relative_epsilon)) << "\n";
+// Prints: true
+
+std::cout << abs_eq(4.0, 4.2, epsilon) << "\n";
+// Prints: true
+
+std::cout << abs_ne(4.0, 4.2, epsilon) << "\n";
+// Prints: false
+
+std::cout << abs_eq_zero(0.2, epsilon) << "\n";
+// Prints: false
+
+std::cout << abs_lt(4.0, 4.25, epsilon) << "\n";
+// Prints: true
+
+std::cout << abs_lte(1.0, 1.2, epsilon) << "\n";
+// Prints: true
+
+std::cout << abs_gt(1.25, 1.0, epsilon) << "\n";
+// Prints: true
+
+std::cout << abs_gte(0.75, 1.0, epsilon) << "\n";
+// Prints: false
+```

master/src/universe/common/autoware_auto_common/include/autoware_auto_common/common/type_traits.hpp

@@ -0,0 +1,222 @@
+// Copyright 2021 Apex.AI, 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.
+//
+// Developed by Apex.AI, Inc.
+
+#include "autoware_auto_common/common/types.hpp"
+#include "autoware_auto_common/common/visibility_control.hpp"
+
+#include <cstdint>
+#include <tuple>
+#include <type_traits>
+
+#ifndef AUTOWARE_AUTO_COMMON__COMMON__TYPE_TRAITS_HPP_
+#define AUTOWARE_AUTO_COMMON__COMMON__TYPE_TRAITS_HPP_
+
+namespace autoware
+{
+namespace common
+{
+namespace type_traits
+{
+
+///
+/// @brief      A helper function to be used in static_assert to indicate an impossible branch.
+///
+/// @details    Typically used when a static_assert is used to guard a certain default
+///             implementation to never be executed and to show a helpful message to the user.
+///
+/// @tparam     T     Any type needed to delay the compilation of this function until it is used.
+///
+/// @return     A boolean that should be false for any type passed into this function.
+///
+template <typename T>
+constexpr inline autoware::common::types::bool8_t COMMON_PUBLIC impossible_branch() noexcept
+{
+  return sizeof(T) == 0;
+}
+
+/// Find an index of a type in a tuple
+template <class QueryT, class TupleT>
+struct COMMON_PUBLIC index
+{
+  static_assert(!std::is_same<TupleT, std::tuple<>>::value, "Could not find QueryT in given tuple");
+};
+
+/// Specialization for a tuple that starts with the HeadT type. End of recursion.
+template <class HeadT, class... Tail>
+struct COMMON_PUBLIC index<HeadT, std::tuple<HeadT, Tail...>>
+: std::integral_constant<std::int32_t, 0>
+{
+};
+
+/// Specialization for a tuple with a type different to QueryT that calls the recursive step.
+template <class QueryT, class HeadT, class... Tail>
+struct COMMON_PUBLIC index<QueryT, std::tuple<HeadT, Tail...>>
+: std::integral_constant<std::int32_t, 1 + index<QueryT, std::tuple<Tail...>>::value>
+{
+};
+
+///
+/// @brief      Visit every element in a tuple.
+///
+///             This specialization indicates the end of the recursive tuple traversal.
+///
+/// @tparam     I         Current index.
+/// @tparam     Callable  Callable type, usually a lambda with one auto input parameter.
+/// @tparam     TypesT    Types in the tuple.
+///
+/// @return     Does not return anything. Capture variables in a lambda to return any values.
+///
+template <std::size_t I = 0UL, typename Callable, typename... TypesT>
+COMMON_PUBLIC inline constexpr typename std::enable_if_t<I == sizeof...(TypesT)> visit(
+  std::tuple<TypesT...> &, Callable) noexcept
+{
+}
+/// @brief      Same as the previous specialization but for const tuple.
+template <std::size_t I = 0UL, typename Callable, typename... TypesT>
+COMMON_PUBLIC inline constexpr typename std::enable_if_t<I == sizeof...(TypesT)> visit(
+  const std::tuple<TypesT...> &, Callable) noexcept
+{
+}
+
+///
+/// @brief      Visit every element in a tuple.
+///
+///             This specialization is used to apply the callable to an element of a tuple and
+///             recursively call this function on the next one.
+///
+/// @param      tuple     The tuple instance
+/// @param[in]  callable  A callable, usually a lambda with one auto input parameter.
+///
+/// @tparam     I         Current index.
+/// @tparam     Callable  Callable type, usually a lambda with one auto input parameter.
+/// @tparam     TypesT    Types in the tuple.
+///
+/// @return     Does not return anything. Capture variables in a lambda to return any values.
+///
+template <std::size_t I = 0UL, typename Callable, typename... TypesT>
+COMMON_PUBLIC inline constexpr typename std::enable_if_t<I != sizeof...(TypesT)> visit(
+  std::tuple<TypesT...> & tuple, Callable callable) noexcept
+{
+  callable(std::get<I>(tuple));
+  visit<I + 1UL, Callable, TypesT...>(tuple, callable);
+}
+/// @brief      Same as the previous specialization but for const tuple.
+template <std::size_t I = 0UL, typename Callable, typename... TypesT>
+COMMON_PUBLIC inline constexpr typename std::enable_if_t<I != sizeof...(TypesT)> visit(
+  const std::tuple<TypesT...> & tuple, Callable callable) noexcept
+{
+  callable(std::get<I>(tuple));
+  visit<I + 1UL, Callable, TypesT...>(tuple, callable);
+}
+
+/// @brief      A class to compute a conjunction over given traits.
+template <class...>
+struct COMMON_PUBLIC conjunction : std::true_type
+{
+};
+/// @brief      A conjunction of another type shall derive from that type.
+template <class TraitT>
+struct COMMON_PUBLIC conjunction<TraitT> : TraitT
+{
+};
+template <class TraitT, class... TraitsTs>
+struct COMMON_PUBLIC conjunction<TraitT, TraitsTs...>
+: std::conditional_t<static_cast<bool>(TraitT::value), conjunction<TraitsTs...>, TraitT>
+{
+};
+
+///
+/// @brief      A trait to check if a tuple has a type.
+///
+/// @details    Taken from https://stackoverflow.com/a/25958302/678093
+///
+/// @tparam     QueryT  A query type.
+/// @tparam     TupleT  A tuple to search the type in.
+///
+template <typename QueryT, typename TupleT>
+struct has_type;
+
+///
+/// @brief      An overload of the general trait that signifies that nothing can be found in an
+///             empty tuple.
+///
+/// @tparam     QueryT     Any type.
+///
+template <typename QueryT>
+struct has_type<QueryT, std::tuple<>> : std::false_type
+{
+};
+
+///
+/// @brief      Recursive override of the main trait.
+///
+/// @tparam     QueryT  Query type.
+/// @tparam     HeadT   Head type in the tuple.
+/// @tparam     TailTs  Rest of the tuple types.
+///
+template <typename QueryT, typename HeadT, typename... TailTs>
+struct has_type<QueryT, std::tuple<HeadT, TailTs...>> : has_type<QueryT, std::tuple<TailTs...>>
+{
+};
+
+///
+/// @brief      End of recursion for the main `has_type` trait. Becomes a `true_type` when the first
+///             type in the tuple matches the query type.
+///
+/// @tparam     QueryT  Query type.
+/// @tparam     TailTs  Other types in the tuple.
+///
+template <typename QueryT, typename... TailTs>
+struct has_type<QueryT, std::tuple<QueryT, TailTs...>> : std::true_type
+{
+};
+
+///
+/// @brief      A trait used to intersect types stored in tuples at compile time. The resulting
+///             typedef `type` will hold a tuple with the intersection of the types provided in the
+///             input tuples.
+///
+/// @details    Taken from https://stackoverflow.com/a/41200732/1763680
+///
+/// @tparam     TupleT1  Tuple 1
+/// @tparam     TupleT2  Tuple 2
+///
+template <typename TupleT1, typename TupleT2>
+struct intersect
+{
+  ///
+  /// @brief      Intersect the types.
+  ///
+  /// @details    This function "iterates" over the types in TupleT1 and checks if those are in
+  ///             TupleT2. If this is true, these types are concatenated into a new tuple.
+  ///
+  template <std::size_t... Indices>
+  static constexpr auto make_intersection(std::index_sequence<Indices...>)
+  {
+    return std::tuple_cat(std::conditional_t<
+                          has_type<std::tuple_element_t<Indices, TupleT1>, TupleT2>::value,
+                          std::tuple<std::tuple_element_t<Indices, TupleT1>>, std::tuple<>>{}...);
+  }
+  /// The resulting tuple type.
+  using type =
+    decltype(make_intersection(std::make_index_sequence<std::tuple_size<TupleT1>::value>{}));
+};
+
+}  // namespace type_traits
+}  // namespace common
+}  // namespace autoware
+
+#endif  // AUTOWARE_AUTO_COMMON__COMMON__TYPE_TRAITS_HPP_

master/src/universe/common/autoware_auto_common/include/autoware_auto_common/common/types.hpp

@@ -0,0 +1,127 @@
+// Copyright 2017-2020 the Autoware Foundation, Arm Limited
+//
+// 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.
+//
+// Co-developed by Tier IV, Inc. and Apex.AI, Inc.
+/// \file
+/// \brief This file includes common type definition
+
+#ifndef AUTOWARE_AUTO_COMMON__COMMON__TYPES_HPP_
+#define AUTOWARE_AUTO_COMMON__COMMON__TYPES_HPP_
+
+#include "autoware_auto_common/common/visibility_control.hpp"
+#include "autoware_auto_common/helper_functions/float_comparisons.hpp"
+
+#include <cstdint>
+#include <limits>
+#include <vector>
+
+namespace autoware
+{
+namespace common
+{
+namespace types
+{
+// Aliases to conform to MISRA C++ Rule 3-9-2 (Directive 4.6 in MISRA C).
+// Similarly, the stdint typedefs should be used instead of plain int, long etc. types.
+// We don't currently require code to comply to MISRA, but we should try to where it is
+// easily possible.
+using bool8_t = bool;
+#if __cplusplus < 201811L || !__cpp_char8_t
+using char8_t = char;
+#endif
+using uchar8_t = unsigned char;
+// If we ever compile on a platform where this is not true, float32_t and float64_t definitions
+// need to be adjusted.
+static_assert(sizeof(float) == 4, "float is assumed to be 32-bit");
+using float32_t = float;
+static_assert(sizeof(double) == 8, "double is assumed to be 64-bit");
+using float64_t = double;
+
+/// pi = tau / 2
+constexpr float32_t PI = 3.14159265359F;
+/// pi/2
+constexpr float32_t PI_2 = 1.5707963267948966F;
+/// tau = 2 pi
+constexpr float32_t TAU = 6.283185307179586476925286766559F;
+
+struct COMMON_PUBLIC PointXYZIF
+{
+  float32_t x{0};
+  float32_t y{0};
+  float32_t z{0};
+  float32_t intensity{0};
+  uint16_t id{0};
+  static constexpr uint16_t END_OF_SCAN_ID = 65535u;
+  friend bool operator==(const PointXYZIF & p1, const PointXYZIF & p2) noexcept
+  {
+    using autoware::common::helper_functions::comparisons::rel_eq;
+    const auto epsilon = std::numeric_limits<float32_t>::epsilon();
+    return rel_eq(p1.x, p2.x, epsilon) && rel_eq(p1.y, p2.y, epsilon) &&
+           rel_eq(p1.z, p2.z, epsilon) && rel_eq(p1.intensity, p2.intensity, epsilon) &&
+           (p1.id == p2.id);
+  }
+};
+
+struct COMMON_PUBLIC PointXYZF
+{
+  float32_t x{0};
+  float32_t y{0};
+  float32_t z{0};
+  uint16_t id{0};
+  static constexpr uint16_t END_OF_SCAN_ID = 65535u;
+  friend bool operator==(const PointXYZF & p1, const PointXYZF & p2) noexcept
+  {
+    using autoware::common::helper_functions::comparisons::rel_eq;
+    const auto epsilon = std::numeric_limits<float32_t>::epsilon();
+    return rel_eq(p1.x, p2.x, epsilon) && rel_eq(p1.y, p2.y, epsilon) &&
+           rel_eq(p1.z, p2.z, epsilon) && (p1.id == p2.id);
+  }
+};
+
+struct COMMON_PUBLIC PointXYZI
+{
+  float32_t x{0.0F};
+  float32_t y{0.0F};
+  float32_t z{0.0F};
+  float32_t intensity{0.0F};
+  friend bool operator==(const PointXYZI & p1, const PointXYZI & p2) noexcept
+  {
+    return helper_functions::comparisons::rel_eq(
+             p1.x, p2.x, std::numeric_limits<float32_t>::epsilon()) &&
+
+           helper_functions::comparisons::rel_eq(
+             p1.y, p2.y, std::numeric_limits<float32_t>::epsilon()) &&
+
+           helper_functions::comparisons::rel_eq(
+             p1.z, p2.z, std::numeric_limits<float32_t>::epsilon()) &&
+
+           helper_functions::comparisons::rel_eq(
+             p1.intensity, p2.intensity, std::numeric_limits<float32_t>::epsilon());
+  }
+};
+
+using PointBlock = std::vector<PointXYZIF>;
+using PointPtrBlock = std::vector<const PointXYZIF *>;
+/// \brief Stores basic configuration information, does some simple validity checking
+static constexpr uint16_t POINT_BLOCK_CAPACITY = 512U;
+
+// TODO(yunus.caliskan): switch to std::void_t when C++17 is available
+/// \brief `std::void_t<> implementation
+template <typename... Ts>
+using void_t = void;
+}  // namespace types
+}  // namespace common
+}  // namespace autoware
+
+#endif  // AUTOWARE_AUTO_COMMON__COMMON__TYPES_HPP_

master/src/universe/common/autoware_auto_common/include/autoware_auto_common/common/visibility_control.hpp

@@ -0,0 +1,38 @@
+// Copyright 2017-2019 the Autoware Foundation
+//
+// 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.
+//
+// Co-developed by Tier IV, Inc. and Apex.AI, Inc.
+
+#ifndef AUTOWARE_AUTO_COMMON__COMMON__VISIBILITY_CONTROL_HPP_
+#define AUTOWARE_AUTO_COMMON__COMMON__VISIBILITY_CONTROL_HPP_
+
+#if defined(_MSC_VER) && defined(_WIN64)
+#if defined(COMMON_BUILDING_DLL) || defined(COMMON_EXPORTS)
+#define COMMON_PUBLIC __declspec(dllexport)
+#define COMMON_LOCAL
+#else  // defined(COMMON_BUILDING_DLL) || defined(COMMON_EXPORTS)
+#define COMMON_PUBLIC __declspec(dllimport)
+#define COMMON_LOCAL
+#endif  // defined(COMMON_BUILDING_DLL) || defined(COMMON_EXPORTS)
+#elif defined(__GNUC__) && defined(__linux__)
+#define COMMON_PUBLIC __attribute__((visibility("default")))
+#define COMMON_LOCAL __attribute__((visibility("hidden")))
+#elif defined(__GNUC__) && defined(__APPLE__)
+#define COMMON_PUBLIC __attribute__((visibility("default")))
+#define COMMON_LOCAL __attribute__((visibility("hidden")))
+#else  // !(defined(__GNUC__) && defined(__APPLE__))
+#error "Unsupported Build Configuration"
+#endif  // _MSC_VER
+
+#endif  // AUTOWARE_AUTO_COMMON__COMMON__VISIBILITY_CONTROL_HPP_

master/src/universe/common/autoware_auto_common/include/autoware_auto_common/helper_functions/angle_utils.hpp

@@ -0,0 +1,66 @@
+// Copyright 2020 Apex.AI, 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.
+//
+// Co-developed by Tier IV, Inc. and Apex.AI, Inc.
+
+#ifndef AUTOWARE_AUTO_COMMON__HELPER_FUNCTIONS__ANGLE_UTILS_HPP_
+#define AUTOWARE_AUTO_COMMON__HELPER_FUNCTIONS__ANGLE_UTILS_HPP_
+
+#include <cmath>
+#include <type_traits>
+
+namespace autoware
+{
+namespace common
+{
+namespace helper_functions
+{
+
+namespace detail
+{
+constexpr auto kDoublePi = 2.0 * M_PI;
+}  // namespace detail
+
+///
+/// @brief      Wrap angle to the [-pi, pi] range.
+///
+/// @details    This method uses the formula suggested in the paper [On wrapping the Kalman filter
+///             and estimating with the SO(2) group](https://arxiv.org/pdf/1708.05551.pdf) and
+///             implements the following formula:
+///             \f$\mathrm{mod}(\alpha + \pi, 2 \pi) - \pi\f$.
+///
+/// @param[in]  angle  The input angle
+///
+/// @tparam     T      Type of scalar
+///
+/// @return     Angle wrapped to the chosen range.
+///
+template <typename T>
+constexpr T wrap_angle(T angle) noexcept
+{
+  auto help_angle = angle + T(M_PI);
+  while (help_angle < T{}) {
+    help_angle += T(detail::kDoublePi);
+  }
+  while (help_angle >= T(detail::kDoublePi)) {
+    help_angle -= T(detail::kDoublePi);
+  }
+  return help_angle - T(M_PI);
+}
+
+}  // namespace helper_functions
+}  // namespace common
+}  // namespace autoware
+
+#endif  // AUTOWARE_AUTO_COMMON__HELPER_FUNCTIONS__ANGLE_UTILS_HPP_

master/src/universe/common/autoware_auto_common/include/autoware_auto_common/helper_functions/bool_comparisons.hpp

@@ -0,0 +1,50 @@
+// Copyright 2020 Mapless AI, Inc.
+//
+// Permission is hereby granted, free of charge, to any person obtaining a copy
+// of this software and associated documentation files (the "Software"), to
+// deal in the Software without restriction, including without limitation the
+// rights to use, copy, modify, merge, publish, distribute, sublicense, and/or
+// sell copies of the Software, and to permit persons to whom the Software is
+// furnished to do so, subject to the following conditions:
+//
+// The above copyright notice and this permission notice shall be included in
+// all copies or substantial portions of the Software.
+//
+// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
+//  AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
+//  LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING
+// FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS
+// IN THE SOFTWARE.
+
+#ifndef AUTOWARE_AUTO_COMMON__HELPER_FUNCTIONS__BOOL_COMPARISONS_HPP_
+#define AUTOWARE_AUTO_COMMON__HELPER_FUNCTIONS__BOOL_COMPARISONS_HPP_
+
+#include "autoware_auto_common/common/types.hpp"
+
+namespace autoware
+{
+namespace common
+{
+namespace helper_functions
+{
+namespace comparisons
+{
+
+/**
+ * @brief Convenience method for performing logical exclusive or ops.
+ * @return True iff exactly one of 'a' and 'b' is true.
+ */
+template <typename T>
+types::bool8_t exclusive_or(const T & a, const T & b)
+{
+  return static_cast<types::bool8_t>(a) != static_cast<types::bool8_t>(b);
+}
+
+}  // namespace comparisons
+}  // namespace helper_functions
+}  // namespace common
+}  // namespace autoware
+
+#endif  // AUTOWARE_AUTO_COMMON__HELPER_FUNCTIONS__BOOL_COMPARISONS_HPP_

master/src/universe/common/autoware_auto_common/include/autoware_auto_common/helper_functions/byte_reader.hpp

@@ -0,0 +1,73 @@
+// Copyright 2017-2019 the Autoware Foundation
+//
+// 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.
+//
+// Co-developed by Tier IV, Inc. and Apex.AI, Inc.
+/// \file
+/// \brief This file includes common helper functions
+
+#ifndef AUTOWARE_AUTO_COMMON__HELPER_FUNCTIONS__BYTE_READER_HPP_
+#define AUTOWARE_AUTO_COMMON__HELPER_FUNCTIONS__BYTE_READER_HPP_
+
+#include <cstdint>
+#include <cstring>
+#include <vector>
+
+namespace autoware
+{
+namespace common
+{
+namespace helper_functions
+{
+/// \brief A utility class to read byte vectors in big-endian order
+class ByteReader
+{
+private:
+  const std::vector<uint8_t> & byte_vector_;
+  std::size_t index_;
+
+public:
+  /// \brief Default constructor, byte reader class
+  /// \param[in] byte_vector A vector to read bytes from
+  explicit ByteReader(const std::vector<uint8_t> & byte_vector)
+  : byte_vector_(byte_vector), index_(0U)
+  {
+  }
+
+  // brief Read bytes and store it in the argument passed in big-endian order
+  /// \param[inout] value Read and store the bytes from the vector matching the size of the argument
+  template <typename T>
+  void read(T & value)
+  {
+    constexpr std::size_t kTypeSize = sizeof(T);
+    union {
+      T value;
+      uint8_t byte_vector[kTypeSize];
+    } tmp;
+
+    for (std::size_t i = 0; i < kTypeSize; ++i) {
+      tmp.byte_vector[i] = byte_vector_[index_ + kTypeSize - 1 - i];
+    }
+
+    value = tmp.value;
+
+    index_ += kTypeSize;
+  }
+
+  void skip(std::size_t count) { index_ += count; }
+};
+}  // namespace helper_functions
+}  // namespace common
+}  // namespace autoware
+
+#endif  // AUTOWARE_AUTO_COMMON__HELPER_FUNCTIONS__BYTE_READER_HPP_

master/src/universe/common/autoware_auto_common/include/autoware_auto_common/helper_functions/crtp.hpp

@@ -0,0 +1,52 @@
+// Copyright 2017-2019 the Autoware Foundation
+//
+// 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.
+//
+// Co-developed by Tier IV, Inc. and Apex.AI, Inc.
+/// \file
+/// \brief This file includes common helper functions
+
+#ifndef AUTOWARE_AUTO_COMMON__HELPER_FUNCTIONS__CRTP_HPP_
+#define AUTOWARE_AUTO_COMMON__HELPER_FUNCTIONS__CRTP_HPP_
+
+namespace autoware
+{
+namespace common
+{
+namespace helper_functions
+{
+template <typename Derived>
+class crtp
+{
+protected:
+  const Derived & impl() const
+  {
+    // This is the CRTP pattern for static polymorphism: this is related, static_cast is the only
+    // way to do this
+    // lint -e{9005, 9176, 1939} NOLINT
+    return *static_cast<const Derived *>(this);
+  }
+
+  Derived & impl()
+  {
+    // This is the CRTP pattern for static polymorphism: this is related, static_cast is the only
+    // way to do this
+    // lint -e{9005, 9176, 1939} NOLINT
+    return *static_cast<Derived *>(this);
+  }
+};
+}  // namespace helper_functions
+}  // namespace common
+}  // namespace autoware
+
+#endif  // AUTOWARE_AUTO_COMMON__HELPER_FUNCTIONS__CRTP_HPP_

master/src/universe/common/autoware_auto_common/include/autoware_auto_common/helper_functions/float_comparisons.hpp

@@ -0,0 +1,149 @@
+// Copyright 2020 Mapless AI, Inc.
+//
+// Permission is hereby granted, free of charge, to any person obtaining a copy
+// of this software and associated documentation files (the "Software"), to
+// deal in the Software without restriction, including without limitation the
+// rights to use, copy, modify, merge, publish, distribute, sublicense, and/or
+// sell copies of the Software, and to permit persons to whom the Software is
+// furnished to do so, subject to the following conditions:
+//
+// The above copyright notice and this permission notice shall be included in
+// all copies or substantial portions of the Software.
+//
+// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
+//  AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
+//  LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING
+// FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS
+// IN THE SOFTWARE.
+
+#ifndef AUTOWARE_AUTO_COMMON__HELPER_FUNCTIONS__FLOAT_COMPARISONS_HPP_
+#define AUTOWARE_AUTO_COMMON__HELPER_FUNCTIONS__FLOAT_COMPARISONS_HPP_
+
+#include <algorithm>
+#include <cmath>
+#include <limits>
+
+namespace autoware
+{
+namespace common
+{
+namespace helper_functions
+{
+namespace comparisons
+{
+
+/**
+ * @brief Check for approximate equality in absolute terms.
+ * @pre eps >= 0
+ * @return True iff 'a' and 'b' are within 'eps' of each other.
+ */
+template <typename T>
+bool abs_eq(const T & a, const T & b, const T & eps)
+{
+  static_assert(
+    std::is_floating_point<T>::value, "Float comparisons only support floating point types.");
+
+  return std::abs(a - b) <= eps;
+}
+
+/**
+ * @brief Check for approximate less than in absolute terms.
+ * @pre eps >= 0
+ * @return True iff 'a' is less than 'b' minus 'eps'.
+ */
+template <typename T>
+bool abs_lt(const T & a, const T & b, const T & eps)
+{
+  return !abs_eq(a, b, eps) && (a < b);
+}
+
+/**
+ * @brief Check for approximate less than or equal in absolute terms.
+ * @pre eps >= 0
+ * @return True iff 'a' is less than or equal to 'b' plus 'eps'.
+ */
+template <typename T>
+bool abs_lte(const T & a, const T & b, const T & eps)
+{
+  return abs_eq(a, b, eps) || (a < b);
+}
+
+/**
+ * @brief Check for approximate greater than or equal in absolute terms.
+ * @pre eps >= 0
+ * @return True iff 'a' is greater than or equal to 'b' minus 'eps'.
+ */
+template <typename T>
+bool abs_gte(const T & a, const T & b, const T & eps)
+{
+  return !abs_lt(a, b, eps);
+}
+
+/**
+ * @brief Check for approximate greater than in absolute terms.
+ * @pre eps >= 0
+ * @return True iff 'a' is greater than 'b' minus 'eps'.
+ */
+template <typename T>
+bool abs_gt(const T & a, const T & b, const T & eps)
+{
+  return !abs_lte(a, b, eps);
+}
+
+/**
+ * @brief Check whether a value is within epsilon of zero.
+ * @pre eps >= 0
+ * @return True iff 'a' is within 'eps' of zero.
+ */
+template <typename T>
+bool abs_eq_zero(const T & a, const T & eps)
+{
+  return abs_eq(a, static_cast<T>(0), eps);
+}
+
+/**
+ * @brief
+ * https://randomascii.wordpress.com/2012/02/25/comparing-floating-point-numbers-2012-edition/
+ * @pre rel_eps >= 0
+ * @return True iff 'a' and 'b' are within relative 'rel_eps' of each other.
+ */
+template <typename T>
+bool rel_eq(const T & a, const T & b, const T & rel_eps)
+{
+  static_assert(
+    std::is_floating_point<T>::value, "Float comparisons only support floating point types.");
+
+  const auto delta = std::abs(a - b);
+  const auto larger = std::max(std::abs(a), std::abs(b));
+  const auto max_rel_delta = (larger * rel_eps);
+  return delta <= max_rel_delta;
+}
+
+// TODO(jeff): As needed, add relative variants of <, <=, >, >=
+
+/**
+ * @brief Check for approximate equality in absolute and relative terms.
+ *
+ * @note This method should be used only if an explicit relative or absolute
+ * comparison is not appropriate for the particular use case.
+ *
+ * @pre abs_eps >= 0
+ * @pre rel_eps >= 0
+ * @return True iff 'a' and 'b' are within 'eps' or 'rel_eps' of each other
+ */
+template <typename T>
+bool approx_eq(const T & a, const T & b, const T & abs_eps, const T & rel_eps)
+{
+  const auto are_absolute_eq = abs_eq(a, b, abs_eps);
+  const auto are_relative_eq = rel_eq(a, b, rel_eps);
+  return are_absolute_eq || are_relative_eq;
+}
+
+}  // namespace comparisons
+}  // namespace helper_functions
+}  // namespace common
+}  // namespace autoware
+
+#endif  // AUTOWARE_AUTO_COMMON__HELPER_FUNCTIONS__FLOAT_COMPARISONS_HPP_

master/src/universe/common/autoware_auto_common/include/autoware_auto_common/helper_functions/mahalanobis_distance.hpp

@@ -0,0 +1,72 @@
+// Copyright 2021 Apex.AI, 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.
+//
+// Co-developed by Tier IV, Inc. and Apex.AI, Inc.
+
+#ifndef AUTOWARE_AUTO_COMMON__HELPER_FUNCTIONS__MAHALANOBIS_DISTANCE_HPP_
+#define AUTOWARE_AUTO_COMMON__HELPER_FUNCTIONS__MAHALANOBIS_DISTANCE_HPP_
+
+#include <Eigen/Cholesky>
+
+namespace autoware
+{
+namespace common
+{
+namespace helper_functions
+{
+/// \brief Calculate square of mahalanobis distance
+/// \tparam T Type of elements in the matrix
+/// \tparam kNumOfStates Number of states
+/// \param sample Single column matrix containing sample whose distance needs to be computed
+/// \param mean Single column matrix containing mean of samples received so far
+/// \param covariance_factor Covariance matrix
+/// \return Square of mahalanobis distance
+template <typename T, std::int32_t kNumOfStates>
+types::float32_t calculate_squared_mahalanobis_distance(
+  const Eigen::Matrix<T, kNumOfStates, 1> & sample, const Eigen::Matrix<T, kNumOfStates, 1> & mean,
+  const Eigen::Matrix<T, kNumOfStates, kNumOfStates> & covariance_factor)
+{
+  using Vector = Eigen::Matrix<T, kNumOfStates, 1>;
+  // This is equivalent to the squared Mahalanobis distance of the form: diff.T * C.inv() * diff
+  // Instead of the covariance matrix C we have its lower-triangular factor L, such that C = L * L.T
+  // squared_mahalanobis_distance = diff.T * C.inv() * diff
+  // = diff.T * (L * L.T).inv() * diff
+  // = diff.T * L.T.inv() * L.inv() * diff
+  // = (L.inv() * diff).T * (L.inv() * diff)
+  // this allows us to efficiently find the squared Mahalanobis distance using (L.inv() * diff),
+  // which can be found as a solution to: L * x = diff.
+  const Vector diff = sample - mean;
+  const Vector x = covariance_factor.ldlt().solve(diff);
+  return x.transpose() * x;
+}
+
+/// \brief Calculate mahalanobis distance
+/// \tparam T Type of elements in the matrix
+/// \tparam kNumOfStates Number of states
+/// \param sample Single column matrix containing sample whose distance needs to be computed
+/// \param mean Single column matrix containing mean of samples received so far
+/// \param covariance_factor Covariance matrix
+/// \return Mahalanobis distance
+template <typename T, std::int32_t kNumOfStates>
+types::float32_t calculate_mahalanobis_distance(
+  const Eigen::Matrix<T, kNumOfStates, 1> & sample, const Eigen::Matrix<T, kNumOfStates, 1> & mean,
+  const Eigen::Matrix<T, kNumOfStates, kNumOfStates> & covariance_factor)
+{
+  return sqrtf(calculate_squared_mahalanobis_distance(sample, mean, covariance_factor));
+}
+}  // namespace helper_functions
+}  // namespace common
+}  // namespace autoware
+
+#endif  // AUTOWARE_AUTO_COMMON__HELPER_FUNCTIONS__MAHALANOBIS_DISTANCE_HPP_

master/src/universe/common/autoware_auto_common/include/autoware_auto_common/helper_functions/message_adapters.hpp

@@ -0,0 +1,115 @@
+// Copyright 2017-2019 the Autoware Foundation
+//
+// 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.
+//
+// Co-developed by Tier IV, Inc. and Apex.AI, Inc.
+/// \file
+/// \brief This file includes common helper functions
+
+#ifndef AUTOWARE_AUTO_COMMON__HELPER_FUNCTIONS__MESSAGE_ADAPTERS_HPP_
+#define AUTOWARE_AUTO_COMMON__HELPER_FUNCTIONS__MESSAGE_ADAPTERS_HPP_
+
+#include <builtin_interfaces/msg/time.hpp>
+
+#include <string>
+
+namespace autoware
+{
+namespace common
+{
+namespace helper_functions
+{
+namespace message_field_adapters
+{
+/// Using alias for Time message
+using TimeStamp = builtin_interfaces::msg::Time;
+
+/// \brief Helper class to check existence of header file in compile time:
+/// https://stackoverflow.com/a/16000226/2325407
+template <typename T, typename = std::nullptr_t>
+struct HasHeader : std::false_type
+{
+};
+
+template <typename T>
+struct HasHeader<T, decltype((void)T::header, nullptr)> : std::true_type
+{
+};
+
+/////////// Template declarations
+
+/// Get frame id from message. std::nullptr_t is used to prevent template ambiguity on
+/// SFINAE specializations. Provide a default value on specializations for a friendly API.
+/// \tparam T Message type.
+/// \param msg Message.
+/// \return Frame id of the message.
+template <typename T, std::nullptr_t>
+const std::string & get_frame_id(const T & msg) noexcept;
+
+/// Get a reference to the frame id from message. std::nullptr_t is used to prevent
+/// template ambiguity on SFINAE specializations. Provide a default value on
+/// specializations for a friendly API.
+/// \tparam T Message type.
+/// \param msg Message.
+/// \return Frame id of the message.
+template <typename T, std::nullptr_t>
+std::string & get_frame_id(T & msg) noexcept;
+
+/// Get stamp from message. std::nullptr_t is used to prevent template ambiguity on
+/// SFINAE specializations. Provide a default value on specializations for a friendly API.
+/// \tparam T Message type.
+/// \param msg Message.
+/// \return Frame id of the message.
+template <typename T, std::nullptr_t>
+const TimeStamp & get_stamp(const T & msg) noexcept;
+
+/// Get a reference to the stamp from message. std::nullptr_t is used to prevent
+/// template ambiguity on SFINAE specializations. Provide a default value on
+/// specializations for a friendly API.
+/// \tparam T Message type.
+/// \param msg Message.
+/// \return Frame id of the message.
+template <typename T, std::nullptr_t>
+TimeStamp & get_stamp(T & msg) noexcept;
+
+/////////////// Default specializations for message types that contain a header.
+template <class T, typename std::enable_if<HasHeader<T>::value, std::nullptr_t>::type = nullptr>
+const std::string & get_frame_id(const T & msg) noexcept
+{
+  return msg.header.frame_id;
+}
+
+template <class T, typename std::enable_if<HasHeader<T>::value, std::nullptr_t>::type = nullptr>
+std::string & get_frame_id(T & msg) noexcept
+{
+  return msg.header.frame_id;
+}
+
+template <class T, typename std::enable_if<HasHeader<T>::value, std::nullptr_t>::type = nullptr>
+TimeStamp & get_stamp(T & msg) noexcept
+{
+  return msg.header.stamp;
+}
+
+template <class T, typename std::enable_if<HasHeader<T>::value, std::nullptr_t>::type = nullptr>
+TimeStamp get_stamp(const T & msg) noexcept
+{
+  return msg.header.stamp;
+}
+
+}  // namespace message_field_adapters
+}  // namespace helper_functions
+}  // namespace common
+}  // namespace autoware
+
+#endif  // AUTOWARE_AUTO_COMMON__HELPER_FUNCTIONS__MESSAGE_ADAPTERS_HPP_

master/src/universe/common/autoware_auto_common/include/autoware_auto_common/helper_functions/template_utils.hpp

@@ -0,0 +1,75 @@
+// Copyright 2021 the Autoware Foundation
+//
+// 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.
+//
+// Co-developed by Tier IV, Inc. and Apex.AI, Inc.
+
+#ifndef AUTOWARE_AUTO_COMMON__HELPER_FUNCTIONS__TEMPLATE_UTILS_HPP_
+#define AUTOWARE_AUTO_COMMON__HELPER_FUNCTIONS__TEMPLATE_UTILS_HPP_
+
+#include "autoware_auto_common/common/types.hpp"
+
+#include <type_traits>
+
+namespace autoware
+{
+namespace common
+{
+namespace helper_functions
+{
+/// This struct is `std::true_type` if the expression is valid for a given template and
+/// `std::false_type` otherwise.
+/// \tparam ExpressionTemplate Expression to be checked in compile time
+/// \tparam T Template parameter to instantiate the expression.
+template <template <typename...> class ExpressionTemplate, typename T, typename = void>
+struct expression_valid : std::false_type
+{
+};
+
+/// This struct is `std::true_type` if the expression is valid for a given template and
+/// `std::false_type` otherwise.
+/// \tparam ExpressionTemplate Expression to be checked in compile time
+/// \tparam T Template parameter to instantiate the expression.
+template <template <typename...> class ExpressionTemplate, typename T>
+struct expression_valid<ExpressionTemplate, T, types::void_t<ExpressionTemplate<T>>>
+: std::true_type
+{
+};
+
+/// This struct is `std::true_type` if the expression is valid for a given template
+/// type with the specified return type and `std::false_type` otherwise.
+/// \tparam ExpressionTemplate Expression to be checked in compile time
+/// \tparam T Template parameter to instantiate the expression.
+/// \tparam ReturnT Return type of the expression.
+template <
+  template <typename...> class ExpressionTemplate, typename T, typename ReturnT, typename = void>
+struct expression_valid_with_return : std::false_type
+{
+};
+
+/// This struct is `std::true_type` if the expression is valid for a given template
+/// type with the specified return type and `std::false_type` otherwise.
+/// \tparam ExpressionTemplate Expression to be checked in compile time
+/// \tparam T Template parameter to instantiate the expression.
+/// \tparam ReturnT Return type of the expression.
+template <template <typename...> class ExpressionTemplate, typename T, typename ReturnT>
+struct expression_valid_with_return<
+  ExpressionTemplate, T, ReturnT,
+  std::enable_if_t<std::is_same<ReturnT, ExpressionTemplate<T>>::value>> : std::true_type
+{
+};
+
+}  // namespace helper_functions
+}  // namespace common
+}  // namespace autoware
+#endif  // AUTOWARE_AUTO_COMMON__HELPER_FUNCTIONS__TEMPLATE_UTILS_HPP_

master/src/universe/common/autoware_auto_common/include/autoware_auto_common/helper_functions/type_name.hpp

@@ -0,0 +1,56 @@
+// Copyright 2021 Apex.AI, 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.
+//
+// Co-developed by Tier IV, Inc. and Apex.AI, Inc.
+
+#ifndef AUTOWARE_AUTO_COMMON__HELPER_FUNCTIONS__TYPE_NAME_HPP_
+#define AUTOWARE_AUTO_COMMON__HELPER_FUNCTIONS__TYPE_NAME_HPP_
+
+#include "autoware_auto_common/common/visibility_control.hpp"
+
+#include <string>
+#include <typeinfo>
+
+#if defined(__clang__) || defined(__GNUC__) || defined(__GNUG__)
+#include <cxxabi.h>
+#endif
+
+namespace autoware
+{
+namespace helper_functions
+{
+
+/// @brief      Get a demangled name of a type.
+template <typename T>
+COMMON_PUBLIC std::string get_type_name()
+{
+#if defined(__clang__) || defined(__GNUC__) || defined(__GNUG__)
+  return abi::__cxa_demangle(typeid(T).name(), NULL, NULL, 0);
+#else
+  // For unsupported compilers return a mangled name.
+  return typeid(T).name();
+#endif
+}
+
+/// @brief      Get a demangled name of a type given its instance.
+template <typename T>
+COMMON_PUBLIC std::string get_type_name(const T &)
+{
+  return get_type_name<T>();
+}
+
+}  // namespace helper_functions
+}  // namespace autoware
+
+#endif  // AUTOWARE_AUTO_COMMON__HELPER_FUNCTIONS__TYPE_NAME_HPP_

master/src/universe/common/autoware_auto_common/package.xml

@@ -0,0 +1,27 @@
+<?xml version="1.0"?>
+<?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
+<package format="3">
+  <name>autoware_auto_common</name>
+  <version>1.0.0</version>
+  <description>Miscellaneous helper functions</description>
+  <maintainer email="opensource@apex.ai">Apex.AI, Inc.</maintainer>
+  <maintainer email="tomoya.kimura@tier4.jp">Tomoya Kimura</maintainer>
+  <maintainer email="shumpei.wakabayashi@tier4.jp">Shumpei Wakabayashi</maintainer>
+  <maintainer email="satoshi.ota@tier4.jp">Satoshi Ota</maintainer>
+  <license>Apache License 2.0</license>
+
+  <buildtool_depend>ament_cmake_auto</buildtool_depend>
+  <buildtool_depend>autoware_cmake</buildtool_depend>
+
+  <depend>builtin_interfaces</depend>
+  <depend>eigen</depend>
+
+  <test_depend>ament_cmake_ros</test_depend>
+  <test_depend>ament_lint_auto</test_depend>
+  <test_depend>autoware_lint_common</test_depend>
+  <test_depend>geometry_msgs</test_depend>
+
+  <export>
+    <build_type>ament_cmake</build_type>
+  </export>
+</package>

master/src/universe/common/autoware_auto_common/test/gtest_main.cpp

@@ -0,0 +1,23 @@
+// Copyright 2018 the Autoware Foundation
+//
+// 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.
+//
+// Co-developed by Tier IV, Inc. and Apex.AI, Inc.
+
+#include "gtest/gtest.h"
+
+int main(int argc, char * argv[])
+{
+  ::testing::InitGoogleTest(&argc, argv);
+  return RUN_ALL_TESTS();
+}

master/src/universe/common/autoware_auto_common/test/test_angle_utils.cpp

@@ -0,0 +1,38 @@
+// Copyright 2021 Apex.AI, 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.
+//
+// Developed by Apex.AI, Inc.
+
+#include "autoware_auto_common/common/types.hpp"
+#include "autoware_auto_common/helper_functions/angle_utils.hpp"
+
+#include <gtest/gtest.h>
+
+namespace
+{
+using autoware::common::helper_functions::wrap_angle;
+using autoware::common::types::float32_t;
+using autoware::common::types::float64_t;
+
+}  // namespace
+
+/// @test       Wrap an angle.
+TEST(TestAngleUtils, WrapAngle)
+{
+  EXPECT_DOUBLE_EQ(wrap_angle(-5.0 * M_PI_2), -M_PI_2);
+  EXPECT_DOUBLE_EQ(wrap_angle(5.0 * M_PI_2), M_PI_2);
+  EXPECT_DOUBLE_EQ(wrap_angle(M_PI), -M_PI);
+  EXPECT_DOUBLE_EQ(wrap_angle(-M_PI), -M_PI);
+  EXPECT_DOUBLE_EQ(wrap_angle(0.0), 0.0);
+}

master/src/universe/common/autoware_auto_common/test/test_bool_comparisons.cpp

@@ -0,0 +1,45 @@
+// Copyright 2020 Mapless AI, Inc.
+//
+// Permission is hereby granted, free of charge, to any person obtaining a copy
+// of this software and associated documentation files (the "Software"), to
+// deal in the Software without restriction, including without limitation the
+// rights to use, copy, modify, merge, publish, distribute, sublicense, and/or
+// sell copies of the Software, and to permit persons to whom the Software is
+// furnished to do so, subject to the following conditions:
+//
+// The above copyright notice and this permission notice shall be included in
+// all copies or substantial portions of the Software.
+//
+// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
+//  AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
+//  LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING
+// FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS
+// IN THE SOFTWARE.
+
+#include "autoware_auto_common/helper_functions/bool_comparisons.hpp"
+
+#include <gtest/gtest.h>
+
+// cppcheck does not like gtest macros inside of namespaces:
+// https://sourceforge.net/p/cppcheck/discussion/general/thread/e68df47b/
+// use a namespace alias instead of putting macros into the namespace
+namespace comp = autoware::common::helper_functions::comparisons;
+
+//------------------------------------------------------------------------------
+
+TEST(HelperFunctionsComparisons, ExclusiveOr)
+{
+  EXPECT_TRUE(comp::exclusive_or(0, 1));
+  EXPECT_TRUE(comp::exclusive_or(1, 0));
+  EXPECT_FALSE(comp::exclusive_or(0, 0));
+  EXPECT_FALSE(comp::exclusive_or(1, 1));
+
+  EXPECT_TRUE(comp::exclusive_or(false, true));
+  EXPECT_TRUE(comp::exclusive_or(true, false));
+  EXPECT_FALSE(comp::exclusive_or(false, false));
+  EXPECT_FALSE(comp::exclusive_or(true, true));
+}
+
+//------------------------------------------------------------------------------

master/src/universe/common/autoware_auto_common/test/test_byte_reader.cpp

@@ -0,0 +1,54 @@
+// Copyright 2019 the Autoware Foundation
+//
+// 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.
+//
+// Co-developed by Tier IV, Inc. and Apex.AI, Inc.
+
+#include "autoware_auto_common/common/types.hpp"
+#include "autoware_auto_common/helper_functions/byte_reader.hpp"
+
+#include <gtest/gtest.h>
+
+#include <vector>
+
+using autoware::common::types::float64_t;
+
+namespace
+{
+class ByteReader : public ::testing::Test
+{
+};
+}  // namespace
+
+// tests serial_driver_node's get_packet function which receives serial packages
+TEST_F(ByteReader, Basic)
+{
+  std::vector<uint8_t> data = {0x00, 0x00, 0x00, 0x17, 0x40, 0x28, 0xAE, 0x14,
+                               0x7A, 0xE1, 0x47, 0xAE, 0x00, 0x00, 0x08};
+
+  autoware::common::helper_functions::ByteReader byte_reader(data);
+
+  uint32_t a = 0;
+  byte_reader.read(a);
+  ASSERT_EQ(a, 23U);
+
+  float64_t b = 0;
+  byte_reader.read(b);
+  ASSERT_EQ(b, 12.34);
+
+  byte_reader.skip(1);
+
+  int16_t c = 0;
+  byte_reader.read(c);
+  ASSERT_EQ(c, 8);
+}

master/src/universe/common/autoware_auto_common/test/test_float_comparisons.cpp

@@ -0,0 +1,159 @@
+// Copyright 2020 Mapless AI, Inc.
+//
+// Permission is hereby granted, free of charge, to any person obtaining a copy
+// of this software and associated documentation files (the "Software"), to
+// deal in the Software without restriction, including without limitation the
+// rights to use, copy, modify, merge, publish, distribute, sublicense, and/or
+// sell copies of the Software, and to permit persons to whom the Software is
+// furnished to do so, subject to the following conditions:
+//
+// The above copyright notice and this permission notice shall be included in
+// all copies or substantial portions of the Software.
+//
+// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
+//  AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
+//  LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING
+// FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS
+// IN THE SOFTWARE.
+
+#include "autoware_auto_common/helper_functions/float_comparisons.hpp"
+
+#include <gtest/gtest.h>
+
+// cppcheck does not like gtest macros inside of namespaces:
+// https://sourceforge.net/p/cppcheck/discussion/general/thread/e68df47b/
+// use a namespace alias instead of putting macros into the namespace
+namespace comp = autoware::common::helper_functions::comparisons;
+
+namespace
+{
+const auto a = 1.317;
+const auto b = 2.0;
+const auto c = -5.2747;
+const auto d = 0.0;
+const auto e = -5.2747177;
+const auto f = -5.2749;
+const auto epsilon = 0.0001;
+}  // namespace
+
+//------------------------------------------------------------------------------
+
+TEST(HelperFunctionsComparisons, AbsEqZero)
+{
+  EXPECT_TRUE(comp::abs_eq_zero(d, epsilon));
+  EXPECT_TRUE(comp::abs_eq_zero(d + epsilon * epsilon, epsilon));
+  EXPECT_FALSE(comp::abs_eq_zero(d + 2.0 * epsilon, epsilon));
+  EXPECT_FALSE(comp::abs_eq_zero(1.0, epsilon));
+  EXPECT_TRUE(comp::abs_eq_zero(0.0, 0.0));
+}
+
+//------------------------------------------------------------------------------
+
+TEST(HelperFunctionsComparisons, AbsEq)
+{
+  EXPECT_TRUE(comp::abs_eq(c, e, epsilon));
+  EXPECT_TRUE(comp::abs_eq(e, c, epsilon));
+  EXPECT_FALSE(comp::abs_eq(c, e, 0.0));
+  EXPECT_FALSE(comp::abs_eq(e, c, 0.0));
+  EXPECT_FALSE(comp::abs_eq(a, b, epsilon));
+  EXPECT_FALSE(comp::abs_eq(b, a, epsilon));
+  EXPECT_TRUE(comp::abs_eq(a, a, epsilon));
+  EXPECT_TRUE(comp::abs_eq(a, a, 0.0));
+}
+
+//------------------------------------------------------------------------------
+
+TEST(HelperFunctionsComparisons, AbsLt)
+{
+  EXPECT_TRUE(comp::abs_lt(f, c, 0.0));
+  EXPECT_TRUE(comp::abs_lt(f, c, epsilon));
+  EXPECT_FALSE(comp::abs_lt(c, f, epsilon));
+  EXPECT_FALSE(comp::abs_lt(d, d, epsilon));
+  EXPECT_FALSE(comp::abs_lt(d, d, 0.0));
+}
+
+//------------------------------------------------------------------------------
+
+TEST(HelperFunctionsComparisons, AbsLte)
+{
+  EXPECT_TRUE(comp::abs_lte(c, e, epsilon));
+  EXPECT_TRUE(comp::abs_lte(e, c, epsilon));
+  EXPECT_FALSE(comp::abs_lte(c, e, 0.0));
+  EXPECT_TRUE(comp::abs_lte(e, c, 0.0));
+  EXPECT_TRUE(comp::abs_lte(c, e, epsilon));
+  EXPECT_TRUE(comp::abs_lte(e, c, epsilon));
+  EXPECT_TRUE(comp::abs_lte(a, b, epsilon));
+  EXPECT_FALSE(comp::abs_lte(b, a, epsilon));
+  EXPECT_TRUE(comp::abs_lte(d, d, epsilon));
+  EXPECT_TRUE(comp::abs_lte(d, d, 0.0));
+}
+
+//------------------------------------------------------------------------------
+
+TEST(HelperFunctionsComparisons, AbsGt)
+{
+  EXPECT_TRUE(comp::abs_gt(c, e, 0.0));
+  EXPECT_FALSE(comp::abs_gt(c, e, epsilon));
+  EXPECT_FALSE(comp::abs_gt(f, c, epsilon));
+  EXPECT_TRUE(comp::abs_gt(c, f, epsilon));
+  EXPECT_FALSE(comp::abs_gt(d, d, epsilon));
+  EXPECT_FALSE(comp::abs_gt(d, d, 0.0));
+}
+
+//------------------------------------------------------------------------------
+
+TEST(HelperFunctionsComparisons, AbsGte)
+{
+  EXPECT_TRUE(comp::abs_gte(c, e, 0.0));
+  EXPECT_FALSE(comp::abs_gte(e, c, 0.0));
+  EXPECT_TRUE(comp::abs_gte(c, e, epsilon));
+  EXPECT_TRUE(comp::abs_gte(e, c, epsilon));
+  EXPECT_FALSE(comp::abs_gte(f, c, epsilon));
+  EXPECT_TRUE(comp::abs_gte(c, f, epsilon));
+  EXPECT_TRUE(comp::abs_gte(d, d, epsilon));
+  EXPECT_TRUE(comp::abs_gte(d, d, 0.0));
+}
+
+//------------------------------------------------------------------------------
+
+TEST(HelperFunctionsComparisons, RelEq)
+{
+  EXPECT_FALSE(comp::rel_eq(c, e, 0.0));
+  EXPECT_FALSE(comp::rel_eq(e, c, 0.0));
+  EXPECT_TRUE(comp::rel_eq(a, a, 0.0));
+
+  EXPECT_TRUE(comp::rel_eq(c, e, 1.0));
+  EXPECT_TRUE(comp::rel_eq(e, c, 1.0));
+  EXPECT_TRUE(comp::rel_eq(a, b, 1.0));
+  EXPECT_TRUE(comp::rel_eq(b, a, 1.0));
+
+  EXPECT_FALSE(comp::rel_eq(1.0, 1.1, 0.01));
+  EXPECT_TRUE(comp::rel_eq(10000.0, 10010.0, 0.01));
+}
+
+//------------------------------------------------------------------------------
+
+TEST(HelperFunctionsComparisons, ApproxEq)
+{
+  EXPECT_TRUE(comp::approx_eq(c, e, epsilon, 0.0));
+  EXPECT_TRUE(comp::approx_eq(e, c, epsilon, 0.0));
+  EXPECT_TRUE(comp::approx_eq(a, a, epsilon, 0.0));
+  EXPECT_TRUE(comp::approx_eq(a, a, 0.0, 0.0));
+
+  EXPECT_FALSE(comp::approx_eq(c, e, 0.0, 0.0));
+  EXPECT_FALSE(comp::approx_eq(e, c, 0.0, 0.0));
+  EXPECT_FALSE(comp::approx_eq(a, b, epsilon, 0.0));
+  EXPECT_FALSE(comp::approx_eq(b, a, epsilon, 0.0));
+
+  EXPECT_TRUE(comp::approx_eq(c, e, 0.0, 1.0));
+  EXPECT_TRUE(comp::approx_eq(e, c, 0.0, 1.0));
+  EXPECT_TRUE(comp::approx_eq(a, b, epsilon, 1.0));
+  EXPECT_TRUE(comp::approx_eq(b, a, epsilon, 1.0));
+
+  EXPECT_TRUE(comp::approx_eq(1.0, 1.1, 0.2, 0.01));
+  EXPECT_TRUE(comp::approx_eq(10000.0, 10010.0, 0.2, 0.01));
+}
+
+//------------------------------------------------------------------------------

master/src/universe/common/autoware_auto_common/test/test_mahalanobis_distance.cpp

@@ -0,0 +1,40 @@
+// Copyright 2021 Apex.AI, 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.
+//
+// Co-developed by Tier IV, Inc. and Apex.AI, Inc.
+#include "autoware_auto_common/common/types.hpp"
+#include "autoware_auto_common/helper_functions/mahalanobis_distance.hpp"
+
+#include <gtest/gtest.h>
+
+TEST(MahalanobisDistanceTest, BasicTest)
+{
+  Eigen::Matrix<autoware::common::types::float32_t, 2, 1> mean;
+  mean << 2.F, 2.F;
+  Eigen::Matrix<autoware::common::types::float32_t, 2, 1> sample;
+  sample << 2.F, 3.F;
+  Eigen::Matrix<autoware::common::types::float32_t, 2, 2> cov;
+  cov << 0.1F, 0.0F, 0.0F, 0.6F;
+
+  // the two states are independent and one has more variance than the other. With samples
+  // equidistant from mean but on two different axes will have vastly different
+  // mahalanobis distance values
+  EXPECT_FLOAT_EQ(
+    autoware::common::helper_functions::calculate_mahalanobis_distance(sample, mean, cov),
+    1.666666666F);
+
+  sample << 3.F, 2.F;
+  EXPECT_FLOAT_EQ(
+    autoware::common::helper_functions::calculate_mahalanobis_distance(sample, mean, cov), 10.0F);
+}

master/src/universe/common/autoware_auto_common/test/test_message_field_adapters.cpp

@@ -0,0 +1,81 @@
+// Copyright 2017-2020 the Autoware Foundation
+//
+// 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.
+//
+// Co-developed by Tier IV, Inc. and Apex.AI, Inc.
+
+#include "autoware_auto_common/helper_functions/message_adapters.hpp"
+
+#include <geometry_msgs/msg/transform_stamped.hpp>
+
+#include <gtest/gtest.h>
+
+#include <memory>
+#include <vector>
+
+using autoware::common::helper_functions::message_field_adapters::get_frame_id;
+using autoware::common::helper_functions::message_field_adapters::get_stamp;
+
+namespace
+{
+builtin_interfaces::msg::Time get_stamp_msg(int t)
+{
+  builtin_interfaces::msg::Time stamp;
+  stamp.sec = 0;
+  stamp.nanosec = t;
+  return stamp;
+}
+}  // namespace
+
+TEST(MessageFieldAdapterTest, ConstHeaderTests)
+{
+  using Message = geometry_msgs::msg::TransformStamped;
+
+  const auto stamp = get_stamp_msg(0);
+  const auto frame_id = "MessageFieldAdapterTest_frame";
+
+  std_msgs::msg::Header header;
+  header.stamp = stamp;
+  header.frame_id = frame_id;
+  const Message msg{Message{}.set__header(header)};
+
+  EXPECT_EQ(stamp, get_stamp(msg));
+  EXPECT_EQ(frame_id, get_frame_id(msg));
+}
+
+TEST(MessageFieldAdapterTest, NonconstHeaderTests)
+{
+  using Message = geometry_msgs::msg::TransformStamped;
+
+  const auto stamp = get_stamp_msg(0);
+  const auto frame_id = "MessageFieldAdapterTest_frame";
+  const auto stamp2 = get_stamp_msg(500);
+  const auto frame_id2 = "MessageFieldAdapterTest_frame2";
+
+  ASSERT_NE(stamp, stamp2);
+  ASSERT_NE(frame_id, frame_id2);
+
+  std_msgs::msg::Header header;
+  header.stamp = stamp;
+  header.frame_id = frame_id;
+  Message msg{Message{}.set__header(header)};
+
+  EXPECT_EQ(stamp, get_stamp(msg));
+  EXPECT_EQ(frame_id, get_frame_id(msg));
+
+  get_stamp(msg) = stamp2;
+  get_frame_id(msg) = frame_id2;
+
+  EXPECT_EQ(stamp2, get_stamp(msg));
+  EXPECT_EQ(frame_id2, get_frame_id(msg));
+}

master/src/universe/common/autoware_auto_common/test/test_template_utils.cpp

@@ -0,0 +1,124 @@
+// Copyright 2021 the Autoware Foundation
+//
+// 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.
+//
+// Co-developed by Tier IV, Inc. and Apex.AI, Inc.
+
+#include "autoware_auto_common/helper_functions/template_utils.hpp"
+
+#include <gtest/gtest.h>
+
+struct CorrectType
+{
+};
+struct FalseType
+{
+};
+
+struct Foo
+{
+  static CorrectType bar(CorrectType, const CorrectType &, CorrectType *) { return CorrectType{}; }
+};
+
+template <template <typename> class Expression, typename... Ts>
+using expression_valid_with_return =
+  ::autoware::common::helper_functions::expression_valid_with_return<Expression, Ts...>;
+template <template <typename> class Expression, typename... Ts>
+using expression_valid = ::autoware::common::helper_functions::expression_valid<Expression, Ts...>;
+
+// Types are defined here and not in the header because these definitions are basically the test
+// code themselves.
+
+// Correct way to call Foo::bar(...)
+template <typename FooT, typename In1, typename In2, typename In3>
+using call_bar_expression = decltype(std::declval<FooT>().bar(
+  std::declval<In1>(), std::declval<const In2 &>(), std::declval<In3 *>()));
+
+// Another correct way to call Foo::bar(...) since a temporary can bind to the const lvalue
+// reference
+template <typename FooT, typename In1, typename In2, typename In3>
+using call_bar_expression2 = decltype(std::declval<FooT>().bar(
+  std::declval<In1>(), std::declval<In2>(), std::declval<In3 *>()));
+
+// Signature mismatch:
+template <typename FooT, typename In1, typename In2, typename In3>
+using false_bar_expression1 =
+  decltype(std::declval<FooT>().bar(std::declval<In1>(), std::declval<In2>(), std::declval<In3>()));
+
+// Signature mismatch:
+template <typename FooT, typename In1, typename In2>
+using false_bar_expression2 =
+  decltype(std::declval<FooT>().bar(std::declval<In1>(), std::declval<const In2 &>()));
+
+// cspell: ignore asdasd
+// Signature mismatch:
+template <typename FooT, typename In1, typename In2, typename In3>
+using false_bar_expression3 = decltype(std::declval<FooT>().asdasd(
+  std::declval<In1>(), std::declval<const In2 &>(), std::declval<In3 *>()));
+
+// Correct signature, correct types:
+template <typename FooT>
+using correct_expression1 = call_bar_expression<FooT, CorrectType, CorrectType, CorrectType>;
+template <typename FooT>
+using correct_expression2 = call_bar_expression2<FooT, CorrectType, CorrectType, CorrectType>;
+
+// Correct signature, false types:
+template <typename FooT>
+using false_expression1 = call_bar_expression<FooT, FalseType, CorrectType, CorrectType>;
+template <typename FooT>
+using false_expression2 = call_bar_expression<FooT, CorrectType, FalseType, CorrectType>;
+template <typename FooT>
+using false_expression3 = call_bar_expression<FooT, FalseType, FalseType, FalseType>;
+
+// False signature, correct types:
+template <typename FooT>
+using false_expression4 = false_bar_expression1<FooT, CorrectType, CorrectType, CorrectType>;
+template <typename FooT>
+using false_expression5 = false_bar_expression3<FooT, CorrectType, CorrectType, CorrectType>;
+
+// False signature, false types:
+template <typename FooT>
+using false_expression6 = false_bar_expression1<FooT, CorrectType, CorrectType, CorrectType>;
+template <typename FooT>
+using false_expression7 = false_bar_expression2<FooT, CorrectType, CorrectType>;
+
+TEST(TestTemplateUtils, ExpressionValid)
+{
+  EXPECT_TRUE((expression_valid<correct_expression1, Foo>::value));
+  EXPECT_TRUE((expression_valid<correct_expression2, Foo>::value));
+  EXPECT_FALSE((expression_valid<false_expression1, Foo>::value));
+  EXPECT_FALSE((expression_valid<false_expression2, Foo>::value));
+  EXPECT_FALSE((expression_valid<false_expression3, Foo>::value));
+  EXPECT_FALSE((expression_valid<false_expression4, Foo>::value));
+  EXPECT_FALSE((expression_valid<false_expression5, Foo>::value));
+  EXPECT_FALSE((expression_valid<false_expression6, Foo>::value));
+  EXPECT_FALSE((expression_valid<false_expression7, Foo>::value));
+}
+
+TEST(TestTemplateUtils, ExpressionReturnValid)
+{
+  EXPECT_TRUE((expression_valid_with_return<correct_expression1, Foo, CorrectType>::value));
+  EXPECT_FALSE((expression_valid_with_return<correct_expression1, Foo, FalseType>::value));
+
+  EXPECT_TRUE((expression_valid_with_return<correct_expression2, Foo, CorrectType>::value));
+  EXPECT_FALSE((expression_valid_with_return<correct_expression2, Foo, FalseType>::value));
+
+  // If an expression is not valid, returning the right type will not be enough.
+  EXPECT_FALSE((expression_valid_with_return<false_expression1, Foo, CorrectType>::value));
+  EXPECT_FALSE((expression_valid_with_return<false_expression2, Foo, CorrectType>::value));
+  EXPECT_FALSE((expression_valid_with_return<false_expression3, Foo, CorrectType>::value));
+  EXPECT_FALSE((expression_valid_with_return<false_expression4, Foo, CorrectType>::value));
+  EXPECT_FALSE((expression_valid_with_return<false_expression5, Foo, CorrectType>::value));
+  EXPECT_FALSE((expression_valid_with_return<false_expression6, Foo, CorrectType>::value));
+  EXPECT_FALSE((expression_valid_with_return<false_expression7, Foo, CorrectType>::value));
+}

master/src/universe/common/autoware_auto_common/test/test_type_name.cpp

@@ -0,0 +1,40 @@
+// Copyright 2021 Apex.AI, 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.
+//
+// Developed by Apex.AI, Inc.
+
+#include "autoware_auto_common/common/types.hpp"
+#include "autoware_auto_common/helper_functions/type_name.hpp"
+
+#include <gtest/gtest.h>
+
+namespace
+{
+using autoware::common::types::float32_t;
+using autoware::common::types::float64_t;
+using autoware::helper_functions::get_type_name;
+
+struct SomeStruct
+{
+};
+}  // namespace
+
+/// @test       Test that type names can be demangled.
+TEST(TestTypeDemangling, Demangle)
+{
+  EXPECT_EQ(get_type_name<float32_t>(), "float");
+  const float64_t val{42.0};
+  EXPECT_EQ(get_type_name(val), "double");
+  EXPECT_EQ(get_type_name<SomeStruct>(), "(anonymous namespace)::SomeStruct");
+}

master/src/universe/common/autoware_auto_common/test/test_type_traits.cpp

@@ -0,0 +1,105 @@
+// Copyright 2021 Apex.AI, 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.
+//
+// Developed by Apex.AI, Inc.
+
+#include "autoware_auto_common/common/type_traits.hpp"
+#include "autoware_auto_common/common/types.hpp"
+
+#include <gtest/gtest.h>
+
+#include <tuple>
+
+namespace
+{
+/// @brief      A simple testing function to check if all types are arithmetic.
+///
+///             Trait "is_arithmetic" is picked at random and any other trait could have been used
+///             instead.
+template <typename... Ts>
+bool all_are_arithmetic()
+{
+  // This is just a random function that we use with conjunction.
+  return autoware::common::type_traits::conjunction<std::is_arithmetic<Ts>...>::value;
+}
+
+using autoware::common::types::float32_t;
+using autoware::common::types::float64_t;
+
+}  // namespace
+
+/// @test       Test that index of a type can be computed.
+TEST(TestCommonTypeTraits, Index)
+{
+  using T = std::tuple<std::int32_t, float64_t>;
+  EXPECT_EQ(0, (autoware::common::type_traits::index<std::int32_t, T>::value));
+  EXPECT_EQ(1, (autoware::common::type_traits::index<float64_t, T>::value));
+}
+
+TEST(TestCommonTypeTraits, Conjunction)
+{
+  EXPECT_TRUE((all_are_arithmetic<std::int32_t, float32_t>()));
+  EXPECT_FALSE(
+    (all_are_arithmetic<std::int32_t, float32_t, std::tuple<std::int32_t, float32_t>>()));
+}
+
+TEST(TestCommonTypeTraits, Visit)
+{
+  const std::tuple<std::int32_t, float64_t> t;
+  std::int32_t counter{};
+  autoware::common::type_traits::visit(t, [&counter](const auto &) { counter++; });
+  EXPECT_EQ(2, counter);
+  float64_t sum{};
+  autoware::common::type_traits::visit(
+    std::make_tuple(2, 42.0F, 23.0),
+    [&sum](const auto & element) { sum += static_cast<float64_t>(element); });
+  EXPECT_DOUBLE_EQ(67.0, sum);
+}
+
+TEST(TestCommonTypeTraits, HasType)
+{
+  struct T1
+  {
+  };
+  struct T2
+  {
+  };
+  struct T3
+  {
+  };
+  EXPECT_TRUE((autoware::common::type_traits::has_type<T1, std::tuple<T1, T2>>::value));
+  EXPECT_FALSE((autoware::common::type_traits::has_type<T3, std::tuple<T1, T2>>::value));
+  EXPECT_FALSE((autoware::common::type_traits::has_type<T1, std::tuple<>>::value));
+}
+
+TEST(TestCommonTypeTraits, TypeIntersection)
+{
+  struct T1
+  {
+  };
+  struct T2
+  {
+  };
+  struct T3
+  {
+  };
+  using A = std::tuple<T1, T2>;
+  using B = std::tuple<T2, T3>;
+  using C = std::tuple<T3>;
+  EXPECT_TRUE(
+    (std::is_same<std::tuple<T2>, autoware::common::type_traits::intersect<A, B>::type>::value));
+  EXPECT_TRUE((std::is_same<A, autoware::common::type_traits::intersect<A, A>::type>::value));
+  EXPECT_TRUE(
+    (std::is_same<std::tuple<>, autoware::common::type_traits::intersect<A, C>::type>::value));
+}

master/src/universe/common/autoware_component_interface_specs/CMakeLists.txt

@@ -0,0 +1,20 @@
+cmake_minimum_required(VERSION 3.14)
+project(autoware_component_interface_specs)
+
+find_package(autoware_cmake REQUIRED)
+autoware_package()
+
+if(BUILD_TESTING)
+  ament_auto_add_gtest(gtest_${PROJECT_NAME}
+    test/gtest_main.cpp
+    test/test_planning.cpp
+    test/test_control.cpp
+    test/test_localization.cpp
+    test/test_system.cpp
+    test/test_map.cpp
+    test/test_perception.cpp
+    test/test_vehicle.cpp
+  )
+endif()
+
+ament_auto_package()

master/src/universe/common/autoware_component_interface_specs/README.md

@@ -0,0 +1,2 @@
+# autoware_component_interface_specs
+该功能包是**Autoware功能组件接口**的规格定义包。

master/src/universe/common/autoware_component_interface_specs/include/autoware/component_interface_specs/control.hpp

@@ -0,0 +1,70 @@
+// Copyright 2022 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__COMPONENT_INTERFACE_SPECS__CONTROL_HPP_
+#define AUTOWARE__COMPONENT_INTERFACE_SPECS__CONTROL_HPP_
+
+#include <rclcpp/qos.hpp>
+
+#include <tier4_control_msgs/msg/is_paused.hpp>
+#include <tier4_control_msgs/msg/is_start_requested.hpp>
+#include <tier4_control_msgs/msg/is_stopped.hpp>
+#include <tier4_control_msgs/srv/set_pause.hpp>
+#include <tier4_control_msgs/srv/set_stop.hpp>
+
+namespace autoware::component_interface_specs::control
+{
+
+struct SetPause
+{
+  using Service = tier4_control_msgs::srv::SetPause;
+  static constexpr char name[] = "/control/vehicle_cmd_gate/set_pause";
+};
+
+struct IsPaused
+{
+  using Message = tier4_control_msgs::msg::IsPaused;
+  static constexpr char name[] = "/control/vehicle_cmd_gate/is_paused";
+  static constexpr size_t depth = 1;
+  static constexpr auto reliability = RMW_QOS_POLICY_RELIABILITY_RELIABLE;
+  static constexpr auto durability = RMW_QOS_POLICY_DURABILITY_TRANSIENT_LOCAL;
+};
+
+struct IsStartRequested
+{
+  using Message = tier4_control_msgs::msg::IsStartRequested;
+  static constexpr char name[] = "/control/vehicle_cmd_gate/is_start_requested";
+  static constexpr size_t depth = 1;
+  static constexpr auto reliability = RMW_QOS_POLICY_RELIABILITY_RELIABLE;
+  static constexpr auto durability = RMW_QOS_POLICY_DURABILITY_TRANSIENT_LOCAL;
+};
+
+struct SetStop
+{
+  using Service = tier4_control_msgs::srv::SetStop;
+  static constexpr char name[] = "/control/vehicle_cmd_gate/set_stop";
+};
+
+struct IsStopped
+{
+  using Message = tier4_control_msgs::msg::IsStopped;
+  static constexpr char name[] = "/control/vehicle_cmd_gate/is_stopped";
+  static constexpr size_t depth = 1;
+  static constexpr auto reliability = RMW_QOS_POLICY_RELIABILITY_RELIABLE;
+  static constexpr auto durability = RMW_QOS_POLICY_DURABILITY_TRANSIENT_LOCAL;
+};
+
+}  // namespace autoware::component_interface_specs::control
+
+#endif  // AUTOWARE__COMPONENT_INTERFACE_SPECS__CONTROL_HPP_

master/src/universe/common/autoware_component_interface_specs/include/autoware/component_interface_specs/localization.hpp

@@ -0,0 +1,63 @@
+// Copyright 2022 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__COMPONENT_INTERFACE_SPECS__LOCALIZATION_HPP_
+#define AUTOWARE__COMPONENT_INTERFACE_SPECS__LOCALIZATION_HPP_
+
+#include <rclcpp/qos.hpp>
+
+#include <autoware_adapi_v1_msgs/msg/localization_initialization_state.hpp>
+#include <geometry_msgs/msg/accel_with_covariance_stamped.hpp>
+#include <nav_msgs/msg/odometry.hpp>
+#include <tier4_localization_msgs/srv/initialize_localization.hpp>
+
+namespace autoware::component_interface_specs::localization
+{
+
+struct Initialize
+{
+  using Service = tier4_localization_msgs::srv::InitializeLocalization;
+  static constexpr char name[] = "/localization/initialize";
+};
+
+struct InitializationState
+{
+  using Message = autoware_adapi_v1_msgs::msg::LocalizationInitializationState;
+  static constexpr char name[] = "/localization/initialization_state";
+  static constexpr size_t depth = 1;
+  static constexpr auto reliability = RMW_QOS_POLICY_RELIABILITY_RELIABLE;
+  static constexpr auto durability = RMW_QOS_POLICY_DURABILITY_TRANSIENT_LOCAL;
+};
+
+struct KinematicState
+{
+  using Message = nav_msgs::msg::Odometry;
+  static constexpr char name[] = "/localization/kinematic_state";
+  static constexpr size_t depth = 1;
+  static constexpr auto reliability = RMW_QOS_POLICY_RELIABILITY_RELIABLE;
+  static constexpr auto durability = RMW_QOS_POLICY_DURABILITY_VOLATILE;
+};
+
+struct Acceleration
+{
+  using Message = geometry_msgs::msg::AccelWithCovarianceStamped;
+  static constexpr char name[] = "/localization/acceleration";
+  static constexpr size_t depth = 1;
+  static constexpr auto reliability = RMW_QOS_POLICY_RELIABILITY_RELIABLE;
+  static constexpr auto durability = RMW_QOS_POLICY_DURABILITY_VOLATILE;
+};
+
+}  // namespace autoware::component_interface_specs::localization
+
+#endif  // AUTOWARE__COMPONENT_INTERFACE_SPECS__LOCALIZATION_HPP_

master/src/universe/common/autoware_component_interface_specs/include/autoware/component_interface_specs/map.hpp

@@ -0,0 +1,36 @@
+// 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__COMPONENT_INTERFACE_SPECS__MAP_HPP_
+#define AUTOWARE__COMPONENT_INTERFACE_SPECS__MAP_HPP_
+
+#include <rclcpp/qos.hpp>
+
+#include <tier4_map_msgs/msg/map_projector_info.hpp>
+
+namespace autoware::component_interface_specs::map
+{
+
+struct MapProjectorInfo
+{
+  using Message = tier4_map_msgs::msg::MapProjectorInfo;
+  static constexpr char name[] = "/map/map_projector_info";
+  static constexpr size_t depth = 1;
+  static constexpr auto reliability = RMW_QOS_POLICY_RELIABILITY_RELIABLE;
+  static constexpr auto durability = RMW_QOS_POLICY_DURABILITY_TRANSIENT_LOCAL;
+};
+
+}  // namespace autoware::component_interface_specs::map
+
+#endif  // AUTOWARE__COMPONENT_INTERFACE_SPECS__MAP_HPP_

master/src/universe/common/autoware_component_interface_specs/include/autoware/component_interface_specs/perception.hpp

@@ -0,0 +1,36 @@
+// Copyright 2022 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__COMPONENT_INTERFACE_SPECS__PERCEPTION_HPP_
+#define AUTOWARE__COMPONENT_INTERFACE_SPECS__PERCEPTION_HPP_
+
+#include <rclcpp/qos.hpp>
+
+#include <autoware_perception_msgs/msg/predicted_objects.hpp>
+
+namespace autoware::component_interface_specs::perception
+{
+
+struct ObjectRecognition
+{
+  using Message = autoware_perception_msgs::msg::PredictedObjects;
+  static constexpr char name[] = "/perception/object_recognition/objects";
+  static constexpr size_t depth = 1;
+  static constexpr auto reliability = RMW_QOS_POLICY_RELIABILITY_RELIABLE;
+  static constexpr auto durability = RMW_QOS_POLICY_DURABILITY_VOLATILE;
+};
+
+}  // namespace autoware::component_interface_specs::perception
+
+#endif  // AUTOWARE__COMPONENT_INTERFACE_SPECS__PERCEPTION_HPP_

master/src/universe/common/autoware_component_interface_specs/include/autoware/component_interface_specs/planning.hpp

@@ -0,0 +1,78 @@
+// Copyright 2022 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__COMPONENT_INTERFACE_SPECS__PLANNING_HPP_
+#define AUTOWARE__COMPONENT_INTERFACE_SPECS__PLANNING_HPP_
+
+#include <rclcpp/qos.hpp>
+
+#include <autoware_planning_msgs/msg/lanelet_route.hpp>
+#include <autoware_planning_msgs/msg/trajectory.hpp>
+#include <tier4_planning_msgs/msg/route_state.hpp>
+#include <tier4_planning_msgs/srv/clear_route.hpp>
+#include <tier4_planning_msgs/srv/set_lanelet_route.hpp>
+#include <tier4_planning_msgs/srv/set_waypoint_route.hpp>
+
+namespace autoware::component_interface_specs::planning
+{
+
+struct SetLaneletRoute
+{
+  using Service = tier4_planning_msgs::srv::SetLaneletRoute;
+  static constexpr char name[] = "/planning/mission_planning/route_selector/main/set_lanelet_route";
+};
+
+struct SetWaypointRoute
+{
+  using Service = tier4_planning_msgs::srv::SetWaypointRoute;
+  static constexpr char name[] =
+    "/planning/mission_planning/route_selector/main/set_waypoint_route";
+};
+
+struct ClearRoute
+{
+  using Service = tier4_planning_msgs::srv::ClearRoute;
+  static constexpr char name[] = "/planning/mission_planning/route_selector/main/clear_route";
+};
+
+struct RouteState
+{
+  using Message = tier4_planning_msgs::msg::RouteState;
+  static constexpr char name[] = "/planning/mission_planning/route_selector/main/state";
+  static constexpr size_t depth = 1;
+  static constexpr auto reliability = RMW_QOS_POLICY_RELIABILITY_RELIABLE;
+  static constexpr auto durability = RMW_QOS_POLICY_DURABILITY_TRANSIENT_LOCAL;
+};
+
+struct LaneletRoute
+{
+  using Message = autoware_planning_msgs::msg::LaneletRoute;
+  static constexpr char name[] = "/planning/mission_planning/route_selector/main/route";
+  static constexpr size_t depth = 1;
+  static constexpr auto reliability = RMW_QOS_POLICY_RELIABILITY_RELIABLE;
+  static constexpr auto durability = RMW_QOS_POLICY_DURABILITY_TRANSIENT_LOCAL;
+};
+
+struct Trajectory
+{
+  using Message = autoware_planning_msgs::msg::Trajectory;
+  static constexpr char name[] = "/planning/scenario_planning/trajectory";
+  static constexpr size_t depth = 1;
+  static constexpr auto reliability = RMW_QOS_POLICY_RELIABILITY_RELIABLE;
+  static constexpr auto durability = RMW_QOS_POLICY_DURABILITY_VOLATILE;
+};
+
+}  // namespace autoware::component_interface_specs::planning
+
+#endif  // AUTOWARE__COMPONENT_INTERFACE_SPECS__PLANNING_HPP_

master/src/universe/common/autoware_component_interface_specs/include/autoware/component_interface_specs/system.hpp

@@ -0,0 +1,60 @@
+// Copyright 2022 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__COMPONENT_INTERFACE_SPECS__SYSTEM_HPP_
+#define AUTOWARE__COMPONENT_INTERFACE_SPECS__SYSTEM_HPP_
+
+#include <rclcpp/qos.hpp>
+
+#include <autoware_adapi_v1_msgs/msg/mrm_state.hpp>
+#include <autoware_adapi_v1_msgs/msg/operation_mode_state.hpp>
+#include <tier4_system_msgs/srv/change_autoware_control.hpp>
+#include <tier4_system_msgs/srv/change_operation_mode.hpp>
+
+namespace autoware::component_interface_specs::system
+{
+
+struct MrmState
+{
+  using Message = autoware_adapi_v1_msgs::msg::MrmState;
+  static constexpr char name[] = "/system/fail_safe/mrm_state";
+  static constexpr size_t depth = 1;
+  static constexpr auto reliability = RMW_QOS_POLICY_RELIABILITY_RELIABLE;
+  static constexpr auto durability = RMW_QOS_POLICY_DURABILITY_VOLATILE;
+};
+
+struct ChangeAutowareControl
+{
+  using Service = tier4_system_msgs::srv::ChangeAutowareControl;
+  static constexpr char name[] = "/system/operation_mode/change_autoware_control";
+};
+
+struct ChangeOperationMode
+{
+  using Service = tier4_system_msgs::srv::ChangeOperationMode;
+  static constexpr char name[] = "/system/operation_mode/change_operation_mode";
+};
+
+struct OperationModeState
+{
+  using Message = autoware_adapi_v1_msgs::msg::OperationModeState;
+  static constexpr char name[] = "/system/operation_mode/state";
+  static constexpr size_t depth = 1;
+  static constexpr auto reliability = RMW_QOS_POLICY_RELIABILITY_RELIABLE;
+  static constexpr auto durability = RMW_QOS_POLICY_DURABILITY_TRANSIENT_LOCAL;
+};
+
+}  // namespace autoware::component_interface_specs::system
+
+#endif  // AUTOWARE__COMPONENT_INTERFACE_SPECS__SYSTEM_HPP_

master/src/universe/common/autoware_component_interface_specs/include/autoware/component_interface_specs/vehicle.hpp

@@ -0,0 +1,100 @@
+// 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__COMPONENT_INTERFACE_SPECS__VEHICLE_HPP_
+#define AUTOWARE__COMPONENT_INTERFACE_SPECS__VEHICLE_HPP_
+
+#include <rclcpp/qos.hpp>
+
+#include <autoware_adapi_v1_msgs/msg/door_status_array.hpp>
+#include <autoware_adapi_v1_msgs/srv/get_door_layout.hpp>
+#include <autoware_adapi_v1_msgs/srv/set_door_command.hpp>
+#include <autoware_vehicle_msgs/msg/gear_report.hpp>
+#include <autoware_vehicle_msgs/msg/hazard_lights_report.hpp>
+#include <autoware_vehicle_msgs/msg/steering_report.hpp>
+#include <autoware_vehicle_msgs/msg/turn_indicators_report.hpp>
+#include <tier4_vehicle_msgs/msg/battery_status.hpp>
+
+namespace autoware::component_interface_specs::vehicle
+{
+
+struct SteeringStatus
+{
+  using Message = autoware_vehicle_msgs::msg::SteeringReport;
+  static constexpr char name[] = "/vehicle/status/steering_status";
+  static constexpr size_t depth = 1;
+  static constexpr auto reliability = RMW_QOS_POLICY_RELIABILITY_RELIABLE;
+  static constexpr auto durability = RMW_QOS_POLICY_DURABILITY_VOLATILE;
+};
+
+struct GearStatus
+{
+  using Message = autoware_vehicle_msgs::msg::GearReport;
+  static constexpr char name[] = "/vehicle/status/gear_status";
+  static constexpr size_t depth = 1;
+  static constexpr auto reliability = RMW_QOS_POLICY_RELIABILITY_RELIABLE;
+  static constexpr auto durability = RMW_QOS_POLICY_DURABILITY_VOLATILE;
+};
+
+struct TurnIndicatorStatus
+{
+  using Message = autoware_vehicle_msgs::msg::TurnIndicatorsReport;
+  static constexpr char name[] = "/vehicle/status/turn_indicators_status";
+  static constexpr size_t depth = 1;
+  static constexpr auto reliability = RMW_QOS_POLICY_RELIABILITY_RELIABLE;
+  static constexpr auto durability = RMW_QOS_POLICY_DURABILITY_VOLATILE;
+};
+
+struct HazardLightStatus
+{
+  using Message = autoware_vehicle_msgs::msg::HazardLightsReport;
+  static constexpr char name[] = "/vehicle/status/hazard_lights_status";
+  static constexpr size_t depth = 1;
+  static constexpr auto reliability = RMW_QOS_POLICY_RELIABILITY_RELIABLE;
+  static constexpr auto durability = RMW_QOS_POLICY_DURABILITY_VOLATILE;
+};
+
+struct EnergyStatus
+{
+  using Message = tier4_vehicle_msgs::msg::BatteryStatus;
+  static constexpr char name[] = "/vehicle/status/battery_charge";
+  static constexpr size_t depth = 1;
+  static constexpr auto reliability = RMW_QOS_POLICY_RELIABILITY_RELIABLE;
+  static constexpr auto durability = RMW_QOS_POLICY_DURABILITY_VOLATILE;
+};
+
+struct DoorCommand
+{
+  using Service = autoware_adapi_v1_msgs::srv::SetDoorCommand;
+  static constexpr char name[] = "/vehicle/doors/command";
+};
+
+struct DoorLayout
+{
+  using Service = autoware_adapi_v1_msgs::srv::GetDoorLayout;
+  static constexpr char name[] = "/vehicle/doors/layout";
+};
+
+struct DoorStatus
+{
+  using Message = autoware_adapi_v1_msgs::msg::DoorStatusArray;
+  static constexpr char name[] = "/vehicle/doors/status";
+  static constexpr size_t depth = 1;
+  static constexpr auto reliability = RMW_QOS_POLICY_RELIABILITY_RELIABLE;
+  static constexpr auto durability = RMW_QOS_POLICY_DURABILITY_TRANSIENT_LOCAL;
+};
+
+}  // namespace autoware::component_interface_specs::vehicle
+
+#endif  // AUTOWARE__COMPONENT_INTERFACE_SPECS__VEHICLE_HPP_

master/src/universe/common/autoware_component_interface_specs/package.xml

@@ -0,0 +1,36 @@
+<?xml version="1.0"?>
+<?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
+<package format="3">
+  <name>autoware_component_interface_specs</name>
+  <version>0.0.0</version>
+  <description>The autoware_component_interface_specs package</description>
+  <maintainer email="isamu.takagi@tier4.jp">Takagi, Isamu</maintainer>
+  <maintainer email="yukihiro.saito@tier4.jp">Yukihiro Saito</maintainer>
+  <license>Apache License 2.0</license>
+
+  <buildtool_depend>ament_cmake_auto</buildtool_depend>
+  <buildtool_depend>autoware_cmake</buildtool_depend>
+
+  <depend>autoware_adapi_v1_msgs</depend>
+  <depend>autoware_perception_msgs</depend>
+  <depend>autoware_planning_msgs</depend>
+  <depend>autoware_vehicle_msgs</depend>
+  <depend>nav_msgs</depend>
+  <depend>rcl</depend>
+  <depend>rclcpp</depend>
+  <depend>rosidl_runtime_cpp</depend>
+  <depend>tier4_control_msgs</depend>
+  <depend>tier4_localization_msgs</depend>
+  <depend>tier4_map_msgs</depend>
+  <depend>tier4_planning_msgs</depend>
+  <depend>tier4_system_msgs</depend>
+  <depend>tier4_vehicle_msgs</depend>
+
+  <test_depend>ament_cmake_gtest</test_depend>
+  <test_depend>ament_lint_auto</test_depend>
+  <test_depend>autoware_lint_common</test_depend>
+
+  <export>
+    <build_type>ament_cmake</build_type>
+  </export>
+</package>

master/src/universe/common/autoware_component_interface_specs/test/gtest_main.cpp

@@ -0,0 +1,21 @@
+// Copyright 2023 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 "gtest/gtest.h"
+
+int main(int argc, char * argv[])
+{
+  ::testing::InitGoogleTest(&argc, argv);
+  return RUN_ALL_TESTS();
+}

master/src/universe/common/autoware_component_interface_specs/test/test_control.cpp

@@ -0,0 +1,46 @@
+// Copyright 2023 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/component_interface_specs/control.hpp"
+#include "gtest/gtest.h"
+
+TEST(control, interface)
+{
+  {
+    using autoware::component_interface_specs::control::IsPaused;
+    IsPaused is_paused;
+    size_t depth = 1;
+    EXPECT_EQ(is_paused.depth, depth);
+    EXPECT_EQ(is_paused.reliability, RMW_QOS_POLICY_RELIABILITY_RELIABLE);
+    EXPECT_EQ(is_paused.durability, RMW_QOS_POLICY_DURABILITY_TRANSIENT_LOCAL);
+  }
+
+  {
+    using autoware::component_interface_specs::control::IsStartRequested;
+    IsStartRequested is_start_requested;
+    size_t depth = 1;
+    EXPECT_EQ(is_start_requested.depth, depth);
+    EXPECT_EQ(is_start_requested.reliability, RMW_QOS_POLICY_RELIABILITY_RELIABLE);
+    EXPECT_EQ(is_start_requested.durability, RMW_QOS_POLICY_DURABILITY_TRANSIENT_LOCAL);
+  }
+
+  {
+    using autoware::component_interface_specs::control::IsStopped;
+    IsStopped is_stopped;
+    size_t depth = 1;
+    EXPECT_EQ(is_stopped.depth, depth);
+    EXPECT_EQ(is_stopped.reliability, RMW_QOS_POLICY_RELIABILITY_RELIABLE);
+    EXPECT_EQ(is_stopped.durability, RMW_QOS_POLICY_DURABILITY_TRANSIENT_LOCAL);
+  }
+}

master/src/universe/common/autoware_component_interface_specs/test/test_localization.cpp

@@ -0,0 +1,46 @@
+// Copyright 2023 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/component_interface_specs/localization.hpp"
+#include "gtest/gtest.h"
+
+TEST(localization, interface)
+{
+  {
+    using autoware::component_interface_specs::localization::InitializationState;
+    InitializationState initialization_state;
+    size_t depth = 1;
+    EXPECT_EQ(initialization_state.depth, depth);
+    EXPECT_EQ(initialization_state.reliability, RMW_QOS_POLICY_RELIABILITY_RELIABLE);
+    EXPECT_EQ(initialization_state.durability, RMW_QOS_POLICY_DURABILITY_TRANSIENT_LOCAL);
+  }
+
+  {
+    using autoware::component_interface_specs::localization::KinematicState;
+    KinematicState kinematic_state;
+    size_t depth = 1;
+    EXPECT_EQ(kinematic_state.depth, depth);
+    EXPECT_EQ(kinematic_state.reliability, RMW_QOS_POLICY_RELIABILITY_RELIABLE);
+    EXPECT_EQ(kinematic_state.durability, RMW_QOS_POLICY_DURABILITY_VOLATILE);
+  }
+
+  {
+    using autoware::component_interface_specs::localization::Acceleration;
+    Acceleration acceleration;
+    size_t depth = 1;
+    EXPECT_EQ(acceleration.depth, depth);
+    EXPECT_EQ(acceleration.reliability, RMW_QOS_POLICY_RELIABILITY_RELIABLE);
+    EXPECT_EQ(acceleration.durability, RMW_QOS_POLICY_DURABILITY_VOLATILE);
+  }
+}

master/src/universe/common/autoware_component_interface_specs/test/test_map.cpp

@@ -0,0 +1,28 @@
+// Copyright 2023 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/component_interface_specs/map.hpp"
+#include "gtest/gtest.h"
+
+TEST(map, interface)
+{
+  {
+    using autoware::component_interface_specs::map::MapProjectorInfo;
+    MapProjectorInfo map_projector;
+    size_t depth = 1;
+    EXPECT_EQ(map_projector.depth, depth);
+    EXPECT_EQ(map_projector.reliability, RMW_QOS_POLICY_RELIABILITY_RELIABLE);
+    EXPECT_EQ(map_projector.durability, RMW_QOS_POLICY_DURABILITY_TRANSIENT_LOCAL);
+  }
+}

master/src/universe/common/autoware_component_interface_specs/test/test_perception.cpp

@@ -0,0 +1,28 @@
+// Copyright 2023 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/component_interface_specs/perception.hpp"
+#include "gtest/gtest.h"
+
+TEST(perception, interface)
+{
+  {
+    using autoware::component_interface_specs::perception::ObjectRecognition;
+    ObjectRecognition object_recognition;
+    size_t depth = 1;
+    EXPECT_EQ(object_recognition.depth, depth);
+    EXPECT_EQ(object_recognition.reliability, RMW_QOS_POLICY_RELIABILITY_RELIABLE);
+    EXPECT_EQ(object_recognition.durability, RMW_QOS_POLICY_DURABILITY_VOLATILE);
+  }
+}

master/src/universe/common/autoware_component_interface_specs/test/test_planning.cpp

@@ -0,0 +1,46 @@
+// Copyright 2023 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/component_interface_specs/planning.hpp"
+#include "gtest/gtest.h"
+
+TEST(planning, interface)
+{
+  {
+    using autoware::component_interface_specs::planning::RouteState;
+    RouteState state;
+    size_t depth = 1;
+    EXPECT_EQ(state.depth, depth);
+    EXPECT_EQ(state.reliability, RMW_QOS_POLICY_RELIABILITY_RELIABLE);
+    EXPECT_EQ(state.durability, RMW_QOS_POLICY_DURABILITY_TRANSIENT_LOCAL);
+  }
+
+  {
+    using autoware::component_interface_specs::planning::LaneletRoute;
+    LaneletRoute route;
+    size_t depth = 1;
+    EXPECT_EQ(route.depth, depth);
+    EXPECT_EQ(route.reliability, RMW_QOS_POLICY_RELIABILITY_RELIABLE);
+    EXPECT_EQ(route.durability, RMW_QOS_POLICY_DURABILITY_TRANSIENT_LOCAL);
+  }
+
+  {
+    using autoware::component_interface_specs::planning::Trajectory;
+    Trajectory trajectory;
+    size_t depth = 1;
+    EXPECT_EQ(trajectory.depth, depth);
+    EXPECT_EQ(trajectory.reliability, RMW_QOS_POLICY_RELIABILITY_RELIABLE);
+    EXPECT_EQ(trajectory.durability, RMW_QOS_POLICY_DURABILITY_VOLATILE);
+  }
+}

master/src/universe/common/autoware_component_interface_specs/test/test_system.cpp

@@ -0,0 +1,37 @@
+// Copyright 2023 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/component_interface_specs/system.hpp"
+#include "gtest/gtest.h"
+
+TEST(system, interface)
+{
+  {
+    using autoware::component_interface_specs::system::MrmState;
+    MrmState state;
+    size_t depth = 1;
+    EXPECT_EQ(state.depth, depth);
+    EXPECT_EQ(state.reliability, RMW_QOS_POLICY_RELIABILITY_RELIABLE);
+    EXPECT_EQ(state.durability, RMW_QOS_POLICY_DURABILITY_VOLATILE);
+  }
+
+  {
+    using autoware::component_interface_specs::system::OperationModeState;
+    OperationModeState state;
+    size_t depth = 1;
+    EXPECT_EQ(state.depth, depth);
+    EXPECT_EQ(state.reliability, RMW_QOS_POLICY_RELIABILITY_RELIABLE);
+    EXPECT_EQ(state.durability, RMW_QOS_POLICY_DURABILITY_TRANSIENT_LOCAL);
+  }
+}

master/src/universe/common/autoware_component_interface_specs/test/test_vehicle.cpp

@@ -0,0 +1,64 @@
+// Copyright 2023 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/component_interface_specs/vehicle.hpp"
+#include "gtest/gtest.h"
+
+TEST(vehicle, interface)
+{
+  {
+    using autoware::component_interface_specs::vehicle::SteeringStatus;
+    SteeringStatus status;
+    size_t depth = 1;
+    EXPECT_EQ(status.depth, depth);
+    EXPECT_EQ(status.reliability, RMW_QOS_POLICY_RELIABILITY_RELIABLE);
+    EXPECT_EQ(status.durability, RMW_QOS_POLICY_DURABILITY_VOLATILE);
+  }
+
+  {
+    using autoware::component_interface_specs::vehicle::GearStatus;
+    GearStatus status;
+    size_t depth = 1;
+    EXPECT_EQ(status.depth, depth);
+    EXPECT_EQ(status.reliability, RMW_QOS_POLICY_RELIABILITY_RELIABLE);
+    EXPECT_EQ(status.durability, RMW_QOS_POLICY_DURABILITY_VOLATILE);
+  }
+
+  {
+    using autoware::component_interface_specs::vehicle::TurnIndicatorStatus;
+    TurnIndicatorStatus status;
+    size_t depth = 1;
+    EXPECT_EQ(status.depth, depth);
+    EXPECT_EQ(status.reliability, RMW_QOS_POLICY_RELIABILITY_RELIABLE);
+    EXPECT_EQ(status.durability, RMW_QOS_POLICY_DURABILITY_VOLATILE);
+  }
+
+  {
+    using autoware::component_interface_specs::vehicle::HazardLightStatus;
+    HazardLightStatus status;
+    size_t depth = 1;
+    EXPECT_EQ(status.depth, depth);
+    EXPECT_EQ(status.reliability, RMW_QOS_POLICY_RELIABILITY_RELIABLE);
+    EXPECT_EQ(status.durability, RMW_QOS_POLICY_DURABILITY_VOLATILE);
+  }
+
+  {
+    using autoware::component_interface_specs::vehicle::EnergyStatus;
+    EnergyStatus status;
+    size_t depth = 1;
+    EXPECT_EQ(status.depth, depth);
+    EXPECT_EQ(status.reliability, RMW_QOS_POLICY_RELIABILITY_RELIABLE);
+    EXPECT_EQ(status.durability, RMW_QOS_POLICY_DURABILITY_VOLATILE);
+  }
+}

master/src/universe/common/autoware_geography_utils/CMakeLists.txt

@@ -0,0 +1,37 @@
+cmake_minimum_required(VERSION 3.14)
+project(autoware_geography_utils)
+
+find_package(autoware_cmake REQUIRED)
+autoware_package()
+
+# GeographicLib
+find_package(PkgConfig)
+find_path(GeographicLib_INCLUDE_DIR GeographicLib/Config.h
+  PATH_SUFFIXES GeographicLib
+)
+set(GeographicLib_INCLUDE_DIRS ${GeographicLib_INCLUDE_DIR})
+find_library(GeographicLib_LIBRARIES NAMES Geographic)
+
+ament_auto_add_library(${PROJECT_NAME} SHARED
+  src/height.cpp
+  src/projection.cpp
+  src/lanelet2_projector.cpp
+)
+
+target_link_libraries(${PROJECT_NAME}
+  ${GeographicLib_LIBRARIES}
+)
+
+if(BUILD_TESTING)
+  find_package(ament_cmake_ros REQUIRED)
+
+  file(GLOB_RECURSE test_files test/*.cpp)
+
+  ament_add_ros_isolated_gtest(test_${PROJECT_NAME} ${test_files})
+
+  target_link_libraries(test_${PROJECT_NAME}
+  ${PROJECT_NAME}
+  )
+endif()
+
+ament_auto_package()

master/src/universe/common/autoware_geography_utils/README.md

@@ -0,0 +1,5 @@
+# geography_utils
+
+## Purpose
+
+This package contains geography-related functions used by other packages, so please refer to them as needed.

master/src/universe/common/autoware_geography_utils/include/autoware/geography_utils/height.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__GEOGRAPHY_UTILS__HEIGHT_HPP_
+#define AUTOWARE__GEOGRAPHY_UTILS__HEIGHT_HPP_
+
+#include <string>
+
+namespace autoware::geography_utils
+{
+
+typedef double (*HeightConversionFunction)(
+  const double height, const double latitude, const double longitude);
+double convert_wgs84_to_egm2008(const double height, const double latitude, const double longitude);
+double convert_egm2008_to_wgs84(const double height, const double latitude, const double longitude);
+double convert_height(
+  const double height, const double latitude, const double longitude,
+  const std::string & source_vertical_datum, const std::string & target_vertical_datum);
+
+}  // namespace autoware::geography_utils
+
+#endif  // AUTOWARE__GEOGRAPHY_UTILS__HEIGHT_HPP_

master/src/universe/common/autoware_geography_utils/include/autoware/geography_utils/lanelet2_projector.hpp

@@ -0,0 +1,32 @@
+// 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__GEOGRAPHY_UTILS__LANELET2_PROJECTOR_HPP_
+#define AUTOWARE__GEOGRAPHY_UTILS__LANELET2_PROJECTOR_HPP_
+
+#include <tier4_map_msgs/msg/map_projector_info.hpp>
+
+#include <lanelet2_io/Projection.h>
+
+#include <memory>
+
+namespace autoware::geography_utils
+{
+using MapProjectorInfo = tier4_map_msgs::msg::MapProjectorInfo;
+
+std::unique_ptr<lanelet::Projector> get_lanelet2_projector(const MapProjectorInfo & projector_info);
+
+}  // namespace autoware::geography_utils
+
+#endif  // AUTOWARE__GEOGRAPHY_UTILS__LANELET2_PROJECTOR_HPP_

master/src/universe/common/autoware_geography_utils/include/autoware/geography_utils/projection.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__GEOGRAPHY_UTILS__PROJECTION_HPP_
+#define AUTOWARE__GEOGRAPHY_UTILS__PROJECTION_HPP_
+
+#include <geographic_msgs/msg/geo_point.hpp>
+#include <geometry_msgs/msg/point.hpp>
+#include <tier4_map_msgs/msg/map_projector_info.hpp>
+
+namespace autoware::geography_utils
+{
+using MapProjectorInfo = tier4_map_msgs::msg::MapProjectorInfo;
+using GeoPoint = geographic_msgs::msg::GeoPoint;
+using LocalPoint = geometry_msgs::msg::Point;
+
+LocalPoint project_forward(const GeoPoint & geo_point, const MapProjectorInfo & projector_info);
+GeoPoint project_reverse(const LocalPoint & local_point, const MapProjectorInfo & projector_info);
+
+}  // namespace autoware::geography_utils
+
+#endif  // AUTOWARE__GEOGRAPHY_UTILS__PROJECTION_HPP_

master/src/universe/common/autoware_geography_utils/package.xml

@@ -0,0 +1,27 @@
+<?xml version="1.0"?>
+<?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
+<package format="3">
+  <name>autoware_geography_utils</name>
+  <version>0.1.0</version>
+  <description>The autoware_geography_utils package</description>
+  <maintainer email="koji.minoda@tier4.jp">Koji Minoda</maintainer>
+  <license>Apache License 2.0</license>
+
+  <buildtool_depend>ament_cmake_auto</buildtool_depend>
+  <buildtool_depend>autoware_cmake</buildtool_depend>
+
+  <depend>autoware_lanelet2_extension</depend>
+  <depend>geographic_msgs</depend>
+  <depend>geographiclib</depend>
+  <depend>geometry_msgs</depend>
+  <depend>lanelet2_io</depend>
+  <depend>tier4_map_msgs</depend>
+
+  <test_depend>ament_cmake_ros</test_depend>
+  <test_depend>ament_lint_auto</test_depend>
+  <test_depend>autoware_lint_common</test_depend>
+
+  <export>
+    <build_type>ament_cmake</build_type>
+  </export>
+</package>

master/src/universe/common/autoware_geography_utils/src/height.cpp

@@ -0,0 +1,63 @@
+// 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.
+
+#include <GeographicLib/Geoid.hpp>
+#include <autoware/geography_utils/height.hpp>
+
+#include <map>
+#include <stdexcept>
+#include <string>
+#include <utility>
+
+namespace autoware::geography_utils
+{
+
+double convert_wgs84_to_egm2008(const double height, const double latitude, const double longitude)
+{
+  GeographicLib::Geoid egm2008("egm2008-1");
+  // cSpell: ignore ELLIPSOIDTOGEOID
+  return egm2008.ConvertHeight(latitude, longitude, height, GeographicLib::Geoid::ELLIPSOIDTOGEOID);
+}
+
+double convert_egm2008_to_wgs84(const double height, const double latitude, const double longitude)
+{
+  GeographicLib::Geoid egm2008("egm2008-1");
+  // cSpell: ignore GEOIDTOELLIPSOID
+  return egm2008.ConvertHeight(latitude, longitude, height, GeographicLib::Geoid::GEOIDTOELLIPSOID);
+}
+
+double convert_height(
+  const double height, const double latitude, const double longitude,
+  const std::string & source_vertical_datum, const std::string & target_vertical_datum)
+{
+  if (source_vertical_datum == target_vertical_datum) {
+    return height;
+  }
+  std::map<std::pair<std::string, std::string>, HeightConversionFunction> conversion_map;
+  conversion_map[{"WGS84", "EGM2008"}] = convert_wgs84_to_egm2008;
+  conversion_map[{"EGM2008", "WGS84"}] = convert_egm2008_to_wgs84;
+
+  auto key = std::make_pair(source_vertical_datum, target_vertical_datum);
+  if (conversion_map.find(key) != conversion_map.end()) {
+    return conversion_map[key](height, latitude, longitude);
+  } else {
+    std::string error_message =
+      "Invalid conversion types: " + std::string(source_vertical_datum.c_str()) + " to " +
+      std::string(target_vertical_datum.c_str());
+
+    throw std::invalid_argument(error_message);
+  }
+}
+
+}  // namespace autoware::geography_utils

master/src/universe/common/autoware_geography_utils/src/lanelet2_projector.cpp

@@ -0,0 +1,54 @@
+// 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.
+
+#include <GeographicLib/Geoid.hpp>
+#include <autoware/geography_utils/lanelet2_projector.hpp>
+#include <autoware_lanelet2_extension/projection/mgrs_projector.hpp>
+#include <autoware_lanelet2_extension/projection/transverse_mercator_projector.hpp>
+
+#include <lanelet2_projection/UTM.h>
+
+namespace autoware::geography_utils
+{
+
+std::unique_ptr<lanelet::Projector> get_lanelet2_projector(const MapProjectorInfo & projector_info)
+{
+  if (projector_info.projector_type == MapProjectorInfo::LOCAL_CARTESIAN_UTM) {
+    lanelet::GPSPoint position{
+      projector_info.map_origin.latitude, projector_info.map_origin.longitude,
+      projector_info.map_origin.altitude};
+    lanelet::Origin origin{position};
+    lanelet::projection::UtmProjector projector{origin};
+    return std::make_unique<lanelet::projection::UtmProjector>(projector);
+
+  } else if (projector_info.projector_type == MapProjectorInfo::MGRS) {
+    lanelet::projection::MGRSProjector projector{};
+    projector.setMGRSCode(projector_info.mgrs_grid);
+    return std::make_unique<lanelet::projection::MGRSProjector>(projector);
+
+  } else if (projector_info.projector_type == MapProjectorInfo::TRANSVERSE_MERCATOR) {
+    lanelet::GPSPoint position{
+      projector_info.map_origin.latitude, projector_info.map_origin.longitude,
+      projector_info.map_origin.altitude};
+    lanelet::Origin origin{position};
+    lanelet::projection::TransverseMercatorProjector projector{origin};
+    return std::make_unique<lanelet::projection::TransverseMercatorProjector>(projector);
+  }
+  const std::string error_msg =
+    "Invalid map projector type: " + projector_info.projector_type +
+    ". Currently supported types: MGRS, LocalCartesianUTM, and TransverseMercator";
+  throw std::invalid_argument(error_msg);
+}
+
+}  // namespace autoware::geography_utils

master/src/universe/common/autoware_geography_utils/src/projection.cpp

@@ -0,0 +1,95 @@
+// 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.
+
+#include <GeographicLib/Geoid.hpp>
+#include <autoware/geography_utils/lanelet2_projector.hpp>
+#include <autoware/geography_utils/projection.hpp>
+#include <autoware_lanelet2_extension/projection/mgrs_projector.hpp>
+
+namespace autoware::geography_utils
+{
+
+Eigen::Vector3d to_basic_point_3d_pt(const LocalPoint src)
+{
+  Eigen::Vector3d dst;
+  dst.x() = src.x;
+  dst.y() = src.y;
+  dst.z() = src.z;
+  return dst;
+}
+
+LocalPoint project_forward(const GeoPoint & geo_point, const MapProjectorInfo & projector_info)
+{
+  std::unique_ptr<lanelet::Projector> projector = get_lanelet2_projector(projector_info);
+  lanelet::GPSPoint position{geo_point.latitude, geo_point.longitude, geo_point.altitude};
+
+  lanelet::BasicPoint3d projected_local_point;
+  if (projector_info.projector_type == MapProjectorInfo::MGRS) {
+    const int mgrs_precision = 9;  // set precision as 100 micro meter
+    const auto mgrs_projector = dynamic_cast<lanelet::projection::MGRSProjector *>(projector.get());
+
+    // project x and y using projector
+    // note that the altitude is ignored in MGRS projection conventionally
+    projected_local_point = mgrs_projector->forward(position, mgrs_precision);
+  } else {
+    // project x and y using projector
+    // note that the original projector such as UTM projector does not compensate for the altitude
+    // offset
+    projected_local_point = projector->forward(position);
+
+    // correct z based on the map origin
+    // note that the converted altitude in local point is in the same vertical datum as the geo
+    // point
+    projected_local_point.z() = geo_point.altitude - projector_info.map_origin.altitude;
+  }
+
+  LocalPoint local_point;
+  local_point.x = projected_local_point.x();
+  local_point.y = projected_local_point.y();
+  local_point.z = projected_local_point.z();
+
+  return local_point;
+}
+
+GeoPoint project_reverse(const LocalPoint & local_point, const MapProjectorInfo & projector_info)
+{
+  std::unique_ptr<lanelet::Projector> projector = get_lanelet2_projector(projector_info);
+
+  lanelet::GPSPoint projected_gps_point;
+  if (projector_info.projector_type == MapProjectorInfo::MGRS) {
+    const auto mgrs_projector = dynamic_cast<lanelet::projection::MGRSProjector *>(projector.get());
+    // project latitude and longitude using projector
+    // note that the z is ignored in MGRS projection conventionally
+    projected_gps_point =
+      mgrs_projector->reverse(to_basic_point_3d_pt(local_point), projector_info.mgrs_grid);
+  } else {
+    // project latitude and longitude using projector
+    // note that the original projector such as UTM projector does not compensate for the altitude
+    // offset
+    projected_gps_point = projector->reverse(to_basic_point_3d_pt(local_point));
+
+    // correct altitude based on the map origin
+    // note that the converted altitude in local point is in the same vertical datum as the geo
+    // point
+    projected_gps_point.ele = local_point.z + projector_info.map_origin.altitude;
+  }
+
+  GeoPoint geo_point;
+  geo_point.latitude = projected_gps_point.lat;
+  geo_point.longitude = projected_gps_point.lon;
+  geo_point.altitude = projected_gps_point.ele;
+  return geo_point;
+}
+
+}  // namespace autoware::geography_utils

master/src/universe/common/autoware_geography_utils/test/test_geography_utils.cpp

@@ -0,0 +1,26 @@
+// 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.
+
+#include "autoware/geography_utils/height.hpp"
+#include "autoware/geography_utils/lanelet2_projector.hpp"
+#include "autoware/geography_utils/projection.hpp"
+
+#include <gtest/gtest.h>
+
+int main(int argc, char * argv[])
+{
+  testing::InitGoogleTest(&argc, argv);
+  bool result = RUN_ALL_TESTS();
+  return result;
+}

master/src/universe/common/autoware_geography_utils/test/test_height.cpp

@@ -0,0 +1,86 @@
+// 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.
+
+#include <autoware/geography_utils/height.hpp>
+
+#include <gtest/gtest.h>
+
+#include <stdexcept>
+#include <string>
+
+// Test case to verify if same source and target datums return original height
+TEST(GeographyUtils, SameSourceTargetDatum)
+{
+  const double height = 10.0;
+  const double latitude = 35.0;
+  const double longitude = 139.0;
+  const std::string datum = "WGS84";
+
+  double converted_height =
+    autoware::geography_utils::convert_height(height, latitude, longitude, datum, datum);
+
+  EXPECT_DOUBLE_EQ(height, converted_height);
+}
+
+// Test case to verify valid source and target datums
+TEST(GeographyUtils, ValidSourceTargetDatum)
+{
+  // Calculated with
+  // https://www.unavco.org/software/geodetic-utilities/geoid-height-calculator/geoid-height-calculator.html
+  const double height = 10.0;
+  const double latitude = 35.0;
+  const double longitude = 139.0;
+  const double target_height = -30.18;
+
+  double converted_height =
+    autoware::geography_utils::convert_height(height, latitude, longitude, "WGS84", "EGM2008");
+
+  EXPECT_NEAR(target_height, converted_height, 0.1);
+}
+
+// Test case to verify invalid source and target datums
+TEST(GeographyUtils, InvalidSourceTargetDatum)
+{
+  const double height = 10.0;
+  const double latitude = 35.0;
+  const double longitude = 139.0;
+
+  EXPECT_THROW(
+    autoware::geography_utils::convert_height(height, latitude, longitude, "INVALID1", "INVALID2"),
+    std::invalid_argument);
+}
+
+// Test case to verify invalid source datums
+TEST(GeographyUtils, InvalidSourceDatum)
+{
+  const double height = 10.0;
+  const double latitude = 35.0;
+  const double longitude = 139.0;
+
+  EXPECT_THROW(
+    autoware::geography_utils::convert_height(height, latitude, longitude, "INVALID1", "WGS84"),
+    std::invalid_argument);
+}
+
+// Test case to verify invalid target datums
+TEST(GeographyUtils, InvalidTargetDatum)
+{
+  const double height = 10.0;
+  const double latitude = 35.0;
+  const double longitude = 139.0;
+
+  EXPECT_THROW(
+    autoware::geography_utils::convert_height(height, latitude, longitude, "WGS84", "INVALID2"),
+    std::invalid_argument);
+}

master/src/universe/common/autoware_geography_utils/test/test_projection.cpp

@@ -0,0 +1,161 @@
+// 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.
+
+#include <autoware/geography_utils/projection.hpp>
+
+#include <gtest/gtest.h>
+
+#include <stdexcept>
+#include <string>
+
+TEST(GeographyUtilsProjection, ProjectForwardToMGRS)
+{
+  // source point
+  geographic_msgs::msg::GeoPoint geo_point;
+  geo_point.latitude = 35.62426;
+  geo_point.longitude = 139.74252;
+  geo_point.altitude = 10.0;
+
+  // target point
+  geometry_msgs::msg::Point local_point;
+  local_point.x = 86128.0;
+  local_point.y = 43002.0;
+  local_point.z = 10.0;
+
+  // projector info
+  tier4_map_msgs::msg::MapProjectorInfo projector_info;
+  projector_info.projector_type = tier4_map_msgs::msg::MapProjectorInfo::MGRS;
+  projector_info.mgrs_grid = "54SUE";
+  projector_info.vertical_datum = tier4_map_msgs::msg::MapProjectorInfo::WGS84;
+
+  // conversion
+  const geometry_msgs::msg::Point converted_point =
+    autoware::geography_utils::project_forward(geo_point, projector_info);
+
+  EXPECT_NEAR(converted_point.x, local_point.x, 1.0);
+  EXPECT_NEAR(converted_point.y, local_point.y, 1.0);
+  EXPECT_NEAR(converted_point.z, local_point.z, 1.0);
+}
+
+TEST(GeographyUtilsProjection, ProjectReverseFromMGRS)
+{
+  // source point
+  geometry_msgs::msg::Point local_point;
+  local_point.x = 86128.0;
+  local_point.y = 43002.0;
+  local_point.z = 10.0;
+
+  // target point
+  geographic_msgs::msg::GeoPoint geo_point;
+  geo_point.latitude = 35.62426;
+  geo_point.longitude = 139.74252;
+  geo_point.altitude = 10.0;
+
+  // projector info
+  tier4_map_msgs::msg::MapProjectorInfo projector_info;
+  projector_info.projector_type = tier4_map_msgs::msg::MapProjectorInfo::MGRS;
+  projector_info.mgrs_grid = "54SUE";
+  projector_info.vertical_datum = tier4_map_msgs::msg::MapProjectorInfo::WGS84;
+
+  // conversion
+  const geographic_msgs::msg::GeoPoint converted_point =
+    autoware::geography_utils::project_reverse(local_point, projector_info);
+
+  EXPECT_NEAR(converted_point.latitude, geo_point.latitude, 0.0001);
+  EXPECT_NEAR(converted_point.longitude, geo_point.longitude, 0.0001);
+  EXPECT_NEAR(converted_point.altitude, geo_point.altitude, 0.0001);
+}
+
+TEST(GeographyUtilsProjection, ProjectForwardAndReverseMGRS)
+{
+  // source point
+  geographic_msgs::msg::GeoPoint geo_point;
+  geo_point.latitude = 35.62426;
+  geo_point.longitude = 139.74252;
+  geo_point.altitude = 10.0;
+
+  // projector info
+  tier4_map_msgs::msg::MapProjectorInfo projector_info;
+  projector_info.projector_type = tier4_map_msgs::msg::MapProjectorInfo::MGRS;
+  projector_info.mgrs_grid = "54SUE";
+  projector_info.vertical_datum = tier4_map_msgs::msg::MapProjectorInfo::WGS84;
+
+  // conversion
+  const geometry_msgs::msg::Point converted_local_point =
+    autoware::geography_utils::project_forward(geo_point, projector_info);
+  const geographic_msgs::msg::GeoPoint converted_geo_point =
+    autoware::geography_utils::project_reverse(converted_local_point, projector_info);
+
+  EXPECT_NEAR(converted_geo_point.latitude, geo_point.latitude, 0.0001);
+  EXPECT_NEAR(converted_geo_point.longitude, geo_point.longitude, 0.0001);
+  EXPECT_NEAR(converted_geo_point.altitude, geo_point.altitude, 0.0001);
+}
+
+TEST(GeographyUtilsProjection, ProjectForwardToLocalCartesianUTMOrigin)
+{
+  // source point
+  geographic_msgs::msg::GeoPoint geo_point;
+  geo_point.latitude = 35.62406;
+  geo_point.longitude = 139.74252;
+  geo_point.altitude = 10.0;
+
+  // target point
+  geometry_msgs::msg::Point local_point;
+  local_point.x = 0.0;
+  local_point.y = -22.18;
+  local_point.z = 20.0;
+
+  // projector info
+  tier4_map_msgs::msg::MapProjectorInfo projector_info;
+  projector_info.projector_type = tier4_map_msgs::msg::MapProjectorInfo::LOCAL_CARTESIAN_UTM;
+  projector_info.vertical_datum = tier4_map_msgs::msg::MapProjectorInfo::WGS84;
+  projector_info.map_origin.latitude = 35.62426;
+  projector_info.map_origin.longitude = 139.74252;
+  projector_info.map_origin.altitude = -10.0;
+
+  // conversion
+  const geometry_msgs::msg::Point converted_point =
+    autoware::geography_utils::project_forward(geo_point, projector_info);
+
+  EXPECT_NEAR(converted_point.x, local_point.x, 1.0);
+  EXPECT_NEAR(converted_point.y, local_point.y, 1.0);
+  EXPECT_NEAR(converted_point.z, local_point.z, 1.0);
+}
+
+TEST(GeographyUtilsProjection, ProjectForwardAndReverseLocalCartesianUTMOrigin)
+{
+  // source point
+  geographic_msgs::msg::GeoPoint geo_point;
+  geo_point.latitude = 35.62426;
+  geo_point.longitude = 139.74252;
+  geo_point.altitude = 10.0;
+
+  // projector info
+  tier4_map_msgs::msg::MapProjectorInfo projector_info;
+  projector_info.projector_type = tier4_map_msgs::msg::MapProjectorInfo::LOCAL_CARTESIAN_UTM;
+  projector_info.vertical_datum = tier4_map_msgs::msg::MapProjectorInfo::WGS84;
+  projector_info.map_origin.latitude = 35.0;
+  projector_info.map_origin.longitude = 139.0;
+  projector_info.map_origin.altitude = 0.0;
+
+  // conversion
+  const geometry_msgs::msg::Point converted_local_point =
+    autoware::geography_utils::project_forward(geo_point, projector_info);
+  const geographic_msgs::msg::GeoPoint converted_geo_point =
+    autoware::geography_utils::project_reverse(converted_local_point, projector_info);
+
+  EXPECT_NEAR(converted_geo_point.latitude, geo_point.latitude, 0.0001);
+  EXPECT_NEAR(converted_geo_point.longitude, geo_point.longitude, 0.0001);
+  EXPECT_NEAR(converted_geo_point.altitude, geo_point.altitude, 0.0001);
+}

master/src/universe/common/autoware_grid_map_utils/CMakeLists.txt

@@ -0,0 +1,42 @@
+cmake_minimum_required(VERSION 3.5)
+project(autoware_grid_map_utils)
+
+if(NOT CMAKE_CXX_STANDARD)
+  set(CMAKE_CXX_STANDARD 17)
+  set(CMAKE_CXX_STANDARD_REQUIRED ON)
+  set(CMAKE_CXX_EXTENSIONS OFF)
+endif()
+if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang")
+  add_compile_options(-Wall -Wextra -Wpedantic)
+endif()
+
+find_package(autoware_cmake REQUIRED)
+autoware_package()
+find_package(ament_cmake REQUIRED)
+
+ament_auto_add_library(${PROJECT_NAME} SHARED
+  DIRECTORY src
+)
+
+target_link_libraries(${PROJECT_NAME})
+
+if(BUILD_TESTING)
+  find_package(ament_lint_auto REQUIRED)
+  ament_lint_auto_find_test_dependencies()
+
+  ament_add_gtest(test_${PROJECT_NAME}
+    test/test_polygon_iterator.cpp
+  )
+  target_link_libraries(test_${PROJECT_NAME}
+    ${PROJECT_NAME}
+  )
+
+  find_package(OpenCV REQUIRED)
+  add_executable(benchmark test/benchmark.cpp)
+  target_link_libraries(benchmark
+    ${PROJECT_NAME}
+    ${OpenCV_LIBS}
+  )
+endif()
+
+ament_auto_package()

master/src/universe/common/autoware_grid_map_utils/README.md

@@ -0,0 +1,51 @@
+# Grid Map Utils
+
+## Overview
+
+This packages contains a re-implementation of the `grid_map::PolygonIterator` used to iterate over
+all cells of a grid map contained inside some polygon.
+
+## Algorithm
+
+This implementation uses the [scan line algorithm](https://en.wikipedia.org/wiki/Scanline_rendering),
+a common algorithm used to draw polygons on a rasterized image.
+The main idea of the algorithm adapted to a grid map is as follow:
+
+- calculate intersections between rows of the grid map and the edges of the polygon edges;
+- calculate for each row the column between each pair of intersections;
+- the resulting `(row, column)` indexes are inside of the polygon.
+
+More details on the scan line algorithm can be found in the References.
+
+## API
+
+The `autoware::grid_map_utils::PolygonIterator` follows the same API as the original [`grid_map::PolygonIterator`](https://docs.ros.org/en/kinetic/api/grid_map_core/html/classgrid__map_1_1PolygonIterator.html).
+
+## Assumptions
+
+The behavior of the `autoware::grid_map_utils::PolygonIterator` is only guaranteed to match the `grid_map::PolygonIterator` if edges of the polygon do not _exactly_ cross any cell center.
+In such a case, whether the crossed cell is considered inside or outside of the polygon can vary due to floating precision error.
+
+## Performances
+
+Benchmarking code is implemented in `test/benchmarking.cpp` and is also used to validate that the `autoware::grid_map_utils::PolygonIterator` behaves exactly like the `grid_map::PolygonIterator`.
+
+The following figure shows a comparison of the runtime between the implementation of this package (`autoware_grid_map_utils`) and the original implementation (`grid_map`).
+The time measured includes the construction of the iterator and the iteration over all indexes and is shown using a logarithmic scale.
+Results were obtained varying the side size of a square grid map with `100 <= n <= 1000` (size=`n` means a grid of `n x n` cells),
+random polygons with a number of vertices `3 <= m <= 100` and with each parameter `(n,m)` repeated 10 times.
+
+![Runtime comparison](media/runtime_comparison.png)
+
+## Future improvements
+
+There exists variations of the scan line algorithm for multiple polygons.
+These can be implemented if we want to iterate over the cells contained in at least one of multiple polygons.
+
+The current implementation imitate the behavior of the original `grid_map::PolygonIterator` where a cell is selected if its center position is inside the polygon.
+This behavior could be changed for example to only return all cells overlapped by the polygon.
+
+## References
+
+- <https://en.wikipedia.org/wiki/Scanline_rendering>
+- <https://web.cs.ucdavis.edu/~ma/ECS175_S00/Notes/0411_b.pdf>

master/src/universe/common/autoware_grid_map_utils/include/autoware/grid_map_utils/polygon_iterator.hpp

@@ -0,0 +1,129 @@
+// Copyright 2022 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__GRID_MAP_UTILS__POLYGON_ITERATOR_HPP_
+#define AUTOWARE__GRID_MAP_UTILS__POLYGON_ITERATOR_HPP_
+
+#include "grid_map_core/TypeDefs.hpp"
+
+#include <grid_map_core/GridMap.hpp>
+#include <grid_map_core/GridMapMath.hpp>
+#include <grid_map_core/Polygon.hpp>
+
+#include <utility>
+#include <vector>
+
+namespace autoware::grid_map_utils
+{
+
+/// @brief Representation of a polygon edge made of 2 vertices
+struct Edge
+{
+  grid_map::Position first;
+  grid_map::Position second;
+
+  Edge(grid_map::Position f, grid_map::Position s) : first(std::move(f)), second(std::move(s)) {}
+
+  /// @brief Sorting operator resulting in edges sorted from highest to lowest x values
+  bool operator<(const Edge & e)
+  {
+    return first.x() > e.first.x() || (first.x() == e.first.x() && second.x() > e.second.x());
+  }
+};
+
+/** @brief A polygon iterator for grid_map::GridMap based on the scan line algorithm.
+    @details This iterator allows to iterate over all cells whose center is inside a polygon. \
+             This reproduces the behavior of the original grid_map::PolygonIterator which uses\
+             a "point in polygon" check for each cell of the gridmap, making it very expensive\
+             to run on large maps. In comparison, the scan line algorithm implemented here is \
+             much more scalable.
+*/
+class PolygonIterator
+{
+public:
+  /// @brief Constructor.
+  /// @details Calculate the indexes of the gridmap that are inside the polygon using the scan line
+  /// algorithm.
+  /// @param grid_map the grid map to iterate on.
+  /// @param polygon the polygonal area to iterate on.
+  PolygonIterator(const grid_map::GridMap & grid_map, const grid_map::Polygon & polygon);
+  /// @brief Compare to another iterator.
+  /// @param other other iterator.
+  /// @return whether the current iterator points to a different address than the other one.
+  bool operator!=(const PolygonIterator & other) const;
+  /// @brief Dereference the iterator with const.
+  /// @return the value to which the iterator is pointing.
+  const grid_map::Index & operator*() const;
+  /// @brief Increase the iterator to the next element.
+  /// @return a reference to the updated iterator.
+  PolygonIterator & operator++();
+  /// @brief Indicates if iterator is past end.
+  /// @return true if iterator is out of scope, false if end has not been reached.
+  [[nodiscard]] bool isPastEnd() const;
+
+private:
+  /** @brief Calculate sorted edges of the given polygon.
+      @details Vertices in an edge are ordered from higher to lower x.
+              Edges are sorted in reverse lexicographical order of x.
+      @param polygon Polygon for which edges are calculated.
+      @return Sorted edges of the polygon.
+  */
+  static std::vector<Edge> calculateSortedEdges(const grid_map::Polygon & polygon);
+
+  /// @brief Calculates intersections between lines (i.e., center of rows) and the polygon edges.
+  /// @param edges Edges of the polygon.
+  /// @param from_to_row ranges of lines to use for intersection.
+  /// @param origin Position of the top-left cell in the grid map.
+  /// @param grid_map grid map.
+  /// @return for each row the vector of y values with an intersection.
+  static std::vector<std::vector<double>> calculateIntersectionsPerLine(
+    const std::vector<Edge> & edges, const std::pair<int, int> from_to_row,
+    const grid_map::Position & origin, const grid_map::GridMap & grid_map);
+
+  /// @brief Calculates the range of rows covering the given edges.
+  /// @details The rows are calculated without any shift that might exist in the grid map.
+  /// @param edges Edges of the polygon.
+  /// @param origin Position of the top-left cell in the grid map.
+  /// @param grid_map grid map.
+  /// @return the range of rows as a pair {first row, last row}.
+  static std::pair<int, int> calculateRowRange(
+    const std::vector<Edge> & edges, const grid_map::Position & origin,
+    const grid_map::GridMap & grid_map);
+
+  // Helper functions
+  /// @brief Increment the current_line_ to the line with intersections
+  void goToNextLine();
+  /// @brief Calculate the initial current_col_ and the current_to_col_ for the current intersection
+  void calculateColumnIndexes();
+  /// @brief Calculate the current_index_ from the current_line_ and current_col_
+  void calculateIndex();
+
+  /// Gridmap info
+  int row_of_first_line_;
+  grid_map::Index map_start_idx_;
+  grid_map::Size map_size_;
+  double map_resolution_;
+  double map_origin_y_;
+  /// Intersections between scan lines and the polygon
+  std::vector<std::vector<double>> intersections_per_line_;
+  std::vector<double>::const_iterator intersection_iter_;
+  /// current indexes
+  grid_map::Index current_index_;
+  size_t current_line_;
+  int current_col_;
+  int current_to_col_;
+};
+}  // namespace autoware::grid_map_utils
+
+#endif  // AUTOWARE__GRID_MAP_UTILS__POLYGON_ITERATOR_HPP_

master/src/universe/common/autoware_grid_map_utils/package.xml

@@ -0,0 +1,25 @@
+<?xml version="1.0"?>
+<?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
+<package format="3">
+  <name>autoware_grid_map_utils</name>
+  <version>0.0.0</version>
+  <description>Utilities for the grid_map library</description>
+  <maintainer email="maxime.clement@tier4.jp">Maxime CLEMENT</maintainer>
+  <license>Apache License 2.0</license>
+
+  <buildtool_depend>autoware_cmake</buildtool_depend>
+  <buildtool_depend>eigen3_cmake_module</buildtool_depend>
+
+  <depend>autoware_universe_utils</depend>
+  <depend>grid_map_core</depend>
+  <depend>grid_map_cv</depend>
+  <depend>libopencv-dev</depend>
+
+  <test_depend>ament_cmake_gtest</test_depend>
+  <test_depend>ament_lint_auto</test_depend>
+  <test_depend>autoware_lint_common</test_depend>
+
+  <export>
+    <build_type>ament_cmake</build_type>
+  </export>
+</package>

master/src/universe/common/autoware_grid_map_utils/src/polygon_iterator.cpp

@@ -0,0 +1,217 @@
+// Copyright 2022 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.
+
+#include "autoware/grid_map_utils/polygon_iterator.hpp"
+
+#include "grid_map_core/GridMap.hpp"
+#include "grid_map_core/Polygon.hpp"
+#include "grid_map_core/TypeDefs.hpp"
+
+#include <algorithm>
+#include <functional>
+#include <utility>
+
+namespace autoware::grid_map_utils
+{
+
+std::vector<Edge> PolygonIterator::calculateSortedEdges(const grid_map::Polygon & polygon)
+{
+  std::vector<Edge> edges;
+  edges.reserve(polygon.nVertices());
+  const auto & vertices = polygon.getVertices();
+  for (auto vertex = vertices.cbegin(); std::next(vertex) != vertices.cend(); ++vertex) {
+    // order pair by decreasing x and ignore horizontal edges (when x is equal)
+    if (vertex->x() > std::next(vertex)->x())
+      edges.emplace_back(*vertex, *std::next(vertex));
+    else if (vertex->x() < std::next(vertex)->x())
+      edges.emplace_back(*std::next(vertex), *vertex);
+  }
+  std::sort(edges.begin(), edges.end());
+  edges.shrink_to_fit();
+  return edges;
+}
+
+std::vector<std::vector<double>> PolygonIterator::calculateIntersectionsPerLine(
+  const std::vector<Edge> & edges, const std::pair<int, int> from_to_row,
+  const grid_map::Position & origin, const grid_map::GridMap & grid_map)
+{
+  const auto from_row = from_to_row.first;
+  const auto to_row = from_to_row.second;
+  // calculate for each line the y value intersecting with the polygon in decreasing order
+  std::vector<std::vector<double>> y_intersections_per_line;
+  y_intersections_per_line.reserve(to_row - from_row + 1);
+  for (auto row = from_row; row <= to_row; ++row) {
+    std::vector<double> y_intersections;
+    const auto line_x = origin.x() - grid_map.getResolution() * row;
+    for (const auto & edge : edges) {
+      // special case when exactly touching a vertex: only count edge for its lowest x
+      // up-down edge (\/) case: count the vertex twice
+      // down-down edge case: count the vertex only once
+      if (edge.second.x() == line_x) {
+        y_intersections.push_back(edge.second.y());
+      } else if (edge.first.x() >= line_x && edge.second.x() < line_x) {
+        const auto diff = edge.first - edge.second;
+        const auto y = edge.second.y() + (line_x - edge.second.x()) * diff.y() / diff.x();
+        y_intersections.push_back(y);
+      } else if (edge.first.x() < line_x) {  // edge below the line
+        break;
+      }
+    }
+    std::sort(y_intersections.begin(), y_intersections.end(), std::greater());
+    // remove pairs outside of map
+    auto iter = y_intersections.cbegin();
+    while (iter != y_intersections.cend() && std::next(iter) != y_intersections.cend() &&
+           *iter >= origin.y() && *std::next(iter) >= origin.y()) {
+      iter = y_intersections.erase(iter);
+      iter = y_intersections.erase(iter);
+    }
+    iter = std::lower_bound(
+      y_intersections.cbegin(), y_intersections.cend(),
+      origin.y() - (grid_map.getSize()(1) - 1) * grid_map.getResolution(), std::greater());
+    while (iter != y_intersections.cend() && std::next(iter) != y_intersections.cend()) {
+      iter = y_intersections.erase(iter);
+      iter = y_intersections.erase(iter);
+    }
+    y_intersections_per_line.push_back(y_intersections);
+  }
+  return y_intersections_per_line;
+}
+
+std::pair<int, int> PolygonIterator::calculateRowRange(
+  const std::vector<Edge> & edges, const grid_map::Position & origin,
+  const grid_map::GridMap & grid_map)
+{
+  const auto min_vertex_x =
+    std::min_element(edges.cbegin(), edges.cend(), [](const Edge & e1, const Edge & e2) {
+      return e1.second.x() < e2.second.x();
+    })->second.x();
+  const auto max_vertex_x = edges.front().first.x();
+
+  const auto dist_min_to_origin = origin.x() - min_vertex_x + grid_map.getResolution();
+  const auto dist_max_to_origin = origin.x() - max_vertex_x + grid_map.getResolution();
+  const auto min_row = std::clamp(
+    static_cast<int>(dist_max_to_origin / grid_map.getResolution()), 0, grid_map.getSize()(0) - 1);
+  const auto max_row = std::clamp(
+    static_cast<int>(dist_min_to_origin / grid_map.getResolution()), 0, grid_map.getSize()(0) - 1);
+
+  return {min_row, max_row};
+}
+
+PolygonIterator::PolygonIterator(
+  const grid_map::GridMap & grid_map, const grid_map::Polygon & polygon)
+{
+  auto poly = polygon;
+  if (poly.nVertices() < 3) return;
+  // repeat the first vertex to get the last edge [last vertex, first vertex]
+  if (poly.getVertex(0) != poly.getVertex(poly.nVertices() - 1)) poly.addVertex(poly.getVertex(0));
+
+  map_start_idx_ = grid_map.getStartIndex();
+  map_resolution_ = grid_map.getResolution();
+  map_size_ = grid_map.getSize();
+  const auto origin = [&]() {
+    grid_map::Position origin;
+    grid_map.getPosition(map_start_idx_, origin);
+    return origin;
+  }();
+  map_origin_y_ = origin.y();
+
+  // We make line scan left -> right / up -> down *in the index frame* (idx[0,0] is pos[up, left]).
+  // In the position frame, this corresponds to high -> low Y values and high -> low X values.
+  const std::vector<Edge> edges = calculateSortedEdges(poly);
+  if (edges.empty()) return;
+  const auto from_to_row = calculateRowRange(edges, origin, grid_map);
+  intersections_per_line_ = calculateIntersectionsPerLine(edges, from_to_row, origin, grid_map);
+
+  row_of_first_line_ = from_to_row.first;
+  current_col_ = 0;
+  current_to_col_ = -1;
+  current_line_ = -1;  // goToNextLine() increments the line so assign -1 to start from 0
+
+  // Initialize iterator to the first (row,column) inside the Polygon
+  if (!intersections_per_line_.empty()) {
+    goToNextLine();
+    if (!isPastEnd()) {
+      calculateColumnIndexes();
+      calculateIndex();
+    }
+  }
+}
+
+bool PolygonIterator::operator!=(const PolygonIterator & other) const
+{
+  return current_line_ != other.current_line_ || current_col_ != other.current_col_;
+}
+
+const grid_map::Index & PolygonIterator::operator*() const
+{
+  return current_index_;
+}
+
+void PolygonIterator::goToNextLine()
+{
+  ++current_line_;
+  while (current_line_ < intersections_per_line_.size() &&
+         intersections_per_line_[current_line_].size() < 2)
+    ++current_line_;
+  if (!isPastEnd()) intersection_iter_ = intersections_per_line_[current_line_].cbegin();
+}
+
+void PolygonIterator::calculateColumnIndexes()
+{
+  const auto dist_from_origin = map_origin_y_ - *intersection_iter_ + map_resolution_;
+  current_col_ =
+    std::clamp(static_cast<int>(dist_from_origin / map_resolution_), 0, map_size_(1) - 1);
+  ++intersection_iter_;
+  const auto dist_to_origin = map_origin_y_ - *intersection_iter_;
+  current_to_col_ =
+    std::clamp(static_cast<int>(dist_to_origin / map_resolution_), 0, map_size_(1) - 1);
+  // Case where intersections do not encompass the center of a cell: iterate again
+  if (current_to_col_ < current_col_) {
+    operator++();
+  }
+}
+
+void PolygonIterator::calculateIndex()
+{
+  current_index_(0) = map_start_idx_(0) + row_of_first_line_ + static_cast<int>(current_line_);
+  grid_map::wrapIndexToRange(current_index_(0), map_size_(0));
+  current_index_(1) = map_start_idx_(1) + current_col_;
+  grid_map::wrapIndexToRange(current_index_(1), map_size_(1));
+}
+
+PolygonIterator & PolygonIterator::operator++()
+{
+  ++current_col_;
+  if (current_col_ > current_to_col_) {
+    ++intersection_iter_;
+    if (
+      intersection_iter_ == intersections_per_line_[current_line_].cend() ||
+      std::next(intersection_iter_) == intersections_per_line_[current_line_].cend()) {
+      goToNextLine();
+    }
+    if (!isPastEnd()) {
+      calculateColumnIndexes();
+    }
+  }
+  if (!isPastEnd()) {
+    calculateIndex();
+  }
+  return *this;
+}
+
+[[nodiscard]] bool PolygonIterator::isPastEnd() const
+{
+  return current_line_ >= intersections_per_line_.size();
+}
+}  // namespace autoware::grid_map_utils

master/src/universe/common/autoware_grid_map_utils/test/benchmark.cpp

@@ -0,0 +1,182 @@
+// Copyright 2022 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.
+
+#include "autoware/grid_map_utils/polygon_iterator.hpp"
+#include "grid_map_core/TypeDefs.hpp"
+#include "grid_map_cv/GridMapCvConverter.hpp"
+#include "grid_map_cv/GridMapCvProcessing.hpp"
+
+#include <autoware/universe_utils/system/stop_watch.hpp>
+#include <grid_map_core/iterators/PolygonIterator.hpp>
+#include <grid_map_cv/grid_map_cv.hpp>
+#include <opencv2/core.hpp>
+#include <opencv2/highgui.hpp>
+#include <opencv2/imgproc.hpp>
+#include <opencv2/opencv.hpp>
+
+#include <algorithm>
+#include <fstream>
+#include <iostream>
+#include <random>
+#include <string>
+#include <vector>
+
+int main(int argc, char * argv[])
+{
+  bool visualize = false;
+  for (int i = 1; i < argc; ++i) {
+    const auto arg = std::string(argv[i]);
+    if (arg == "-v" || arg == "--visualize") {
+      visualize = true;
+    }
+  }
+  std::ofstream result_file;
+  result_file.open("benchmark_results.csv");
+  result_file
+    << "#Size PolygonVertices PolygonIndexes grid_map_utils_constructor grid_map_utils_iteration "
+       "grid_map_constructor grid_map_iteration\n";
+  autoware::universe_utils::StopWatch<std::chrono::milliseconds> stopwatch;
+
+  constexpr auto nb_iterations = 10;
+  constexpr auto polygon_side_vertices =
+    25;  // number of vertex per side of the square base_polygon
+  const auto grid_map_length = grid_map::Length(10.0, 10.0);
+  std::random_device r;
+  std::default_random_engine engine(0);
+  // TODO(Maxime CLEMENT): moving breaks the polygon visualization
+  std::uniform_real_distribution move_dist(-2.0, 2.0);
+  std::uniform_real_distribution poly_x_offset(-2.0, 2.0);
+  std::uniform_real_distribution poly_y_offset(-2.0, 2.0);
+
+  grid_map::Polygon base_polygon;
+  const auto top_left = grid_map::Position(-grid_map_length.x() / 2, grid_map_length.y() / 2);
+  const auto top_right = grid_map::Position(grid_map_length.x() / 2, grid_map_length.y() / 2);
+  const auto bot_right = grid_map::Position(grid_map_length.x() / 2, -grid_map_length.y() / 2);
+  const auto bot_left = grid_map::Position(-grid_map_length.x() / 2, -grid_map_length.y() / 2);
+  const auto top_vector = top_right - top_left;
+  for (double i = 0; i < polygon_side_vertices; ++i) {
+    const auto factor = i / polygon_side_vertices;
+    base_polygon.addVertex(top_left + factor * top_vector);
+  }
+  const auto right_vector = bot_right - top_right;
+  for (double i = 0; i < polygon_side_vertices; ++i) {
+    const auto factor = i / polygon_side_vertices;
+    base_polygon.addVertex(top_right + factor * right_vector);
+  }
+  const auto bot_vector = bot_left - bot_right;
+  for (double i = 0; i < polygon_side_vertices; ++i) {
+    const auto factor = i / polygon_side_vertices;
+    base_polygon.addVertex(bot_right + factor * bot_vector);
+  }
+  const auto left_vector = top_left - bot_left;
+  for (double i = 0; i < polygon_side_vertices; ++i) {
+    const auto factor = i / polygon_side_vertices;
+    base_polygon.addVertex(bot_left + factor * left_vector);
+  }
+
+  for (auto grid_map_size = 100; grid_map_size <= 1000; grid_map_size += 100) {
+    std::cout << "Map of size " << grid_map_size << " by " << grid_map_size << std::endl;
+    const auto resolution = grid_map_length(0) / grid_map_size;
+    grid_map::GridMap map({"layer"});
+    map.setGeometry(grid_map_length, resolution);
+    for (auto vertices = 3ul; vertices <= base_polygon.nVertices(); ++vertices) {
+      auto polygon_indexes = 0.0;
+      std::cout << "\tPolygon with " << vertices << " vertices" << std::endl;
+
+      double grid_map_utils_constructor_duration{};
+      double grid_map_constructor_duration{};
+      double grid_map_utils_iteration_duration{};
+      double grid_map_iteration_duration{};
+
+      for (auto iteration = 0; iteration < nb_iterations; ++iteration) {
+        map.setGeometry(grid_map::Length(10.0, 10.0), resolution, grid_map::Position(0.0, 0.0));
+        const auto move = grid_map::Position(move_dist(engine), move_dist(engine));
+        map.move(move);
+
+        // generate random sub-polygon of base_polygon with some noise
+        grid_map::Polygon polygon;
+        std::vector<size_t> indexes(base_polygon.nVertices());
+        for (size_t i = 0; i <= base_polygon.nVertices(); ++i) indexes[i] = i;
+        std::shuffle(indexes.begin(), indexes.end(), std::default_random_engine(iteration));
+        indexes.resize(vertices);
+        std::sort(indexes.begin(), indexes.end());
+        for (const auto idx : indexes) {
+          const auto offset = grid_map::Position(poly_x_offset(engine), poly_y_offset(engine));
+          polygon.addVertex(base_polygon.getVertex(idx) + offset);
+        }
+        stopwatch.tic("gmu_ctor");
+        autoware::grid_map_utils::PolygonIterator grid_map_utils_iterator(map, polygon);
+        grid_map_utils_constructor_duration += stopwatch.toc("gmu_ctor");
+        stopwatch.tic("gm_ctor");
+        grid_map::PolygonIterator grid_map_iterator(map, polygon);
+        grid_map_constructor_duration += stopwatch.toc("gm_ctor");
+        bool diff = false;
+        while (!grid_map_utils_iterator.isPastEnd() && !grid_map_iterator.isPastEnd()) {
+          stopwatch.tic("gmu_iter");
+          const auto gmu_idx = *grid_map_utils_iterator;
+          ++grid_map_utils_iterator;
+          grid_map_utils_iteration_duration += stopwatch.toc("gmu_iter");
+          stopwatch.tic("gm_iter");
+          const auto gm_idx = *grid_map_iterator;
+          ++grid_map_iterator;
+          grid_map_iteration_duration += stopwatch.toc("gm_iter");
+          ++polygon_indexes;
+
+          if (gmu_idx.x() != gm_idx.x() || gmu_idx.y() != gm_idx.y()) {
+            diff = true;
+          }
+        }
+        if (grid_map_iterator.isPastEnd() != grid_map_utils_iterator.isPastEnd()) {
+          diff = true;
+        }
+        if (diff || visualize) {
+          // Prepare images of the cells selected by the two PolygonIterators
+          auto gridmap = map;
+          for (autoware::grid_map_utils::PolygonIterator iterator(map, polygon);
+               !iterator.isPastEnd(); ++iterator)
+            map.at("layer", *iterator) = 100;
+          for (grid_map::PolygonIterator iterator(gridmap, polygon); !iterator.isPastEnd();
+               ++iterator)
+            gridmap.at("layer", *iterator) = 100;
+
+          cv::Mat img;
+          cv::Mat custom_img;
+          cv::Mat gm_img;
+          cv::Mat diff_img;
+
+          grid_map::GridMapCvConverter::toImage<unsigned char, 1>(
+            map, "layer", CV_8UC1, 0.0, 100, img);
+          cv::resize(img, custom_img, cv::Size(500, 500), cv::INTER_LINEAR);
+          grid_map::GridMapCvConverter::toImage<unsigned char, 1>(
+            gridmap, "layer", CV_8UC1, 0.0, 100, img);
+          cv::resize(img, gm_img, cv::Size(500, 500), cv::INTER_LINEAR);
+          cv::compare(custom_img, gm_img, diff_img, cv::CMP_EQ);
+
+          cv::imshow("custom", custom_img);
+          cv::imshow("grid_map", gm_img);
+          cv::imshow("diff", diff_img);
+          cv::waitKey(0);
+          cv::destroyAllWindows();
+        }
+      }
+      // print results to file
+      result_file << grid_map_size << " " << vertices << " " << polygon_indexes / nb_iterations
+                  << " " << grid_map_utils_constructor_duration / nb_iterations << " "
+                  << grid_map_utils_iteration_duration / nb_iterations << " "
+                  << grid_map_constructor_duration / nb_iterations << " "
+                  << grid_map_iteration_duration / nb_iterations << "\n";
+    }
+  }
+  result_file.close();
+}

master/src/universe/common/autoware_grid_map_utils/test/test_polygon_iterator.cpp

@@ -0,0 +1,280 @@
+// Copyright 2022 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.
+
+#include "autoware/grid_map_utils/polygon_iterator.hpp"
+
+#include <autoware/universe_utils/system/stop_watch.hpp>
+#include <grid_map_core/iterators/PolygonIterator.hpp>
+
+// gtest
+#include <gtest/gtest.h>
+
+// Vector
+#include <random>
+#include <string>
+#include <vector>
+
+using grid_map::GridMap;
+using grid_map::Index;
+using grid_map::Length;
+using grid_map::Polygon;
+using grid_map::Position;
+
+// Copied from grid_map::PolygonIterator
+TEST(PolygonIterator, FullCover)
+{
+  std::vector<std::string> types;
+  types.emplace_back("type");
+  GridMap map(types);
+  map.setGeometry(Length(8.0, 5.0), 1.0, Position(0.0, 0.0));  // bufferSize(8, 5)
+
+  Polygon polygon;
+  polygon.addVertex(Position(-100.0, 100.0));
+  polygon.addVertex(Position(100.0, 100.0));
+  polygon.addVertex(Position(100.0, -100.0));
+  polygon.addVertex(Position(-100.0, -100.0));
+
+  autoware::grid_map_utils::PolygonIterator iterator(map, polygon);
+
+  EXPECT_FALSE(iterator.isPastEnd());
+  EXPECT_EQ(0, (*iterator)(0));
+  EXPECT_EQ(0, (*iterator)(1));
+
+  ++iterator;
+  EXPECT_FALSE(iterator.isPastEnd());
+  EXPECT_EQ(0, (*iterator)(0));
+  EXPECT_EQ(1, (*iterator)(1));
+
+  ++iterator;
+  EXPECT_FALSE(iterator.isPastEnd());
+  EXPECT_EQ(0, (*iterator)(0));
+  EXPECT_EQ(2, (*iterator)(1));
+
+  for (int i = 0; i < 37; ++i) ++iterator;
+
+  EXPECT_FALSE(iterator.isPastEnd());
+  EXPECT_EQ(7, (*iterator)(0));
+  EXPECT_EQ(4, (*iterator)(1));
+
+  ++iterator;
+  EXPECT_TRUE(iterator.isPastEnd());
+}
+
+// Copied from grid_map::PolygonIterator
+TEST(PolygonIterator, Outside)
+{
+  GridMap map({"types"});
+  map.setGeometry(Length(8.0, 5.0), 1.0, Position(0.0, 0.0));  // bufferSize(8, 5)
+
+  Polygon polygon;
+  polygon.addVertex(Position(99.0, 101.0));
+  polygon.addVertex(Position(101.0, 101.0));
+  polygon.addVertex(Position(101.0, 99.0));
+  polygon.addVertex(Position(99.0, 99.0));
+
+  autoware::grid_map_utils::PolygonIterator iterator(map, polygon);
+
+  EXPECT_TRUE(iterator.isPastEnd());
+}
+
+// Copied from grid_map::PolygonIterator
+TEST(PolygonIterator, Square)
+{
+  GridMap map({"types"});
+  map.setGeometry(Length(8.0, 5.0), 1.0, Position(0.0, 0.0));  // bufferSize(8, 5)
+
+  Polygon polygon;
+  polygon.addVertex(Position(-1.0, 1.5));
+  polygon.addVertex(Position(1.0, 1.5));
+  polygon.addVertex(Position(1.0, -1.5));
+  polygon.addVertex(Position(-1.0, -1.5));
+
+  autoware::grid_map_utils::PolygonIterator iterator(map, polygon);
+
+  EXPECT_FALSE(iterator.isPastEnd());
+  EXPECT_EQ(3, (*iterator)(0));
+  EXPECT_EQ(1, (*iterator)(1));
+
+  ++iterator;
+  EXPECT_FALSE(iterator.isPastEnd());
+  EXPECT_EQ(3, (*iterator)(0));
+  EXPECT_EQ(2, (*iterator)(1));
+
+  ++iterator;
+  EXPECT_FALSE(iterator.isPastEnd());
+  EXPECT_EQ(3, (*iterator)(0));
+  EXPECT_EQ(3, (*iterator)(1));
+
+  ++iterator;
+  EXPECT_FALSE(iterator.isPastEnd());
+  EXPECT_EQ(4, (*iterator)(0));
+  EXPECT_EQ(1, (*iterator)(1));
+
+  ++iterator;
+  EXPECT_FALSE(iterator.isPastEnd());
+  EXPECT_EQ(4, (*iterator)(0));
+  EXPECT_EQ(2, (*iterator)(1));
+
+  ++iterator;
+  EXPECT_FALSE(iterator.isPastEnd());
+  EXPECT_EQ(4, (*iterator)(0));
+  EXPECT_EQ(3, (*iterator)(1));
+
+  ++iterator;
+  EXPECT_TRUE(iterator.isPastEnd());
+}
+
+// Copied from grid_map::PolygonIterator
+TEST(PolygonIterator, TopLeftTriangle)
+{
+  GridMap map({"types"});
+  map.setGeometry(Length(8.0, 5.0), 1.0, Position(0.0, 0.0));  // bufferSize(8, 5)
+
+  Polygon polygon;
+  polygon.addVertex(Position(-40.1, 20.6));
+  polygon.addVertex(Position(40.1, 20.4));
+  polygon.addVertex(Position(-40.1, -20.6));
+
+  autoware::grid_map_utils::PolygonIterator iterator(map, polygon);
+
+  EXPECT_FALSE(iterator.isPastEnd());
+  EXPECT_EQ(0, (*iterator)(0));
+  EXPECT_EQ(0, (*iterator)(1));
+
+  ++iterator;
+  EXPECT_FALSE(iterator.isPastEnd());
+  EXPECT_EQ(1, (*iterator)(0));
+  EXPECT_EQ(0, (*iterator)(1));
+}
+
+// Copied from grid_map::PolygonIterator
+TEST(PolygonIterator, MoveMap)
+{
+  GridMap map({"layer"});
+  map.setGeometry(Length(8.0, 5.0), 1.0, Position(0.0, 0.0));  // bufferSize(8, 5)
+  map.move(Position(2.0, 0.0));
+
+  Polygon polygon;
+  polygon.addVertex(Position(6.1, 1.6));
+  polygon.addVertex(Position(0.9, 1.6));
+  polygon.addVertex(Position(0.9, -1.6));
+  polygon.addVertex(Position(6.1, -1.6));
+  autoware::grid_map_utils::PolygonIterator iterator(map, polygon);
+
+  EXPECT_FALSE(iterator.isPastEnd());
+  EXPECT_EQ(6, (*iterator)(0));
+  EXPECT_EQ(1, (*iterator)(1));
+
+  ++iterator;
+  EXPECT_FALSE(iterator.isPastEnd());
+  EXPECT_EQ(6, (*iterator)(0));
+  EXPECT_EQ(2, (*iterator)(1));
+
+  for (int i = 0; i < 4; ++i) ++iterator;
+
+  EXPECT_FALSE(iterator.isPastEnd());
+  EXPECT_EQ(7, (*iterator)(0));
+  EXPECT_EQ(3, (*iterator)(1));
+
+  ++iterator;
+  EXPECT_FALSE(iterator.isPastEnd());
+  EXPECT_EQ(0, (*iterator)(0));
+  EXPECT_EQ(1, (*iterator)(1));
+
+  for (int i = 0; i < 8; ++i) ++iterator;
+
+  EXPECT_FALSE(iterator.isPastEnd());
+  EXPECT_EQ(2, (*iterator)(0));
+  EXPECT_EQ(3, (*iterator)(1));
+
+  ++iterator;
+  EXPECT_TRUE(iterator.isPastEnd());
+}
+
+// This test shows a difference when an edge passes exactly through the center of a cell
+TEST(PolygonIterator, Difference)
+{
+  GridMap map({"layer"});
+  map.setGeometry(Length(5.0, 5.0), 1.0, Position(0.0, 0.0));  // bufferSize(8, 5)
+
+  // Triangle where the hypotenuse is an exact diagonal of the map: difference.
+  Polygon polygon;
+  polygon.addVertex(Position(2.5, 2.5));
+  polygon.addVertex(Position(-2.5, 2.5));
+  polygon.addVertex(Position(-2.5, -2.5));
+  autoware::grid_map_utils::PolygonIterator iterator(map, polygon);
+  grid_map::PolygonIterator gm_iterator(map, polygon);
+  bool diff = false;
+  while (!iterator.isPastEnd() && !gm_iterator.isPastEnd()) {
+    if ((*gm_iterator)(0) != (*iterator)(0) || (*gm_iterator)(1) != (*iterator)(1)) diff = true;
+    ++iterator;
+    ++gm_iterator;
+  }
+  if (iterator.isPastEnd() != gm_iterator.isPastEnd()) {
+    diff = true;
+  }
+  EXPECT_TRUE(diff);
+
+  // Triangle where the hypotenuse does not cross any cell center: no difference.
+  polygon.removeVertices();
+  polygon.addVertex(Position(2.5, 2.1));
+  polygon.addVertex(Position(-2.5, 2.5));
+  polygon.addVertex(Position(-2.5, -2.9));
+  iterator = autoware::grid_map_utils::PolygonIterator(map, polygon);
+  gm_iterator = grid_map::PolygonIterator(map, polygon);
+  diff = false;
+  while (!iterator.isPastEnd() && !gm_iterator.isPastEnd()) {
+    if ((*gm_iterator)(0) != (*iterator)(0) || (*gm_iterator)(1) != (*iterator)(1)) diff = true;
+    ++iterator;
+    ++gm_iterator;
+  }
+  if (iterator.isPastEnd() != gm_iterator.isPastEnd()) {
+    diff = true;
+  }
+  EXPECT_FALSE(diff);
+}
+
+TEST(PolygonIterator, SelfCrossingPolygon)
+{
+  GridMap map({"layer"});
+  map.setGeometry(Length(5.0, 5.0), 1.0, Position(0.0, 0.0));  // bufferSize(8, 5)
+
+  // Hour-glass shape
+  Polygon polygon;
+  polygon.addVertex(Position(2.5, 2.9));
+  polygon.addVertex(Position(2.5, -2.9));
+  polygon.addVertex(Position(-2.5, 2.5));
+  polygon.addVertex(Position(-2.5, -2.5));
+  autoware::grid_map_utils::PolygonIterator iterator(map, polygon);
+  grid_map::PolygonIterator gm_iterator(map, polygon);
+
+  const std::vector<Index> expected_indexes = {
+    Index(0, 0), Index(0, 1), Index(0, 2), Index(0, 3), Index(0, 4), Index(1, 1), Index(1, 2),
+    Index(1, 3), Index(2, 2), Index(3, 2), Index(4, 1), Index(4, 2), Index(4, 3)};
+  bool diff = false;
+  size_t i = 0;
+  while (!iterator.isPastEnd() && !gm_iterator.isPastEnd()) {
+    if ((*gm_iterator)(0) != (*iterator)(0) || (*gm_iterator)(1) != (*iterator)(1)) diff = true;
+    ASSERT_TRUE(i < expected_indexes.size());
+    EXPECT_EQ((*iterator)(0), expected_indexes[i](0));
+    EXPECT_EQ((*iterator)(1), expected_indexes[i](1));
+    ++i;
+    ++iterator;
+    ++gm_iterator;
+  }
+  if (iterator.isPastEnd() != gm_iterator.isPastEnd()) {
+    diff = true;
+  }
+  EXPECT_FALSE(diff);
+}

master/src/universe/common/autoware_interpolation/CMakeLists.txt

@@ -0,0 +1,24 @@
+cmake_minimum_required(VERSION 3.14)
+project(autoware_interpolation)
+
+find_package(autoware_cmake REQUIRED)
+autoware_package()
+
+ament_auto_add_library(autoware_interpolation SHARED
+  src/linear_interpolation.cpp
+  src/spline_interpolation.cpp
+  src/spline_interpolation_points_2d.cpp
+  src/spherical_linear_interpolation.cpp
+)
+
+if(BUILD_TESTING)
+  file(GLOB_RECURSE test_files test/**/*.cpp)
+
+  ament_add_ros_isolated_gtest(test_interpolation ${test_files})
+
+  target_link_libraries(test_interpolation
+    autoware_interpolation
+  )
+endif()
+
+ament_auto_package()

master/src/universe/common/autoware_interpolation/README.md

@@ -0,0 +1,109 @@
+# Interpolation package
+
+This package supplies linear and spline interpolation functions.
+
+## Linear Interpolation
+
+`lerp(src_val, dst_val, ratio)` (for scalar interpolation) interpolates `src_val` and `dst_val` with `ratio`.
+This will be replaced with `std::lerp(src_val, dst_val, ratio)` in `C++20`.
+
+`lerp(base_keys, base_values, query_keys)` (for vector interpolation) applies linear regression to each two continuous points whose x values are`base_keys` and whose y values are `base_values`.
+Then it calculates interpolated values on y-axis for `query_keys` on x-axis.
+
+## Spline Interpolation
+
+`spline(base_keys, base_values, query_keys)` (for vector interpolation) applies spline regression to each two continuous points whose x values are`base_keys` and whose y values are `base_values`.
+Then it calculates interpolated values on y-axis for `query_keys` on x-axis.
+
+### Evaluation of calculation cost
+
+We evaluated calculation cost of spline interpolation for 100 points, and adopted the best one which is tridiagonal matrix algorithm.
+Methods except for tridiagonal matrix algorithm exists in `spline_interpolation` package, which has been removed from Autoware.
+
+| Method                            | Calculation time |
+| --------------------------------- | ---------------- |
+| Tridiagonal Matrix Algorithm      | 0.007 [ms]       |
+| Preconditioned Conjugate Gradient | 0.024 [ms]       |
+| Successive Over-Relaxation        | 0.074 [ms]       |
+
+### Spline Interpolation Algorithm
+
+Assuming that the size of `base_keys` ($x_i$) and `base_values` ($y_i$) are $N + 1$, we aim to calculate spline interpolation with the following equation to interpolate between $y_i$ and $y_{i+1}$.
+
+$$
+Y_i(x) = a_i (x - x_i)^3 + b_i (x - x_i)^2 + c_i (x - x_i) + d_i \ \ \ (i = 0, \dots, N-1)
+$$
+
+Constraints on spline interpolation are as follows.
+The number of constraints is $4N$, which is equal to the number of variables of spline interpolation.
+
+$$
+\begin{align}
+Y_i (x_i) & = y_i \ \ \ (i = 0, \dots, N-1) \\
+Y_i (x_{i+1}) & = y_{i+1} \ \ \ (i = 0, \dots, N-1) \\
+Y'_i (x_{i+1}) & = Y'_{i+1} (x_{i+1}) \ \ \ (i = 0, \dots, N-2) \\
+Y''_i (x_{i+1}) & = Y''_{i+1} (x_{i+1}) \ \ \ (i = 0, \dots, N-2) \\
+Y''_0 (x_0) & = 0 \\
+Y''_{N-1} (x_N) & = 0
+\end{align}
+$$
+
+According to [this article](https://www.mk-mode.com/rails/docs/INTERPOLATION_SPLINE.pdf), spline interpolation is formulated as the following linear equation.
+
+$$
+\begin{align}
+ \begin{pmatrix}
+    2(h_0 + h_1) & h_1 \\
+    h_0 & 2 (h_1 + h_2) & h_2 & & O \\
+        &     &     & \ddots \\
+    O &     &     &       & h_{N-2} & 2 (h_{N-2} + h_{N-1})
+ \end{pmatrix}
+ \begin{pmatrix}
+    v_1 \\ v_2 \\ v_3 \\ \vdots \\ v_{N-1}
+ \end{pmatrix}=
+ \begin{pmatrix}
+    w_1 \\ w_2 \\ w_3 \\ \vdots \\ w_{N-1}
+ \end{pmatrix}
+\end{align}
+$$
+
+where
+
+$$
+\begin{align}
+h_i & = x_{i+1} - x_i \ \ \ (i = 0, \dots, N-1) \\
+w_i & = 6 \left(\frac{y_{i+1} - y_{i+1}}{h_i} - \frac{y_i - y_{i-1}}{h_{i-1}}\right) \ \ \ (i = 1, \dots, N-1)
+\end{align}
+$$
+
+The coefficient matrix of this linear equation is tridiagonal matrix. Therefore, it can be solve with tridiagonal matrix algorithm, which can solve linear equations without gradient descent methods.
+
+Solving this linear equation with tridiagonal matrix algorithm, we can calculate coefficients of spline interpolation as follows.
+
+$$
+\begin{align}
+a_i & = \frac{v_{i+1} - v_i}{6 (x_{i+1} - x_i)} \ \ \ (i = 0, \dots, N-1) \\
+b_i & = \frac{v_i}{2} \ \ \ (i = 0, \dots, N-1) \\
+c_i & = \frac{y_{i+1} - y_i}{x_{i+1} - x_i} - \frac{1}{6}(x_{i+1} - x_i)(2 v_i + v_{i+1}) \ \ \ (i = 0, \dots, N-1) \\
+d_i & = y_i \ \ \ (i = 0, \dots, N-1)
+\end{align}
+$$
+
+### Tridiagonal Matrix Algorithm
+
+We solve tridiagonal linear equation according to [this article](https://www.iist.ac.in/sites/default/files/people/tdma.pdf) where variables of linear equation are expressed as follows in the implementation.
+
+$$
+\begin{align}
+ \begin{pmatrix}
+    b_0 & c_0 &     & \\
+    a_0 & b_1 & c_2 & O \\
+        &     & \ddots \\
+    O &     & a_{N-2} &  b_{N-1}
+ \end{pmatrix}
+x =
+\begin{pmatrix}
+    d_0 \\ d_2 \\ d_3 \\ \vdots \\ d_{N-1}
+ \end{pmatrix}
+\end{align}
+$$

master/src/universe/common/autoware_interpolation/include/autoware/interpolation/interpolation_utils.hpp

@@ -0,0 +1,114 @@
+// 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__INTERPOLATION__INTERPOLATION_UTILS_HPP_
+#define AUTOWARE__INTERPOLATION__INTERPOLATION_UTILS_HPP_
+
+#include <algorithm>
+#include <array>
+#include <stdexcept>
+#include <vector>
+
+namespace autoware::interpolation
+{
+inline bool isIncreasing(const std::vector<double> & x)
+{
+  if (x.empty()) {
+    throw std::invalid_argument("Points is empty.");
+  }
+
+  for (size_t i = 0; i < x.size() - 1; ++i) {
+    if (x.at(i) >= x.at(i + 1)) {
+      return false;
+    }
+  }
+
+  return true;
+}
+
+inline bool isNotDecreasing(const std::vector<double> & x)
+{
+  if (x.empty()) {
+    throw std::invalid_argument("Points is empty.");
+  }
+
+  for (size_t i = 0; i < x.size() - 1; ++i) {
+    if (x.at(i) > x.at(i + 1)) {
+      return false;
+    }
+  }
+
+  return true;
+}
+
+inline std::vector<double> validateKeys(
+  const std::vector<double> & base_keys, const std::vector<double> & query_keys)
+{
+  // when vectors are empty
+  if (base_keys.empty() || query_keys.empty()) {
+    throw std::invalid_argument("Points is empty.");
+  }
+
+  // when size of vectors are less than 2
+  if (base_keys.size() < 2) {
+    throw std::invalid_argument(
+      "The size of points is less than 2. base_keys.size() = " + std::to_string(base_keys.size()));
+  }
+
+  // when indices are not sorted
+  if (!isIncreasing(base_keys) || !isNotDecreasing(query_keys)) {
+    throw std::invalid_argument("Either base_keys or query_keys is not sorted.");
+  }
+
+  // when query_keys is out of base_keys (This function does not allow exterior division.)
+  constexpr double epsilon = 1e-3;
+  if (
+    query_keys.front() < base_keys.front() - epsilon ||
+    base_keys.back() + epsilon < query_keys.back()) {
+    throw std::invalid_argument("query_keys is out of base_keys");
+  }
+
+  // NOTE: Due to calculation error of double, a query key may be slightly out of base keys.
+  //       Therefore, query keys are cropped here.
+  auto validated_query_keys = query_keys;
+  validated_query_keys.front() = std::max(validated_query_keys.front(), base_keys.front());
+  validated_query_keys.back() = std::min(validated_query_keys.back(), base_keys.back());
+
+  return validated_query_keys;
+}
+
+template <class T>
+void validateKeysAndValues(
+  const std::vector<double> & base_keys, const std::vector<T> & base_values)
+{
+  // when vectors are empty
+  if (base_keys.empty() || base_values.empty()) {
+    throw std::invalid_argument("Points is empty.");
+  }
+
+  // when size of vectors are less than 2
+  if (base_keys.size() < 2 || base_values.size() < 2) {
+    throw std::invalid_argument(
+      "The size of points is less than 2. base_keys.size() = " + std::to_string(base_keys.size()) +
+      ", base_values.size() = " + std::to_string(base_values.size()));
+  }
+
+  // when sizes of indices and values are not same
+  if (base_keys.size() != base_values.size()) {
+    throw std::invalid_argument("The size of base_keys and base_values are not the same.");
+  }
+}
+}  // namespace autoware::interpolation
+
+#endif  // AUTOWARE__INTERPOLATION__INTERPOLATION_UTILS_HPP_

master/src/universe/common/autoware_interpolation/include/autoware/interpolation/linear_interpolation.hpp

@@ -0,0 +1,35 @@
+// 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__INTERPOLATION__LINEAR_INTERPOLATION_HPP_
+#define AUTOWARE__INTERPOLATION__LINEAR_INTERPOLATION_HPP_
+
+#include "autoware/interpolation/interpolation_utils.hpp"
+
+#include <vector>
+
+namespace autoware::interpolation
+{
+double lerp(const double src_val, const double dst_val, const double ratio);
+
+std::vector<double> lerp(
+  const std::vector<double> & base_keys, const std::vector<double> & base_values,
+  const std::vector<double> & query_keys);
+
+double lerp(
+  const std::vector<double> & base_keys, const std::vector<double> & base_values,
+  const double query_key);
+}  // namespace autoware::interpolation
+
+#endif  // AUTOWARE__INTERPOLATION__LINEAR_INTERPOLATION_HPP_

master/src/universe/common/autoware_interpolation/include/autoware/interpolation/spherical_linear_interpolation.hpp

@@ -0,0 +1,48 @@
+// Copyright 2022 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__INTERPOLATION__SPHERICAL_LINEAR_INTERPOLATION_HPP_
+#define AUTOWARE__INTERPOLATION__SPHERICAL_LINEAR_INTERPOLATION_HPP_
+
+#include "autoware/interpolation/interpolation_utils.hpp"
+
+#include <geometry_msgs/msg/quaternion.hpp>
+
+#include <tf2/utils.h>
+
+#ifdef ROS_DISTRO_GALACTIC
+#include <tf2_geometry_msgs/tf2_geometry_msgs.h>
+#else
+#include <tf2_geometry_msgs/tf2_geometry_msgs.hpp>
+#endif
+
+#include <vector>
+
+namespace autoware::interpolation
+{
+geometry_msgs::msg::Quaternion slerp(
+  const geometry_msgs::msg::Quaternion & src_quat, const geometry_msgs::msg::Quaternion & dst_quat,
+  const double ratio);
+
+std::vector<geometry_msgs::msg::Quaternion> slerp(
+  const std::vector<double> & base_keys,
+  const std::vector<geometry_msgs::msg::Quaternion> & base_values,
+  const std::vector<double> & query_keys);
+
+geometry_msgs::msg::Quaternion lerpOrientation(
+  const geometry_msgs::msg::Quaternion & o_from, const geometry_msgs::msg::Quaternion & o_to,
+  const double ratio);
+}  // namespace autoware::interpolation
+
+#endif  // AUTOWARE__INTERPOLATION__SPHERICAL_LINEAR_INTERPOLATION_HPP_

master/src/universe/common/autoware_interpolation/include/autoware/interpolation/spline_interpolation.hpp

@@ -0,0 +1,97 @@
+// 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__INTERPOLATION__SPLINE_INTERPOLATION_HPP_
+#define AUTOWARE__INTERPOLATION__SPLINE_INTERPOLATION_HPP_
+
+#include "autoware/interpolation/interpolation_utils.hpp"
+#include "autoware/universe_utils/geometry/geometry.hpp"
+
+#include <Eigen/Core>
+
+#include <algorithm>
+#include <cmath>
+#include <iostream>
+#include <numeric>
+#include <vector>
+
+namespace autoware::interpolation
+{
+// static spline interpolation functions
+std::vector<double> spline(
+  const std::vector<double> & base_keys, const std::vector<double> & base_values,
+  const std::vector<double> & query_keys);
+std::vector<double> splineByAkima(
+  const std::vector<double> & base_keys, const std::vector<double> & base_values,
+  const std::vector<double> & query_keys);
+
+// non-static 1-dimensional spline interpolation
+//
+// Usage:
+// ```
+// SplineInterpolation spline;
+// // memorize pre-interpolation result internally
+// spline.calcSplineCoefficients(base_keys, base_values);
+// const auto interpolation_result1 = spline.getSplineInterpolatedValues(
+//   base_keys, query_keys1);
+// const auto interpolation_result2 = spline.getSplineInterpolatedValues(
+//   base_keys, query_keys2);
+// ```
+class SplineInterpolation
+{
+public:
+  SplineInterpolation() = default;
+  SplineInterpolation(
+    const std::vector<double> & base_keys, const std::vector<double> & base_values)
+  {
+    calcSplineCoefficients(base_keys, base_values);
+  }
+
+  //!< @brief get values of spline interpolation on designated sampling points.
+  //!< @details Assuming that query_keys are t vector for sampling, and interpolation is for x,
+  //            meaning that spline interpolation was applied to x(t),
+  //            return value will be x(t) vector
+  std::vector<double> getSplineInterpolatedValues(const std::vector<double> & query_keys) const;
+
+  //!< @brief get 1st differential values of spline interpolation on designated sampling points.
+  //!< @details Assuming that query_keys are t vector for sampling, and interpolation is for x,
+  //            meaning that spline interpolation was applied to x(t),
+  //            return value will be dx/dt(t) vector
+  std::vector<double> getSplineInterpolatedDiffValues(const std::vector<double> & query_keys) const;
+
+  //!< @brief get 2nd differential values of spline interpolation on designated sampling points.
+  //!< @details Assuming that query_keys are t vector for sampling, and interpolation is for x,
+  //            meaning that spline interpolation was applied to x(t),
+  //            return value will be d^2/dt^2(t) vector
+  std::vector<double> getSplineInterpolatedQuadDiffValues(
+    const std::vector<double> & query_keys) const;
+
+  size_t getSize() const { return base_keys_.size(); }
+
+private:
+  Eigen::VectorXd a_;
+  Eigen::VectorXd b_;
+  Eigen::VectorXd c_;
+  Eigen::VectorXd d_;
+
+  std::vector<double> base_keys_;
+
+  void calcSplineCoefficients(
+    const std::vector<double> & base_keys, const std::vector<double> & base_values);
+
+  Eigen::Index get_index(const double & key) const;
+};
+}  // namespace autoware::interpolation
+
+#endif  // AUTOWARE__INTERPOLATION__SPLINE_INTERPOLATION_HPP_

master/src/universe/common/autoware_interpolation/include/autoware/interpolation/spline_interpolation_points_2d.hpp

@@ -0,0 +1,89 @@
+// 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__INTERPOLATION__SPLINE_INTERPOLATION_POINTS_2D_HPP_
+#define AUTOWARE__INTERPOLATION__SPLINE_INTERPOLATION_POINTS_2D_HPP_
+
+#include "autoware/interpolation/spline_interpolation.hpp"
+
+#include <vector>
+
+namespace autoware::interpolation
+{
+
+template <typename T>
+std::vector<double> splineYawFromPoints(const std::vector<T> & points);
+
+// non-static points spline interpolation
+// NOTE: We can calculate yaw from the x and y by interpolation derivatives.
+//
+// Usage:
+// ```
+// SplineInterpolationPoints2d spline;
+// // memorize pre-interpolation result internally
+// spline.calcSplineCoefficients(base_keys, base_values);
+// const auto interpolation_result1 = spline.getSplineInterpolatedPoint(
+//   base_keys, query_keys1);
+// const auto interpolation_result2 = spline.getSplineInterpolatedPoint(
+//   base_keys, query_keys2);
+// const auto yaw_interpolation_result = spline.getSplineInterpolatedYaw(
+//   base_keys, query_keys1);
+// ```
+class SplineInterpolationPoints2d
+{
+public:
+  SplineInterpolationPoints2d() = default;
+  template <typename T>
+  explicit SplineInterpolationPoints2d(const std::vector<T> & points)
+  {
+    std::vector<geometry_msgs::msg::Point> points_inner;
+    for (const auto & p : points) {
+      points_inner.push_back(autoware::universe_utils::getPoint(p));
+    }
+    calcSplineCoefficientsInner(points_inner);
+  }
+
+  // TODO(murooka) implement these functions
+  // std::vector<geometry_msgs::msg::Point> getSplineInterpolatedPoints(const double width);
+  // std::vector<geometry_msgs::msg::Pose> getSplineInterpolatedPoses(const double width);
+
+  // pose (= getSplineInterpolatedPoint + getSplineInterpolatedYaw)
+  geometry_msgs::msg::Pose getSplineInterpolatedPose(const size_t idx, const double s) const;
+
+  // point
+  geometry_msgs::msg::Point getSplineInterpolatedPoint(const size_t idx, const double s) const;
+
+  // yaw
+  double getSplineInterpolatedYaw(const size_t idx, const double s) const;
+  std::vector<double> getSplineInterpolatedYaws() const;
+
+  // curvature
+  double getSplineInterpolatedCurvature(const size_t idx, const double s) const;
+  std::vector<double> getSplineInterpolatedCurvatures() const;
+
+  size_t getSize() const { return base_s_vec_.size(); }
+  size_t getOffsetIndex(const size_t idx, const double offset) const;
+  double getAccumulatedLength(const size_t idx) const;
+
+private:
+  void calcSplineCoefficientsInner(const std::vector<geometry_msgs::msg::Point> & points);
+  SplineInterpolation spline_x_;
+  SplineInterpolation spline_y_;
+  SplineInterpolation spline_z_;
+
+  std::vector<double> base_s_vec_;
+};
+}  // namespace autoware::interpolation
+
+#endif  // AUTOWARE__INTERPOLATION__SPLINE_INTERPOLATION_POINTS_2D_HPP_

master/src/universe/common/autoware_interpolation/include/autoware/interpolation/zero_order_hold.hpp

@@ -0,0 +1,81 @@
+// Copyright 2022 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__INTERPOLATION__ZERO_ORDER_HOLD_HPP_
+#define AUTOWARE__INTERPOLATION__ZERO_ORDER_HOLD_HPP_
+
+#include "autoware/interpolation/interpolation_utils.hpp"
+
+#include <vector>
+
+namespace autoware::interpolation
+{
+inline std::vector<size_t> calc_closest_segment_indices(
+  const std::vector<double> & base_keys, const std::vector<double> & query_keys,
+  const double overlap_threshold = 1e-3)
+{
+  // throw exception for invalid arguments
+  const auto validated_query_keys = validateKeys(base_keys, query_keys);
+
+  std::vector<size_t> closest_segment_indices(validated_query_keys.size());
+  size_t closest_segment_idx = 0;
+  for (size_t i = 0; i < validated_query_keys.size(); ++i) {
+    // Check if query_key is closes to the terminal point of the base keys
+    if (base_keys.back() - overlap_threshold < validated_query_keys.at(i)) {
+      closest_segment_idx = base_keys.size() - 1;
+    } else {
+      for (size_t j = base_keys.size() - 1; j > closest_segment_idx; --j) {
+        if (
+          base_keys.at(j - 1) - overlap_threshold < validated_query_keys.at(i) &&
+          validated_query_keys.at(i) < base_keys.at(j)) {
+          // find closest segment in base keys
+          closest_segment_idx = j - 1;
+          break;
+        }
+      }
+    }
+
+    closest_segment_indices.at(i) = closest_segment_idx;
+  }
+
+  return closest_segment_indices;
+}
+
+template <class T>
+std::vector<T> zero_order_hold(
+  const std::vector<double> & base_keys, const std::vector<T> & base_values,
+  const std::vector<size_t> & closest_segment_indices)
+{
+  // throw exception for invalid arguments
+  validateKeysAndValues(base_keys, base_values);
+
+  std::vector<T> query_values(closest_segment_indices.size());
+  for (size_t i = 0; i < closest_segment_indices.size(); ++i) {
+    query_values.at(i) = base_values.at(closest_segment_indices.at(i));
+  }
+
+  return query_values;
+}
+
+template <class T>
+std::vector<T> zero_order_hold(
+  const std::vector<double> & base_keys, const std::vector<T> & base_values,
+  const std::vector<double> & query_keys, const double overlap_threshold = 1e-3)
+{
+  return zero_order_hold(
+    base_keys, base_values, calc_closest_segment_indices(base_keys, query_keys, overlap_threshold));
+}
+}  // namespace autoware::interpolation
+
+#endif  // AUTOWARE__INTERPOLATION__ZERO_ORDER_HOLD_HPP_

master/src/universe/common/autoware_interpolation/package.xml

@@ -0,0 +1,24 @@
+<?xml version="1.0"?>
+<?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
+<package format="3">
+  <name>autoware_interpolation</name>
+  <version>0.1.0</version>
+  <description>The spline interpolation package</description>
+  <maintainer email="fumiya.watanabe@tier4.jp">Fumiya Watanabe</maintainer>
+  <maintainer email="takayuki.murooka@tier4.jp">Takayuki Murooka</maintainer>
+  <license>Apache License 2.0</license>
+
+  <buildtool_depend>ament_cmake_auto</buildtool_depend>
+  <buildtool_depend>autoware_cmake</buildtool_depend>
+
+  <depend>autoware_universe_utils</depend>
+  <depend>eigen</depend>
+
+  <test_depend>ament_cmake_ros</test_depend>
+  <test_depend>ament_lint_auto</test_depend>
+  <test_depend>autoware_lint_common</test_depend>
+
+  <export>
+    <build_type>ament_cmake</build_type>
+  </export>
+</package>

master/src/universe/common/autoware_interpolation/src/linear_interpolation.cpp

@@ -0,0 +1,59 @@
+// 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.
+
+#include "autoware/interpolation/linear_interpolation.hpp"
+
+#include <vector>
+
+namespace autoware::interpolation
+{
+double lerp(const double src_val, const double dst_val, const double ratio)
+{
+  return src_val + (dst_val - src_val) * ratio;
+}
+
+std::vector<double> lerp(
+  const std::vector<double> & base_keys, const std::vector<double> & base_values,
+  const std::vector<double> & query_keys)
+{
+  // throw exception for invalid arguments
+  const auto validated_query_keys = validateKeys(base_keys, query_keys);
+  validateKeysAndValues(base_keys, base_values);
+
+  // calculate linear interpolation
+  std::vector<double> query_values;
+  size_t key_index = 0;
+  for (const auto query_key : validated_query_keys) {
+    while (base_keys.at(key_index + 1) < query_key) {
+      ++key_index;
+    }
+
+    const double src_val = base_values.at(key_index);
+    const double dst_val = base_values.at(key_index + 1);
+    const double ratio = (query_key - base_keys.at(key_index)) /
+                         (base_keys.at(key_index + 1) - base_keys.at(key_index));
+
+    const double interpolated_val = lerp(src_val, dst_val, ratio);
+    query_values.push_back(interpolated_val);
+  }
+
+  return query_values;
+}
+
+double lerp(
+  const std::vector<double> & base_keys, const std::vector<double> & base_values, double query_key)
+{
+  return lerp(base_keys, base_values, std::vector<double>{query_key}).front();
+}
+}  // namespace autoware::interpolation

master/src/universe/common/autoware_interpolation/src/spherical_linear_interpolation.cpp

@@ -0,0 +1,71 @@
+// Copyright 2022 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.
+
+#include "autoware/interpolation/spherical_linear_interpolation.hpp"
+
+namespace autoware::interpolation
+{
+geometry_msgs::msg::Quaternion slerp(
+  const geometry_msgs::msg::Quaternion & src_quat, const geometry_msgs::msg::Quaternion & dst_quat,
+  const double ratio)
+{
+  tf2::Quaternion src_tf;
+  tf2::Quaternion dst_tf;
+  tf2::fromMsg(src_quat, src_tf);
+  tf2::fromMsg(dst_quat, dst_tf);
+  const auto interpolated_quat = tf2::slerp(src_tf, dst_tf, ratio);
+  return tf2::toMsg(interpolated_quat);
+}
+
+std::vector<geometry_msgs::msg::Quaternion> slerp(
+  const std::vector<double> & base_keys,
+  const std::vector<geometry_msgs::msg::Quaternion> & base_values,
+  const std::vector<double> & query_keys)
+{
+  // throw exception for invalid arguments
+  const auto validated_query_keys = validateKeys(base_keys, query_keys);
+  validateKeysAndValues(base_keys, base_values);
+
+  // calculate linear interpolation
+  std::vector<geometry_msgs::msg::Quaternion> query_values;
+  size_t key_index = 0;
+  for (const auto query_key : validated_query_keys) {
+    while (base_keys.at(key_index + 1) < query_key) {
+      ++key_index;
+    }
+
+    const auto src_quat = base_values.at(key_index);
+    const auto dst_quat = base_values.at(key_index + 1);
+    const double ratio = (query_key - base_keys.at(key_index)) /
+                         (base_keys.at(key_index + 1) - base_keys.at(key_index));
+
+    const auto interpolated_quat = slerp(src_quat, dst_quat, ratio);
+    query_values.push_back(interpolated_quat);
+  }
+
+  return query_values;
+}
+
+geometry_msgs::msg::Quaternion lerpOrientation(
+  const geometry_msgs::msg::Quaternion & o_from, const geometry_msgs::msg::Quaternion & o_to,
+  const double ratio)
+{
+  tf2::Quaternion q_from, q_to;
+  tf2::fromMsg(o_from, q_from);
+  tf2::fromMsg(o_to, q_to);
+
+  const auto q_interpolated = q_from.slerp(q_to, ratio);
+  return tf2::toMsg(q_interpolated);
+}
+}  // namespace autoware::interpolation

master/src/universe/common/autoware_interpolation/src/spline_interpolation.cpp

@@ -0,0 +1,247 @@
+// 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.
+
+#include "autoware/interpolation/spline_interpolation.hpp"
+
+#include <cstdint>
+#include <vector>
+
+namespace autoware::interpolation
+{
+Eigen::VectorXd solve_tridiagonal_matrix_algorithm(
+  const Eigen::Ref<const Eigen::VectorXd> & a, const Eigen::Ref<const Eigen::VectorXd> & b,
+  const Eigen::Ref<const Eigen::VectorXd> & c, const Eigen::Ref<const Eigen::VectorXd> & d)
+{
+  const auto n = d.size();
+
+  if (n == 1) {
+    return d.array() / b.array();
+  }
+
+  Eigen::VectorXd c_prime = Eigen::VectorXd::Zero(n);
+  Eigen::VectorXd d_prime = Eigen::VectorXd::Zero(n);
+  Eigen::VectorXd x = Eigen::VectorXd::Zero(n);
+
+  // Forward sweep
+  c_prime(0) = c(0) / b(0);
+  d_prime(0) = d(0) / b(0);
+
+  for (auto i = 1; i < n; i++) {
+    const double m = 1.0 / (b(i) - a(i - 1) * c_prime(i - 1));
+    c_prime(i) = i < n - 1 ? c(i) * m : 0;
+    d_prime(i) = (d(i) - a(i - 1) * d_prime(i - 1)) * m;
+  }
+
+  // Back substitution
+  x(n - 1) = d_prime(n - 1);
+
+  for (int64_t i = n - 2; i >= 0; i--) {
+    x(i) = d_prime(i) - c_prime(i) * x(i + 1);
+  }
+
+  return x;
+}
+
+std::vector<double> spline(
+  const std::vector<double> & base_keys, const std::vector<double> & base_values,
+  const std::vector<double> & query_keys)
+{
+  // calculate spline coefficients
+  SplineInterpolation interpolator(base_keys, base_values);
+
+  // interpolate base_keys at query_keys
+  return interpolator.getSplineInterpolatedValues(query_keys);
+}
+
+std::vector<double> splineByAkima(
+  const std::vector<double> & base_keys, const std::vector<double> & base_values,
+  const std::vector<double> & query_keys)
+{
+  constexpr double epsilon = 1e-5;
+
+  // calculate m
+  std::vector<double> m_values;
+  for (size_t i = 0; i < base_keys.size() - 1; ++i) {
+    const double m_val =
+      (base_values.at(i + 1) - base_values.at(i)) / (base_keys.at(i + 1) - base_keys.at(i));
+    m_values.push_back(m_val);
+  }
+
+  // calculate s
+  std::vector<double> s_values;
+  for (size_t i = 0; i < base_keys.size(); ++i) {
+    if (i == 0) {
+      s_values.push_back(m_values.front());
+      continue;
+    } else if (i == base_keys.size() - 1) {
+      s_values.push_back(m_values.back());
+      continue;
+    } else if (i == 1 || i == base_keys.size() - 2) {
+      const double s_val = (m_values.at(i - 1) + m_values.at(i)) / 2.0;
+      s_values.push_back(s_val);
+      continue;
+    }
+
+    const double denom = std::abs(m_values.at(i + 1) - m_values.at(i)) +
+                         std::abs(m_values.at(i - 1) - m_values.at(i - 2));
+    if (std::abs(denom) < epsilon) {
+      const double s_val = (m_values.at(i - 1) + m_values.at(i)) / 2.0;
+      s_values.push_back(s_val);
+      continue;
+    }
+
+    const double s_val = (std::abs(m_values.at(i + 1) - m_values.at(i)) * m_values.at(i - 1) +
+                          std::abs(m_values.at(i - 1) - m_values.at(i - 2)) * m_values.at(i)) /
+                         denom;
+    s_values.push_back(s_val);
+  }
+
+  // calculate cubic coefficients
+  std::vector<double> a;
+  std::vector<double> b;
+  std::vector<double> c;
+  std::vector<double> d;
+  for (size_t i = 0; i < base_keys.size() - 1; ++i) {
+    a.push_back(
+      (s_values.at(i) + s_values.at(i + 1) - 2.0 * m_values.at(i)) /
+      std::pow(base_keys.at(i + 1) - base_keys.at(i), 2));
+    b.push_back(
+      (3.0 * m_values.at(i) - 2.0 * s_values.at(i) - s_values.at(i + 1)) /
+      (base_keys.at(i + 1) - base_keys.at(i)));
+    c.push_back(s_values.at(i));
+    d.push_back(base_values.at(i));
+  }
+
+  // interpolate
+  std::vector<double> res;
+  size_t j = 0;
+  for (const auto & query_key : query_keys) {
+    while (base_keys.at(j + 1) < query_key) {
+      ++j;
+    }
+
+    const double ds = query_key - base_keys.at(j);
+    res.push_back(d.at(j) + (c.at(j) + (b.at(j) + a.at(j) * ds) * ds) * ds);
+  }
+  return res;
+}
+
+Eigen::Index SplineInterpolation::get_index(const double & key) const
+{
+  const auto it = std::lower_bound(base_keys_.begin(), base_keys_.end(), key);
+  return std::clamp(
+    static_cast<int>(std::distance(base_keys_.begin(), it)) - 1, 0,
+    static_cast<int>(base_keys_.size()) - 2);
+}
+
+void SplineInterpolation::calcSplineCoefficients(
+  const std::vector<double> & base_keys, const std::vector<double> & base_values)
+{
+  // throw exceptions for invalid arguments
+  autoware::interpolation::validateKeysAndValues(base_keys, base_values);
+  const Eigen::VectorXd x = Eigen::Map<const Eigen::VectorXd>(
+    base_keys.data(), static_cast<Eigen::Index>(base_keys.size()));
+  const Eigen::VectorXd y = Eigen::Map<const Eigen::VectorXd>(
+    base_values.data(), static_cast<Eigen::Index>(base_values.size()));
+
+  const auto n = x.size();
+
+  if (n == 2) {
+    a_ = Eigen::VectorXd::Zero(1);
+    b_ = Eigen::VectorXd::Zero(1);
+    c_ = Eigen::VectorXd::Zero(1);
+    d_ = Eigen::VectorXd::Zero(1);
+    c_[0] = (y[1] - y[0]) / (x[1] - x[0]);
+    d_[0] = y[0];
+    base_keys_ = base_keys;
+    return;
+  }
+
+  // Create Tridiagonal matrix
+  Eigen::VectorXd v(n);
+  const Eigen::VectorXd h = x.segment(1, n - 1) - x.segment(0, n - 1);
+  const Eigen::VectorXd a = h.segment(1, n - 3);
+  const Eigen::VectorXd b = 2 * (h.segment(0, n - 2) + h.segment(1, n - 2));
+  const Eigen::VectorXd c = h.segment(1, n - 3);
+  const Eigen::VectorXd y_diff = y.segment(1, n - 1) - y.segment(0, n - 1);
+  const Eigen::VectorXd d = 6 * (y_diff.segment(1, n - 2).array() / h.tail(n - 2).array() -
+                                 y_diff.segment(0, n - 2).array() / h.head(n - 2).array());
+
+  // Solve tridiagonal matrix
+  v.segment(1, n - 2) = solve_tridiagonal_matrix_algorithm(a, b, c, d);
+  v[0] = 0;
+  v[n - 1] = 0;
+
+  // Calculate spline coefficients
+  a_ = (v.tail(n - 1) - v.head(n - 1)).array() / 6.0 / (x.tail(n - 1) - x.head(n - 1)).array();
+  b_ = v.segment(0, n - 1) / 2.0;
+  c_ = (y.tail(n - 1) - y.head(n - 1)).array() / (x.tail(n - 1) - x.head(n - 1)).array() -
+       (x.tail(n - 1) - x.head(n - 1)).array() *
+         (2 * v.segment(0, n - 1).array() + v.segment(1, n - 1).array()) / 6.0;
+  d_ = y.head(n - 1);
+  base_keys_ = base_keys;
+}
+
+std::vector<double> SplineInterpolation::getSplineInterpolatedValues(
+  const std::vector<double> & query_keys) const
+{
+  // throw exceptions for invalid arguments
+  const auto validated_query_keys = autoware::interpolation::validateKeys(base_keys_, query_keys);
+  std::vector<double> interpolated_values;
+  interpolated_values.reserve(query_keys.size());
+
+  for (const auto & key : query_keys) {
+    const auto idx = get_index(key);
+    const auto dx = key - base_keys_[idx];
+    interpolated_values.emplace_back(
+      a_[idx] * dx * dx * dx + b_[idx] * dx * dx + c_[idx] * dx + d_[idx]);
+  }
+
+  return interpolated_values;
+}
+
+std::vector<double> SplineInterpolation::getSplineInterpolatedDiffValues(
+  const std::vector<double> & query_keys) const
+{
+  // throw exceptions for invalid arguments
+  const auto validated_query_keys = autoware::interpolation::validateKeys(base_keys_, query_keys);
+  std::vector<double> interpolated_diff_values;
+  interpolated_diff_values.reserve(query_keys.size());
+
+  for (const auto & key : query_keys) {
+    const auto idx = get_index(key);
+    const auto dx = key - base_keys_[idx];
+    interpolated_diff_values.emplace_back(3 * a_[idx] * dx * dx + 2 * b_[idx] * dx + c_[idx]);
+  }
+
+  return interpolated_diff_values;
+}
+
+std::vector<double> SplineInterpolation::getSplineInterpolatedQuadDiffValues(
+  const std::vector<double> & query_keys) const
+{
+  // throw exceptions for invalid arguments
+  const auto validated_query_keys = autoware::interpolation::validateKeys(base_keys_, query_keys);
+  std::vector<double> interpolated_quad_diff_values;
+  interpolated_quad_diff_values.reserve(query_keys.size());
+
+  for (const auto & key : query_keys) {
+    const auto idx = get_index(key);
+    const auto dx = key - base_keys_[idx];
+    interpolated_quad_diff_values.emplace_back(6 * a_[idx] * dx + 2 * b_[idx]);
+  }
+
+  return interpolated_quad_diff_values;
+}
+}  // namespace autoware::interpolation

master/src/universe/common/autoware_interpolation/src/spline_interpolation_points_2d.cpp

@@ -0,0 +1,212 @@
+// 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.
+
+#include "autoware/interpolation/spline_interpolation_points_2d.hpp"
+
+#include <vector>
+
+namespace autoware::interpolation
+{
+std::vector<double> calcEuclidDist(const std::vector<double> & x, const std::vector<double> & y)
+{
+  if (x.size() != y.size()) {
+    return std::vector<double>{};
+  }
+
+  std::vector<double> dist_v;
+  dist_v.push_back(0.0);
+  for (size_t i = 0; i < x.size() - 1; ++i) {
+    const double dx = x.at(i + 1) - x.at(i);
+    const double dy = y.at(i + 1) - y.at(i);
+    dist_v.push_back(dist_v.at(i) + std::hypot(dx, dy));
+  }
+
+  return dist_v;
+}
+
+std::array<std::vector<double>, 4> getBaseValues(
+  const std::vector<geometry_msgs::msg::Point> & points)
+{
+  // calculate x, y
+  std::vector<double> base_x;
+  std::vector<double> base_y;
+  std::vector<double> base_z;
+  for (size_t i = 0; i < points.size(); i++) {
+    const auto & current_pos = points.at(i);
+    if (i > 0) {
+      const auto & prev_pos = points.at(i - 1);
+      if (
+        std::fabs(current_pos.x - prev_pos.x) < 1e-6 &&
+        std::fabs(current_pos.y - prev_pos.y) < 1e-6) {
+        continue;
+      }
+    }
+    base_x.push_back(current_pos.x);
+    base_y.push_back(current_pos.y);
+    base_z.push_back(current_pos.z);
+  }
+
+  // calculate base_keys, base_values
+  if (base_x.size() < 2 || base_y.size() < 2 || base_z.size() < 2) {
+    throw std::logic_error("The number of unique points is not enough.");
+  }
+
+  const std::vector<double> base_s = calcEuclidDist(base_x, base_y);
+
+  return {base_s, base_x, base_y, base_z};
+}
+
+template <typename T>
+std::vector<double> splineYawFromPoints(const std::vector<T> & points)
+{
+  // calculate spline coefficients
+  SplineInterpolationPoints2d interpolator(points);
+
+  // interpolate base_keys at query_keys
+  std::vector<double> yaw_vec;
+  for (size_t i = 0; i < points.size(); ++i) {
+    const double yaw = interpolator.getSplineInterpolatedYaw(i, 0.0);
+    yaw_vec.push_back(yaw);
+  }
+  return yaw_vec;
+}
+template std::vector<double> splineYawFromPoints(
+  const std::vector<geometry_msgs::msg::Point> & points);
+
+geometry_msgs::msg::Pose SplineInterpolationPoints2d::getSplineInterpolatedPose(
+  const size_t idx, const double s) const
+{
+  geometry_msgs::msg::Pose pose;
+  pose.position = getSplineInterpolatedPoint(idx, s);
+  pose.orientation =
+    autoware::universe_utils::createQuaternionFromYaw(getSplineInterpolatedYaw(idx, s));
+  return pose;
+}
+
+geometry_msgs::msg::Point SplineInterpolationPoints2d::getSplineInterpolatedPoint(
+  const size_t idx, const double s) const
+{
+  if (base_s_vec_.size() <= idx) {
+    throw std::out_of_range("idx is out of range.");
+  }
+
+  double whole_s = base_s_vec_.at(idx) + s;
+  if (whole_s < base_s_vec_.front()) {
+    whole_s = base_s_vec_.front();
+  }
+  if (whole_s > base_s_vec_.back()) {
+    whole_s = base_s_vec_.back();
+  }
+
+  const double x = spline_x_.getSplineInterpolatedValues({whole_s}).at(0);
+  const double y = spline_y_.getSplineInterpolatedValues({whole_s}).at(0);
+  const double z = spline_z_.getSplineInterpolatedValues({whole_s}).at(0);
+
+  geometry_msgs::msg::Point geom_point;
+  geom_point.x = x;
+  geom_point.y = y;
+  geom_point.z = z;
+  return geom_point;
+}
+
+double SplineInterpolationPoints2d::getSplineInterpolatedYaw(const size_t idx, const double s) const
+{
+  if (base_s_vec_.size() <= idx) {
+    throw std::out_of_range("idx is out of range.");
+  }
+
+  const double whole_s =
+    std::clamp(base_s_vec_.at(idx) + s, base_s_vec_.front(), base_s_vec_.back());
+
+  const double diff_x = spline_x_.getSplineInterpolatedDiffValues({whole_s}).at(0);
+  const double diff_y = spline_y_.getSplineInterpolatedDiffValues({whole_s}).at(0);
+
+  return std::atan2(diff_y, diff_x);
+}
+
+std::vector<double> SplineInterpolationPoints2d::getSplineInterpolatedYaws() const
+{
+  std::vector<double> yaw_vec;
+  for (size_t i = 0; i < spline_x_.getSize(); ++i) {
+    const double yaw = getSplineInterpolatedYaw(i, 0.0);
+    yaw_vec.push_back(yaw);
+  }
+  return yaw_vec;
+}
+
+double SplineInterpolationPoints2d::getSplineInterpolatedCurvature(
+  const size_t idx, const double s) const
+{
+  if (base_s_vec_.size() <= idx) {
+    throw std::out_of_range("idx is out of range.");
+  }
+
+  const double whole_s =
+    std::clamp(base_s_vec_.at(idx) + s, base_s_vec_.front(), base_s_vec_.back());
+
+  const double diff_x = spline_x_.getSplineInterpolatedDiffValues({whole_s}).at(0);
+  const double diff_y = spline_y_.getSplineInterpolatedDiffValues({whole_s}).at(0);
+
+  const double quad_diff_x = spline_x_.getSplineInterpolatedQuadDiffValues({whole_s}).at(0);
+  const double quad_diff_y = spline_y_.getSplineInterpolatedQuadDiffValues({whole_s}).at(0);
+
+  return (diff_x * quad_diff_y - quad_diff_x * diff_y) /
+         std::pow(std::pow(diff_x, 2) + std::pow(diff_y, 2), 1.5);
+}
+
+std::vector<double> SplineInterpolationPoints2d::getSplineInterpolatedCurvatures() const
+{
+  std::vector<double> curvature_vec;
+  for (size_t i = 0; i < spline_x_.getSize(); ++i) {
+    const double curvature = getSplineInterpolatedCurvature(i, 0.0);
+    curvature_vec.push_back(curvature);
+  }
+  return curvature_vec;
+}
+
+size_t SplineInterpolationPoints2d::getOffsetIndex(const size_t idx, const double offset) const
+{
+  const double whole_s = base_s_vec_.at(idx) + offset;
+  for (size_t s_idx = 0; s_idx < base_s_vec_.size(); ++s_idx) {
+    if (whole_s < base_s_vec_.at(s_idx)) {
+      return s_idx;
+    }
+  }
+  return base_s_vec_.size() - 1;
+}
+
+double SplineInterpolationPoints2d::getAccumulatedLength(const size_t idx) const
+{
+  if (base_s_vec_.size() <= idx) {
+    throw std::out_of_range("idx is out of range.");
+  }
+  return base_s_vec_.at(idx);
+}
+
+void SplineInterpolationPoints2d::calcSplineCoefficientsInner(
+  const std::vector<geometry_msgs::msg::Point> & points)
+{
+  const auto base = getBaseValues(points);
+
+  base_s_vec_ = base.at(0);
+  const auto & base_x_vec = base.at(1);
+  const auto & base_y_vec = base.at(2);
+  const auto & base_z_vec = base.at(3);
+
+  // calculate spline coefficients
+  spline_x_ = SplineInterpolation(base_s_vec_, base_x_vec);
+  spline_y_ = SplineInterpolation(base_s_vec_, base_y_vec);
+  spline_z_ = SplineInterpolation(base_s_vec_, base_z_vec);
+}
+}  // namespace autoware::interpolation

master/src/universe/common/autoware_interpolation/test/src/test_interpolation.cpp

@@ -0,0 +1,21 @@
+// Copyright 2021 TierIV
+//
+// 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 <gtest/gtest.h>
+
+int main(int argc, char * argv[])
+{
+  testing::InitGoogleTest(&argc, argv);
+  return RUN_ALL_TESTS();
+}

master/src/universe/common/autoware_interpolation/test/src/test_interpolation_utils.cpp

@@ -0,0 +1,140 @@
+// 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.
+
+#include "autoware/interpolation/interpolation_utils.hpp"
+
+#include <gtest/gtest.h>
+
+#include <vector>
+
+constexpr double epsilon = 1e-6;
+
+TEST(interpolation_utils, isIncreasing)
+{
+  // empty
+  const std::vector<double> empty_vec;
+  EXPECT_THROW(autoware::interpolation::isIncreasing(empty_vec), std::invalid_argument);
+
+  // increase
+  const std::vector<double> increasing_vec{0.0, 1.5, 3.0, 4.5, 6.0};
+  EXPECT_EQ(autoware::interpolation::isIncreasing(increasing_vec), true);
+
+  // not decrease
+  const std::vector<double> not_increasing_vec{0.0, 1.5, 1.5, 4.5, 6.0};
+  EXPECT_EQ(autoware::interpolation::isIncreasing(not_increasing_vec), false);
+
+  // decrease
+  const std::vector<double> decreasing_vec{0.0, 1.5, 1.2, 4.5, 6.0};
+  EXPECT_EQ(autoware::interpolation::isIncreasing(decreasing_vec), false);
+}
+
+TEST(interpolation_utils, isNotDecreasing)
+{
+  // empty
+  const std::vector<double> empty_vec;
+  EXPECT_THROW(autoware::interpolation::isNotDecreasing(empty_vec), std::invalid_argument);
+
+  // increase
+  const std::vector<double> increasing_vec{0.0, 1.5, 3.0, 4.5, 6.0};
+  EXPECT_EQ(autoware::interpolation::isNotDecreasing(increasing_vec), true);
+
+  // not decrease
+  const std::vector<double> not_increasing_vec{0.0, 1.5, 1.5, 4.5, 6.0};
+  EXPECT_EQ(autoware::interpolation::isNotDecreasing(not_increasing_vec), true);
+
+  // decrease
+  const std::vector<double> decreasing_vec{0.0, 1.5, 1.2, 4.5, 6.0};
+  EXPECT_EQ(autoware::interpolation::isNotDecreasing(decreasing_vec), false);
+}
+
+TEST(interpolation_utils, validateKeys)
+{
+  using autoware::interpolation::validateKeys;
+
+  const std::vector<double> base_keys{0.0, 1.0, 2.0, 3.0};
+  const std::vector<double> query_keys{0.0, 1.0, 2.0, 3.0};
+
+  // valid
+  EXPECT_NO_THROW(validateKeys(base_keys, query_keys));
+
+  // empty
+  const std::vector<double> empty_vec;
+  EXPECT_THROW(validateKeys(empty_vec, query_keys), std::invalid_argument);
+  EXPECT_THROW(validateKeys(base_keys, empty_vec), std::invalid_argument);
+
+  // size is less than 2
+  const std::vector<double> short_vec{0.0};
+  EXPECT_THROW(validateKeys(short_vec, query_keys), std::invalid_argument);
+
+  // partly not increase
+  const std::vector<double> partly_not_increasing_vec{0.0, 0.0, 2.0, 3.0};
+  // NOTE: base_keys must be strictly monotonous increasing vector
+  EXPECT_THROW(validateKeys(partly_not_increasing_vec, query_keys), std::invalid_argument);
+  // NOTE: query_keys is allowed to be monotonous non-decreasing vector
+  EXPECT_NO_THROW(validateKeys(base_keys, partly_not_increasing_vec));
+
+  // decrease
+  const std::vector<double> decreasing_vec{0.0, -1.0, 2.0, 3.0};
+  EXPECT_THROW(validateKeys(decreasing_vec, query_keys), std::invalid_argument);
+  EXPECT_THROW(validateKeys(base_keys, decreasing_vec), std::invalid_argument);
+
+  // out of range
+  const std::vector<double> front_out_query_keys{-1.0, 1.0, 2.0, 3.0};
+  EXPECT_THROW(validateKeys(base_keys, front_out_query_keys), std::invalid_argument);
+
+  const std::vector<double> back_out_query_keys{0.0, 1.0, 2.0, 4.0};
+  EXPECT_THROW(validateKeys(base_keys, back_out_query_keys), std::invalid_argument);
+
+  {  // validated key check in normal case
+    const std::vector<double> normal_query_keys{0.5, 1.5, 3.0};
+    const auto validated_query_keys = validateKeys(base_keys, normal_query_keys);
+    for (size_t i = 0; i < normal_query_keys.size(); ++i) {
+      EXPECT_EQ(normal_query_keys.at(i), validated_query_keys.at(i));
+    }
+  }
+
+  {  // validated key check in case slightly out of range
+    constexpr double slightly_out_of_range_epsilon = 1e-6;
+    const std::vector<double> slightly_out_of_range__query_keys{
+      0.0 - slightly_out_of_range_epsilon, 3.0 + slightly_out_of_range_epsilon};
+    const auto validated_query_keys = validateKeys(base_keys, slightly_out_of_range__query_keys);
+    EXPECT_NEAR(validated_query_keys.at(0), 0.0, 1e-10);
+    EXPECT_NEAR(validated_query_keys.at(1), 3.0, 1e-10);
+  }
+}
+
+TEST(interpolation_utils, validateKeysAndValues)
+{
+  using autoware::interpolation::validateKeysAndValues;
+
+  const std::vector<double> base_keys{0.0, 1.0, 2.0, 3.0};
+  const std::vector<double> base_values{0.0, 1.0, 2.0, 3.0};
+
+  // valid
+  EXPECT_NO_THROW(validateKeysAndValues(base_keys, base_values));
+
+  // empty
+  const std::vector<double> empty_vec;
+  EXPECT_THROW(validateKeysAndValues(empty_vec, base_values), std::invalid_argument);
+  EXPECT_THROW(validateKeysAndValues(base_keys, empty_vec), std::invalid_argument);
+
+  // size is less than 2
+  const std::vector<double> short_vec{0.0};
+  EXPECT_THROW(validateKeysAndValues(short_vec, base_values), std::invalid_argument);
+  EXPECT_THROW(validateKeysAndValues(base_keys, short_vec), std::invalid_argument);
+
+  // size is different
+  const std::vector<double> different_size_base_values{0.0, 1.0, 2.0};
+  EXPECT_THROW(validateKeysAndValues(base_keys, different_size_base_values), std::invalid_argument);
+}

master/src/universe/common/autoware_interpolation/test/src/test_linear_interpolation.cpp

@@ -0,0 +1,95 @@
+// 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.
+
+#include "autoware/interpolation/linear_interpolation.hpp"
+
+#include <gtest/gtest.h>
+
+#include <limits>
+#include <vector>
+
+constexpr double epsilon = 1e-6;
+
+TEST(linear_interpolation, lerp_scalar)
+{
+  EXPECT_EQ(autoware::interpolation::lerp(0.0, 1.0, 0.3), 0.3);
+  EXPECT_EQ(autoware::interpolation::lerp(-0.5, 12.3, 0.3), 3.34);
+}
+
+TEST(linear_interpolation, lerp_vector)
+{
+  {  // straight: query_keys is same as base_keys
+    const std::vector<double> base_keys{0.0, 1.0, 2.0, 3.0, 4.0};
+    const std::vector<double> base_values{0.0, 1.5, 3.0, 4.5, 6.0};
+    const std::vector<double> query_keys = base_keys;
+    const std::vector<double> ans = base_values;
+
+    const auto query_values = autoware::interpolation::lerp(base_keys, base_values, query_keys);
+    for (size_t i = 0; i < query_values.size(); ++i) {
+      EXPECT_NEAR(query_values.at(i), ans.at(i), epsilon);
+    }
+  }
+
+  {  // straight: query_keys is random
+    const std::vector<double> base_keys{0.0, 1.0, 2.0, 3.0, 4.0};
+    const std::vector<double> base_values{0.0, 1.5, 3.0, 4.5, 6.0};
+    const std::vector<double> query_keys{0.0, 0.7, 1.9, 4.0};
+    const std::vector<double> ans{0.0, 1.05, 2.85, 6.0};
+
+    const auto query_values = autoware::interpolation::lerp(base_keys, base_values, query_keys);
+    for (size_t i = 0; i < query_values.size(); ++i) {
+      EXPECT_NEAR(query_values.at(i), ans.at(i), epsilon);
+    }
+  }
+
+  {  // curve: query_keys is same as base_keys
+    const std::vector<double> base_keys{-1.5, 1.0, 5.0, 10.0, 15.0, 20.0};
+    const std::vector<double> base_values{-1.2, 0.5, 1.0, 1.2, 2.0, 1.0};
+    const std::vector<double> query_keys = base_keys;
+    const std::vector<double> ans = base_values;
+
+    const auto query_values = autoware::interpolation::lerp(base_keys, base_values, query_keys);
+    for (size_t i = 0; i < query_values.size(); ++i) {
+      EXPECT_NEAR(query_values.at(i), ans.at(i), epsilon);
+    }
+  }
+
+  {  // curve: query_keys is same as random
+    const std::vector<double> base_keys{-1.5, 1.0, 5.0, 10.0, 15.0, 20.0};
+    const std::vector<double> base_values{-1.2, 0.5, 1.0, 1.2, 2.0, 1.0};
+    const std::vector<double> query_keys{0.0, 8.0, 18.0};
+    const std::vector<double> ans{-0.18, 1.12, 1.4};
+
+    const auto query_values = autoware::interpolation::lerp(base_keys, base_values, query_keys);
+    for (size_t i = 0; i < query_values.size(); ++i) {
+      EXPECT_NEAR(query_values.at(i), ans.at(i), epsilon);
+    }
+  }
+}
+
+TEST(linear_interpolation, lerp_scalar_query)
+{
+  {  // curve: query_keys is same as random
+    const std::vector<double> base_keys{-1.5, 1.0, 5.0, 10.0, 15.0, 20.0};
+    const std::vector<double> base_values{-1.2, 0.5, 1.0, 1.2, 2.0, 1.0};
+    const std::vector<double> query_keys{0.0, 8.0, 18.0};
+    const std::vector<double> ans{-0.18, 1.12, 1.4};
+
+    for (size_t i = 0; i < query_keys.size(); ++i) {
+      const auto query_value =
+        autoware::interpolation::lerp(base_keys, base_values, query_keys.at(i));
+      EXPECT_NEAR(query_value, ans.at(i), epsilon);
+    }
+  }
+}

master/src/universe/common/autoware_interpolation/test/src/test_spherical_linear_interpolation.cpp

@@ -0,0 +1,137 @@
+// 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.
+
+#include "autoware/interpolation/spherical_linear_interpolation.hpp"
+
+#include <gtest/gtest.h>
+
+#include <limits>
+#include <vector>
+
+constexpr double epsilon = 1e-6;
+
+namespace
+{
+inline geometry_msgs::msg::Quaternion createQuaternionFromRPY(
+  const double roll, const double pitch, const double yaw)
+{
+  tf2::Quaternion q;
+  q.setRPY(roll, pitch, yaw);
+  return tf2::toMsg(q);
+}
+}  // namespace
+
+TEST(slerp, spline_scalar)
+{
+  using autoware::interpolation::slerp;
+
+  // Same value
+  {
+    const double src_yaw = 0.0;
+    const double dst_yaw = 0.0;
+    const auto src_quat = createQuaternionFromRPY(0.0, 0.0, src_yaw);
+    const auto dst_quat = createQuaternionFromRPY(0.0, 0.0, dst_yaw);
+
+    const auto ans_quat = createQuaternionFromRPY(0.0, 0.0, 0.0);
+
+    for (double ratio = -2.0; ratio < 2.0 + epsilon; ratio += 0.1) {
+      const auto interpolated_quat = slerp(src_quat, dst_quat, ratio);
+      EXPECT_NEAR(ans_quat.x, interpolated_quat.x, epsilon);
+      EXPECT_NEAR(ans_quat.y, interpolated_quat.y, epsilon);
+      EXPECT_NEAR(ans_quat.z, interpolated_quat.z, epsilon);
+      EXPECT_NEAR(ans_quat.w, interpolated_quat.w, epsilon);
+    }
+  }
+
+  // Random Value
+  {
+    const double src_yaw = 0.0;
+    const double dst_yaw = M_PI;
+    const auto src_quat = createQuaternionFromRPY(0.0, 0.0, src_yaw);
+    const auto dst_quat = createQuaternionFromRPY(0.0, 0.0, dst_yaw);
+
+    for (double ratio = -2.0; ratio < 2.0 + epsilon; ratio += 0.1) {
+      const auto interpolated_quat = slerp(src_quat, dst_quat, ratio);
+
+      const double ans_yaw = M_PI * ratio;
+      tf2::Quaternion ans;
+      ans.setRPY(0, 0, ans_yaw);
+      const geometry_msgs::msg::Quaternion ans_quat = tf2::toMsg(ans);
+
+      EXPECT_NEAR(ans_quat.x, interpolated_quat.x, epsilon);
+      EXPECT_NEAR(ans_quat.y, interpolated_quat.y, epsilon);
+      EXPECT_NEAR(ans_quat.z, interpolated_quat.z, epsilon);
+      EXPECT_NEAR(ans_quat.w, interpolated_quat.w, epsilon);
+    }
+  }
+}
+
+TEST(slerp, spline_vector)
+{
+  using autoware::interpolation::slerp;
+
+  // query keys are same as base keys
+  {
+    const std::vector<double> base_keys{0.0, 1.0, 2.0, 3.0, 4.0};
+    std::vector<geometry_msgs::msg::Quaternion> base_values;
+    for (size_t i = 0; i < 5; ++i) {
+      const auto quat = createQuaternionFromRPY(0.0, 0.0, i * M_PI / 5.0);
+      base_values.push_back(quat);
+    }
+    const std::vector<double> query_keys = base_keys;
+    const auto ans = base_values;
+
+    const auto results = slerp(base_keys, base_values, query_keys);
+
+    for (size_t i = 0; i < results.size(); ++i) {
+      const auto interpolated_quat = results.at(i);
+      const auto ans_quat = ans.at(i);
+
+      EXPECT_NEAR(ans_quat.x, interpolated_quat.x, epsilon);
+      EXPECT_NEAR(ans_quat.y, interpolated_quat.y, epsilon);
+      EXPECT_NEAR(ans_quat.z, interpolated_quat.z, epsilon);
+      EXPECT_NEAR(ans_quat.w, interpolated_quat.w, epsilon);
+    }
+  }
+
+  // random
+  {
+    const std::vector<double> base_keys{0.0, 1.0, 2.0, 3.0, 4.0};
+    std::vector<geometry_msgs::msg::Quaternion> base_values;
+    for (size_t i = 0; i < 5; ++i) {
+      const auto quat = createQuaternionFromRPY(0.0, 0.0, i * M_PI / 5.0);
+      base_values.push_back(quat);
+    }
+    const std::vector<double> query_keys = {0.0, 0.1, 1.5, 2.6, 3.1, 3.8};
+    std::vector<geometry_msgs::msg::Quaternion> ans(query_keys.size());
+    ans.at(0) = createQuaternionFromRPY(0.0, 0.0, 0.0);
+    ans.at(1) = createQuaternionFromRPY(0.0, 0.0, 0.1 * M_PI / 5.0);
+    ans.at(2) = createQuaternionFromRPY(0.0, 0.0, 0.5 * M_PI / 5.0 + M_PI / 5.0);
+    ans.at(3) = createQuaternionFromRPY(0.0, 0.0, 0.6 * M_PI / 5.0 + 2.0 * M_PI / 5.0);
+    ans.at(4) = createQuaternionFromRPY(0.0, 0.0, 0.1 * M_PI / 5.0 + 3.0 * M_PI / 5.0);
+    ans.at(5) = createQuaternionFromRPY(0.0, 0.0, 0.8 * M_PI / 5.0 + 3.0 * M_PI / 5.0);
+
+    const auto results = slerp(base_keys, base_values, query_keys);
+
+    for (size_t i = 0; i < results.size(); ++i) {
+      const auto interpolated_quat = results.at(i);
+      const auto ans_quat = ans.at(i);
+
+      EXPECT_NEAR(ans_quat.x, interpolated_quat.x, epsilon);
+      EXPECT_NEAR(ans_quat.y, interpolated_quat.y, epsilon);
+      EXPECT_NEAR(ans_quat.z, interpolated_quat.z, epsilon);
+      EXPECT_NEAR(ans_quat.w, interpolated_quat.w, epsilon);
+    }
+  }
+}

master/src/universe/common/autoware_interpolation/test/src/test_spline_interpolation.cpp

@@ -0,0 +1,279 @@
+// 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.
+
+#include "autoware/interpolation/spline_interpolation.hpp"
+#include "autoware/universe_utils/geometry/geometry.hpp"
+
+#include <gtest/gtest.h>
+
+#include <limits>
+#include <vector>
+
+constexpr double epsilon = 1e-6;
+
+using autoware::interpolation::SplineInterpolation;
+
+TEST(spline_interpolation, spline)
+{
+  {  // straight: query_keys is same as base_keys
+    const std::vector<double> base_keys{0.0, 1.0, 2.0, 3.0, 4.0};
+    const std::vector<double> base_values{0.0, 1.5, 3.0, 4.5, 6.0};
+    const std::vector<double> query_keys = base_keys;
+    const std::vector<double> ans = base_values;
+
+    const auto query_values = autoware::interpolation::spline(base_keys, base_values, query_keys);
+    for (size_t i = 0; i < query_values.size(); ++i) {
+      EXPECT_NEAR(query_values.at(i), ans.at(i), epsilon);
+    }
+  }
+
+  {  // straight: query_keys is random
+    const std::vector<double> base_keys{0.0, 1.0, 2.0, 3.0, 4.0};
+    const std::vector<double> base_values{0.0, 1.5, 3.0, 4.5, 6.0};
+    const std::vector<double> query_keys{0.0, 0.7, 1.9, 4.0};
+    const std::vector<double> ans{0.0, 1.05, 2.85, 6.0};
+
+    const auto query_values = autoware::interpolation::spline(base_keys, base_values, query_keys);
+    for (size_t i = 0; i < query_values.size(); ++i) {
+      EXPECT_NEAR(query_values.at(i), ans.at(i), epsilon);
+    }
+  }
+
+  {  // curve: query_keys is same as base_keys
+    const std::vector<double> base_keys{-1.5, 1.0, 5.0, 10.0, 15.0, 20.0};
+    const std::vector<double> base_values{-1.2, 0.5, 1.0, 1.2, 2.0, 1.0};
+    const std::vector<double> query_keys = base_keys;
+    const std::vector<double> ans = base_values;
+
+    const auto query_values = autoware::interpolation::spline(base_keys, base_values, query_keys);
+    for (size_t i = 0; i < query_values.size(); ++i) {
+      EXPECT_NEAR(query_values.at(i), ans.at(i), epsilon);
+    }
+  }
+
+  {  // curve: query_keys is random
+    const std::vector<double> base_keys{-1.5, 1.0, 5.0, 10.0, 15.0, 20.0};
+    const std::vector<double> base_values{-1.2, 0.5, 1.0, 1.2, 2.0, 1.0};
+    const std::vector<double> query_keys{0.0, 8.0, 18.0};
+    const std::vector<double> ans{-0.076114, 1.001217, 1.573640};
+
+    const auto query_values = autoware::interpolation::spline(base_keys, base_values, query_keys);
+    for (size_t i = 0; i < query_values.size(); ++i) {
+      EXPECT_NEAR(query_values.at(i), ans.at(i), epsilon);
+    }
+  }
+
+  {  // straight: size of base_keys is 2 (edge case in the implementation)
+    const std::vector<double> base_keys{0.0, 1.0};
+    const std::vector<double> base_values{0.0, 1.5};
+    const std::vector<double> query_keys = base_keys;
+    const std::vector<double> ans = base_values;
+
+    const auto query_values = autoware::interpolation::spline(base_keys, base_values, query_keys);
+    for (size_t i = 0; i < query_values.size(); ++i) {
+      EXPECT_NEAR(query_values.at(i), ans.at(i), epsilon);
+    }
+  }
+
+  {  // straight: size of base_keys is 3 (edge case in the implementation)
+    const std::vector<double> base_keys{0.0, 1.0, 2.0};
+    const std::vector<double> base_values{0.0, 1.5, 3.0};
+    const std::vector<double> query_keys = base_keys;
+    const std::vector<double> ans = base_values;
+
+    const auto query_values = autoware::interpolation::spline(base_keys, base_values, query_keys);
+    for (size_t i = 0; i < query_values.size(); ++i) {
+      EXPECT_NEAR(query_values.at(i), ans.at(i), epsilon);
+    }
+  }
+
+  {  // curve: query_keys is random. size of base_keys is 3 (edge case in the implementation)
+    const std::vector<double> base_keys{-1.5, 1.0, 5.0};
+    const std::vector<double> base_values{-1.2, 0.5, 1.0};
+    const std::vector<double> query_keys{-1.0, 0.0, 4.0};
+    const std::vector<double> ans{-0.808769, -0.077539, 1.035096};
+
+    const auto query_values = autoware::interpolation::spline(base_keys, base_values, query_keys);
+    for (size_t i = 0; i < query_values.size(); ++i) {
+      EXPECT_NEAR(query_values.at(i), ans.at(i), epsilon);
+    }
+  }
+
+  {  // When the query keys changes suddenly (edge case of spline interpolation).
+    const std::vector<double> base_keys = {0.0, 1.0, 1.0001, 2.0, 3.0, 4.0};
+    const std::vector<double> base_values = {0.0, 0.0, 0.1, 0.1, 0.1, 0.1};
+    const std::vector<double> query_keys = {0.0, 1.0, 1.5, 2.0, 3.0, 4.0};
+    const std::vector<double> ans = {0.0, 0.0, 158.738293, 0.1, 0.1, 0.1};
+
+    const auto query_values = autoware::interpolation::spline(base_keys, base_values, query_keys);
+    for (size_t i = 0; i < query_values.size(); ++i) {
+      EXPECT_NEAR(query_values.at(i), ans.at(i), epsilon);
+    }
+  }
+}
+
+TEST(spline_interpolation, splineByAkima)
+{
+  {  // straight: query_keys is same as base_keys
+    const std::vector<double> base_keys{0.0, 1.0, 2.0, 3.0, 4.0};
+    const std::vector<double> base_values{0.0, 1.5, 3.0, 4.5, 6.0};
+    const std::vector<double> query_keys = base_keys;
+    const std::vector<double> ans = base_values;
+
+    const auto query_values =
+      autoware::interpolation::splineByAkima(base_keys, base_values, query_keys);
+    for (size_t i = 0; i < query_values.size(); ++i) {
+      EXPECT_NEAR(query_values.at(i), ans.at(i), epsilon);
+    }
+  }
+
+  {  // straight: query_keys is random
+    const std::vector<double> base_keys{0.0, 1.0, 2.0, 3.0, 4.0};
+    const std::vector<double> base_values{0.0, 1.5, 3.0, 4.5, 6.0};
+    const std::vector<double> query_keys{0.0, 0.7, 1.9, 4.0};
+    const std::vector<double> ans{0.0, 1.05, 2.85, 6.0};
+
+    const auto query_values =
+      autoware::interpolation::splineByAkima(base_keys, base_values, query_keys);
+    for (size_t i = 0; i < query_values.size(); ++i) {
+      EXPECT_NEAR(query_values.at(i), ans.at(i), epsilon);
+    }
+  }
+
+  {  // curve: query_keys is same as base_keys
+    const std::vector<double> base_keys{-1.5, 1.0, 5.0, 10.0, 15.0, 20.0};
+    const std::vector<double> base_values{-1.2, 0.5, 1.0, 1.2, 2.0, 1.0};
+    const std::vector<double> query_keys = base_keys;
+    const std::vector<double> ans = base_values;
+
+    const auto query_values =
+      autoware::interpolation::splineByAkima(base_keys, base_values, query_keys);
+    for (size_t i = 0; i < query_values.size(); ++i) {
+      EXPECT_NEAR(query_values.at(i), ans.at(i), epsilon);
+    }
+  }
+
+  {  // curve: query_keys is random
+    const std::vector<double> base_keys{-1.5, 1.0, 5.0, 10.0, 15.0, 20.0};
+    const std::vector<double> base_values{-1.2, 0.5, 1.0, 1.2, 2.0, 1.0};
+    const std::vector<double> query_keys{0.0, 8.0, 18.0};
+    const std::vector<double> ans{-0.0801, 1.110749, 1.4864};
+
+    const auto query_values =
+      autoware::interpolation::splineByAkima(base_keys, base_values, query_keys);
+    for (size_t i = 0; i < query_values.size(); ++i) {
+      EXPECT_NEAR(query_values.at(i), ans.at(i), epsilon);
+    }
+  }
+
+  {  // straight: size of base_keys is 2 (edge case in the implementation)
+    const std::vector<double> base_keys{0.0, 1.0};
+    const std::vector<double> base_values{0.0, 1.5};
+    const std::vector<double> query_keys = base_keys;
+    const std::vector<double> ans = base_values;
+
+    const auto query_values =
+      autoware::interpolation::splineByAkima(base_keys, base_values, query_keys);
+    for (size_t i = 0; i < query_values.size(); ++i) {
+      EXPECT_NEAR(query_values.at(i), ans.at(i), epsilon);
+    }
+  }
+
+  {  // straight: size of base_keys is 3 (edge case in the implementation)
+    const std::vector<double> base_keys{0.0, 1.0, 2.0};
+    const std::vector<double> base_values{0.0, 1.5, 3.0};
+    const std::vector<double> query_keys = base_keys;
+    const std::vector<double> ans = base_values;
+
+    const auto query_values =
+      autoware::interpolation::splineByAkima(base_keys, base_values, query_keys);
+    for (size_t i = 0; i < query_values.size(); ++i) {
+      EXPECT_NEAR(query_values.at(i), ans.at(i), epsilon);
+    }
+  }
+
+  {  // curve: query_keys is random. size of base_keys is 3 (edge case in the implementation)
+    const std::vector<double> base_keys{-1.5, 1.0, 5.0};
+    const std::vector<double> base_values{-1.2, 0.5, 1.0};
+    const std::vector<double> query_keys{-1.0, 0.0, 4.0};
+    const std::vector<double> ans{-0.8378, -0.0801, 0.927031};
+
+    const auto query_values =
+      autoware::interpolation::splineByAkima(base_keys, base_values, query_keys);
+    for (size_t i = 0; i < query_values.size(); ++i) {
+      EXPECT_NEAR(query_values.at(i), ans.at(i), epsilon);
+    }
+  }
+
+  {  // When the query keys changes suddenly (edge case of spline interpolation).
+    const std::vector<double> base_keys = {0.0, 1.0, 1.0001, 2.0, 3.0, 4.0};
+    const std::vector<double> base_values = {0.0, 0.0, 0.1, 0.1, 0.1, 0.1};
+    const std::vector<double> query_keys = {0.0, 1.0, 1.5, 2.0, 3.0, 4.0};
+    const std::vector<double> ans = {0.0, 0.0, 0.1, 0.1, 0.1, 0.1};
+
+    const auto query_values =
+      autoware::interpolation::splineByAkima(base_keys, base_values, query_keys);
+    for (size_t i = 0; i < query_values.size(); ++i) {
+      EXPECT_NEAR(query_values.at(i), ans.at(i), epsilon);
+    }
+  }
+}
+
+TEST(spline_interpolation, SplineInterpolation)
+{
+  {
+    // curve: query_keys is random
+    const std::vector<double> base_keys{-1.5, 1.0, 5.0, 10.0, 15.0, 20.0};
+    const std::vector<double> base_values{-1.2, 0.5, 1.0, 1.2, 2.0, 1.0};
+    const std::vector<double> query_keys{0.0, 8.0, 18.0};
+    const std::vector<double> ans{-0.076114, 1.001217, 1.573640};
+
+    SplineInterpolation s(base_keys, base_values);
+    const std::vector<double> query_values = s.getSplineInterpolatedValues(query_keys);
+
+    for (size_t i = 0; i < query_values.size(); ++i) {
+      EXPECT_NEAR(query_values.at(i), ans.at(i), epsilon);
+    }
+  }
+
+  {
+    // getSplineInterpolatedDiffValues
+    const std::vector<double> base_keys{-1.5, 1.0, 5.0, 10.0, 15.0, 20.0};
+    const std::vector<double> base_values{-1.2, 0.5, 1.0, 1.2, 2.0, 1.0};
+    const std::vector<double> query_keys{0.0, 8.0, 12.0, 18.0};
+    const std::vector<double> ans{0.671343, 0.049289, 0.209471, -0.253746};
+
+    SplineInterpolation s(base_keys, base_values);
+    const std::vector<double> query_values = s.getSplineInterpolatedDiffValues(query_keys);
+
+    for (size_t i = 0; i < query_values.size(); ++i) {
+      EXPECT_NEAR(query_values.at(i), ans.at(i), epsilon);
+    }
+  }
+
+  {
+    // getSplineInterpolatedQuadDiffValues
+    const std::vector<double> base_keys{-1.5, 1.0, 5.0, 10.0, 15.0, 20.0};
+    const std::vector<double> base_values{-1.2, 0.5, 1.0, 1.2, 2.0, 1.0};
+    const std::vector<double> query_keys{0.0, 8.0, 12.0, 18.0};
+    const std::vector<double> ans{-0.155829, 0.043097, -0.011143, -0.049611};
+
+    SplineInterpolation s(base_keys, base_values);
+    const std::vector<double> query_values = s.getSplineInterpolatedQuadDiffValues(query_keys);
+
+    for (size_t i = 0; i < query_values.size(); ++i) {
+      EXPECT_NEAR(query_values.at(i), ans.at(i), epsilon);
+    }
+  }
+}

master/src/universe/common/autoware_interpolation/test/src/test_spline_interpolation_points_2d.cpp

@@ -0,0 +1,223 @@
+// 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.
+
+#include "autoware/interpolation/spline_interpolation.hpp"
+#include "autoware/interpolation/spline_interpolation_points_2d.hpp"
+#include "autoware/universe_utils/geometry/geometry.hpp"
+
+#include <gtest/gtest.h>
+
+#include <limits>
+#include <vector>
+
+constexpr double epsilon = 1e-6;
+
+using autoware::interpolation::SplineInterpolationPoints2d;
+
+TEST(spline_interpolation, splineYawFromPoints)
+{
+  using autoware::universe_utils::createPoint;
+
+  {  // straight
+    std::vector<geometry_msgs::msg::Point> points;
+    points.push_back(createPoint(0.0, 0.0, 0.0));
+    points.push_back(createPoint(1.0, 1.5, 0.0));
+    points.push_back(createPoint(2.0, 3.0, 0.0));
+    points.push_back(createPoint(3.0, 4.5, 0.0));
+    points.push_back(createPoint(4.0, 6.0, 0.0));
+
+    const std::vector<double> ans{0.9827937, 0.9827937, 0.9827937, 0.9827937, 0.9827937};
+
+    const auto yaws = autoware::interpolation::splineYawFromPoints(points);
+    for (size_t i = 0; i < yaws.size(); ++i) {
+      EXPECT_NEAR(yaws.at(i), ans.at(i), epsilon);
+    }
+  }
+
+  {  // curve
+    std::vector<geometry_msgs::msg::Point> points;
+    points.push_back(createPoint(-2.0, -10.0, 0.0));
+    points.push_back(createPoint(2.0, 1.5, 0.0));
+    points.push_back(createPoint(3.0, 3.0, 0.0));
+    points.push_back(createPoint(5.0, 10.0, 0.0));
+    points.push_back(createPoint(10.0, 12.5, 0.0));
+
+    const std::vector<double> ans{1.368174, 0.961318, 1.086098, 0.938357, 0.278594};
+    const auto yaws = autoware::interpolation::splineYawFromPoints(points);
+    for (size_t i = 0; i < yaws.size(); ++i) {
+      EXPECT_NEAR(yaws.at(i), ans.at(i), epsilon);
+    }
+  }
+
+  {  // size of base_keys is 1 (infeasible to interpolate)
+    std::vector<geometry_msgs::msg::Point> points;
+    points.push_back(createPoint(1.0, 0.0, 0.0));
+
+    EXPECT_THROW(autoware::interpolation::splineYawFromPoints(points), std::logic_error);
+  }
+
+  {  // straight: size of base_keys is 2 (edge case in the implementation)
+    std::vector<geometry_msgs::msg::Point> points;
+    points.push_back(createPoint(1.0, 0.0, 0.0));
+    points.push_back(createPoint(2.0, 1.5, 0.0));
+
+    const std::vector<double> ans{0.9827937, 0.9827937};
+
+    const auto yaws = autoware::interpolation::splineYawFromPoints(points);
+    for (size_t i = 0; i < yaws.size(); ++i) {
+      EXPECT_NEAR(yaws.at(i), ans.at(i), epsilon);
+    }
+  }
+
+  {  // straight: size of base_keys is 3 (edge case in the implementation)
+    std::vector<geometry_msgs::msg::Point> points;
+    points.push_back(createPoint(1.0, 0.0, 0.0));
+    points.push_back(createPoint(2.0, 1.5, 0.0));
+    points.push_back(createPoint(3.0, 3.0, 0.0));
+
+    const std::vector<double> ans{0.9827937, 0.9827937, 0.9827937};
+
+    const auto yaws = autoware::interpolation::splineYawFromPoints(points);
+    for (size_t i = 0; i < yaws.size(); ++i) {
+      EXPECT_NEAR(yaws.at(i), ans.at(i), epsilon);
+    }
+  }
+}
+
+TEST(spline_interpolation, SplineInterpolationPoints2d)
+{
+  using autoware::universe_utils::createPoint;
+
+  // curve
+  std::vector<geometry_msgs::msg::Point> points;
+  points.push_back(createPoint(-2.0, -10.0, 0.0));
+  points.push_back(createPoint(2.0, 1.5, 0.0));
+  points.push_back(createPoint(3.0, 3.0, 0.0));
+  points.push_back(createPoint(5.0, 10.0, 0.0));
+  points.push_back(createPoint(10.0, 12.5, 0.0));
+
+  SplineInterpolationPoints2d s(points);
+
+  {  // point
+    // front
+    const auto front_point = s.getSplineInterpolatedPoint(0, 0.0);
+    EXPECT_NEAR(front_point.x, -2.0, epsilon);
+    EXPECT_NEAR(front_point.y, -10.0, epsilon);
+
+    // back
+    const auto back_point = s.getSplineInterpolatedPoint(4, 0.0);
+    EXPECT_NEAR(back_point.x, 10.0, epsilon);
+    EXPECT_NEAR(back_point.y, 12.5, epsilon);
+
+    // random
+    const auto random_point = s.getSplineInterpolatedPoint(3, 0.5);
+    EXPECT_NEAR(random_point.x, 5.28974, epsilon);
+    EXPECT_NEAR(random_point.y, 10.3450319, epsilon);
+
+    // out of range of total length
+    const auto front_out_point = s.getSplineInterpolatedPoint(0.0, -0.1);
+    EXPECT_NEAR(front_out_point.x, -2.0, epsilon);
+    EXPECT_NEAR(front_out_point.y, -10.0, epsilon);
+
+    const auto back_out_point = s.getSplineInterpolatedPoint(4.0, 0.1);
+    EXPECT_NEAR(back_out_point.x, 10.0, epsilon);
+    EXPECT_NEAR(back_out_point.y, 12.5, epsilon);
+
+    // out of range of index
+    EXPECT_THROW(s.getSplineInterpolatedPoint(-1, 0.0), std::out_of_range);
+    EXPECT_THROW(s.getSplineInterpolatedPoint(5, 0.0), std::out_of_range);
+  }
+
+  {  // yaw
+    // front
+    EXPECT_NEAR(s.getSplineInterpolatedYaw(0, 0.0), 1.368174, epsilon);
+
+    // back
+    EXPECT_NEAR(s.getSplineInterpolatedYaw(4, 0.0), 0.278594, epsilon);
+
+    // random
+    EXPECT_NEAR(s.getSplineInterpolatedYaw(3, 0.5), 0.808580, epsilon);
+
+    // out of range of total length
+    EXPECT_NEAR(s.getSplineInterpolatedYaw(0.0, -0.1), 1.368174, epsilon);
+    EXPECT_NEAR(s.getSplineInterpolatedYaw(4, 0.1), 0.278594, epsilon);
+
+    // out of range of index
+    EXPECT_THROW(s.getSplineInterpolatedYaw(-1, 0.0), std::out_of_range);
+    EXPECT_THROW(s.getSplineInterpolatedYaw(5, 0.0), std::out_of_range);
+  }
+
+  {  // curvature
+    // front
+    EXPECT_NEAR(s.getSplineInterpolatedCurvature(0, 0.0), 0.0, epsilon);
+
+    // back
+    EXPECT_NEAR(s.getSplineInterpolatedCurvature(4, 0.0), 0.0, epsilon);
+
+    // random
+    EXPECT_NEAR(s.getSplineInterpolatedCurvature(3, 0.5), -0.271073, epsilon);
+
+    // out of range of total length
+    EXPECT_NEAR(s.getSplineInterpolatedCurvature(0.0, -0.1), 0.0, epsilon);
+    EXPECT_NEAR(s.getSplineInterpolatedCurvature(4, 0.1), 0.0, epsilon);
+
+    // out of range of index
+    EXPECT_THROW(s.getSplineInterpolatedCurvature(-1, 0.0), std::out_of_range);
+    EXPECT_THROW(s.getSplineInterpolatedCurvature(5, 0.0), std::out_of_range);
+  }
+
+  {  // accumulated distance
+    // front
+    EXPECT_NEAR(s.getAccumulatedLength(0), 0.0, epsilon);
+
+    // back
+    EXPECT_NEAR(s.getAccumulatedLength(4), 26.8488511, epsilon);
+
+    // random
+    EXPECT_NEAR(s.getAccumulatedLength(3), 21.2586811, epsilon);
+
+    // out of range of index
+    EXPECT_THROW(s.getAccumulatedLength(-1), std::out_of_range);
+    EXPECT_THROW(s.getAccumulatedLength(5), std::out_of_range);
+  }
+
+  // size of base_keys is 1 (infeasible to interpolate)
+  std::vector<geometry_msgs::msg::Point> single_points;
+  single_points.push_back(createPoint(1.0, 0.0, 0.0));
+  EXPECT_THROW(SplineInterpolationPoints2d{single_points}, std::logic_error);
+}
+
+TEST(spline_interpolation, SplineInterpolationPoints2dPolymorphism)
+{
+  using autoware::universe_utils::createPoint;
+  using autoware_planning_msgs::msg::TrajectoryPoint;
+
+  std::vector<geometry_msgs::msg::Point> points;
+  points.push_back(createPoint(-2.0, -10.0, 0.0));
+  points.push_back(createPoint(2.0, 1.5, 0.0));
+  points.push_back(createPoint(3.0, 3.0, 0.0));
+
+  std::vector<TrajectoryPoint> trajectory_points;
+  for (const auto & p : points) {
+    TrajectoryPoint tp;
+    tp.pose.position = p;
+    trajectory_points.push_back(tp);
+  }
+
+  SplineInterpolationPoints2d s_point(points);
+  s_point.getSplineInterpolatedPoint(0, 0.);
+
+  SplineInterpolationPoints2d s_traj_point(trajectory_points);
+  s_traj_point.getSplineInterpolatedPoint(0, 0.);
+}

master/src/universe/common/autoware_interpolation/test/src/test_zero_order_hold.cpp

@@ -0,0 +1,158 @@
+// Copyright 2022 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.
+
+#include "autoware/interpolation/zero_order_hold.hpp"
+
+#include <gtest/gtest.h>
+
+#include <limits>
+#include <vector>
+
+constexpr double epsilon = 1e-6;
+
+TEST(zero_order_hold_interpolation, vector_interpolation)
+{
+  {  // straight: query_keys is same as base_keys
+    const std::vector<double> base_keys{0.0, 1.0, 2.0, 3.0, 4.0};
+    const std::vector<double> base_values{0.0, 1.5, 2.5, 3.5, 0.0};
+    const std::vector<double> query_keys = base_keys;
+    const std::vector<double> ans = base_values;
+
+    const auto query_values =
+      autoware::interpolation::zero_order_hold(base_keys, base_values, query_keys);
+    for (size_t i = 0; i < query_values.size(); ++i) {
+      EXPECT_NEAR(query_values.at(i), ans.at(i), epsilon);
+    }
+  }
+
+  {  // straight: query_keys is random
+    const std::vector<double> base_keys{0.0, 1.0, 2.0, 3.0, 4.0};
+    const std::vector<double> base_values{0.0, 1.5, 3.0, 4.5, 6.0};
+    const std::vector<double> query_keys{0.0, 0.7, 1.9, 4.0};
+    const std::vector<double> ans{0.0, 0.0, 1.5, 6.0};
+
+    const auto query_values =
+      autoware::interpolation::zero_order_hold(base_keys, base_values, query_keys);
+    for (size_t i = 0; i < query_values.size(); ++i) {
+      EXPECT_NEAR(query_values.at(i), ans.at(i), epsilon);
+    }
+  }
+
+  {  // curve: query_keys is same as base_keys
+    const std::vector<double> base_keys{-1.5, 1.0, 5.0, 10.0, 15.0, 20.0};
+    const std::vector<double> base_values{-1.2, 0.5, 1.0, 1.2, 2.0, 1.0};
+    const std::vector<double> query_keys = base_keys;
+    const std::vector<double> ans = base_values;
+
+    const auto query_values =
+      autoware::interpolation::zero_order_hold(base_keys, base_values, query_keys);
+    for (size_t i = 0; i < query_values.size(); ++i) {
+      EXPECT_NEAR(query_values.at(i), ans.at(i), epsilon);
+    }
+  }
+
+  {  // curve: query_keys is same as random
+    const std::vector<double> base_keys{-1.5, 1.0, 5.0, 10.0, 15.0, 20.0};
+    const std::vector<double> base_values{-1.2, 0.5, 1.0, 1.2, 2.0, 1.0};
+    const std::vector<double> query_keys{0.0, 8.0, 18.0};
+    const std::vector<double> ans{-1.2, 1.0, 2.0};
+
+    const auto query_values =
+      autoware::interpolation::zero_order_hold(base_keys, base_values, query_keys);
+    for (size_t i = 0; i < query_values.size(); ++i) {
+      EXPECT_NEAR(query_values.at(i), ans.at(i), epsilon);
+    }
+  }
+
+  {  // Boundary Condition
+    const std::vector<double> base_keys{0.0, 1.0, 2.0, 3.0, 4.0};
+    const std::vector<double> base_values{0.0, 1.5, 2.5, 3.5, 0.0};
+    const std::vector<double> query_keys{0.0, 1.0, 2.0, 3.0, 4.0 - 0.001};
+    const std::vector<double> ans = {0.0, 1.5, 2.5, 3.5, 3.5};
+
+    const auto query_values =
+      autoware::interpolation::zero_order_hold(base_keys, base_values, query_keys);
+    for (size_t i = 0; i < query_values.size(); ++i) {
+      EXPECT_NEAR(query_values.at(i), ans.at(i), epsilon);
+    }
+  }
+
+  {  // Boundary Condition
+    const std::vector<double> base_keys{0.0, 1.0, 2.0, 3.0, 4.0};
+    const std::vector<double> base_values{0.0, 1.5, 2.5, 3.5, 0.0};
+    const std::vector<double> query_keys{0.0, 1.0, 2.0, 3.0, 4.0 - 0.0001};
+    const std::vector<double> ans = {0.0, 1.5, 2.5, 3.5, 0.0};
+
+    const auto query_values =
+      autoware::interpolation::zero_order_hold(base_keys, base_values, query_keys);
+    for (size_t i = 0; i < query_values.size(); ++i) {
+      EXPECT_NEAR(query_values.at(i), ans.at(i), epsilon);
+    }
+  }
+}
+
+TEST(zero_order_hold_interpolation, vector_interpolation_no_double_interpolation)
+{
+  {  // straight: query_keys is same as base_keys
+    const std::vector<double> base_keys{0.0, 1.0, 2.0, 3.0, 4.0};
+    const std::vector<bool> base_values{true, true, false, true, true};
+    const std::vector<double> query_keys = base_keys;
+    const auto ans = base_values;
+
+    const auto query_values =
+      autoware::interpolation::zero_order_hold(base_keys, base_values, query_keys);
+    for (size_t i = 0; i < query_values.size(); ++i) {
+      EXPECT_EQ(query_values.at(i), ans.at(i));
+    }
+  }
+
+  {  // straight: query_keys is random
+    const std::vector<double> base_keys{0.0, 1.0, 2.0, 3.0, 4.0};
+    const std::vector<double> base_values{true, false, false, true, false};
+    const std::vector<double> query_keys{0.0, 0.7, 1.9, 4.0};
+    const std::vector<bool> ans = {true, true, false, false};
+
+    const auto query_values =
+      autoware::interpolation::zero_order_hold(base_keys, base_values, query_keys);
+    for (size_t i = 0; i < query_values.size(); ++i) {
+      EXPECT_EQ(query_values.at(i), ans.at(i));
+    }
+  }
+
+  {  // Boundary Condition
+    const std::vector<double> base_keys{0.0, 1.0, 2.0, 3.0, 4.0};
+    const std::vector<bool> base_values{true, true, false, true, false};
+    const std::vector<double> query_keys{0.0, 1.0, 2.0, 3.0, 4.0 - 0.001};
+    const std::vector<double> ans = {true, true, false, true, true};
+
+    const auto query_values =
+      autoware::interpolation::zero_order_hold(base_keys, base_values, query_keys);
+    for (size_t i = 0; i < query_values.size(); ++i) {
+      EXPECT_NEAR(query_values.at(i), ans.at(i), epsilon);
+    }
+  }
+
+  {  // Boundary Condition
+    const std::vector<double> base_keys{0.0, 1.0, 2.0, 3.0, 4.0};
+    const std::vector<double> base_values{true, false, true, true, false};
+    const std::vector<double> query_keys{0.0, 1.0, 2.0, 3.0, 4.0 - 0.0001};
+    const std::vector<double> ans = base_values;
+
+    const auto query_values =
+      autoware::interpolation::zero_order_hold(base_keys, base_values, query_keys);
+    for (size_t i = 0; i < query_values.size(); ++i) {
+      EXPECT_NEAR(query_values.at(i), ans.at(i), epsilon);
+    }
+  }
+}

master/src/universe/common/autoware_kalman_filter/CMakeLists.txt

@@ -0,0 +1,29 @@
+cmake_minimum_required(VERSION 3.14)
+project(autoware_kalman_filter)
+
+find_package(autoware_cmake REQUIRED)
+autoware_package()
+
+find_package(eigen3_cmake_module REQUIRED)
+find_package(Eigen3 REQUIRED)
+
+include_directories(
+  SYSTEM
+    ${EIGEN3_INCLUDE_DIR}
+)
+
+ament_auto_add_library(${PROJECT_NAME} SHARED
+  src/kalman_filter.cpp
+  src/time_delay_kalman_filter.cpp
+  include/autoware/kalman_filter/kalman_filter.hpp
+  include/autoware/kalman_filter/time_delay_kalman_filter.hpp
+)
+
+if(BUILD_TESTING)
+  file(GLOB_RECURSE test_files test/*.cpp)
+  ament_add_ros_isolated_gtest(test_${PROJECT_NAME} ${test_files})
+
+  target_link_libraries(test_${PROJECT_NAME} ${PROJECT_NAME})
+endif()
+
+ament_auto_package()

master/src/universe/common/autoware_kalman_filter/README.md

@@ -0,0 +1,9 @@
+# kalman_filter
+
+## Purpose
+
+This common package contains the kalman filter with time delay and the calculation of the kalman filter.
+
+## Assumptions / Known limits
+
+TBD.

master/src/universe/common/autoware_kalman_filter/include/autoware/kalman_filter/kalman_filter.hpp

@@ -0,0 +1,214 @@
+// Copyright 2018-2019 Autoware Foundation
+//
+// 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__KALMAN_FILTER__KALMAN_FILTER_HPP_
+#define AUTOWARE__KALMAN_FILTER__KALMAN_FILTER_HPP_
+
+#include <Eigen/Core>
+#include <Eigen/LU>
+
+namespace autoware::kalman_filter
+{
+
+/**
+ * @file kalman_filter.h
+ * @brief kalman filter class
+ * @author Takamasa Horibe
+ * @date 2019.05.01
+ */
+
+class KalmanFilter
+{
+public:
+  /**
+   * @brief No initialization constructor.
+   */
+  KalmanFilter();
+
+  /**
+   * @brief constructor with initialization
+   * @param x initial state
+   * @param A coefficient matrix of x for process model
+   * @param B coefficient matrix of u for process model
+   * @param C coefficient matrix of x for measurement model
+   * @param Q covariance matrix for process model
+   * @param R covariance matrix for measurement model
+   * @param P initial covariance of estimated state
+   */
+  KalmanFilter(
+    const Eigen::MatrixXd & x, const Eigen::MatrixXd & A, const Eigen::MatrixXd & B,
+    const Eigen::MatrixXd & C, const Eigen::MatrixXd & Q, const Eigen::MatrixXd & R,
+    const Eigen::MatrixXd & P);
+
+  /**
+   * @brief destructor
+   */
+  ~KalmanFilter();
+
+  /**
+   * @brief initialization of kalman filter
+   * @param x initial state
+   * @param A coefficient matrix of x for process model
+   * @param B coefficient matrix of u for process model
+   * @param C coefficient matrix of x for measurement model
+   * @param Q covariance matrix for process model
+   * @param R covariance matrix for measurement model
+   * @param P initial covariance of estimated state
+   */
+  bool init(
+    const Eigen::MatrixXd & x, const Eigen::MatrixXd & A, const Eigen::MatrixXd & B,
+    const Eigen::MatrixXd & C, const Eigen::MatrixXd & Q, const Eigen::MatrixXd & R,
+    const Eigen::MatrixXd & P);
+
+  /**
+   * @brief initialization of kalman filter
+   * @param x initial state
+   * @param P initial covariance of estimated state
+   */
+  bool init(const Eigen::MatrixXd & x, const Eigen::MatrixXd & P0);
+
+  /**
+   * @brief set A of process model
+   * @param A coefficient matrix of x for process model
+   */
+  void setA(const Eigen::MatrixXd & A);
+
+  /**
+   * @brief set B of process model
+   * @param B coefficient matrix of u for process model
+   */
+  void setB(const Eigen::MatrixXd & B);
+
+  /**
+   * @brief set C of measurement model
+   * @param C coefficient matrix of x for measurement model
+   */
+  void setC(const Eigen::MatrixXd & C);
+
+  /**
+   * @brief set covariance matrix Q for process model
+   * @param Q covariance matrix for process model
+   */
+  void setQ(const Eigen::MatrixXd & Q);
+
+  /**
+   * @brief set covariance matrix R for measurement model
+   * @param R covariance matrix for measurement model
+   */
+  void setR(const Eigen::MatrixXd & R);
+
+  /**
+   * @brief get current kalman filter state
+   * @param x kalman filter state
+   */
+  void getX(Eigen::MatrixXd & x) const;
+
+  /**
+   * @brief get current kalman filter covariance
+   * @param P kalman filter covariance
+   */
+  void getP(Eigen::MatrixXd & P) const;
+
+  /**
+   * @brief get component of current kalman filter state
+   * @param i index of kalman filter state
+   * @return value of i's component of the kalman filter state x[i]
+   */
+  double getXelement(unsigned int i) const;
+
+  /**
+   * @brief calculate kalman filter state and covariance by prediction model with A, B, Q matrix.
+   * This is mainly for EKF with variable matrix.
+   * @param u input for model
+   * @param A coefficient matrix of x for process model
+   * @param B coefficient matrix of u for process model
+   * @param Q covariance matrix for process model
+   * @return bool to check matrix operations are being performed properly
+   */
+  bool predict(
+    const Eigen::MatrixXd & u, const Eigen::MatrixXd & A, const Eigen::MatrixXd & B,
+    const Eigen::MatrixXd & Q);
+
+  /**
+   * @brief calculate kalman filter covariance with prediction model with x, A, Q matrix. This is
+   * mainly for EKF with variable matrix.
+   * @param x_next predicted state
+   * @param A coefficient matrix of x for process model
+   * @param Q covariance matrix for process model
+   * @return bool to check matrix operations are being performed properly
+   */
+  bool predict(
+    const Eigen::MatrixXd & x_next, const Eigen::MatrixXd & A, const Eigen::MatrixXd & Q);
+
+  /**
+   * @brief calculate kalman filter covariance with prediction model with x, A, Q matrix. This is
+   * mainly for EKF with variable matrix.
+   * @param x_next predicted state
+   * @param A coefficient matrix of x for process model
+   * @return bool to check matrix operations are being performed properly
+   */
+  bool predict(const Eigen::MatrixXd & x_next, const Eigen::MatrixXd & A);
+
+  /**
+   * @brief calculate kalman filter state by prediction model with A, B and Q being class member
+   * variables.
+   * @param u input for the model
+   * @return bool to check matrix operations are being performed properly
+   */
+  bool predict(const Eigen::MatrixXd & u);
+
+  /**
+   * @brief calculate kalman filter state by measurement model with y_pred, C and R matrix. This is
+   * mainly for EKF with variable matrix.
+   * @param y measured values
+   * @param y output values expected from measurement model
+   * @param C coefficient matrix of x for measurement model
+   * @param R covariance matrix for measurement model
+   * @return bool to check matrix operations are being performed properly
+   */
+  bool update(
+    const Eigen::MatrixXd & y, const Eigen::MatrixXd & y_pred, const Eigen::MatrixXd & C,
+    const Eigen::MatrixXd & R);
+
+  /**
+   * @brief calculate kalman filter state by measurement model with C and R matrix. This is mainly
+   * for EKF with variable matrix.
+   * @param y measured values
+   * @param C coefficient matrix of x for measurement model
+   * @param R covariance matrix for measurement model
+   * @return bool to check matrix operations are being performed properly
+   */
+  bool update(const Eigen::MatrixXd & y, const Eigen::MatrixXd & C, const Eigen::MatrixXd & R);
+
+  /**
+   * @brief calculate kalman filter state by measurement model with C and R being class member
+   * variables.
+   * @param y measured values
+   * @return bool to check matrix operations are being performed properly
+   */
+  bool update(const Eigen::MatrixXd & y);
+
+protected:
+  Eigen::MatrixXd x_;  //!< @brief current estimated state
+  Eigen::MatrixXd
+    A_;  //!< @brief coefficient matrix of x for process model x[k+1] = A*x[k] + B*u[k]
+  Eigen::MatrixXd
+    B_;  //!< @brief coefficient matrix of u for process model x[k+1] = A*x[k] + B*u[k]
+  Eigen::MatrixXd C_;  //!< @brief coefficient matrix of x for measurement model y[k] = C * x[k]
+  Eigen::MatrixXd Q_;  //!< @brief covariance matrix for process model x[k+1] = A*x[k] + B*u[k]
+  Eigen::MatrixXd R_;  //!< @brief covariance matrix for measurement model y[k] = C * x[k]
+  Eigen::MatrixXd P_;  //!< @brief covariance of estimated state
+};
+}  // namespace autoware::kalman_filter
+#endif  // AUTOWARE__KALMAN_FILTER__KALMAN_FILTER_HPP_

master/src/universe/common/autoware_kalman_filter/include/autoware/kalman_filter/time_delay_kalman_filter.hpp

@@ -0,0 +1,89 @@
+// Copyright 2018-2019 Autoware Foundation
+//
+// 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__KALMAN_FILTER__TIME_DELAY_KALMAN_FILTER_HPP_
+#define AUTOWARE__KALMAN_FILTER__TIME_DELAY_KALMAN_FILTER_HPP_
+
+#include "autoware/kalman_filter/kalman_filter.hpp"
+
+#include <Eigen/Core>
+#include <Eigen/LU>
+
+#include <iostream>
+
+namespace autoware::kalman_filter
+{
+/**
+ * @file time_delay_kalman_filter.h
+ * @brief kalman filter with delayed measurement class
+ * @author Takamasa Horibe
+ * @date 2019.05.01
+ */
+
+class TimeDelayKalmanFilter : public KalmanFilter
+{
+public:
+  /**
+   * @brief No initialization constructor.
+   */
+  TimeDelayKalmanFilter();
+
+  /**
+   * @brief initialization of kalman filter
+   * @param x initial state
+   * @param P0 initial covariance of estimated state
+   * @param max_delay_step Maximum number of delay steps, which determines the dimension of the
+   * extended kalman filter
+   */
+  void init(const Eigen::MatrixXd & x, const Eigen::MatrixXd & P, const int max_delay_step);
+
+  /**
+   * @brief get latest time estimated state
+   */
+  Eigen::MatrixXd getLatestX() const;
+
+  /**
+   * @brief get latest time estimation covariance
+   */
+  Eigen::MatrixXd getLatestP() const;
+
+  /**
+   * @brief calculate kalman filter covariance by precision model with time delay. This is mainly
+   * for EKF of nonlinear process model.
+   * @param x_next predicted state by prediction model
+   * @param A coefficient matrix of x for process model
+   * @param Q covariance matrix for process model
+   */
+  bool predictWithDelay(
+    const Eigen::MatrixXd & x_next, const Eigen::MatrixXd & A, const Eigen::MatrixXd & Q);
+
+  /**
+   * @brief calculate kalman filter covariance by measurement model with time delay. This is mainly
+   * for EKF of nonlinear process model.
+   * @param y measured values
+   * @param C coefficient matrix of x for measurement model
+   * @param R covariance matrix for measurement model
+   * @param delay_step measurement delay
+   */
+  bool updateWithDelay(
+    const Eigen::MatrixXd & y, const Eigen::MatrixXd & C, const Eigen::MatrixXd & R,
+    const int delay_step);
+
+private:
+  int max_delay_step_;  //!< @brief maximum number of delay steps
+  int dim_x_;           //!< @brief dimension of latest state
+  int dim_x_ex_;        //!< @brief dimension of extended state with dime delay
+};
+}  // namespace autoware::kalman_filter
+#endif  // AUTOWARE__KALMAN_FILTER__TIME_DELAY_KALMAN_FILTER_HPP_

master/src/universe/common/autoware_kalman_filter/package.xml

@@ -0,0 +1,28 @@
+<?xml version="1.0"?>
+<?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
+<package format="3">
+  <name>autoware_kalman_filter</name>
+  <version>0.1.0</version>
+  <description>The kalman filter package</description>
+  <maintainer email="yukihiro.saito@tier4.jp">Yukihiro Saito</maintainer>
+  <maintainer email="takeshi.ishita@tier4.jp">Takeshi Ishita</maintainer>
+  <maintainer email="koji.minoda@tier4.jp">Koji Minoda</maintainer>
+  <license>Apache License 2.0</license>
+
+  <author email="takamasa.horibe@tier4.jp">Takamasa Horibe</author>
+
+  <buildtool_depend>ament_cmake_auto</buildtool_depend>
+  <buildtool_depend>autoware_cmake</buildtool_depend>
+
+  <build_depend>eigen</build_depend>
+  <build_depend>eigen3_cmake_module</build_depend>
+
+  <test_depend>ament_cmake_cppcheck</test_depend>
+  <test_depend>ament_cmake_ros</test_depend>
+  <test_depend>ament_lint_auto</test_depend>
+  <test_depend>autoware_lint_common</test_depend>
+
+  <export>
+    <build_type>ament_cmake</build_type>
+  </export>
+</package>

master/src/universe/common/autoware_kalman_filter/src/kalman_filter.cpp

@@ -0,0 +1,161 @@
+// Copyright 2018-2019 Autoware Foundation
+//
+// 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/kalman_filter/kalman_filter.hpp"
+
+namespace autoware::kalman_filter
+{
+KalmanFilter::KalmanFilter()
+{
+}
+KalmanFilter::KalmanFilter(
+  const Eigen::MatrixXd & x, const Eigen::MatrixXd & A, const Eigen::MatrixXd & B,
+  const Eigen::MatrixXd & C, const Eigen::MatrixXd & Q, const Eigen::MatrixXd & R,
+  const Eigen::MatrixXd & P)
+{
+  init(x, A, B, C, Q, R, P);
+}
+KalmanFilter::~KalmanFilter()
+{
+}
+bool KalmanFilter::init(
+  const Eigen::MatrixXd & x, const Eigen::MatrixXd & A, const Eigen::MatrixXd & B,
+  const Eigen::MatrixXd & C, const Eigen::MatrixXd & Q, const Eigen::MatrixXd & R,
+  const Eigen::MatrixXd & P)
+{
+  if (
+    x.cols() == 0 || x.rows() == 0 || A.cols() == 0 || A.rows() == 0 || B.cols() == 0 ||
+    B.rows() == 0 || C.cols() == 0 || C.rows() == 0 || Q.cols() == 0 || Q.rows() == 0 ||
+    R.cols() == 0 || R.rows() == 0 || P.cols() == 0 || P.rows() == 0) {
+    return false;
+  }
+  x_ = x;
+  A_ = A;
+  B_ = B;
+  C_ = C;
+  Q_ = Q;
+  R_ = R;
+  P_ = P;
+  return true;
+}
+bool KalmanFilter::init(const Eigen::MatrixXd & x, const Eigen::MatrixXd & P0)
+{
+  if (x.cols() == 0 || x.rows() == 0 || P0.cols() == 0 || P0.rows() == 0) {
+    return false;
+  }
+  x_ = x;
+  P_ = P0;
+  return true;
+}
+
+void KalmanFilter::setA(const Eigen::MatrixXd & A)
+{
+  A_ = A;
+}
+void KalmanFilter::setB(const Eigen::MatrixXd & B)
+{
+  B_ = B;
+}
+void KalmanFilter::setC(const Eigen::MatrixXd & C)
+{
+  C_ = C;
+}
+void KalmanFilter::setQ(const Eigen::MatrixXd & Q)
+{
+  Q_ = Q;
+}
+void KalmanFilter::setR(const Eigen::MatrixXd & R)
+{
+  R_ = R;
+}
+void KalmanFilter::getX(Eigen::MatrixXd & x) const
+{
+  x = x_;
+}
+void KalmanFilter::getP(Eigen::MatrixXd & P) const
+{
+  P = P_;
+}
+double KalmanFilter::getXelement(unsigned int i) const
+{
+  return x_(i);
+}
+
+bool KalmanFilter::predict(
+  const Eigen::MatrixXd & x_next, const Eigen::MatrixXd & A, const Eigen::MatrixXd & Q)
+{
+  if (
+    x_.rows() != x_next.rows() || A.cols() != P_.rows() || Q.cols() != Q.rows() ||
+    A.rows() != Q.cols()) {
+    return false;
+  }
+  x_ = x_next;
+  P_ = A * P_ * A.transpose() + Q;
+  return true;
+}
+bool KalmanFilter::predict(const Eigen::MatrixXd & x_next, const Eigen::MatrixXd & A)
+{
+  return predict(x_next, A, Q_);
+}
+
+bool KalmanFilter::predict(
+  const Eigen::MatrixXd & u, const Eigen::MatrixXd & A, const Eigen::MatrixXd & B,
+  const Eigen::MatrixXd & Q)
+{
+  if (A.cols() != x_.rows() || B.cols() != u.rows()) {
+    return false;
+  }
+  const Eigen::MatrixXd x_next = A * x_ + B * u;
+  return predict(x_next, A, Q);
+}
+bool KalmanFilter::predict(const Eigen::MatrixXd & u)
+{
+  return predict(u, A_, B_, Q_);
+}
+
+bool KalmanFilter::update(
+  const Eigen::MatrixXd & y, const Eigen::MatrixXd & y_pred, const Eigen::MatrixXd & C,
+  const Eigen::MatrixXd & R)
+{
+  if (
+    P_.cols() != C.cols() || R.rows() != R.cols() || R.rows() != C.rows() ||
+    y.rows() != y_pred.rows() || y.rows() != C.rows()) {
+    return false;
+  }
+  const Eigen::MatrixXd PCT = P_ * C.transpose();
+  const Eigen::MatrixXd K = PCT * ((R + C * PCT).inverse());
+
+  if (isnan(K.array()).any() || isinf(K.array()).any()) {
+    return false;
+  }
+
+  x_ = x_ + K * (y - y_pred);
+  P_ = P_ - K * (C * P_);
+  return true;
+}
+
+bool KalmanFilter::update(
+  const Eigen::MatrixXd & y, const Eigen::MatrixXd & C, const Eigen::MatrixXd & R)
+{
+  if (C.cols() != x_.rows()) {
+    return false;
+  }
+  const Eigen::MatrixXd y_pred = C * x_;
+  return update(y, y_pred, C, R);
+}
+bool KalmanFilter::update(const Eigen::MatrixXd & y)
+{
+  return update(y, C_, R_);
+}
+}  // namespace autoware::kalman_filter

master/src/universe/common/autoware_kalman_filter/src/time_delay_kalman_filter.cpp

@@ -0,0 +1,107 @@
+// Copyright 2018-2019 Autoware Foundation
+//
+// 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/kalman_filter/time_delay_kalman_filter.hpp"
+
+namespace autoware::kalman_filter
+{
+TimeDelayKalmanFilter::TimeDelayKalmanFilter()
+{
+}
+
+void TimeDelayKalmanFilter::init(
+  const Eigen::MatrixXd & x, const Eigen::MatrixXd & P0, const int max_delay_step)
+{
+  max_delay_step_ = max_delay_step;
+  dim_x_ = x.rows();
+  dim_x_ex_ = dim_x_ * max_delay_step;
+
+  x_ = Eigen::MatrixXd::Zero(dim_x_ex_, 1);
+  P_ = Eigen::MatrixXd::Zero(dim_x_ex_, dim_x_ex_);
+
+  for (int i = 0; i < max_delay_step_; ++i) {
+    x_.block(i * dim_x_, 0, dim_x_, 1) = x;
+    P_.block(i * dim_x_, i * dim_x_, dim_x_, dim_x_) = P0;
+  }
+}
+
+Eigen::MatrixXd TimeDelayKalmanFilter::getLatestX() const
+{
+  return x_.block(0, 0, dim_x_, 1);
+}
+
+Eigen::MatrixXd TimeDelayKalmanFilter::getLatestP() const
+{
+  return P_.block(0, 0, dim_x_, dim_x_);
+}
+
+bool TimeDelayKalmanFilter::predictWithDelay(
+  const Eigen::MatrixXd & x_next, const Eigen::MatrixXd & A, const Eigen::MatrixXd & Q)
+{
+  /*
+   * time delay model:
+   *
+   *     [A   0   0]      [P11   P12   P13]      [Q   0   0]
+   * A = [I   0   0], P = [P21   P22   P23], Q = [0   0   0]
+   *     [0   I   0]      [P31   P32   P33]      [0   0   0]
+   *
+   * covariance calculation in prediction : P = A * P * A' + Q
+   *
+   *     [A*P11*A'*+Q  A*P11  A*P12]
+   * P = [     P11*A'    P11    P12]
+   *     [     P21*A'    P21    P22]
+   */
+
+  const int d_dim_x = dim_x_ex_ - dim_x_;
+
+  /* slide states in the time direction */
+  Eigen::MatrixXd x_tmp = Eigen::MatrixXd::Zero(dim_x_ex_, 1);
+  x_tmp.block(0, 0, dim_x_, 1) = x_next;
+  x_tmp.block(dim_x_, 0, d_dim_x, 1) = x_.block(0, 0, d_dim_x, 1);
+  x_ = x_tmp;
+
+  /* update P with delayed measurement A matrix structure */
+  Eigen::MatrixXd P_tmp = Eigen::MatrixXd::Zero(dim_x_ex_, dim_x_ex_);
+  P_tmp.block(0, 0, dim_x_, dim_x_) = A * P_.block(0, 0, dim_x_, dim_x_) * A.transpose() + Q;
+  P_tmp.block(0, dim_x_, dim_x_, d_dim_x) = A * P_.block(0, 0, dim_x_, d_dim_x);
+  P_tmp.block(dim_x_, 0, d_dim_x, dim_x_) = P_.block(0, 0, d_dim_x, dim_x_) * A.transpose();
+  P_tmp.block(dim_x_, dim_x_, d_dim_x, d_dim_x) = P_.block(0, 0, d_dim_x, d_dim_x);
+  P_ = P_tmp;
+
+  return true;
+}
+
+bool TimeDelayKalmanFilter::updateWithDelay(
+  const Eigen::MatrixXd & y, const Eigen::MatrixXd & C, const Eigen::MatrixXd & R,
+  const int delay_step)
+{
+  if (delay_step >= max_delay_step_) {
+    std::cerr << "delay step is larger than max_delay_step. ignore update." << std::endl;
+    return false;
+  }
+
+  const int dim_y = y.rows();
+
+  /* set measurement matrix */
+  Eigen::MatrixXd C_ex = Eigen::MatrixXd::Zero(dim_y, dim_x_ex_);
+  C_ex.block(0, dim_x_ * delay_step, dim_y, dim_x_) = C;
+
+  /* update */
+  if (!update(y, C_ex, R)) {
+    return false;
+  }
+
+  return true;
+}
+}  // namespace autoware::kalman_filter

master/src/universe/common/autoware_kalman_filter/test/test_kalman_filter.cpp

@@ -0,0 +1,96 @@
+// Copyright 2023 The Autoware Foundation
+//
+// 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/kalman_filter/kalman_filter.hpp"
+
+#include <gtest/gtest.h>
+
+using autoware::kalman_filter::KalmanFilter;
+
+TEST(kalman_filter, kf)
+{
+  KalmanFilter kf_;
+
+  Eigen::MatrixXd x_t(2, 1);
+  x_t << 1, 2;
+
+  Eigen::MatrixXd P_t(2, 2);
+  P_t << 1, 0, 0, 1;
+
+  Eigen::MatrixXd Q_t(2, 2);
+  Q_t << 0.01, 0, 0, 0.01;
+
+  Eigen::MatrixXd R_t(2, 2);
+  R_t << 0.09, 0, 0, 0.09;
+
+  Eigen::MatrixXd C_t(2, 2);
+  C_t << 1, 0, 0, 1;
+
+  Eigen::MatrixXd A_t(2, 2);
+  A_t << 1, 0, 0, 1;
+
+  Eigen::MatrixXd B_t(2, 2);
+  B_t << 1, 0, 0, 1;
+
+  // Initialize the filter and check if initialization was successful
+  EXPECT_TRUE(kf_.init(x_t, A_t, B_t, C_t, Q_t, R_t, P_t));
+
+  // Perform prediction
+  Eigen::MatrixXd u_t(2, 1);
+  u_t << 0.1, 0.1;
+  EXPECT_TRUE(kf_.predict(u_t));
+
+  // Check the updated state and covariance matrix
+  Eigen::MatrixXd x_predict_expected = A_t * x_t + B_t * u_t;
+  Eigen::MatrixXd P_predict_expected = A_t * P_t * A_t.transpose() + Q_t;
+
+  Eigen::MatrixXd x_predict;
+  kf_.getX(x_predict);
+  Eigen::MatrixXd P_predict;
+  kf_.getP(P_predict);
+
+  EXPECT_NEAR(x_predict(0, 0), x_predict_expected(0, 0), 1e-5);
+  EXPECT_NEAR(x_predict(1, 0), x_predict_expected(1, 0), 1e-5);
+  EXPECT_NEAR(P_predict(0, 0), P_predict_expected(0, 0), 1e-5);
+  EXPECT_NEAR(P_predict(1, 1), P_predict_expected(1, 1), 1e-5);
+
+  // Perform update
+  Eigen::MatrixXd y_t(2, 1);
+  y_t << 1.05, 2.05;
+  EXPECT_TRUE(kf_.update(y_t));
+
+  // Check the updated state and covariance matrix
+  const Eigen::MatrixXd PCT_t = P_predict_expected * C_t.transpose();
+  const Eigen::MatrixXd K_t = PCT_t * ((R_t + C_t * PCT_t).inverse());
+  const Eigen::MatrixXd y_pred = C_t * x_predict_expected;
+  Eigen::MatrixXd x_update_expected = x_predict_expected + K_t * (y_t - y_pred);
+  Eigen::MatrixXd P_update_expected = P_predict_expected - K_t * (C_t * P_predict_expected);
+
+  Eigen::MatrixXd x_update;
+  kf_.getX(x_update);
+  Eigen::MatrixXd P_update;
+  kf_.getP(P_update);
+
+  EXPECT_NEAR(x_update(0, 0), x_update_expected(0, 0), 1e-5);
+  EXPECT_NEAR(x_update(1, 0), x_update_expected(1, 0), 1e-5);
+  EXPECT_NEAR(P_update(0, 0), P_update_expected(0, 0), 1e-5);
+  EXPECT_NEAR(P_update(1, 1), P_update_expected(1, 1), 1e-5);
+}
+
+int main(int argc, char * argv[])
+{
+  testing::InitGoogleTest(&argc, argv);
+  bool result = RUN_ALL_TESTS();
+  return result;
+}

master/src/universe/common/autoware_kalman_filter/test/test_time_delay_kalman_filter.cpp

@@ -0,0 +1,123 @@
+// Copyright 2023 The Autoware Foundation
+//
+// 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/kalman_filter/time_delay_kalman_filter.hpp"
+
+#include <gtest/gtest.h>
+
+using autoware::kalman_filter::TimeDelayKalmanFilter;
+
+TEST(time_delay_kalman_filter, td_kf)
+{
+  TimeDelayKalmanFilter td_kf_;
+
+  Eigen::MatrixXd x_t(3, 1);
+  x_t << 1.0, 2.0, 3.0;
+  Eigen::MatrixXd P_t(3, 3);
+  P_t << 0.1, 0.0, 0.0, 0.0, 0.2, 0.0, 0.0, 0.0, 0.3;
+  const int max_delay_step = 5;
+  const int dim_x = x_t.rows();
+  const int dim_x_ex = dim_x * max_delay_step;
+  // Initialize the filter
+  td_kf_.init(x_t, P_t, max_delay_step);
+
+  // Check if initialization was successful
+  Eigen::MatrixXd x_init = td_kf_.getLatestX();
+  Eigen::MatrixXd P_init = td_kf_.getLatestP();
+  Eigen::MatrixXd x_ex_t = Eigen::MatrixXd::Zero(dim_x_ex, 1);
+  Eigen::MatrixXd P_ex_t = Eigen::MatrixXd::Zero(dim_x_ex, dim_x_ex);
+  for (int i = 0; i < max_delay_step; ++i) {
+    x_ex_t.block(i * dim_x, 0, dim_x, 1) = x_t;
+    P_ex_t.block(i * dim_x, i * dim_x, dim_x, dim_x) = P_t;
+  }
+
+  EXPECT_EQ(x_init.rows(), 3);
+  EXPECT_EQ(x_init.cols(), 1);
+  EXPECT_EQ(P_init.rows(), 3);
+  EXPECT_EQ(P_init.cols(), 3);
+  EXPECT_NEAR(x_init(0, 0), x_t(0, 0), 1e-5);
+  EXPECT_NEAR(x_init(1, 0), x_t(1, 0), 1e-5);
+  EXPECT_NEAR(x_init(2, 0), x_t(2, 0), 1e-5);
+  EXPECT_NEAR(P_init(0, 0), P_t(0, 0), 1e-5);
+  EXPECT_NEAR(P_init(1, 1), P_t(1, 1), 1e-5);
+  EXPECT_NEAR(P_init(2, 2), P_t(2, 2), 1e-5);
+
+  // Define prediction parameters
+  Eigen::MatrixXd A_t(3, 3);
+  A_t << 2.0, 0.0, 0.0, 0.0, 2.0, 0.0, 0.0, 0.0, 2.0;
+  Eigen::MatrixXd Q_t(3, 3);
+  Q_t << 0.01, 0.0, 0.0, 0.0, 0.02, 0.0, 0.0, 0.0, 0.03;
+  Eigen::MatrixXd x_next(3, 1);
+  x_next << 2.0, 4.0, 6.0;
+
+  // Perform prediction
+  EXPECT_TRUE(td_kf_.predictWithDelay(x_next, A_t, Q_t));
+
+  // Check the prediction state and covariance matrix
+  Eigen::MatrixXd x_predict = td_kf_.getLatestX();
+  Eigen::MatrixXd P_predict = td_kf_.getLatestP();
+  Eigen::MatrixXd x_tmp = Eigen::MatrixXd::Zero(dim_x_ex, 1);
+  x_tmp.block(0, 0, dim_x, 1) = A_t * x_t;
+  x_tmp.block(dim_x, 0, dim_x_ex - dim_x, 1) = x_ex_t.block(0, 0, dim_x_ex - dim_x, 1);
+  x_ex_t = x_tmp;
+  Eigen::MatrixXd x_predict_expected = x_ex_t.block(0, 0, dim_x, 1);
+  Eigen::MatrixXd P_tmp = Eigen::MatrixXd::Zero(dim_x_ex, dim_x_ex);
+  P_tmp.block(0, 0, dim_x, dim_x) = A_t * P_ex_t.block(0, 0, dim_x, dim_x) * A_t.transpose() + Q_t;
+  P_tmp.block(0, dim_x, dim_x, dim_x_ex - dim_x) =
+    A_t * P_ex_t.block(0, 0, dim_x, dim_x_ex - dim_x);
+  P_tmp.block(dim_x, 0, dim_x_ex - dim_x, dim_x) =
+    P_ex_t.block(0, 0, dim_x_ex - dim_x, dim_x) * A_t.transpose();
+  P_tmp.block(dim_x, dim_x, dim_x_ex - dim_x, dim_x_ex - dim_x) =
+    P_ex_t.block(0, 0, dim_x_ex - dim_x, dim_x_ex - dim_x);
+  P_ex_t = P_tmp;
+  Eigen::MatrixXd P_predict_expected = P_ex_t.block(0, 0, dim_x, dim_x);
+  EXPECT_NEAR(x_predict(0, 0), x_predict_expected(0, 0), 1e-5);
+  EXPECT_NEAR(x_predict(1, 0), x_predict_expected(1, 0), 1e-5);
+  EXPECT_NEAR(x_predict(2, 0), x_predict_expected(2, 0), 1e-5);
+  EXPECT_NEAR(P_predict(0, 0), P_predict_expected(0, 0), 1e-5);
+  EXPECT_NEAR(P_predict(1, 1), P_predict_expected(1, 1), 1e-5);
+  EXPECT_NEAR(P_predict(2, 2), P_predict_expected(2, 2), 1e-5);
+
+  // Define update parameters
+  Eigen::MatrixXd C_t(3, 3);
+  C_t << 0.5, 0.0, 0.0, 0.0, 0.5, 0.0, 0.0, 0.0, 0.5;
+  Eigen::MatrixXd R_t(3, 3);
+  R_t << 0.001, 0.0, 0.0, 0.0, 0.002, 0.0, 0.0, 0.0, 0.003;
+  Eigen::MatrixXd y_t(3, 1);
+  y_t << 1.05, 2.05, 3.05;
+  const int delay_step = 2;  // Choose an appropriate delay step
+  const int dim_y = y_t.rows();
+
+  // Perform update
+  EXPECT_TRUE(td_kf_.updateWithDelay(y_t, C_t, R_t, delay_step));
+
+  // Check the updated state and covariance matrix
+  Eigen::MatrixXd x_update = td_kf_.getLatestX();
+  Eigen::MatrixXd P_update = td_kf_.getLatestP();
+
+  Eigen::MatrixXd C_ex_t = Eigen::MatrixXd::Zero(dim_y, dim_x_ex);
+  const Eigen::MatrixXd PCT_t = P_ex_t * C_ex_t.transpose();
+  const Eigen::MatrixXd K_t = PCT_t * ((R_t + C_ex_t * PCT_t).inverse());
+  const Eigen::MatrixXd y_pred = C_ex_t * x_ex_t;
+  x_ex_t = x_ex_t + K_t * (y_t - y_pred);
+  P_ex_t = P_ex_t - K_t * (C_ex_t * P_ex_t);
+  Eigen::MatrixXd x_update_expected = x_ex_t.block(0, 0, dim_x, 1);
+  Eigen::MatrixXd P_update_expected = P_ex_t.block(0, 0, dim_x, dim_x);
+  EXPECT_NEAR(x_update(0, 0), x_update_expected(0, 0), 1e-5);
+  EXPECT_NEAR(x_update(1, 0), x_update_expected(1, 0), 1e-5);
+  EXPECT_NEAR(x_update(2, 0), x_update_expected(2, 0), 1e-5);
+  EXPECT_NEAR(P_update(0, 0), P_update_expected(0, 0), 1e-5);
+  EXPECT_NEAR(P_update(1, 1), P_update_expected(1, 1), 1e-5);
+  EXPECT_NEAR(P_update(2, 2), P_update_expected(2, 2), 1e-5);
+}

master/src/universe/common/autoware_motion_utils/CMakeLists.txt

@@ -0,0 +1,51 @@
+cmake_minimum_required(VERSION 3.14)
+project(autoware_motion_utils)
+
+option(BUILD_EXAMPLES "Build examples" OFF)
+
+find_package(autoware_cmake REQUIRED)
+autoware_package()
+
+find_package(Boost REQUIRED)
+
+ament_auto_add_library(autoware_motion_utils SHARED
+  DIRECTORY src
+)
+
+if(BUILD_TESTING)
+  find_package(ament_cmake_ros REQUIRED)
+
+  file(GLOB_RECURSE test_files test/**/*.cpp)
+
+  ament_add_ros_isolated_gtest(test_autoware_motion_utils ${test_files})
+
+  target_link_libraries(test_autoware_motion_utils
+    autoware_motion_utils
+  )
+endif()
+
+if(BUILD_EXAMPLES)
+  message(STATUS "Building examples")
+
+  include(FetchContent)
+  fetchcontent_declare(
+    matplotlibcpp17
+    GIT_REPOSITORY https://github.com/soblin/matplotlibcpp17.git
+    GIT_TAG master
+  )
+  fetchcontent_makeavailable(matplotlibcpp17)
+
+  file(GLOB_RECURSE example_files examples/*.cpp)
+
+  foreach(example_file ${example_files})
+    get_filename_component(example_name ${example_file} NAME_WE)
+    ament_auto_add_executable(${example_name} ${example_file})
+    set_source_files_properties(${example_file} PROPERTIES COMPILE_FLAGS -Wno-error -Wno-attributes -Wno-unused-parameter)
+    target_link_libraries(${example_name}
+      autoware_motion_utils
+      matplotlibcpp17::matplotlibcpp17
+    )
+  endforeach()
+endif()
+
+ament_auto_package()

master/src/universe/common/autoware_motion_utils/README.md

@@ -0,0 +1,104 @@
+# Motion Utils package
+
+## Definition of terms
+
+### Segment
+
+`Segment` in Autoware is the line segment between two successive points as follows.
+
+![segment](./media/segment.svg){: style="width:600px"}
+
+The nearest segment index and nearest point index to a certain position is not always th same.
+Therefore, we prepare two different utility functions to calculate a nearest index for points and segments.
+
+## Nearest index search
+
+In this section, the nearest index and nearest segment index search is explained.
+
+We have the same functions for the nearest index search and nearest segment index search.
+Taking for the example the nearest index search, we have two types of functions.
+
+The first function finds the nearest index with distance and yaw thresholds.
+
+```cpp
+template <class T>
+size_t findFirstNearestIndexWithSoftConstraints(
+  const T & points, const geometry_msgs::msg::Pose & pose,
+  const double dist_threshold = std::numeric_limits<double>::max(),
+  const double yaw_threshold = std::numeric_limits<double>::max());
+```
+
+This function finds the first local solution within thresholds.
+The reason to find the first local one is to deal with some edge cases explained in the next subsection.
+
+There are default parameters for thresholds arguments so that you can decide which thresholds to pass to the function.
+
+1. When both the distance and yaw thresholds are given.
+   - First, try to find the nearest index with both the distance and yaw thresholds.
+   - If not found, try to find again with only the distance threshold.
+   - If not found, find without any thresholds.
+2. When only distance are given.
+   - First, try to find the nearest index the distance threshold.
+   - If not found, find without any thresholds.
+3. When no thresholds are given.
+   - Find the nearest index.
+
+The second function finds the nearest index in the lane whose id is `lane_id`.
+
+```cpp
+size_t findNearestIndexFromLaneId(
+  const tier4_planning_msgs::msg::PathWithLaneId & path,
+  const geometry_msgs::msg::Point & pos, const int64_t lane_id);
+```
+
+### Application to various object
+
+Many node packages often calculate the nearest index of objects.
+We will explain the recommended method to calculate it.
+
+#### Nearest index for the ego
+
+Assuming that the path length before the ego is short enough, we expect to find the correct nearest index in the following edge cases by `findFirstNearestIndexWithSoftConstraints` with both distance and yaw thresholds.
+Blue circles describes the distance threshold from the base link position and two blue lines describe the yaw threshold against the base link orientation.
+Among points in these cases, the correct nearest point which is red can be found.
+
+![ego_nearest_search](./media/ego_nearest_search.svg)
+
+Therefore, the implementation is as follows.
+
+```cpp
+const size_t ego_nearest_idx = findFirstNearestIndexWithSoftConstraints(points, ego_pose, ego_nearest_dist_threshold, ego_nearest_yaw_threshold);
+const size_t ego_nearest_seg_idx = findFirstNearestIndexWithSoftConstraints(points, ego_pose, ego_nearest_dist_threshold, ego_nearest_yaw_threshold);
+```
+
+#### Nearest index for dynamic objects
+
+For the ego nearest index, the orientation is considered in addition to the position since the ego is supposed to follow the points.
+However, for the dynamic objects (e.g., predicted object), sometimes its orientation may be different from the points order, e.g. the dynamic object driving backward although the ego is driving forward.
+
+Therefore, the yaw threshold should not be considered for the dynamic object.
+The implementation is as follows.
+
+```cpp
+const size_t dynamic_obj_nearest_idx = findFirstNearestIndexWithSoftConstraints(points, dynamic_obj_pose, dynamic_obj_nearest_dist_threshold);
+const size_t dynamic_obj_nearest_seg_idx = findFirstNearestIndexWithSoftConstraints(points, dynamic_obj_pose, dynamic_obj_nearest_dist_threshold);
+```
+
+#### Nearest index for traffic objects
+
+In lanelet maps, traffic objects belong to the specific lane.
+With this specific lane's id, the correct nearest index can be found.
+
+The implementation is as follows.
+
+```cpp
+// first extract `lane_id` which the traffic object belong to.
+const size_t traffic_obj_nearest_idx = findNearestIndexFromLaneId(path_with_lane_id, traffic_obj_pos, lane_id);
+const size_t traffic_obj_nearest_seg_idx = findNearestSegmentIndexFromLaneId(path_with_lane_id, traffic_obj_pos, lane_id);
+```
+
+## For developers
+
+Some of the template functions in `trajectory.hpp` are mostly used for specific types (`autoware_planning_msgs::msg::PathPoint`, `autoware_planning_msgs::msg::PathPoint`, `autoware_planning_msgs::msg::TrajectoryPoint`), so they are exported as `extern template` functions to speed-up compilation time.
+
+`autoware_motion_utils.hpp` header file was removed because the source files that directly/indirectly include this file took a long time for preprocessing.

master/src/universe/common/autoware_motion_utils/docs/vehicle/vehicle.md

@@ -0,0 +1,169 @@
+# vehicle utils
+
+Vehicle utils provides a convenient library used to check vehicle status.
+
+## Feature
+
+The library contains following classes.
+
+### vehicle_stop_checker
+
+This class check whether the vehicle is stopped or not based on localization result.
+
+#### Subscribed Topics
+
+| Name                            | Type                      | Description      |
+| ------------------------------- | ------------------------- | ---------------- |
+| `/localization/kinematic_state` | `nav_msgs::msg::Odometry` | vehicle odometry |
+
+#### Parameters
+
+| Name                       | Type   | Default Value | Explanation                 |
+| -------------------------- | ------ | ------------- | --------------------------- |
+| `velocity_buffer_time_sec` | double | 10.0          | odometry buffering time [s] |
+
+#### Member functions
+
+```c++
+bool isVehicleStopped(const double stop_duration)
+```
+
+- Check simply whether the vehicle is stopped based on the localization result.
+- Returns `true` if the vehicle is stopped, even if system outputs a non-zero target velocity.
+
+#### Example Usage
+
+Necessary includes:
+
+```c++
+#include <autoware/universe_utils/vehicle/vehicle_state_checker.hpp>
+```
+
+1.Create a checker instance.
+
+```c++
+class SampleNode : public rclcpp::Node
+{
+public:
+  SampleNode() : Node("sample_node")
+  {
+    vehicle_stop_checker_ = std::make_unique<VehicleStopChecker>(this);
+  }
+
+  std::unique_ptr<VehicleStopChecker> vehicle_stop_checker_;
+
+  bool sampleFunc();
+
+  ...
+}
+```
+
+2.Check the vehicle state.
+
+```c++
+
+bool SampleNode::sampleFunc()
+{
+  ...
+
+  const auto result_1 = vehicle_stop_checker_->isVehicleStopped();
+
+  ...
+
+  const auto result_2 = vehicle_stop_checker_->isVehicleStopped(3.0);
+
+  ...
+}
+
+```
+
+### vehicle_arrival_checker
+
+This class check whether the vehicle arrive at stop point based on localization and planning result.
+
+#### Subscribed Topics
+
+| Name                                     | Type                                      | Description      |
+| ---------------------------------------- | ----------------------------------------- | ---------------- |
+| `/localization/kinematic_state`          | `nav_msgs::msg::Odometry`                 | vehicle odometry |
+| `/planning/scenario_planning/trajectory` | `autoware_planning_msgs::msg::Trajectory` | trajectory       |
+
+#### Parameters
+
+| Name                       | Type   | Default Value | Explanation                                                            |
+| -------------------------- | ------ | ------------- | ---------------------------------------------------------------------- |
+| `velocity_buffer_time_sec` | double | 10.0          | odometry buffering time [s]                                            |
+| `th_arrived_distance_m`    | double | 1.0           | threshold distance to check if vehicle has arrived at target point [m] |
+
+#### Member functions
+
+```c++
+bool isVehicleStopped(const double stop_duration)
+```
+
+- Check simply whether the vehicle is stopped based on the localization result.
+- Returns `true` if the vehicle is stopped, even if system outputs a non-zero target velocity.
+
+```c++
+bool isVehicleStoppedAtStopPoint(const double stop_duration)
+```
+
+- Check whether the vehicle is stopped at stop point based on the localization and planning result.
+- Returns `true` if the vehicle is not only stopped but also arrived at stop point.
+
+#### Example Usage
+
+Necessary includes:
+
+```c++
+#include <autoware/universe_utils/vehicle/vehicle_state_checker.hpp>
+```
+
+1.Create a checker instance.
+
+```c++
+class SampleNode : public rclcpp::Node
+{
+public:
+  SampleNode() : Node("sample_node")
+  {
+    vehicle_arrival_checker_ = std::make_unique<VehicleArrivalChecker>(this);
+  }
+
+  std::unique_ptr<VehicleArrivalChecker> vehicle_arrival_checker_;
+
+  bool sampleFunc();
+
+  ...
+}
+```
+
+2.Check the vehicle state.
+
+```c++
+
+bool SampleNode::sampleFunc()
+{
+  ...
+
+  const auto result_1 = vehicle_arrival_checker_->isVehicleStopped();
+
+  ...
+
+  const auto result_2 = vehicle_arrival_checker_->isVehicleStopped(3.0);
+
+  ...
+
+  const auto result_3 = vehicle_arrival_checker_->isVehicleStoppedAtStopPoint();
+
+  ...
+
+  const auto result_4 = vehicle_arrival_checker_->isVehicleStoppedAtStopPoint(3.0);
+
+  ...
+}
+```
+
+## Assumptions / Known limits
+
+`vehicle_stop_checker` and `vehicle_arrival_checker` cannot check whether the vehicle is stopped more than `velocity_buffer_time_sec` second.

master/src/universe/common/autoware_motion_utils/examples/example_interpolator.cpp

@@ -0,0 +1,116 @@
+// Copyright 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.
+
+#include "autoware/motion_utils/trajectory_container/interpolator/akima_spline.hpp"
+#include "autoware/motion_utils/trajectory_container/interpolator/cubic_spline.hpp"
+#include "autoware/motion_utils/trajectory_container/interpolator/interpolator.hpp"
+#include "autoware/motion_utils/trajectory_container/interpolator/linear.hpp"
+#include "autoware/motion_utils/trajectory_container/interpolator/nearest_neighbor.hpp"
+
+#include <autoware/motion_utils/trajectory_container/interpolator.hpp>
+
+#include <matplotlibcpp17/pyplot.h>
+
+#include <random>
+#include <vector>
+
+int main()
+{
+  pybind11::scoped_interpreter guard{};
+  auto plt = matplotlibcpp17::pyplot::import();
+
+  // create random values
+  std::vector<double> bases = {0.0, 1.0, 2.0, 3.0, 4.0, 5.0, 6.0, 7.0, 8.0};
+  std::vector<double> values;
+  std::random_device seed_gen;
+  std::mt19937 engine(seed_gen());
+  std::uniform_real_distribution<> dist(-1.0, 1.0);
+  for (size_t i = 0; i < bases.size(); ++i) {
+    values.push_back(dist(engine));
+  }
+  // Scatter Data
+  plt.scatter(Args(bases, values));
+
+  using autoware::motion_utils::trajectory_container::interpolator::InterpolatorInterface;
+  // Linear Interpolator
+  {
+    using autoware::motion_utils::trajectory_container::interpolator::Linear;
+    auto interpolator = *Linear::Builder{}.set_bases(bases).set_values(values).build();
+    std::vector<double> x;
+    std::vector<double> y;
+    for (double i = bases.front(); i < bases.back(); i += 0.01) {
+      x.push_back(i);
+      y.push_back(interpolator.compute(i));
+    }
+    plt.plot(Args(x, y), Kwargs("label"_a = "Linear"));
+  }
+
+  // AkimaSpline Interpolator
+  {
+    using autoware::motion_utils::trajectory_container::interpolator::AkimaSpline;
+
+    auto interpolator = *AkimaSpline::Builder{}.set_bases(bases).set_values(values).build();
+    std::vector<double> x;
+    std::vector<double> y;
+    for (double i = bases.front(); i < bases.back(); i += 0.01) {
+      x.push_back(i);
+      y.push_back(interpolator.compute(i));
+    }
+    plt.plot(Args(x, y), Kwargs("label"_a = "AkimaSpline"));
+  }
+
+  // CubicSpline Interpolator
+  {
+    using autoware::motion_utils::trajectory_container::interpolator::CubicSpline;
+    auto interpolator = *CubicSpline::Builder{}.set_bases(bases).set_values(values).build();
+    std::vector<double> x;
+    std::vector<double> y;
+    for (double i = bases.front(); i < bases.back(); i += 0.01) {
+      x.push_back(i);
+      y.push_back(interpolator.compute(i));
+    }
+    plt.plot(Args(x, y), Kwargs("label"_a = "CubicSpline"));
+  }
+
+  // NearestNeighbor Interpolator
+  {
+    using autoware::motion_utils::trajectory_container::interpolator::NearestNeighbor;
+    auto interpolator =
+      *NearestNeighbor<double>::Builder{}.set_bases(bases).set_values(values).build();
+    std::vector<double> x;
+    std::vector<double> y;
+    for (double i = bases.front(); i < bases.back(); i += 0.01) {
+      x.push_back(i);
+      y.push_back(interpolator.compute(i));
+    }
+    plt.plot(Args(x, y), Kwargs("label"_a = "NearestNeighbor"));
+  }
+
+  // Stairstep Interpolator
+  {
+    using autoware::motion_utils::trajectory_container::interpolator::Stairstep;
+    auto interpolator = *Stairstep<double>::Builder{}.set_bases(bases).set_values(values).build();
+    std::vector<double> x;
+    std::vector<double> y;
+    for (double i = bases.front(); i < bases.back(); i += 0.01) {
+      x.push_back(i);
+      y.push_back(interpolator.compute(i));
+    }
+    plt.plot(Args(x, y), Kwargs("label"_a = "Stairstep"));
+  }
+
+  plt.legend();
+  plt.show();
+  return 0;
+}

master/src/universe/common/autoware_motion_utils/include/autoware/motion_utils/constants.hpp

@@ -0,0 +1,23 @@
+// Copyright 2022 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__MOTION_UTILS__CONSTANTS_HPP_
+#define AUTOWARE__MOTION_UTILS__CONSTANTS_HPP_
+
+namespace autoware::motion_utils
+{
+constexpr double overlap_threshold = 0.1;
+}  // namespace autoware::motion_utils
+
+#endif  // AUTOWARE__MOTION_UTILS__CONSTANTS_HPP_

master/src/universe/common/autoware_motion_utils/include/autoware/motion_utils/distance/distance.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__MOTION_UTILS__DISTANCE__DISTANCE_HPP_
+#define AUTOWARE__MOTION_UTILS__DISTANCE__DISTANCE_HPP_
+
+#include <algorithm>
+#include <cmath>
+#include <iostream>
+#include <optional>
+#include <tuple>
+#include <vector>
+
+namespace autoware::motion_utils
+{
+std::optional<double> calcDecelDistWithJerkAndAccConstraints(
+  const double current_vel, const double target_vel, const double current_acc, const double acc_min,
+  const double jerk_acc, const double jerk_dec);
+
+}  // namespace autoware::motion_utils
+
+#endif  // AUTOWARE__MOTION_UTILS__DISTANCE__DISTANCE_HPP_

master/src/universe/common/autoware_motion_utils/include/autoware/motion_utils/factor/velocity_factor_interface.hpp

@@ -0,0 +1,54 @@
+
+// Copyright 2022-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__MOTION_UTILS__FACTOR__VELOCITY_FACTOR_INTERFACE_HPP_
+#define AUTOWARE__MOTION_UTILS__FACTOR__VELOCITY_FACTOR_INTERFACE_HPP_
+
+#include <autoware_adapi_v1_msgs/msg/planning_behavior.hpp>
+#include <autoware_adapi_v1_msgs/msg/velocity_factor.hpp>
+#include <autoware_adapi_v1_msgs/msg/velocity_factor_array.hpp>
+#include <geometry_msgs/msg/pose.hpp>
+
+#include <string>
+#include <vector>
+
+namespace autoware::motion_utils
+{
+using autoware_adapi_v1_msgs::msg::PlanningBehavior;
+using autoware_adapi_v1_msgs::msg::VelocityFactor;
+using VelocityFactorBehavior = VelocityFactor::_behavior_type;
+using VelocityFactorStatus = VelocityFactor::_status_type;
+using geometry_msgs::msg::Pose;
+
+class VelocityFactorInterface
+{
+public:
+  [[nodiscard]] VelocityFactor get() const { return velocity_factor_; }
+  void init(const VelocityFactorBehavior & behavior) { behavior_ = behavior; }
+  void reset() { velocity_factor_.behavior = PlanningBehavior::UNKNOWN; }
+
+  template <class PointType>
+  void set(
+    const std::vector<PointType> & points, const Pose & curr_pose, const Pose & stop_pose,
+    const VelocityFactorStatus status, const std::string & detail = "");
+
+private:
+  VelocityFactorBehavior behavior_{VelocityFactor::UNKNOWN};
+  VelocityFactor velocity_factor_{};
+};
+
+}  // namespace autoware::motion_utils
+
+#endif  // AUTOWARE__MOTION_UTILS__FACTOR__VELOCITY_FACTOR_INTERFACE_HPP_

master/src/universe/common/autoware_motion_utils/include/autoware/motion_utils/marker/marker_helper.hpp

@@ -0,0 +1,50 @@
+// 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__MOTION_UTILS__MARKER__MARKER_HELPER_HPP_
+#define AUTOWARE__MOTION_UTILS__MARKER__MARKER_HELPER_HPP_
+
+#include <rclcpp/time.hpp>
+
+#include <visualization_msgs/msg/marker_array.hpp>
+
+#include <string>
+
+namespace autoware::motion_utils
+{
+using geometry_msgs::msg::Pose;
+
+visualization_msgs::msg::MarkerArray createStopVirtualWallMarker(
+  const Pose & pose, const std::string & module_name, const rclcpp::Time & now, const int32_t id,
+  const double longitudinal_offset = 0.0, const std::string & ns_prefix = "",
+  const bool is_driving_forward = true);
+
+visualization_msgs::msg::MarkerArray createSlowDownVirtualWallMarker(
+  const Pose & pose, const std::string & module_name, const rclcpp::Time & now, const int32_t id,
+  const double longitudinal_offset = 0.0, const std::string & ns_prefix = "",
+  const bool is_driving_forward = true);
+
+visualization_msgs::msg::MarkerArray createDeadLineVirtualWallMarker(
+  const Pose & pose, const std::string & module_name, const rclcpp::Time & now, const int32_t id,
+  const double longitudinal_offset = 0.0, const std::string & ns_prefix = "",
+  const bool is_driving_forward = true);
+
+visualization_msgs::msg::MarkerArray createDeletedStopVirtualWallMarker(
+  const rclcpp::Time & now, const int32_t id);
+
+visualization_msgs::msg::MarkerArray createDeletedSlowDownVirtualWallMarker(
+  const rclcpp::Time & now, const int32_t id);
+}  // namespace autoware::motion_utils
+
+#endif  // AUTOWARE__MOTION_UTILS__MARKER__MARKER_HELPER_HPP_