Initial import of FaRui Campus ADS v3.2

slave/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()

slave/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
+```

slave/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_

slave/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_

slave/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_

slave/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_

slave/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_

slave/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_

slave/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_

slave/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_

slave/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_

slave/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_

slave/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_

slave/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_

slave/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>

slave/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();
+}

slave/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);
+}

slave/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));
+}
+
+//------------------------------------------------------------------------------

slave/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);
+}

slave/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));
+}
+
+//------------------------------------------------------------------------------

slave/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);
+}

slave/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));
+}

slave/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));
+}

slave/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");
+}

slave/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));
+}

slave/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()

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

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

slave/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_

slave/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_

slave/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_

slave/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_

slave/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_

slave/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_

slave/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_

slave/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>

slave/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();
+}

slave/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);
+  }
+}

slave/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);
+  }
+}

slave/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);
+  }
+}

slave/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);
+  }
+}

slave/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);
+  }
+}

slave/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);
+  }
+}

slave/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);
+  }
+}

slave/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()

slave/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}
+$$

slave/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_

slave/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_

slave/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_

slave/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_

slave/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_

slave/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_

slave/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>

slave/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

slave/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

slave/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

slave/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

slave/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();
+}

slave/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);
+}

slave/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);
+    }
+  }
+}

slave/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);
+    }
+  }
+}

slave/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);
+    }
+  }
+}

slave/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.);
+}

slave/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);
+    }
+  }
+}

slave/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()

slave/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.

slave/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.

slave/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;
+}

slave/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_

slave/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_

slave/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_

slave/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_

slave/src/universe/common/autoware_motion_utils/include/autoware/motion_utils/marker/virtual_wall_marker_creator.hpp

@@ -0,0 +1,81 @@
+// 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__MARKER__VIRTUAL_WALL_MARKER_CREATOR_HPP_
+#define AUTOWARE__MOTION_UTILS__MARKER__VIRTUAL_WALL_MARKER_CREATOR_HPP_
+
+#include <rclcpp/time.hpp>
+
+#include <geometry_msgs/msg/pose.hpp>
+#include <visualization_msgs/msg/marker_array.hpp>
+
+#include <functional>
+#include <string>
+#include <unordered_map>
+#include <vector>
+
+namespace autoware::motion_utils
+{
+
+/// @brief type of virtual wall associated with different marker styles and namespace
+enum VirtualWallType { stop, slowdown, deadline };
+/// @brief virtual wall to be visualized in rviz
+struct VirtualWall
+{
+  geometry_msgs::msg::Pose pose{};
+  std::string text{};
+  std::string ns{};
+  VirtualWallType style = stop;
+  double longitudinal_offset{};
+  bool is_driving_forward{true};
+};
+using VirtualWalls = std::vector<VirtualWall>;
+
+/// @brief class to manage the creation of virtual wall markers
+/// @details creates both ADD and DELETE markers
+class VirtualWallMarkerCreator
+{
+  struct MarkerCount
+  {
+    size_t previous = 0UL;
+    size_t current = 0UL;
+  };
+
+  using create_wall_function = std::function<visualization_msgs::msg::MarkerArray(
+    const geometry_msgs::msg::Pose & pose, const std::string & module_name,
+    const rclcpp::Time & now, const int32_t id, const double longitudinal_offset,
+    const std::string & ns_prefix, const bool is_driving_forward)>;
+
+  VirtualWalls virtual_walls_;
+  std::unordered_map<std::string, MarkerCount> marker_count_per_namespace_;
+
+  /// @brief internal cleanup: clear the stored markers and remove unused namespace from the map
+  void cleanup();
+
+public:
+  /// @brief add a virtual wall
+  /// @param virtual_wall virtual wall to add
+  void add_virtual_wall(const VirtualWall & virtual_wall);
+  /// @brief add virtual walls
+  /// @param virtual_walls virtual walls to add
+  void add_virtual_walls(const VirtualWalls & walls);
+
+  /// @brief create markers for the stored virtual walls
+  /// @details also create DELETE markers for the namespace+ids that are no longer used
+  /// @param now current time to be used for displaying the markers
+  visualization_msgs::msg::MarkerArray create_markers(const rclcpp::Time & now = rclcpp::Time());
+};
+}  // namespace autoware::motion_utils
+
+#endif  // AUTOWARE__MOTION_UTILS__MARKER__VIRTUAL_WALL_MARKER_CREATOR_HPP_

slave/src/universe/common/autoware_motion_utils/include/autoware/motion_utils/resample/resample.hpp

@@ -0,0 +1,239 @@
+// 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__RESAMPLE__RESAMPLE_HPP_
+#define AUTOWARE__MOTION_UTILS__RESAMPLE__RESAMPLE_HPP_
+
+#include "autoware_planning_msgs/msg/path.hpp"
+#include "autoware_planning_msgs/msg/trajectory.hpp"
+#include "tier4_planning_msgs/msg/path_with_lane_id.hpp"
+
+#include <vector>
+
+namespace autoware::motion_utils
+{
+/**
+ * @brief A resampling function for a path(points). Note that in a default setting, position xy are
+ *        resampled by spline interpolation, position z are resampled by linear interpolation, and
+ *        orientation of the resampled path are calculated by a forward difference method
+ *        based on the interpolated position x and y.
+ * @param input_path input path(point) to resample
+ * @param resampled_arclength arclength that contains length of each resampling points from initial
+ *        point
+ * @param use_akima_spline_for_xy If true, it uses linear interpolation to resample position x and
+ *        y. Otherwise, it uses spline interpolation
+ * @param use_lerp_for_z If true, it uses linear interpolation to resample position z.
+ *        Otherwise, it uses spline interpolation
+ * @return resampled path(poses)
+ */
+std::vector<geometry_msgs::msg::Point> resamplePointVector(
+  const std::vector<geometry_msgs::msg::Point> & points,
+  const std::vector<double> & resampled_arclength, const bool use_akima_spline_for_xy = false,
+  const bool use_lerp_for_z = true);
+
+/**
+ * @brief A resampling function for a path(position). Note that in a default setting, position xy
+ * are resampled by spline interpolation, position z are resampled by linear interpolation, and
+ *        orientation of the resampled path are calculated by a forward difference method
+ *        based on the interpolated position x and y.
+ * @param input_path input path(position) to resample
+ * @param resample_interval resampling interval
+ * @param use_akima_spline_for_xy If true, it uses linear interpolation to resample position x and
+ *        y. Otherwise, it uses spline interpolation
+ * @param use_lerp_for_z If true, it uses linear interpolation to resample position z.
+ *        Otherwise, it uses spline interpolation
+ * @return resampled path(poses)
+ */
+std::vector<geometry_msgs::msg::Point> resamplePointVector(
+  const std::vector<geometry_msgs::msg::Point> & points, const double resample_interval,
+  const bool use_akima_spline_for_xy = false, const bool use_lerp_for_z = true);
+
+/**
+ * @brief A resampling function for a path(poses). Note that in a default setting, position xy are
+ *        resampled by spline interpolation, position z are resampled by linear interpolation, and
+ *        orientation of the resampled path are calculated by a forward difference method
+ *        based on the interpolated position x and y.
+ * @param input_path input path(poses) to resample
+ * @param resampled_arclength arclength that contains length of each resampling points from initial
+ *        point
+ * @param use_akima_spline_for_xy If true, it uses linear interpolation to resample position x and
+ *        y. Otherwise, it uses spline interpolation
+ * @param use_lerp_for_z If true, it uses linear interpolation to resample position z.
+ *        Otherwise, it uses spline interpolation
+ * @return resampled path(poses)
+ */
+std::vector<geometry_msgs::msg::Pose> resamplePoseVector(
+  const std::vector<geometry_msgs::msg::Pose> & points,
+  const std::vector<double> & resampled_arclength, const bool use_akima_spline_for_xy = false,
+  const bool use_lerp_for_z = true);
+
+/**
+ * @brief A resampling function for a path(poses). Note that in a default setting, position xy are
+ *        resampled by spline interpolation, position z are resampled by linear interpolation, and
+ *        orientation of the resampled path are calculated by a forward difference method
+ *        based on the interpolated position x and y.
+ * @param input_path input path(poses) to resample
+ * @param resample_interval resampling interval
+ * @param use_akima_spline_for_xy If true, it uses linear interpolation to resample position x and
+ *        y. Otherwise, it uses spline interpolation
+ * @param use_lerp_for_z If true, it uses linear interpolation to resample position z.
+ *        Otherwise, it uses spline interpolation
+ * @return resampled path(poses)
+ */
+std::vector<geometry_msgs::msg::Pose> resamplePoseVector(
+  const std::vector<geometry_msgs::msg::Pose> & points, const double resample_interval,
+  const bool use_akima_spline_for_xy = false, const bool use_lerp_for_z = true);
+
+/**
+ * @brief A resampling function for a path with lane id. Note that in a default setting, position xy
+ *        are resampled by spline interpolation, position z are resampled by linear interpolation,
+ *        longitudinal and lateral velocity are resampled by zero_order_hold, and heading rate is
+ *        resampled by linear interpolation. Orientation of the resampled path are calculated by a
+ *        forward difference method based on the interpolated position x and y. Moreover, lane_ids
+ *        and is_final are also interpolated by zero order hold
+ * @param input_path input path to resample
+ * @param resampled_arclength arclength that contains length of each resampling points from initial
+ *        point
+ * @param use_akima_spline_for_xy If true, it uses linear interpolation to resample position x and
+ *        y. Otherwise, it uses spline interpolation
+ * @param use_lerp_for_z If true, it uses linear interpolation to resample position z.
+ *        Otherwise, it uses spline interpolation
+ * @param use_zero_order_hold_for_v If true, it uses zero_order_hold to resample
+ *        longitudinal and lateral velocity. Otherwise, it uses linear interpolation
+ * @return resampled path
+ */
+tier4_planning_msgs::msg::PathWithLaneId resamplePath(
+  const tier4_planning_msgs::msg::PathWithLaneId & input_path,
+  const std::vector<double> & resampled_arclength, const bool use_akima_spline_for_xy = false,
+  const bool use_lerp_for_z = true, const bool use_zero_order_hold_for_v = true);
+
+/**
+ * @brief A resampling function for a path with lane id. Note that in a default setting, position xy
+ *        are resampled by spline interpolation, position z are resampled by linear interpolation,
+ *        longitudinal and lateral velocity are resampled by zero_order_hold, and heading rate is
+ *        resampled by linear interpolation. Orientation of the resampled path are calculated by a
+ *        forward difference method based on the interpolated position x and y. Moreover, lane_ids
+ *        and is_final are also interpolated by zero order hold
+ * @param input_path input path to resample
+ * @param resampled_interval resampling interval point
+ * @param use_akima_spline_for_xy If true, it uses linear interpolation to resample position x and
+ *        y. Otherwise, it uses spline interpolation
+ * @param use_lerp_for_z If true, it uses linear interpolation to resample position z.
+ *        Otherwise, it uses spline interpolation
+ * @param use_zero_order_hold_for_v If true, it uses zero_order_hold to resample
+ *        longitudinal and lateral velocity. Otherwise, it uses linear interpolation
+ * @param resample_input_path_stop_point If true, resample closest stop point in input path
+ * @return resampled path
+ */
+tier4_planning_msgs::msg::PathWithLaneId resamplePath(
+  const tier4_planning_msgs::msg::PathWithLaneId & input_path, const double resample_interval,
+  const bool use_akima_spline_for_xy = false, const bool use_lerp_for_z = true,
+  const bool use_zero_order_hold_for_v = true, const bool resample_input_path_stop_point = true);
+
+/**
+ * @brief A resampling function for a path. Note that in a default setting, position xy are
+ *        resampled by spline interpolation, position z are resampled by linear interpolation,
+ *        longitudinal and lateral velocity are resampled by zero_order_hold, and heading rate is
+ *        resampled by linear interpolation. Orientation of the resampled path are calculated by a
+ *        forward difference method based on the interpolated position x and y.
+ * @param input_path input path to resample
+ * @param resampled_arclength arclength that contains length of each resampling points from initial
+ *        point
+ * @param use_akima_spline_for_xy If true, it uses linear interpolation to resample position x and
+ *        y. Otherwise, it uses spline interpolation
+ * @param use_lerp_for_z If true, it uses linear interpolation to resample position z.
+ *        Otherwise, it uses spline interpolation
+ * @param use_zero_order_hold_for_v If true, it uses zero_order_hold to resample
+ *        longitudinal and lateral velocity. Otherwise, it uses linear interpolation
+ * @return resampled path
+ */
+autoware_planning_msgs::msg::Path resamplePath(
+  const autoware_planning_msgs::msg::Path & input_path,
+  const std::vector<double> & resampled_arclength, const bool use_akima_spline_for_xy = false,
+  const bool use_lerp_for_z = true, const bool use_zero_order_hold_for_v = true);
+
+/**
+ * @brief A resampling function for a path. Note that in a default setting, position xy
+ *        are resampled by spline interpolation, position z are resampled by linear interpolation,
+ *        longitudinal and lateral velocity are resampled by zero_order_hold, and heading rate is
+ *        resampled by linear interpolation. Orientation of the resampled path are calculated by a
+ *        forward difference method based on the interpolated position x and y.
+ * @param input_path input path to resample
+ * @param resampled_interval resampling interval point
+ * @param use_akima_spline_for_xy If true, it uses linear interpolation to resample position x and
+ *        y. Otherwise, it uses spline interpolation
+ * @param use_lerp_for_z If true, it uses linear interpolation to resample position z.
+ *        Otherwise, it uses spline interpolation
+ * @param use_zero_order_hold_for_v If true, it uses zero_order_hold to resample
+ *        longitudinal and lateral velocity. Otherwise, it uses linear interpolation
+ * @param resample_input_path_stop_point If true, resample closest stop point in input path
+ * @return resampled path
+ */
+autoware_planning_msgs::msg::Path resamplePath(
+  const autoware_planning_msgs::msg::Path & input_path, const double resample_interval,
+  const bool use_akima_spline_for_xy = false, const bool use_lerp_for_z = true,
+  const bool use_zero_order_hold_for_twist = true,
+  const bool resample_input_path_stop_point = true);
+
+/**
+ * @brief A resampling function for a trajectory. Note that in a default setting, position xy are
+ *        resampled by spline interpolation, position z are resampled by linear interpolation, twist
+ *        informaiton(velocity and acceleration) are resampled by zero_order_hold, and heading rate
+ *        is resampled by linear interpolation. The rest of the category is resampled by linear
+ *        interpolation. Orientation of the resampled path are calculated by a forward difference
+ *        method based on the interpolated position x and y.
+ * @param input_trajectory input trajectory to resample
+ * @param resampled_arclength arclength that contains length of each resampling points from initial
+ *        point
+ * @param use_akima_spline_for_xy If true, it uses linear interpolation to resample position x and
+ *        y. Otherwise, it uses spline interpolation
+ * @param use_lerp_for_z If true, it uses linear interpolation to resample position z.
+ *        Otherwise, it uses spline interpolation
+ * @param use_zero_order_hold_for_twist If true, it uses zero_order_hold to resample
+ *        longitudinal, lateral velocity and acceleration. Otherwise, it uses linear interpolation
+ * @return resampled trajectory
+ */
+autoware_planning_msgs::msg::Trajectory resampleTrajectory(
+  const autoware_planning_msgs::msg::Trajectory & input_trajectory,
+  const std::vector<double> & resampled_arclength, const bool use_akima_spline_for_xy = false,
+  const bool use_lerp_for_z = true, const bool use_zero_order_hold_for_twist = true);
+
+/**
+ * @brief A resampling function for a trajectory. This function resamples closest stop point,
+ *        terminal point and points by resample interval. Note that in a default setting, position
+ * xy are resampled by spline interpolation, position z are resampled by linear interpolation, twist
+ *        informaiton(velocity and acceleration) are resampled by zero_order_hold, and heading rate
+ *        is resampled by linear interpolation. The rest of the category is resampled by linear
+ *        interpolation. Orientation of the resampled path are calculated by a forward difference
+ *        method based on the interpolated position x and y.
+ * @param input_trajectory input trajectory to resample
+ * @param resampled_interval resampling interval
+ * @param use_akima_spline_for_xy If true, it uses linear interpolation to resample position x and
+ *        y. Otherwise, it uses spline interpolation
+ * @param use_lerp_for_z If true, it uses linear interpolation to resample position z.
+ *        Otherwise, it uses spline interpolation
+ * @param use_zero_order_hold_for_twist If true, it uses zero_order_hold to resample
+ *        longitudinal, lateral velocity and acceleration. Otherwise, it uses linear interpolation
+ * @param resample_input_trajectory_stop_point If true, resample closest stop point in input
+ *        trajectory
+ * @return resampled trajectory
+ */
+autoware_planning_msgs::msg::Trajectory resampleTrajectory(
+  const autoware_planning_msgs::msg::Trajectory & input_trajectory, const double resample_interval,
+  const bool use_akima_spline_for_xy = false, const bool use_lerp_for_z = true,
+  const bool use_zero_order_hold_for_twist = true,
+  const bool resample_input_trajectory_stop_point = true);
+}  // namespace autoware::motion_utils
+
+#endif  // AUTOWARE__MOTION_UTILS__RESAMPLE__RESAMPLE_HPP_

slave/src/universe/common/autoware_motion_utils/include/autoware/motion_utils/resample/resample_utils.hpp

@@ -0,0 +1,127 @@
+// 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__RESAMPLE__RESAMPLE_UTILS_HPP_
+#define AUTOWARE__MOTION_UTILS__RESAMPLE__RESAMPLE_UTILS_HPP_
+
+#include "autoware/universe_utils/system/backtrace.hpp"
+
+#include <autoware/motion_utils/constants.hpp>
+#include <autoware/motion_utils/trajectory/trajectory.hpp>
+#include <autoware/universe_utils/geometry/geometry.hpp>
+
+#include <vector>
+
+namespace resample_utils
+{
+constexpr double close_s_threshold = 1e-6;
+
+static inline rclcpp::Logger get_logger()
+{
+  constexpr const char * logger{"autoware_motion_utils.resample_utils"};
+  return rclcpp::get_logger(logger);
+}
+
+template <class T>
+bool validate_size(const T & points)
+{
+  return points.size() >= 2;
+}
+
+template <class T>
+bool validate_resampling_range(const T & points, const std::vector<double> & resampling_intervals)
+{
+  const double points_length = autoware::motion_utils::calcArcLength(points);
+  return points_length >= resampling_intervals.back();
+}
+
+template <class T>
+bool validate_points_duplication(const T & points)
+{
+  for (size_t i = 0; i < points.size() - 1; ++i) {
+    const auto & curr_pt = autoware::universe_utils::getPoint(points.at(i));
+    const auto & next_pt = autoware::universe_utils::getPoint(points.at(i + 1));
+    const double ds = autoware::universe_utils::calcDistance2d(curr_pt, next_pt);
+    if (ds < close_s_threshold) {
+      return false;
+    }
+  }
+
+  return true;
+}
+
+template <class T>
+bool validate_arguments(const T & input_points, const std::vector<double> & resampling_intervals)
+{
+  // Check size of the arguments
+  if (!validate_size(input_points)) {
+    RCLCPP_DEBUG(get_logger(), "invalid argument: The number of input points is less than 2");
+    autoware::universe_utils::print_backtrace();
+    return false;
+  }
+  if (!validate_size(resampling_intervals)) {
+    RCLCPP_DEBUG(
+      get_logger(), "invalid argument: The number of resampling intervals is less than 2");
+    autoware::universe_utils::print_backtrace();
+    return false;
+  }
+
+  // Check resampling range
+  if (!validate_resampling_range(input_points, resampling_intervals)) {
+    RCLCPP_DEBUG(get_logger(), "invalid argument: resampling interval is longer than input points");
+    autoware::universe_utils::print_backtrace();
+    return false;
+  }
+
+  // Check duplication
+  if (!validate_points_duplication(input_points)) {
+    RCLCPP_DEBUG(get_logger(), "invalid argument: input points has some duplicated points");
+    autoware::universe_utils::print_backtrace();
+    return false;
+  }
+
+  return true;
+}
+
+template <class T>
+bool validate_arguments(const T & input_points, const double resampling_interval)
+{
+  // Check size of the arguments
+  if (!validate_size(input_points)) {
+    RCLCPP_DEBUG(get_logger(), "invalid argument: The number of input points is less than 2");
+    autoware::universe_utils::print_backtrace();
+    return false;
+  }
+
+  // check resampling interval
+  if (resampling_interval < autoware::motion_utils::overlap_threshold) {
+    RCLCPP_DEBUG(
+      get_logger(), "invalid argument: resampling interval is less than %f",
+      autoware::motion_utils::overlap_threshold);
+    autoware::universe_utils::print_backtrace();
+    return false;
+  }
+
+  // Check duplication
+  if (!validate_points_duplication(input_points)) {
+    RCLCPP_DEBUG(get_logger(), "invalid argument: input points has some duplicated points");
+    autoware::universe_utils::print_backtrace();
+    return false;
+  }
+
+  return true;
+}
+}  // namespace resample_utils
+
+#endif  // AUTOWARE__MOTION_UTILS__RESAMPLE__RESAMPLE_UTILS_HPP_

slave/src/universe/common/autoware_motion_utils/include/autoware/motion_utils/trajectory/conversion.hpp

@@ -0,0 +1,120 @@
+// 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__TRAJECTORY__CONVERSION_HPP_
+#define AUTOWARE__MOTION_UTILS__TRAJECTORY__CONVERSION_HPP_
+
+#include "autoware_planning_msgs/msg/detail/path__struct.hpp"
+#include "autoware_planning_msgs/msg/detail/trajectory__struct.hpp"
+#include "autoware_planning_msgs/msg/detail/trajectory_point__struct.hpp"
+#include "std_msgs/msg/header.hpp"
+#include "tier4_planning_msgs/msg/detail/path_with_lane_id__struct.hpp"
+
+#include <vector>
+
+namespace autoware::motion_utils
+{
+using TrajectoryPoints = std::vector<autoware_planning_msgs::msg::TrajectoryPoint>;
+
+/**
+ * @brief Convert std::vector<autoware_planning_msgs::msg::TrajectoryPoint> to
+ * autoware_planning_msgs::msg::Trajectory. This function is temporarily added for porting to
+ * autoware_msgs. We should consider whether to remove this function after the porting is done.
+ * @attention This function just clips
+ * std::vector<autoware_planning_msgs::msg::TrajectoryPoint> up to the capacity of Trajectory.
+ * Therefore, the error handling out of this function is necessary if the size of the input greater
+ * than the capacity.
+ * @todo Decide how to handle the situation that we need to use the trajectory with the size of
+ * points larger than the capacity. (Tier IV)
+ */
+autoware_planning_msgs::msg::Trajectory convertToTrajectory(
+  const std::vector<autoware_planning_msgs::msg::TrajectoryPoint> & trajectory,
+  const std_msgs::msg::Header & header = std_msgs::msg::Header{});
+
+/**
+ * @brief Convert autoware_planning_msgs::msg::Trajectory to
+ * std::vector<autoware_planning_msgs::msg::TrajectoryPoint>.
+ */
+std::vector<autoware_planning_msgs::msg::TrajectoryPoint> convertToTrajectoryPointArray(
+  const autoware_planning_msgs::msg::Trajectory & trajectory);
+
+template <class T>
+autoware_planning_msgs::msg::Path convertToPath([[maybe_unused]] const T & input)
+{
+  static_assert(sizeof(T) == 0, "Only specializations of convertToPath can be used.");
+  throw std::logic_error("Only specializations of convertToPath can be used.");
+}
+
+template <>
+inline autoware_planning_msgs::msg::Path convertToPath(
+  const tier4_planning_msgs::msg::PathWithLaneId & input)
+{
+  autoware_planning_msgs::msg::Path output{};
+  output.header = input.header;
+  output.left_bound = input.left_bound;
+  output.right_bound = input.right_bound;
+  output.points.resize(input.points.size());
+  for (size_t i = 0; i < input.points.size(); ++i) {
+    output.points.at(i) = input.points.at(i).point;
+  }
+  return output;
+}
+
+template <class T>
+TrajectoryPoints convertToTrajectoryPoints([[maybe_unused]] const T & input)
+{
+  static_assert(sizeof(T) == 0, "Only specializations of convertToTrajectoryPoints can be used.");
+  throw std::logic_error("Only specializations of convertToTrajectoryPoints can be used.");
+}
+
+template <>
+inline TrajectoryPoints convertToTrajectoryPoints(
+  const tier4_planning_msgs::msg::PathWithLaneId & input)
+{
+  TrajectoryPoints output{};
+  for (const auto & p : input.points) {
+    autoware_planning_msgs::msg::TrajectoryPoint tp;
+    tp.pose = p.point.pose;
+    tp.longitudinal_velocity_mps = p.point.longitudinal_velocity_mps;
+    // since path point doesn't have acc for now
+    tp.acceleration_mps2 = 0;
+    output.emplace_back(tp);
+  }
+  return output;
+}
+
+template <class T>
+tier4_planning_msgs::msg::PathWithLaneId convertToPathWithLaneId([[maybe_unused]] const T & input)
+{
+  static_assert(sizeof(T) == 0, "Only specializations of convertToPathWithLaneId can be used.");
+  throw std::logic_error("Only specializations of convertToPathWithLaneId can be used.");
+}
+
+template <>
+inline tier4_planning_msgs::msg::PathWithLaneId convertToPathWithLaneId(
+  const TrajectoryPoints & input)
+{
+  tier4_planning_msgs::msg::PathWithLaneId output{};
+  for (const auto & p : input) {
+    tier4_planning_msgs::msg::PathPointWithLaneId pp;
+    pp.point.pose = p.pose;
+    pp.point.longitudinal_velocity_mps = p.longitudinal_velocity_mps;
+    output.points.emplace_back(pp);
+  }
+  return output;
+}
+
+}  // namespace autoware::motion_utils
+
+#endif  // AUTOWARE__MOTION_UTILS__TRAJECTORY__CONVERSION_HPP_

slave/src/universe/common/autoware_motion_utils/include/autoware/motion_utils/trajectory/interpolation.hpp

@@ -0,0 +1,96 @@
+// 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__TRAJECTORY__INTERPOLATION_HPP_
+#define AUTOWARE__MOTION_UTILS__TRAJECTORY__INTERPOLATION_HPP_
+
+#include "autoware/universe_utils/geometry/geometry.hpp"
+
+#include "autoware_planning_msgs/msg/trajectory.hpp"
+#include "tier4_planning_msgs/msg/path_with_lane_id.hpp"
+
+#include <boost/optional.hpp>
+
+#include <algorithm>
+#include <limits>
+
+namespace autoware::motion_utils
+{
+/**
+ * @brief An interpolation function that finds the closest interpolated point on the trajectory from
+ * the given pose
+ * @param trajectory input trajectory
+ * @param target_pose target_pose
+ * @param use_zero_order_for_twist flag to decide wether to use zero order hold interpolation for
+ * twist information
+ * @return resampled path(poses)
+ */
+autoware_planning_msgs::msg::TrajectoryPoint calcInterpolatedPoint(
+  const autoware_planning_msgs::msg::Trajectory & trajectory,
+  const geometry_msgs::msg::Pose & target_pose, const bool use_zero_order_hold_for_twist = false,
+  const double dist_threshold = std::numeric_limits<double>::max(),
+  const double yaw_threshold = std::numeric_limits<double>::max());
+
+/**
+ * @brief An interpolation function that finds the closest interpolated point on the path from
+ * the given pose
+ * @param path input path
+ * @param target_pose target_pose
+ * @param use_zero_order_for_twist flag to decide wether to use zero order hold interpolation for
+ * twist information
+ * @return resampled path(poses)
+ */
+tier4_planning_msgs::msg::PathPointWithLaneId calcInterpolatedPoint(
+  const tier4_planning_msgs::msg::PathWithLaneId & path,
+  const geometry_msgs::msg::Pose & target_pose, const bool use_zero_order_hold_for_twist = false,
+  const double dist_threshold = std::numeric_limits<double>::max(),
+  const double yaw_threshold = std::numeric_limits<double>::max());
+
+/**
+ * @brief An interpolation function that finds the closest interpolated point on the path that is a
+ * certain length away from the given pose
+ * @param points input path
+ * @param target_length length from the front point of the path
+ * @return resampled pose
+ */
+template <class T>
+geometry_msgs::msg::Pose calcInterpolatedPose(const T & points, const double target_length)
+{
+  if (points.empty()) {
+    geometry_msgs::msg::Pose interpolated_pose;
+    return interpolated_pose;
+  }
+
+  if (points.size() < 2 || target_length < 0.0) {
+    return autoware::universe_utils::getPose(points.front());
+  }
+
+  double accumulated_length = 0;
+  for (size_t i = 0; i < points.size() - 1; ++i) {
+    const auto & curr_pose = autoware::universe_utils::getPose(points.at(i));
+    const auto & next_pose = autoware::universe_utils::getPose(points.at(i + 1));
+    const double length = autoware::universe_utils::calcDistance3d(curr_pose, next_pose);
+    if (accumulated_length + length > target_length) {
+      const double ratio = (target_length - accumulated_length) / std::max(length, 1e-6);
+      return autoware::universe_utils::calcInterpolatedPose(curr_pose, next_pose, ratio);
+    }
+    accumulated_length += length;
+  }
+
+  return autoware::universe_utils::getPose(points.back());
+}
+
+}  // namespace autoware::motion_utils
+
+#endif  // AUTOWARE__MOTION_UTILS__TRAJECTORY__INTERPOLATION_HPP_

slave/src/universe/common/autoware_motion_utils/include/autoware/motion_utils/trajectory/path_shift.hpp

@@ -0,0 +1,71 @@
+// 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.
+
+#ifndef AUTOWARE__MOTION_UTILS__TRAJECTORY__PATH_SHIFT_HPP_
+#define AUTOWARE__MOTION_UTILS__TRAJECTORY__PATH_SHIFT_HPP_
+
+namespace autoware::motion_utils
+{
+/**
+ * @brief Calculates the velocity required for shifting
+ * @param lateral lateral distance
+ * @param jerk lateral jerk
+ * @param longitudinal_distance longitudinal distance
+ * @return velocity
+ */
+double calc_feasible_velocity_from_jerk(
+  const double lateral, const double jerk, const double longitudinal_distance);
+
+/**
+ * @brief Calculates the lateral distance required for shifting
+ * @param longitudinal longitudinal distance
+ * @param jerk lateral jerk
+ * @param velocity velocity
+ * @return lateral distance
+ */
+double calc_lateral_dist_from_jerk(
+  const double longitudinal, const double jerk, const double velocity);
+
+/**
+ * @brief Calculates the lateral distance required for shifting
+ * @param lateral lateral distance
+ * @param jerk lateral jerk
+ * @param velocity velocity
+ * @return longitudinal distance
+ */
+double calc_longitudinal_dist_from_jerk(
+  const double lateral, const double jerk, const double velocity);
+
+/**
+ * @brief Calculates the total time required for shifting
+ * @param lateral lateral distance
+ * @param jerk lateral jerk
+ * @param acc lateral acceleration
+ * @return time
+ */
+double calc_shift_time_from_jerk(const double lateral, const double jerk, const double acc);
+
+/**
+ * @brief Calculates the required jerk from lateral/longitudinal distance
+ * @param lateral lateral distance
+ * @param longitudinal longitudinal distance
+ * @param velocity velocity
+ * @return jerk
+ */
+double calc_jerk_from_lat_lon_distance(
+  const double lateral, const double longitudinal, const double velocity);
+
+}  // namespace autoware::motion_utils
+
+#endif  // AUTOWARE__MOTION_UTILS__TRAJECTORY__PATH_SHIFT_HPP_

slave/src/universe/common/autoware_motion_utils/include/autoware/motion_utils/trajectory/path_with_lane_id.hpp

@@ -0,0 +1,46 @@
+// 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__TRAJECTORY__PATH_WITH_LANE_ID_HPP_
+#define AUTOWARE__MOTION_UTILS__TRAJECTORY__PATH_WITH_LANE_ID_HPP_
+
+#include "tier4_planning_msgs/msg/path_with_lane_id.hpp"
+#include <geometry_msgs/msg/point.hpp>
+
+#include <optional>
+#include <utility>
+namespace autoware::motion_utils
+{
+std::optional<std::pair<size_t, size_t>> getPathIndexRangeWithLaneId(
+  const tier4_planning_msgs::msg::PathWithLaneId & path, const int64_t target_lane_id);
+
+size_t findNearestIndexFromLaneId(
+  const tier4_planning_msgs::msg::PathWithLaneId & path, const geometry_msgs::msg::Point & pos,
+  const int64_t lane_id);
+
+size_t findNearestSegmentIndexFromLaneId(
+  const tier4_planning_msgs::msg::PathWithLaneId & path, const geometry_msgs::msg::Point & pos,
+  const int64_t lane_id);
+
+// @brief Calculates the path to be followed by the rear wheel center in order to make the vehicle
+// center follow the input path
+// @param [in] path with position to be followed by the center of the vehicle
+// @param [out] PathWithLaneId to be followed by the rear wheel center follow to make the vehicle
+// center follow the input path NOTE: rear_to_cog is supposed to be positive
+tier4_planning_msgs::msg::PathWithLaneId convertToRearWheelCenter(
+  const tier4_planning_msgs::msg::PathWithLaneId & path, const double rear_to_cog,
+  const bool enable_last_point_compensation = true);
+}  // namespace autoware::motion_utils
+
+#endif  // AUTOWARE__MOTION_UTILS__TRAJECTORY__PATH_WITH_LANE_ID_HPP_

slave/src/universe/common/autoware_motion_utils/include/autoware/motion_utils/trajectory_container/interpolator.hpp

@@ -0,0 +1,23 @@
+// 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.
+
+#ifndef AUTOWARE__MOTION_UTILS__TRAJECTORY_CONTAINER__INTERPOLATOR_HPP_
+#define AUTOWARE__MOTION_UTILS__TRAJECTORY_CONTAINER__INTERPOLATOR_HPP_
+#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/linear.hpp>
+#include <autoware/motion_utils/trajectory_container/interpolator/nearest_neighbor.hpp>
+#include <autoware/motion_utils/trajectory_container/interpolator/spherical_linear.hpp>
+#include <autoware/motion_utils/trajectory_container/interpolator/stairstep.hpp>
+#endif  // AUTOWARE__MOTION_UTILS__TRAJECTORY_CONTAINER__INTERPOLATOR_HPP_

slave/src/universe/common/autoware_motion_utils/include/autoware/motion_utils/trajectory_container/interpolator/akima_spline.hpp

@@ -0,0 +1,94 @@
+// 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.
+
+#ifndef AUTOWARE__MOTION_UTILS__TRAJECTORY_CONTAINER__INTERPOLATOR__AKIMA_SPLINE_HPP_
+#define AUTOWARE__MOTION_UTILS__TRAJECTORY_CONTAINER__INTERPOLATOR__AKIMA_SPLINE_HPP_
+
+#include "autoware/motion_utils/trajectory_container/interpolator/detail/interpolator_mixin.hpp"
+
+#include <Eigen/Dense>
+
+#include <vector>
+
+namespace autoware::motion_utils::trajectory_container::interpolator
+{
+
+/**
+ * @brief Class for Akima spline interpolation.
+ *
+ * This class provides methods to perform Akima spline interpolation on a set of data points.
+ */
+class AkimaSpline : public detail::InterpolatorMixin<AkimaSpline, double>
+{
+private:
+  Eigen::VectorXd a_, b_, c_, d_;  ///< Coefficients for the Akima spline.
+
+  /**
+   * @brief Compute the spline parameters.
+   *
+   * This method computes the coefficients for the Akima spline.
+   *
+   * @param bases The bases values.
+   * @param values The values to interpolate.
+   */
+  void compute_parameters(
+    const Eigen::Ref<const Eigen::VectorXd> & bases,
+    const Eigen::Ref<const Eigen::VectorXd> & values);
+
+  /**
+   * @brief Build the interpolator with the given values.
+   *
+   * @param bases The bases values.
+   * @param values The values to interpolate.
+   */
+  void build_impl(const std::vector<double> & bases, const std::vector<double> & values) override;
+
+  /**
+   * @brief Compute the interpolated value at the given point.
+   *
+   * @param s The point at which to compute the interpolated value.
+   * @return The interpolated value.
+   */
+  [[nodiscard]] double compute_impl(const double & s) const override;
+
+  /**
+   * @brief Compute the first derivative at the given point.
+   *
+   * @param s The point at which to compute the first derivative.
+   * @return The first derivative.
+   */
+  [[nodiscard]] double compute_first_derivative_impl(const double & s) const override;
+
+  /**
+   * @brief Compute the second derivative at the given point.
+   *
+   * @param s The point at which to compute the second derivative.
+   * @return The second derivative.
+   */
+  [[nodiscard]] double compute_second_derivative_impl(const double & s) const override;
+
+public:
+  AkimaSpline() = default;
+
+  /**
+   * @brief Get the minimum number of required points for the interpolator.
+   *
+   * @return The minimum number of required points.
+   */
+  [[nodiscard]] size_t minimum_required_points() const override { return 5; }
+};
+
+}  // namespace autoware::motion_utils::trajectory_container::interpolator
+
+#endif  // AUTOWARE__MOTION_UTILS__TRAJECTORY_CONTAINER__INTERPOLATOR__AKIMA_SPLINE_HPP_

slave/src/universe/common/autoware_motion_utils/include/autoware/motion_utils/trajectory_container/interpolator/cubic_spline.hpp

@@ -0,0 +1,96 @@
+// 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.
+
+#ifndef AUTOWARE__MOTION_UTILS__TRAJECTORY_CONTAINER__INTERPOLATOR__CUBIC_SPLINE_HPP_
+#define AUTOWARE__MOTION_UTILS__TRAJECTORY_CONTAINER__INTERPOLATOR__CUBIC_SPLINE_HPP_
+
+#include "autoware/motion_utils/trajectory_container/interpolator/detail/interpolator_mixin.hpp"
+
+#include <Eigen/Dense>
+
+#include <vector>
+
+namespace autoware::motion_utils::trajectory_container::interpolator
+{
+
+/**
+ * @brief Class for cubic spline interpolation.
+ *
+ * This class provides methods to perform cubic spline interpolation on a set of data points.
+ */
+class CubicSpline : public detail::InterpolatorMixin<CubicSpline, double>
+{
+private:
+  Eigen::VectorXd a_, b_, c_, d_;  ///< Coefficients for the cubic spline.
+  Eigen::VectorXd h_;              ///< Interval sizes between bases points.
+
+  /**
+   * @brief Compute the spline parameters.
+   *
+   * This method computes the coefficients for the cubic spline.
+   *
+   * @param bases The bases values.
+   * @param values The values to interpolate.
+   */
+  void compute_parameters(
+    const Eigen::Ref<const Eigen::VectorXd> & bases,
+    const Eigen::Ref<const Eigen::VectorXd> & values);
+
+  /**
+   * @brief Build the interpolator with the given values.
+   *
+   * @param bases The bases values.
+   * @param values The values to interpolate.
+   * @return True if the interpolator was built successfully, false otherwise.
+   */
+  void build_impl(const std::vector<double> & bases, const std::vector<double> & values) override;
+
+  /**
+   * @brief Compute the interpolated value at the given point.
+   *
+   * @param s The point at which to compute the interpolated value.
+   * @return The interpolated value.
+   */
+  [[nodiscard]] double compute_impl(const double & s) const override;
+
+  /**
+   * @brief Compute the first derivative at the given point.
+   *
+   * @param s The point at which to compute the first derivative.
+   * @return The first derivative.
+   */
+  [[nodiscard]] double compute_first_derivative_impl(const double & s) const override;
+
+  /**
+   * @brief Compute the second derivative at the given point.
+   *
+   * @param s The point at which to compute the second derivative.
+   * @return The second derivative.
+   */
+  [[nodiscard]] double compute_second_derivative_impl(const double & s) const override;
+
+public:
+  CubicSpline() = default;
+
+  /**
+   * @brief Get the minimum number of required points for the interpolator.
+   *
+   * @return The minimum number of required points.
+   */
+  [[nodiscard]] size_t minimum_required_points() const override { return 4; }
+};
+
+}  // namespace autoware::motion_utils::trajectory_container::interpolator
+
+#endif  // AUTOWARE__MOTION_UTILS__TRAJECTORY_CONTAINER__INTERPOLATOR__CUBIC_SPLINE_HPP_

slave/src/universe/common/autoware_motion_utils/include/autoware/motion_utils/trajectory_container/interpolator/detail/interpolator_common_interface.hpp

@@ -0,0 +1,143 @@
+// 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.
+
+// clang-format off
+#ifndef AUTOWARE__MOTION_UTILS__TRAJECTORY_CONTAINER__INTERPOLATOR__DETAIL__INTERPOLATOR_COMMON_INTERFACE_HPP_  // NOLINT
+#define AUTOWARE__MOTION_UTILS__TRAJECTORY_CONTAINER__INTERPOLATOR__DETAIL__INTERPOLATOR_COMMON_INTERFACE_HPP_  // NOLINT
+// clang-format on
+
+#include <Eigen/Dense>
+#include <rclcpp/logging.hpp>
+
+#include <vector>
+
+namespace autoware::motion_utils::trajectory_container::interpolator::detail
+{
+/**
+ * @brief Base class for interpolation implementations.
+ *
+ * This class provides the basic interface and common functionality for different types
+ * of interpolation. It is intended to be subclassed by specific interpolation algorithms.
+ *
+ * @tparam T The type of the values being interpolated.
+ */
+template <typename T>
+class InterpolatorCommonInterface
+{
+protected:
+  std::vector<double> bases_;  ///< bases values for the interpolation.
+
+  /**
+   * @brief Get the start of the interpolation range.
+   */
+  [[nodiscard]] double start() const { return bases_.front(); }
+
+  /**
+   * @brief Get the end of the interpolation range.
+   */
+  [[nodiscard]] double end() const { return bases_.back(); }
+
+  /**
+   * @brief Compute the interpolated value at the given point.
+   *
+   * This method should be overridden by subclasses to provide the specific interpolation logic.
+   *
+   * @param s The point at which to compute the interpolated value.
+   * @return The interpolated value.
+   */
+  [[nodiscard]] virtual T compute_impl(const double & s) const = 0;
+
+  /**
+   * @brief Build the interpolator with the given values.
+   *
+   * This method should be overridden by subclasses to provide the specific build logic.
+   *
+   * @param bases The bases values.
+   * @param values The values to interpolate.
+   */
+  virtual void build_impl(const std::vector<double> & bases, const std::vector<T> & values) = 0;
+
+  /**
+   * @brief Validate the input to the compute method.
+   *
+   * Checks that the interpolator has been built and that the input value is within range.
+   *
+   * @param s The input value.
+   * @throw std::runtime_error if the interpolator has not been built.
+   */
+  void validate_compute_input(const double & s) const
+  {
+    if (s < start() || s > end()) {
+      RCLCPP_WARN(
+        rclcpp::get_logger("Interpolator"),
+        "Input value %f is outside the range of the interpolator [%f, %f].", s, start(), end());
+    }
+  }
+
+  [[nodiscard]] int32_t get_index(const double & s, bool end_inclusive = true) const
+  {
+    if (end_inclusive && s == end()) {
+      return static_cast<int32_t>(bases_.size()) - 2;
+    }
+    auto comp = [](const double & a, const double & b) { return a <= b; };
+    return std::distance(bases_.begin(), std::lower_bound(bases_.begin(), bases_.end(), s, comp)) -
+           1;
+  }
+
+public:
+  /**
+   * @brief Build the interpolator with the given bases and values.
+   *
+   * @param bases The bases values.
+   * @param values The values to interpolate.
+   * @return True if the interpolator was built successfully, false otherwise.
+   */
+  bool build(const std::vector<double> & bases, const std::vector<T> & values)
+  {
+    if (bases.size() != values.size()) {
+      return false;
+    }
+    if (bases.size() < minimum_required_points()) {
+      return false;
+    }
+    build_impl(bases, values);
+    return true;
+  }
+
+  /**
+   * @brief Get the minimum number of required points for the interpolator.
+   *
+   * This method should be overridden by subclasses to return the specific requirement.
+   *
+   * @return The minimum number of required points.
+   */
+  [[nodiscard]] virtual size_t minimum_required_points() const = 0;
+
+  /**
+   * @brief Compute the interpolated value at the given point.
+   *
+   * @param s The point at which to compute the interpolated value.
+   * @return The interpolated value.
+   */
+  [[nodiscard]] T compute(const double & s) const
+  {
+    validate_compute_input(s);
+    return compute_impl(s);
+  }
+};
+}  // namespace autoware::motion_utils::trajectory_container::interpolator::detail
+
+// clang-format off
+#endif  // AUTOWARE__MOTION_UTILS__TRAJECTORY_CONTAINER__INTERPOLATOR__DETAIL__INTERPOLATOR_COMMON_INTERFACE_HPP_  // NOLINT
+// clang-format on

slave/src/universe/common/autoware_motion_utils/include/autoware/motion_utils/trajectory_container/interpolator/detail/interpolator_mixin.hpp

@@ -0,0 +1,90 @@
+// 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.
+
+// clang-format off
+#ifndef AUTOWARE__MOTION_UTILS__TRAJECTORY_CONTAINER__INTERPOLATOR__DETAIL__INTERPOLATOR_MIXIN_HPP_  // NOLINT
+#define AUTOWARE__MOTION_UTILS__TRAJECTORY_CONTAINER__INTERPOLATOR__DETAIL__INTERPOLATOR_MIXIN_HPP_  // NOLINT
+// clang-format on
+
+#include "autoware/motion_utils/trajectory_container/interpolator/interpolator.hpp"
+
+#include <Eigen/Dense>
+
+#include <memory>
+#include <optional>
+#include <utility>
+#include <vector>
+
+namespace autoware::motion_utils::trajectory_container::interpolator::detail
+{
+
+/**
+ * @brief Base class for interpolator implementations.
+ *
+ * This class implements the core functionality for interpolator implementations.
+ *
+ * @tparam InterpolatorType The type of the interpolator implementation.
+ * @tparam T The type of the values being interpolated.
+ */
+template <class InterpolatorType, class T>
+struct InterpolatorMixin : public InterpolatorInterface<T>
+{
+  std::shared_ptr<InterpolatorInterface<T>> clone() const override
+  {
+    return std::make_shared<InterpolatorType>(static_cast<const InterpolatorType &>(*this));
+  }
+
+  class Builder
+  {
+  private:
+    std::vector<double> bases_;
+    std::vector<T> values_;
+
+  public:
+    [[nodiscard]] Builder & set_bases(const Eigen::Ref<const Eigen::VectorXd> & bases)
+    {
+      bases_ = std::vector<double>(bases.begin(), bases.end());
+      return *this;
+    }
+
+    [[nodiscard]] Builder & set_bases(const std::vector<double> & bases)
+    {
+      bases_ = bases;
+      return *this;
+    }
+
+    [[nodiscard]] Builder & set_values(const std::vector<T> & values)
+    {
+      values_ = values;
+      return *this;
+    }
+
+    template <typename... Args>
+    [[nodiscard]] std::optional<InterpolatorType> build(Args &&... args)
+    {
+      auto interpolator = InterpolatorType(std::forward<Args>(args)...);
+      bool success = interpolator.build(bases_, values_);
+      if (!success) {
+        return std::nullopt;
+      }
+      return interpolator;
+    }
+  };
+};
+
+}  // namespace autoware::motion_utils::trajectory_container::interpolator::detail
+
+// clang-format off
+#endif  // AUTOWARE__MOTION_UTILS__TRAJECTORY_CONTAINER__INTERPOLATOR__DETAIL__INTERPOLATOR_MIXIN_HPP_  // NOLINT
+// clang-format on

slave/src/universe/common/autoware_motion_utils/include/autoware/motion_utils/trajectory_container/interpolator/detail/nearest_neighbor_common_impl.hpp

@@ -0,0 +1,85 @@
+// 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.
+
+// clang-format off
+#ifndef AUTOWARE__MOTION_UTILS__TRAJECTORY_CONTAINER__INTERPOLATOR__DETAIL__NEAREST_NEIGHBOR_COMMON_IMPL_HPP_  // NOLINT
+#define AUTOWARE__MOTION_UTILS__TRAJECTORY_CONTAINER__INTERPOLATOR__DETAIL__NEAREST_NEIGHBOR_COMMON_IMPL_HPP_  // NOLINT
+// clang-format on
+
+#include "autoware/motion_utils/trajectory_container/interpolator/detail/interpolator_mixin.hpp"
+
+#include <vector>
+
+namespace autoware::motion_utils::trajectory_container::interpolator
+{
+
+template <typename T>
+class NearestNeighbor;
+
+namespace detail
+{
+/**
+ * @brief Common Implementation of nearest neighbor.
+ *
+ * This class implements the core functionality for nearest neighbor interpolation.
+ *
+ * @tparam T The type of the values being interpolated.
+ */
+template <typename T>
+class NearestNeighborCommonImpl : public detail::InterpolatorMixin<NearestNeighbor<T>, T>
+{
+protected:
+  std::vector<T> values_;  ///< Interpolation values.
+
+  /**
+   * @brief Compute the interpolated value at the given point.
+   *
+   * @param s The point at which to compute the interpolated value.
+   * @return The interpolated value.
+   */
+  [[nodiscard]] T compute_impl(const double & s) const override
+  {
+    const int32_t idx = this->get_index(s);
+    return (std::abs(s - this->bases_[idx]) <= std::abs(s - this->bases_[idx + 1]))
+             ? this->values_.at(idx)
+             : this->values_.at(idx + 1);
+  }
+
+  /**
+   * @brief Build the interpolator with the given values.
+   *
+   * @param bases The bases values.
+   * @param values The values to interpolate.
+   * @return True if the interpolator was built successfully, false otherwise.
+   */
+  void build_impl(const std::vector<double> & bases, const std::vector<T> & values) override
+  {
+    this->bases_ = bases;
+    this->values_ = values;
+  }
+
+public:
+  /**
+   * @brief Get the minimum number of required points for the interpolator.
+   *
+   * @return The minimum number of required points.
+   */
+  [[nodiscard]] size_t minimum_required_points() const override { return 1; }
+};
+
+}  // namespace detail
+}  // namespace autoware::motion_utils::trajectory_container::interpolator
+// clang-format off
+#endif  // AUTOWARE__MOTION_UTILS__TRAJECTORY_CONTAINER__INTERPOLATOR__DETAIL__NEAREST_NEIGHBOR_COMMON_IMPL_HPP_  // NOLINT
+// clang-format on

slave/src/universe/common/autoware_motion_utils/include/autoware/motion_utils/trajectory_container/interpolator/detail/stairstep_common_impl.hpp

@@ -0,0 +1,85 @@
+// 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.
+
+// clang-format off
+#ifndef AUTOWARE__MOTION_UTILS__TRAJECTORY_CONTAINER__INTERPOLATOR__DETAIL__STAIRSTEP_COMMON_IMPL_HPP_  // NOLINT
+#define AUTOWARE__MOTION_UTILS__TRAJECTORY_CONTAINER__INTERPOLATOR__DETAIL__STAIRSTEP_COMMON_IMPL_HPP_  // NOLINT
+// clang-format on
+
+#include "autoware/motion_utils/trajectory_container/interpolator/detail/interpolator_mixin.hpp"
+
+#include <vector>
+
+namespace autoware::motion_utils::trajectory_container::interpolator
+{
+
+template <typename T>
+class Stairstep;
+
+namespace detail
+{
+
+/**
+ * @brief Base class for stairstep interpolation.
+ *
+ * This class implements the core functionality for stairstep interpolation.
+ *
+ * @tparam T The type of the values being interpolated.
+ */
+template <typename T>
+class StairstepCommonImpl : public detail::InterpolatorMixin<Stairstep<T>, T>
+{
+protected:
+  std::vector<T> values_;  ///< Interpolation values.
+
+  /**
+   * @brief Compute the interpolated value at the given point.
+   *
+   * @param s The point at which to compute the interpolated value.
+   * @return The interpolated value.
+   */
+  [[nodiscard]] T compute_impl(const double & s) const override
+  {
+    const int32_t idx = this->get_index(s, false);
+    return this->values_.at(idx);
+  }
+  /**
+   * @brief Build the interpolator with the given values.
+   *
+   * @param bases The bases values.
+   * @param values The values to interpolate.
+   */
+  void build_impl(const std::vector<double> & bases, const std::vector<T> & values) override
+  {
+    this->bases_ = bases;
+    this->values_ = values;
+  }
+
+public:
+  /**
+   * @brief Default constructor.
+   */
+  StairstepCommonImpl() = default;
+
+  /**
+   * @brief Get the minimum number of required points for the interpolator.
+   */
+  [[nodiscard]] size_t minimum_required_points() const override { return 2; }
+};
+}  // namespace detail
+}  // namespace autoware::motion_utils::trajectory_container::interpolator
+
+// clang-format off
+#endif  // AUTOWARE__MOTION_UTILS__TRAJECTORY_CONTAINER__INTERPOLATOR__DETAIL__STAIRSTEP_COMMON_IMPL_HPP_  // NOLINT
+// clang-format on

slave/src/universe/common/autoware_motion_utils/include/autoware/motion_utils/trajectory_container/interpolator/interpolator.hpp

@@ -0,0 +1,97 @@
+// 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.
+
+#ifndef AUTOWARE__MOTION_UTILS__TRAJECTORY_CONTAINER__INTERPOLATOR__INTERPOLATOR_HPP_
+#define AUTOWARE__MOTION_UTILS__TRAJECTORY_CONTAINER__INTERPOLATOR__INTERPOLATOR_HPP_
+
+#include "autoware/motion_utils/trajectory_container/interpolator/detail/interpolator_common_interface.hpp"
+
+#include <memory>
+
+namespace autoware::motion_utils::trajectory_container::interpolator
+{
+/**
+ * @brief Template class for interpolation.
+ *
+ * This class serves as the base class for specific interpolation types.
+ *
+ * @tparam T The type of the values being interpolated. (e.g. double, int, etc.)
+ */
+template <typename T>
+class InterpolatorInterface : public detail::InterpolatorCommonInterface<T>
+{
+public:
+  [[nodiscard]] virtual std::shared_ptr<InterpolatorInterface<T>> clone() const = 0;
+};
+
+/**
+ * @brief Specialization of Interpolator for double values.
+ *
+ * This class adds methods for computing first and second derivatives.
+ */
+template <>
+class InterpolatorInterface<double> : public detail::InterpolatorCommonInterface<double>
+{
+protected:
+  /**
+   * @brief Compute the first derivative at the given point.
+   *
+   * This method should be overridden by subclasses to provide the specific logic.
+   *
+   * @param s The point at which to compute the first derivative.
+   * @return The first derivative.
+   */
+  [[nodiscard]] virtual double compute_first_derivative_impl(const double & s) const = 0;
+
+  /**
+   * @brief Compute the second derivative at the given point.
+   *
+   * This method should be overridden by subclasses to provide the specific logic.
+   *
+   * @param s The point at which to compute the second derivative.
+   * @return The second derivative.
+   */
+  [[nodiscard]] virtual double compute_second_derivative_impl(const double & s) const = 0;
+
+public:
+  /**
+   * @brief Compute the first derivative at the given point.
+   *
+   * @param s The point at which to compute the first derivative.
+   * @return The first derivative.
+   */
+  [[nodiscard]] double compute_first_derivative(const double & s) const
+  {
+    this->validate_compute_input(s);
+    return compute_first_derivative_impl(s);
+  }
+
+  /**
+   * @brief Compute the second derivative at the given point.
+   *
+   * @param s The point at which to compute the second derivative.
+   * @return The second derivative.
+   */
+  [[nodiscard]] double compute_second_derivative(const double & s) const
+  {
+    this->validate_compute_input(s);
+    return compute_second_derivative_impl(s);
+  }
+
+  [[nodiscard]] virtual std::shared_ptr<InterpolatorInterface<double>> clone() const = 0;
+};
+
+}  // namespace autoware::motion_utils::trajectory_container::interpolator
+
+#endif  // AUTOWARE__MOTION_UTILS__TRAJECTORY_CONTAINER__INTERPOLATOR__INTERPOLATOR_HPP_

slave/src/universe/common/autoware_motion_utils/include/autoware/motion_utils/trajectory_container/interpolator/linear.hpp

@@ -0,0 +1,86 @@
+// 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.
+
+#ifndef AUTOWARE__MOTION_UTILS__TRAJECTORY_CONTAINER__INTERPOLATOR__LINEAR_HPP_
+#define AUTOWARE__MOTION_UTILS__TRAJECTORY_CONTAINER__INTERPOLATOR__LINEAR_HPP_
+
+#include "autoware/motion_utils/trajectory_container/interpolator/detail/interpolator_mixin.hpp"
+
+#include <Eigen/Dense>
+
+#include <vector>
+
+namespace autoware::motion_utils::trajectory_container::interpolator
+{
+
+/**
+ * @brief Class for linear interpolation.
+ *
+ * This class provides methods to perform linear interpolation on a set of data points.
+ */
+class Linear : public detail::InterpolatorMixin<Linear, double>
+{
+private:
+  Eigen::VectorXd values_;  ///< Interpolation values.
+
+  /**
+   * @brief Build the interpolator with the given values.
+   *
+   * @param bases The bases values.
+   * @param values The values to interpolate.
+   * @return True if the interpolator was built successfully, false otherwise.
+   */
+  void build_impl(const std::vector<double> & bases, const std::vector<double> & values) override;
+
+  /**
+   * @brief Compute the interpolated value at the given point.
+   *
+   * @param s The point at which to compute the interpolated value.
+   * @return The interpolated value.
+   */
+  [[nodiscard]] double compute_impl(const double & s) const override;
+
+  /**
+   * @brief Compute the first derivative at the given point.
+   *
+   * @param s The point at which to compute the first derivative.
+   * @return The first derivative.
+   */
+  [[nodiscard]] double compute_first_derivative_impl(const double & s) const override;
+
+  /**
+   * @brief Compute the second derivative at the given point.
+   *
+   * @param s The point at which to compute the second derivative.
+   * @return The second derivative.
+   */
+  [[nodiscard]] double compute_second_derivative_impl(const double &) const override;
+
+public:
+  /**
+   * @brief Default constructor.
+   */
+  Linear() = default;
+
+  /**
+   * @brief Get the minimum number of required points for the interpolator.
+   *
+   * @return The minimum number of required points.
+   */
+  [[nodiscard]] size_t minimum_required_points() const override;
+};
+
+}  // namespace autoware::motion_utils::trajectory_container::interpolator
+
+#endif  // AUTOWARE__MOTION_UTILS__TRAJECTORY_CONTAINER__INTERPOLATOR__LINEAR_HPP_

slave/src/universe/common/autoware_motion_utils/include/autoware/motion_utils/trajectory_container/interpolator/nearest_neighbor.hpp

@@ -0,0 +1,78 @@
+// 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.
+
+#ifndef AUTOWARE__MOTION_UTILS__TRAJECTORY_CONTAINER__INTERPOLATOR__NEAREST_NEIGHBOR_HPP_
+#define AUTOWARE__MOTION_UTILS__TRAJECTORY_CONTAINER__INTERPOLATOR__NEAREST_NEIGHBOR_HPP_
+
+#include "autoware/motion_utils/trajectory_container/interpolator/detail/nearest_neighbor_common_impl.hpp"
+
+namespace autoware::motion_utils::trajectory_container::interpolator
+{
+
+/**
+ * @brief Template class for nearest neighbor interpolation.
+ *
+ * This class provides methods to perform nearest neighbor interpolation on a set of data points.
+ *
+ * @tparam T The type of the values being interpolated.
+ */
+template <typename T>
+class NearestNeighbor;
+
+/**
+ * @brief Template class for nearest neighbor interpolation.
+ *
+ * This class provides the interface for nearest neighbor interpolation.
+ *
+ * @tparam T The type of the values being interpolated.
+ */
+template <typename T>
+class NearestNeighbor : public detail::NearestNeighborCommonImpl<T>
+{
+public:
+  NearestNeighbor() = default;
+};
+
+/**
+ * @brief Specialization of NearestNeighbor for double values.
+ *
+ * This class provides methods to perform nearest neighbor interpolation on double values.
+ */
+template <>
+class NearestNeighbor<double> : public detail::NearestNeighborCommonImpl<double>
+{
+private:
+  /**
+   * @brief Compute the first derivative at the given point.
+   *
+   * @param s The point at which to compute the first derivative.
+   * @return The first derivative.
+   */
+  [[nodiscard]] double compute_first_derivative_impl(const double &) const override { return 0.0; }
+
+  /**
+   * @brief Compute the second derivative at the given point.
+   *
+   * @param s The point at which to compute the second derivative.
+   * @return The second derivative.
+   */
+  [[nodiscard]] double compute_second_derivative_impl(const double &) const override { return 0.0; }
+
+public:
+  NearestNeighbor() = default;
+};
+
+}  // namespace autoware::motion_utils::trajectory_container::interpolator
+
+#endif  // AUTOWARE__MOTION_UTILS__TRAJECTORY_CONTAINER__INTERPOLATOR__NEAREST_NEIGHBOR_HPP_

slave/src/universe/common/autoware_motion_utils/include/autoware/motion_utils/trajectory_container/interpolator/spherical_linear.hpp

@@ -0,0 +1,73 @@
+// 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.
+
+#ifndef AUTOWARE__MOTION_UTILS__TRAJECTORY_CONTAINER__INTERPOLATOR__SPHERICAL_LINEAR_HPP_
+#define AUTOWARE__MOTION_UTILS__TRAJECTORY_CONTAINER__INTERPOLATOR__SPHERICAL_LINEAR_HPP_
+
+#include "autoware/motion_utils/trajectory_container/interpolator/detail/interpolator_mixin.hpp"
+
+#include <geometry_msgs/msg/quaternion.hpp>
+
+#include <vector>
+
+namespace autoware::motion_utils::trajectory_container::interpolator
+{
+
+/**
+ * @brief Class for SphericalLinear interpolation.
+ *
+ * This class provides methods to perform SphericalLinear interpolation on a set of data points.
+ */
+class SphericalLinear
+: public detail::InterpolatorMixin<SphericalLinear, geometry_msgs::msg::Quaternion>
+{
+private:
+  std::vector<geometry_msgs::msg::Quaternion> quaternions_;
+
+  /**
+   * @brief Build the interpolator with the given values.
+   *
+   * @param bases The bases values.
+   * @param values The values to interpolate.
+   * @return True if the interpolator was built successfully, false otherwise.
+   */
+  void build_impl(
+    const std::vector<double> & bases,
+    const std::vector<geometry_msgs::msg::Quaternion> & quaternions) override;
+
+  /**
+   * @brief Compute the interpolated value at the given point.
+   *
+   * @param s The point at which to compute the interpolated value.
+   * @return The interpolated value.
+   */
+  [[nodiscard]] geometry_msgs::msg::Quaternion compute_impl(const double & s) const override;
+
+public:
+  /**
+   * @brief Default constructor.
+   */
+  SphericalLinear() = default;
+
+  /**
+   * @brief Get the minimum number of required points for the interpolator.
+   *
+   * @return The minimum number of required points.
+   */
+  [[nodiscard]] size_t minimum_required_points() const override;
+};
+
+}  // namespace autoware::motion_utils::trajectory_container::interpolator
+
+#endif  // AUTOWARE__MOTION_UTILS__TRAJECTORY_CONTAINER__INTERPOLATOR__SPHERICAL_LINEAR_HPP_

slave/src/universe/common/autoware_motion_utils/include/autoware/motion_utils/trajectory_container/interpolator/stairstep.hpp

@@ -0,0 +1,78 @@
+// 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.
+
+#ifndef AUTOWARE__MOTION_UTILS__TRAJECTORY_CONTAINER__INTERPOLATOR__STAIRSTEP_HPP_
+#define AUTOWARE__MOTION_UTILS__TRAJECTORY_CONTAINER__INTERPOLATOR__STAIRSTEP_HPP_
+
+#include "autoware/motion_utils/trajectory_container/interpolator/detail/stairstep_common_impl.hpp"
+
+namespace autoware::motion_utils::trajectory_container::interpolator
+{
+
+/**
+ * @brief Template class for stairstep interpolation.
+ *
+ * This class provides methods to perform stairstep interpolation on a set of data points.
+ *
+ * @tparam T The type of the values being interpolated.
+ */
+template <typename T>
+class Stairstep;
+
+/**
+ * @brief Template class for stairstep interpolation.
+ *
+ * This class provides the interface for stairstep interpolation.
+ *
+ * @tparam T The type of the values being interpolated.
+ */
+template <typename T>
+class Stairstep : public detail::StairstepCommonImpl<T>
+{
+public:
+  Stairstep() = default;
+};
+
+/**
+ * @brief Specialization of Stairstep for double values.
+ *
+ * This class provides methods to perform stairstep interpolation on double values.
+ */
+template <>
+class Stairstep<double> : public detail::StairstepCommonImpl<double>
+{
+private:
+  /**
+   * @brief Compute the first derivative at the given point.
+   *
+   * @param s The point at which to compute the first derivative.
+   * @return The first derivative.
+   */
+  [[nodiscard]] double compute_first_derivative_impl(const double &) const override { return 0.0; }
+
+  /**
+   * @brief Compute the second derivative at the given point.
+   *
+   * @param s The point at which to compute the second derivative.
+   * @return The second derivative.
+   */
+  [[nodiscard]] double compute_second_derivative_impl(const double &) const override { return 0.0; }
+
+public:
+  Stairstep() = default;
+};
+
+}  // namespace autoware::motion_utils::trajectory_container::interpolator
+
+#endif  // AUTOWARE__MOTION_UTILS__TRAJECTORY_CONTAINER__INTERPOLATOR__STAIRSTEP_HPP_

slave/src/universe/common/autoware_motion_utils/include/autoware/motion_utils/vehicle/vehicle_state_checker.hpp

@@ -0,0 +1,82 @@
+// 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__VEHICLE__VEHICLE_STATE_CHECKER_HPP_
+#define AUTOWARE__MOTION_UTILS__VEHICLE__VEHICLE_STATE_CHECKER_HPP_
+
+#include <rclcpp/rclcpp.hpp>
+
+#include <autoware_planning_msgs/msg/trajectory.hpp>
+#include <geometry_msgs/msg/twist_stamped.hpp>
+#include <nav_msgs/msg/odometry.hpp>
+
+#include <deque>
+
+namespace autoware::motion_utils
+{
+
+using autoware_planning_msgs::msg::Trajectory;
+using geometry_msgs::msg::TwistStamped;
+using nav_msgs::msg::Odometry;
+
+class VehicleStopCheckerBase
+{
+public:
+  VehicleStopCheckerBase(rclcpp::Node * node, double buffer_duration);
+  rclcpp::Logger getLogger() { return logger_; }
+  void addTwist(const TwistStamped & twist);
+  bool isVehicleStopped(const double stop_duration = 0.0) const;
+
+protected:
+  rclcpp::Clock::SharedPtr clock_;
+  rclcpp::Logger logger_;
+
+private:
+  double buffer_duration_;
+  std::deque<TwistStamped> twist_buffer_;
+};
+
+class VehicleStopChecker : public VehicleStopCheckerBase
+{
+public:
+  explicit VehicleStopChecker(rclcpp::Node * node);
+
+protected:
+  rclcpp::Subscription<Odometry>::SharedPtr sub_odom_;
+  Odometry::ConstSharedPtr odometry_ptr_;
+
+private:
+  static constexpr double velocity_buffer_time_sec = 10.0;
+  void onOdom(const Odometry::ConstSharedPtr msg);
+};
+
+class VehicleArrivalChecker : public VehicleStopChecker
+{
+public:
+  explicit VehicleArrivalChecker(rclcpp::Node * node);
+
+  bool isVehicleStoppedAtStopPoint(const double stop_duration = 0.0) const;
+
+private:
+  static constexpr double th_arrived_distance_m = 1.0;
+
+  rclcpp::Subscription<Trajectory>::SharedPtr sub_trajectory_;
+
+  Trajectory::ConstSharedPtr trajectory_ptr_;
+
+  void onTrajectory(const Trajectory::ConstSharedPtr msg);
+};
+}  // namespace autoware::motion_utils
+
+#endif  // AUTOWARE__MOTION_UTILS__VEHICLE__VEHICLE_STATE_CHECKER_HPP_

slave/src/universe/common/autoware_motion_utils/media/ego_nearest_search.svg

@@ -0,0 +1,280 @@
+<?xml version="1.0" encoding="UTF-8"?>
+<!DOCTYPE svg PUBLIC "-//W3C//DTD SVG 1.1//EN" "http://www.w3.org/Graphics/SVG/1.1/DTD/svg11.dtd">
+<svg
+  xmlns="http://www.w3.org/2000/svg"
+  xmlns:xlink="http://www.w3.org/1999/xlink"
+  version="1.1"
+  width="957px"
+  height="472px"
+  viewBox="-0.5 -0.5 957 472"
+  content="&lt;mxfile host=&quot;Electron&quot; modified=&quot;2022-08-15T04:23:17.706Z&quot; agent=&quot;5.0 (X11; Linux x86_64) AppleWebKit/537.36 (KHTML, like Gecko) draw.io/13.0.1 Chrome/80.0.3987.163 Electron/8.2.1 Safari/537.36&quot; etag=&quot;hdvt5EoOSeSXZq3sAR4b&quot; version=&quot;13.0.1&quot; type=&quot;device&quot;&gt;&lt;diagram id=&quot;FmUAXtwAyzemAjJw8FDx&quot; name=&quot;Page-1&quot;&gt;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&lt;/diagram&gt;&lt;/mxfile&gt;"
+>
+  <defs/>
+  <g>
+    <rect x="805" y="105" width="50" height="30" fill-opacity="0.5" fill="#f5f5f5" stroke="#000000" stroke-opacity="0.5" transform="rotate(-186,830,120)" pointer-events="all"/>
+    <path
+      d="M 238 239.35 L 238 427 Q 238 437 248 437 L 448 437 Q 458 437 458 427 L 458 387 Q 458 377 458 367 L 458 327 Q 458 317 448 317 L 18 317"
+      fill="none"
+      stroke="#97d077"
+      stroke-width="3"
+      stroke-miterlimit="10"
+      pointer-events="stroke"
+    />
+    <path d="M 238 230.35 L 242.5 239.35 L 233.5 239.35 Z" fill="#97d077" stroke="#97d077" stroke-width="3" stroke-miterlimit="10" pointer-events="all"/>
+    <ellipse cx="238" cy="333.33" rx="5" ry="5" fill="#ff3333" stroke="#b85450" pointer-events="all"/>
+    <rect x="862" y="110" width="40" height="20" fill="none" stroke="none" pointer-events="all"/>
+    <g transform="translate(-0.5 -0.5)">
+      <switch>
+        <foreignObject style="overflow: visible; text-align: left;" pointer-events="none" width="100%" height="100%" requiredFeatures="http://www.w3.org/TR/SVG11/feature#Extensibility">
+          <div
+            xmlns="http://www.w3.org/1999/xhtml"
+            style="display: flex; align-items: unsafe center; justify-content: unsafe center; width: 38px; height: 1px; padding-top: 120px; margin-left: 863px;"
+          >
+            <div style="box-sizing: border-box; font-size: 0; text-align: center; ">
+              <div style="display: inline-block; font-size: 12px; font-family: Helvetica; color: #FF3333; line-height: 1.2; pointer-events: all; white-space: normal; word-wrap: normal; ">nearest</div>
+            </div>
+          </div>
+        </foreignObject>
+        <text x="882" y="124" fill="#FF3333" font-family="Helvetica" font-size="12px" text-anchor="middle">nearest</text>
+      </switch>
+    </g>
+    <ellipse cx="211" cy="98" rx="95" ry="95" fill="none" stroke="#3333ff" stroke-dasharray="3 3" pointer-events="all"/>
+    <path d="M 254.38 44.51 Q 276 57 284 80 Q 292 103 278.04 127.42" fill="none" stroke="#3333ff" stroke-miterlimit="10" pointer-events="stroke"/>
+    <path d="M 250.92 42.51 L 255.38 42.78 L 253.38 46.24 Z" fill="#3333ff" stroke="#3333ff" stroke-miterlimit="10" pointer-events="all"/>
+    <path d="M 276.55 130.03 L 276.8 125.56 L 278.04 127.42 L 280.28 127.55 Z" fill="#3333ff" stroke="#3333ff" stroke-miterlimit="10" pointer-events="all"/>
+    <g transform="translate(-0.5 -0.5)">
+      <switch>
+        <foreignObject style="overflow: visible; text-align: left;" pointer-events="none" width="100%" height="100%" requiredFeatures="http://www.w3.org/TR/SVG11/feature#Extensibility">
+          <div xmlns="http://www.w3.org/1999/xhtml" style="display: flex; align-items: unsafe center; justify-content: unsafe center; width: 1px; height: 1px; padding-top: 75px; margin-left: 277px;">
+            <div style="box-sizing: border-box; font-size: 0; text-align: center; ">
+              <div style="display: inline-block; font-size: 9px; font-family: Helvetica; color: #3333FF; line-height: 1.2; pointer-events: all; background-color: #ffffff; white-space: nowrap; ">
+                2 * yaw
+                <br style="font-size: 9px"/>
+                threshold
+              </div>
+            </div>
+          </div>
+        </foreignObject>
+        <text x="277" y="77" fill="#3333FF" font-family="Helvetica" font-size="9px" text-anchor="middle">2 * yaw...</text>
+      </switch>
+    </g>
+    <path
+      d="M 188 107 L 518 107 Q 528 107 528 117 L 528 187 Q 528 197 518 197 L 458 197 Q 448 197 440.19 190.75 L 355.81 123.25 Q 348 117 338 117 L 20.35 117"
+      fill="none"
+      stroke="#97d077"
+      stroke-width="3"
+      stroke-miterlimit="10"
+      pointer-events="stroke"
+    />
+    <path d="M 11.35 117 L 20.35 112.5 L 20.35 121.5 Z" fill="#97d077" stroke="#97d077" stroke-width="3" stroke-miterlimit="10" pointer-events="all"/>
+    <ellipse cx="33" cy="107" rx="5" ry="5" fill="#b9ff92" stroke="#000000" pointer-events="all"/>
+    <path d="M 8 107 L 28 107" fill="none" stroke="#97d077" stroke-width="3" stroke-miterlimit="10" pointer-events="stroke"/>
+    <ellipse cx="183" cy="107" rx="5" ry="5" fill="#ff3333" stroke="#b85450" pointer-events="all"/>
+    <path d="M 38 107 L 178 107" fill="none" stroke="#97d077" stroke-width="3" stroke-miterlimit="10" pointer-events="stroke"/>
+    <ellipse cx="263" cy="107" rx="5" ry="5" fill="#b9ff92" stroke="#000000" pointer-events="all"/>
+    <ellipse cx="343" cy="107" rx="5" ry="5" fill="#b9ff92" stroke="#000000" pointer-events="all"/>
+    <ellipse cx="423" cy="107" rx="5" ry="5" fill="#b9ff92" stroke="#000000" pointer-events="all"/>
+    <ellipse cx="493" cy="107" rx="5" ry="5" fill="#b9ff92" stroke="#000000" pointer-events="all"/>
+    <ellipse cx="528" cy="139" rx="5" ry="5" fill="#b9ff92" stroke="#000000" pointer-events="all"/>
+    <ellipse cx="408" cy="165" rx="5" ry="5" fill="#b9ff92" stroke="#000000" pointer-events="all"/>
+    <ellipse cx="523" cy="196" rx="5" ry="5" fill="#b9ff92" stroke="#000000" pointer-events="all"/>
+    <ellipse cx="465" cy="197" rx="5" ry="5" fill="#b9ff92" stroke="#000000" pointer-events="all"/>
+    <ellipse cx="193" cy="117" rx="5" ry="5" fill="#b9ff92" stroke="#000000" pointer-events="all"/>
+    <ellipse cx="111" cy="117" rx="5" ry="5" fill="#b9ff92" stroke="#000000" pointer-events="all"/>
+    <ellipse cx="272" cy="117" rx="5" ry="5" fill="#b9ff92" stroke="#000000" pointer-events="all"/>
+    <ellipse cx="356" cy="124" rx="5" ry="5" fill="#b9ff92" stroke="#000000" pointer-events="all"/>
+    <ellipse cx="45" cy="118" rx="5" ry="5" fill="#b9ff92" stroke="#000000" pointer-events="all"/>
+    <rect x="148" y="84.67" width="40" height="20" fill="none" stroke="none" pointer-events="all"/>
+    <g transform="translate(-0.5 -0.5)">
+      <switch>
+        <foreignObject style="overflow: visible; text-align: left;" pointer-events="none" width="100%" height="100%" requiredFeatures="http://www.w3.org/TR/SVG11/feature#Extensibility">
+          <div xmlns="http://www.w3.org/1999/xhtml" style="display: flex; align-items: unsafe center; justify-content: unsafe center; width: 38px; height: 1px; padding-top: 95px; margin-left: 149px;">
+            <div style="box-sizing: border-box; font-size: 0; text-align: center; ">
+              <div style="display: inline-block; font-size: 12px; font-family: Helvetica; color: #FF3333; line-height: 1.2; pointer-events: all; white-space: normal; word-wrap: normal; ">nearest</div>
+            </div>
+          </div>
+        </foreignObject>
+        <text x="168" y="98" fill="#FF3333" font-family="Helvetica" font-size="12px" text-anchor="middle">nearest</text>
+      </switch>
+    </g>
+    <ellipse cx="103" cy="107" rx="5" ry="5" fill="#b9ff92" stroke="#000000" pointer-events="all"/>
+    <path
+      d="M 580.35 113 L 828 113 Q 838 113 848 113 L 859 113 Q 869 113 868.5 103.01 L 868.25 98.01 Q 868 93 858 93 L 853 93 Q 848 93 838 93 L 818 93"
+      fill="none"
+      stroke="#97d077"
+      stroke-width="3"
+      stroke-miterlimit="10"
+      pointer-events="stroke"
+    />
+    <path d="M 571.35 113 L 580.35 108.5 L 580.35 117.5 Z" fill="#97d077" stroke="#97d077" stroke-width="3" stroke-miterlimit="10" pointer-events="all"/>
+    <ellipse cx="59" cy="317" rx="5" ry="5" fill="#b9ff92" stroke="#000000" pointer-events="all"/>
+    <ellipse cx="149" cy="318" rx="5" ry="5" fill="#b9ff92" stroke="#000000" pointer-events="all"/>
+    <ellipse cx="251" cy="317" rx="5" ry="5" fill="#b9ff92" stroke="#000000" pointer-events="all"/>
+    <ellipse cx="238" cy="250" rx="5" ry="5" fill="#b9ff92" stroke="#000000" pointer-events="all"/>
+    <ellipse cx="359" cy="317" rx="5" ry="5" fill="#b9ff92" stroke="#000000" pointer-events="all"/>
+    <ellipse cx="458" cy="338" rx="5" ry="5" fill="#b9ff92" stroke="#000000" pointer-events="all"/>
+    <ellipse cx="459" cy="420" rx="5" ry="5" fill="#b9ff92" stroke="#000000" pointer-events="all"/>
+    <ellipse cx="245" cy="437" rx="5" ry="5" fill="#b9ff92" stroke="#000000" pointer-events="all"/>
+    <ellipse cx="361" cy="437" rx="5" ry="5" fill="#b9ff92" stroke="#000000" pointer-events="all"/>
+    <rect x="206" y="313" width="50" height="30" fill-opacity="0.5" fill="#f5f5f5" stroke="#000000" stroke-opacity="0.5" transform="rotate(-96,231,328)" pointer-events="all"/>
+    <path
+      d="M 934.65 317 L 691 317 Q 681 317 681 327 L 681 452 Q 681 462 691 462 L 835 462 Q 845 462 845 452 L 845 318 Q 845 308 835 308 L 578 308"
+      fill="none"
+      stroke="#97d077"
+      stroke-width="3"
+      stroke-miterlimit="10"
+      pointer-events="stroke"
+    />
+    <path d="M 943.65 317 L 934.65 321.5 L 934.65 312.5 Z" fill="#97d077" stroke="#97d077" stroke-width="3" stroke-miterlimit="10" pointer-events="all"/>
+    <rect x="248" y="323.33" width="40" height="20" fill="none" stroke="none" pointer-events="all"/>
+    <g transform="translate(-0.5 -0.5)">
+      <switch>
+        <foreignObject style="overflow: visible; text-align: left;" pointer-events="none" width="100%" height="100%" requiredFeatures="http://www.w3.org/TR/SVG11/feature#Extensibility">
+          <div
+            xmlns="http://www.w3.org/1999/xhtml"
+            style="display: flex; align-items: unsafe center; justify-content: unsafe center; width: 38px; height: 1px; padding-top: 333px; margin-left: 249px;"
+          >
+            <div style="box-sizing: border-box; font-size: 0; text-align: center; ">
+              <div style="display: inline-block; font-size: 12px; font-family: Helvetica; color: #FF3333; line-height: 1.2; pointer-events: all; white-space: normal; word-wrap: normal; ">nearest</div>
+            </div>
+          </div>
+        </foreignObject>
+        <text x="268" y="337" fill="#FF3333" font-family="Helvetica" font-size="12px" text-anchor="middle">nearest</text>
+      </switch>
+    </g>
+    <ellipse cx="853" cy="113" rx="5" ry="5" fill="#ff3333" stroke="#b85450" pointer-events="all"/>
+    <ellipse cx="837" cy="93" rx="5" ry="5" fill="#b9ff92" stroke="#000000" pointer-events="all"/>
+    <ellipse cx="788" cy="113" rx="5" ry="5" fill="#b9ff92" stroke="#000000" pointer-events="all"/>
+    <ellipse cx="728" cy="113" rx="5" ry="5" fill="#b9ff92" stroke="#000000" pointer-events="all"/>
+    <ellipse cx="665" cy="113" rx="5" ry="5" fill="#b9ff92" stroke="#000000" pointer-events="all"/>
+    <ellipse cx="597" cy="308" rx="5" ry="5" fill="#b9ff92" stroke="#000000" pointer-events="all"/>
+    <ellipse cx="609" cy="112" rx="5" ry="5" fill="#b9ff92" stroke="#000000" pointer-events="all"/>
+    <ellipse cx="657" cy="308" rx="5" ry="5" fill="#b9ff92" stroke="#000000" pointer-events="all"/>
+    <ellipse cx="790" cy="308" rx="5" ry="5" fill="#b9ff92" stroke="#000000" pointer-events="all"/>
+    <ellipse cx="845" cy="331" rx="5" ry="5" fill="#b9ff92" stroke="#000000" pointer-events="all"/>
+    <ellipse cx="845" cy="382" rx="5" ry="5" fill="#b9ff92" stroke="#000000" pointer-events="all"/>
+    <ellipse cx="729" cy="462" rx="5" ry="5" fill="#b9ff92" stroke="#000000" pointer-events="all"/>
+    <ellipse cx="802" cy="462" rx="5" ry="5" fill="#b9ff92" stroke="#000000" pointer-events="all"/>
+    <ellipse cx="845" cy="432" rx="5" ry="5" fill="#b9ff92" stroke="#000000" pointer-events="all"/>
+    <ellipse cx="681" cy="429" rx="5" ry="5" fill="#b9ff92" stroke="#000000" pointer-events="all"/>
+    <ellipse cx="681" cy="368" rx="5" ry="5" fill="#b9ff92" stroke="#000000" pointer-events="all"/>
+    <ellipse cx="706" cy="317" rx="5" ry="5" fill="#b9ff92" stroke="#000000" pointer-events="all"/>
+    <ellipse cx="776" cy="317" rx="5" ry="5" fill="#b9ff92" stroke="#000000" pointer-events="all"/>
+    <ellipse cx="836" cy="317" rx="5" ry="5" fill="#b9ff92" stroke="#000000" pointer-events="all"/>
+    <ellipse cx="897" cy="317" rx="5" ry="5" fill="#b9ff92" stroke="#000000" pointer-events="all"/>
+    <rect x="677" y="283" width="40" height="20" fill="none" stroke="none" pointer-events="all"/>
+    <g transform="translate(-0.5 -0.5)">
+      <switch>
+        <foreignObject style="overflow: visible; text-align: left;" pointer-events="none" width="100%" height="100%" requiredFeatures="http://www.w3.org/TR/SVG11/feature#Extensibility">
+          <div
+            xmlns="http://www.w3.org/1999/xhtml"
+            style="display: flex; align-items: unsafe center; justify-content: unsafe center; width: 38px; height: 1px; padding-top: 293px; margin-left: 678px;"
+          >
+            <div style="box-sizing: border-box; font-size: 0; text-align: center; ">
+              <div style="display: inline-block; font-size: 12px; font-family: Helvetica; color: #FF3333; line-height: 1.2; pointer-events: all; white-space: normal; word-wrap: normal; ">nearest</div>
+            </div>
+          </div>
+        </foreignObject>
+        <text x="697" y="297" fill="#FF3333" font-family="Helvetica" font-size="12px" text-anchor="middle">nearest</text>
+      </switch>
+    </g>
+    <ellipse cx="719" cy="308" rx="5" ry="5" fill="#ff3333" stroke="#b85450" pointer-events="all"/>
+    <path d="M 221 302.53 L 237 313.53 L 221 324.53 Z" fill="none" stroke="#000000" stroke-opacity="0.5" stroke-miterlimit="10" transform="rotate(-96.3,229,313.53)" pointer-events="all"/>
+    <rect x="711" y="299.67" width="50" height="30" fill-opacity="0.5" fill="#f5f5f5" stroke="#000000" stroke-opacity="0.5" pointer-events="all"/>
+    <path d="M 742 303.67 L 758 314.67 L 742 325.67 Z" fill="none" stroke="#000000" stroke-opacity="0.5" stroke-miterlimit="10" pointer-events="all"/>
+    <rect x="196" y="82.67" width="50" height="30" fill-opacity="0.5" fill="#f5f5f5" stroke="#000000" stroke-opacity="0.5" transform="rotate(-15,221,97.67)" pointer-events="all"/>
+    <path d="M 227 82.67 L 243 93.67 L 227 104.67 Z" fill="none" stroke="#000000" stroke-opacity="0.5" stroke-miterlimit="10" transform="rotate(-15,235,93.67)" pointer-events="all"/>
+    <path d="M 808 111 L 824 122 L 808 133 Z" fill="none" stroke="#000000" stroke-opacity="0.5" stroke-miterlimit="10" transform="rotate(-185.7,816,122)" pointer-events="all"/>
+    <path d="M 234 344 L 168.54 289.89" fill="none" stroke="#3333ff" stroke-miterlimit="10" stroke-dasharray="3 3" pointer-events="stroke"/>
+    <path d="M 234 344 L 273.82 271.02" fill="none" stroke="#3333ff" stroke-miterlimit="10" stroke-dasharray="3 3" pointer-events="stroke"/>
+    <ellipse cx="229" cy="340" rx="95" ry="95" fill="none" stroke="#3333ff" stroke-dasharray="3 3" transform="rotate(-84.5,229,340)" pointer-events="all"/>
+    <path d="M 843 118 L 788.42 182.89" fill="none" stroke="#3333ff" stroke-miterlimit="10" stroke-dasharray="3 3" pointer-events="stroke"/>
+    <path d="M 843 118 L 768.63 77.79" fill="none" stroke="#3333ff" stroke-miterlimit="10" stroke-dasharray="3 3" pointer-events="stroke"/>
+    <ellipse cx="838" cy="122" rx="95" ry="95" fill="none" stroke="#3333ff" stroke-dasharray="3 3" transform="rotate(-175,838,122)" pointer-events="all"/>
+    <path d="M 720 316 L 784.4 260.64" fill="none" stroke="#3333ff" stroke-miterlimit="10" stroke-dasharray="3 3" pointer-events="stroke"/>
+    <path d="M 720 316 L 785.64 367.59" fill="none" stroke="#3333ff" stroke-miterlimit="10" stroke-dasharray="3 3" pointer-events="stroke"/>
+    <ellipse cx="725" cy="312" rx="95" ry="95" fill="none" stroke="#3333ff" stroke-dasharray="3 3" transform="rotate(15,725,312)" pointer-events="all"/>
+    <path d="M 206 102 L 255.08 33.02" fill="none" stroke="#3333ff" stroke-miterlimit="10" stroke-dasharray="3 3" pointer-events="stroke"/>
+    <path d="M 204.38 98.21 L 169.87 17.43" fill="none" stroke="#3333ff" stroke-miterlimit="10" pointer-events="stroke"/>
+    <path d="M 205.56 100.97 L 202.15 98.08 L 204.38 98.21 L 205.83 96.51 Z" fill="#3333ff" stroke="#3333ff" stroke-miterlimit="10" pointer-events="all"/>
+    <path d="M 168.69 14.67 L 172.1 17.56 L 169.87 17.43 L 168.42 19.13 Z" fill="#3333ff" stroke="#3333ff" stroke-miterlimit="10" pointer-events="all"/>
+    <g transform="translate(-0.5 -0.5)">
+      <switch>
+        <foreignObject style="overflow: visible; text-align: left;" pointer-events="none" width="100%" height="100%" requiredFeatures="http://www.w3.org/TR/SVG11/feature#Extensibility">
+          <div xmlns="http://www.w3.org/1999/xhtml" style="display: flex; align-items: unsafe center; justify-content: unsafe center; width: 1px; height: 1px; padding-top: 48px; margin-left: 184px;">
+            <div style="box-sizing: border-box; font-size: 0; text-align: center; ">
+              <div style="display: inline-block; font-size: 9px; font-family: Helvetica; color: #3333FF; line-height: 1.2; pointer-events: all; background-color: #ffffff; white-space: nowrap; ">
+                distance
+                <br style="font-size: 9px"/>
+                threshold
+              </div>
+            </div>
+          </div>
+        </foreignObject>
+        <text x="184" y="51" fill="#3333FF" font-family="Helvetica" font-size="9px" text-anchor="middle">distance...</text>
+      </switch>
+    </g>
+    <path d="M 206 102 L 283.96 136" fill="none" stroke="#3333ff" stroke-miterlimit="10" stroke-dasharray="3 3" pointer-events="stroke"/>
+    <rect x="17" y="3" width="40" height="20" fill="none" stroke="none" pointer-events="all"/>
+    <g transform="translate(-0.5 -0.5)">
+      <switch>
+        <foreignObject style="overflow: visible; text-align: left;" pointer-events="none" width="100%" height="100%" requiredFeatures="http://www.w3.org/TR/SVG11/feature#Extensibility">
+          <div xmlns="http://www.w3.org/1999/xhtml" style="display: flex; align-items: unsafe center; justify-content: unsafe center; width: 38px; height: 1px; padding-top: 13px; margin-left: 18px;">
+            <div style="box-sizing: border-box; font-size: 0; text-align: center; ">
+              <div style="display: inline-block; font-size: 23px; font-family: Helvetica; color: #000000; line-height: 1.2; pointer-events: all; white-space: normal; word-wrap: normal; ">1.</div>
+            </div>
+          </div>
+        </foreignObject>
+        <text x="37" y="20" fill="#000000" font-family="Helvetica" font-size="23px" text-anchor="middle">1.</text>
+      </switch>
+    </g>
+    <rect x="580" y="3" width="40" height="20" fill="none" stroke="none" pointer-events="all"/>
+    <g transform="translate(-0.5 -0.5)">
+      <switch>
+        <foreignObject style="overflow: visible; text-align: left;" pointer-events="none" width="100%" height="100%" requiredFeatures="http://www.w3.org/TR/SVG11/feature#Extensibility">
+          <div xmlns="http://www.w3.org/1999/xhtml" style="display: flex; align-items: unsafe center; justify-content: unsafe center; width: 38px; height: 1px; padding-top: 13px; margin-left: 581px;">
+            <div style="box-sizing: border-box; font-size: 0; text-align: center; ">
+              <div style="display: inline-block; font-size: 23px; font-family: Helvetica; color: #000000; line-height: 1.2; pointer-events: all; white-space: normal; word-wrap: normal; ">2.</div>
+            </div>
+          </div>
+        </foreignObject>
+        <text x="600" y="20" fill="#000000" font-family="Helvetica" font-size="23px" text-anchor="middle">2.</text>
+      </switch>
+    </g>
+    <rect x="17" y="225" width="40" height="20" fill="none" stroke="none" pointer-events="all"/>
+    <g transform="translate(-0.5 -0.5)">
+      <switch>
+        <foreignObject style="overflow: visible; text-align: left;" pointer-events="none" width="100%" height="100%" requiredFeatures="http://www.w3.org/TR/SVG11/feature#Extensibility">
+          <div xmlns="http://www.w3.org/1999/xhtml" style="display: flex; align-items: unsafe center; justify-content: unsafe center; width: 38px; height: 1px; padding-top: 235px; margin-left: 18px;">
+            <div style="box-sizing: border-box; font-size: 0; text-align: center; ">
+              <div style="display: inline-block; font-size: 23px; font-family: Helvetica; color: #000000; line-height: 1.2; pointer-events: all; white-space: normal; word-wrap: normal; ">3.</div>
+            </div>
+          </div>
+        </foreignObject>
+        <text x="37" y="242" fill="#000000" font-family="Helvetica" font-size="23px" text-anchor="middle">3.</text>
+      </switch>
+    </g>
+    <rect x="580" y="225" width="40" height="20" fill="none" stroke="none" pointer-events="all"/>
+    <g transform="translate(-0.5 -0.5)">
+      <switch>
+        <foreignObject style="overflow: visible; text-align: left;" pointer-events="none" width="100%" height="100%" requiredFeatures="http://www.w3.org/TR/SVG11/feature#Extensibility">
+          <div
+            xmlns="http://www.w3.org/1999/xhtml"
+            style="display: flex; align-items: unsafe center; justify-content: unsafe center; width: 38px; height: 1px; padding-top: 235px; margin-left: 581px;"
+          >
+            <div style="box-sizing: border-box; font-size: 0; text-align: center; ">
+              <div style="display: inline-block; font-size: 23px; font-family: Helvetica; color: #000000; line-height: 1.2; pointer-events: all; white-space: normal; word-wrap: normal; ">4.</div>
+            </div>
+          </div>
+        </foreignObject>
+        <text x="600" y="242" fill="#000000" font-family="Helvetica" font-size="23px" text-anchor="middle">4.</text>
+      </switch>
+    </g>
+  </g>
+  <switch>
+    <g requiredFeatures="http://www.w3.org/TR/SVG11/feature#Extensibility"/>
+    <a transform="translate(0,-5)" xlink:href="https://desk.draw.io/support/solutions/articles/16000042487" target="_blank">
+      <text text-anchor="middle" font-size="10px" x="50%" y="100%">Viewer does not support full SVG 1.1</text>
+    </a>
+  </switch>
+</svg>

slave/src/universe/common/autoware_motion_utils/media/segment.svg

@@ -0,0 +1,344 @@
+<?xml version="1.0" encoding="UTF-8"?>
+<!DOCTYPE svg PUBLIC "-//W3C//DTD SVG 1.1//EN" "http://www.w3.org/Graphics/SVG/1.1/DTD/svg11.dtd">
+<svg
+  xmlns="http://www.w3.org/2000/svg"
+  xmlns:xlink="http://www.w3.org/1999/xlink"
+  version="1.1"
+  width="433px"
+  height="221px"
+  viewBox="-0.5 -0.5 433 221"
+  content="&lt;mxfile host=&quot;Electron&quot; modified=&quot;2022-08-26T05:32:15.213Z&quot; agent=&quot;5.0 (X11; Linux x86_64) AppleWebKit/537.36 (KHTML, like Gecko) draw.io/13.0.1 Chrome/80.0.3987.163 Electron/8.2.1 Safari/537.36&quot; etag=&quot;d0TAoQKMk17RHRW6v7GV&quot; version=&quot;13.0.1&quot; type=&quot;device&quot;&gt;&lt;diagram id=&quot;FmUAXtwAyzemAjJw8FDx&quot; name=&quot;Page-1&quot;&gt;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&lt;/diagram&gt;&lt;/mxfile&gt;"
+  style="background-color: rgb(255, 255, 255);"
+>
+  <defs/>
+  <g>
+    <path d="M 236 49 Q 255 50 269.5 53 Q 284 56 301 64" fill="none" stroke="#0829ff" stroke-width="7" stroke-miterlimit="10" pointer-events="stroke"/>
+    <path d="M 386 209 Q 376 129 336 89 Q 296 49 231 49 Q 166 49 126 84 Q 86 119 76 209" fill="none" stroke="#97d077" stroke-width="3" stroke-miterlimit="10" pointer-events="stroke"/>
+    <ellipse cx="386" cy="210" rx="5" ry="5" fill="#b9ff92" stroke="#000000" pointer-events="all"/>
+    <ellipse cx="90" cy="140" rx="5" ry="5" fill="#b9ff92" stroke="#000000" pointer-events="all"/>
+    <ellipse cx="125" cy="86" rx="5" ry="5" fill="#b9ff92" stroke="#000000" pointer-events="all"/>
+    <ellipse cx="75" cy="209" rx="5" ry="5" fill="#b9ff92" stroke="#000000" pointer-events="all"/>
+    <ellipse cx="176" cy="56" rx="5" ry="5" fill="#b9ff92" stroke="#000000" pointer-events="all"/>
+    <ellipse cx="348" cy="101" rx="5" ry="5" fill="#b9ff92" stroke="#000000" pointer-events="all"/>
+    <ellipse cx="374" cy="152" rx="5" ry="5" fill="#b9ff92" stroke="#000000" pointer-events="all"/>
+    <ellipse cx="235.5" cy="49.5" rx="6.5" ry="6.5" fill="#b9ff92" stroke="#ff0000" stroke-width="3" pointer-events="all"/>
+    <ellipse cx="300" cy="63" rx="5" ry="5" fill="#b9ff92" stroke="#000000" pointer-events="all"/>
+    <rect x="0" y="199" width="66" height="20" fill="none" stroke="none" pointer-events="all"/>
+    <g transform="translate(-0.5 -0.5)">
+      <switch>
+        <foreignObject style="overflow: visible; text-align: left;" pointer-events="none" width="100%" height="100%" requiredFeatures="http://www.w3.org/TR/SVG11/feature#Extensibility">
+          <div xmlns="http://www.w3.org/1999/xhtml" style="display: flex; align-items: unsafe center; justify-content: unsafe center; width: 64px; height: 1px; padding-top: 209px; margin-left: 1px;">
+            <div style="box-sizing: border-box; font-size: 0; text-align: center; ">
+              <div style="display: inline-block; font-size: 9px; font-family: Helvetica; color: #FF3333; line-height: 1.2; pointer-events: all; white-space: normal; word-wrap: normal; ">
+                point index
+              </div>
+            </div>
+          </div>
+        </foreignObject>
+        <text x="33" y="212" fill="#FF3333" font-family="Helvetica" font-size="9px" text-anchor="middle">point index</text>
+      </switch>
+    </g>
+    <rect x="245" y="8" width="50" height="30" fill-opacity="0.5" fill="#f5f5f5" stroke="#000000" stroke-opacity="0.5" transform="rotate(8,270,23)" pointer-events="all"/>
+    <path d="M 276 14 L 292 25 L 276 36 Z" fill="none" stroke="#000000" stroke-opacity="0.5" stroke-miterlimit="10" transform="rotate(7.7,284,25)" pointer-events="all"/>
+    <rect x="46" y="124" width="66" height="20" fill="none" stroke="none" pointer-events="all"/>
+    <g transform="translate(-0.5 -0.5)">
+      <switch>
+        <foreignObject style="overflow: visible; text-align: left;" pointer-events="none" width="100%" height="100%" requiredFeatures="http://www.w3.org/TR/SVG11/feature#Extensibility">
+          <div xmlns="http://www.w3.org/1999/xhtml" style="display: flex; align-items: unsafe center; justify-content: unsafe center; width: 64px; height: 1px; padding-top: 134px; margin-left: 47px;">
+            <div style="box-sizing: border-box; font-size: 0; text-align: center; ">
+              <div style="display: inline-block; font-size: 9px; font-family: Helvetica; color: #FF3333; line-height: 1.2; pointer-events: all; white-space: normal; word-wrap: normal; ">1</div>
+            </div>
+          </div>
+        </foreignObject>
+        <text x="79" y="137" fill="#FF3333" font-family="Helvetica" font-size="9px" text-anchor="middle">1</text>
+      </switch>
+    </g>
+    <rect x="82" y="70" width="66" height="20" fill="none" stroke="none" pointer-events="all"/>
+    <g transform="translate(-0.5 -0.5)">
+      <switch>
+        <foreignObject style="overflow: visible; text-align: left;" pointer-events="none" width="100%" height="100%" requiredFeatures="http://www.w3.org/TR/SVG11/feature#Extensibility">
+          <div xmlns="http://www.w3.org/1999/xhtml" style="display: flex; align-items: unsafe center; justify-content: unsafe center; width: 64px; height: 1px; padding-top: 80px; margin-left: 83px;">
+            <div style="box-sizing: border-box; font-size: 0; text-align: center; ">
+              <div style="display: inline-block; font-size: 9px; font-family: Helvetica; color: #FF3333; line-height: 1.2; pointer-events: all; white-space: normal; word-wrap: normal; ">2</div>
+            </div>
+          </div>
+        </foreignObject>
+        <text x="115" y="83" fill="#FF3333" font-family="Helvetica" font-size="9px" text-anchor="middle">2</text>
+      </switch>
+    </g>
+    <rect x="78" y="171" width="96" height="20" fill="none" stroke="none" pointer-events="all"/>
+    <g transform="translate(-0.5 -0.5)">
+      <switch>
+        <foreignObject style="overflow: visible; text-align: left;" pointer-events="none" width="100%" height="100%" requiredFeatures="http://www.w3.org/TR/SVG11/feature#Extensibility">
+          <div xmlns="http://www.w3.org/1999/xhtml" style="display: flex; align-items: unsafe center; justify-content: unsafe center; width: 94px; height: 1px; padding-top: 181px; margin-left: 79px;">
+            <div style="box-sizing: border-box; font-size: 0; text-align: center; ">
+              <div style="display: inline-block; font-size: 9px; font-family: Helvetica; color: #0829FF; line-height: 1.2; pointer-events: all; white-space: normal; word-wrap: normal; ">
+                segment index
+              </div>
+            </div>
+          </div>
+        </foreignObject>
+        <text x="126" y="184" fill="#0829FF" font-family="Helvetica" font-size="9px" text-anchor="middle">segment index</text>
+      </switch>
+    </g>
+    <rect x="80" y="106" width="66" height="20" fill="none" stroke="none" pointer-events="all"/>
+    <g transform="translate(-0.5 -0.5)">
+      <switch>
+        <foreignObject style="overflow: visible; text-align: left;" pointer-events="none" width="100%" height="100%" requiredFeatures="http://www.w3.org/TR/SVG11/feature#Extensibility">
+          <div xmlns="http://www.w3.org/1999/xhtml" style="display: flex; align-items: unsafe center; justify-content: unsafe center; width: 64px; height: 1px; padding-top: 116px; margin-left: 81px;">
+            <div style="box-sizing: border-box; font-size: 0; text-align: center; ">
+              <div style="display: inline-block; font-size: 9px; font-family: Helvetica; color: #0829FF; line-height: 1.2; pointer-events: all; white-space: normal; word-wrap: normal; ">1</div>
+            </div>
+          </div>
+        </foreignObject>
+        <text x="113" y="119" fill="#0829FF" font-family="Helvetica" font-size="9px" text-anchor="middle">1</text>
+      </switch>
+    </g>
+    <rect x="341" y="169" width="66" height="20" fill="none" stroke="none" pointer-events="all"/>
+    <g transform="translate(-0.5 -0.5)">
+      <switch>
+        <foreignObject style="overflow: visible; text-align: left;" pointer-events="none" width="100%" height="100%" requiredFeatures="http://www.w3.org/TR/SVG11/feature#Extensibility">
+          <div
+            xmlns="http://www.w3.org/1999/xhtml"
+            style="display: flex; align-items: unsafe center; justify-content: unsafe center; width: 64px; height: 1px; padding-top: 179px; margin-left: 342px;"
+          >
+            <div style="box-sizing: border-box; font-size: 0; text-align: center; ">
+              <div style="display: inline-block; font-size: 9px; font-family: Helvetica; color: #0829FF; line-height: 1.2; pointer-events: all; white-space: normal; word-wrap: normal; ">7</div>
+            </div>
+          </div>
+        </foreignObject>
+        <text x="374" y="182" fill="#0829FF" font-family="Helvetica" font-size="9px" text-anchor="middle">7</text>
+      </switch>
+    </g>
+    <rect x="366" y="198" width="66" height="20" fill="none" stroke="none" pointer-events="all"/>
+    <g transform="translate(-0.5 -0.5)">
+      <switch>
+        <foreignObject style="overflow: visible; text-align: left;" pointer-events="none" width="100%" height="100%" requiredFeatures="http://www.w3.org/TR/SVG11/feature#Extensibility">
+          <div
+            xmlns="http://www.w3.org/1999/xhtml"
+            style="display: flex; align-items: unsafe center; justify-content: unsafe center; width: 64px; height: 1px; padding-top: 208px; margin-left: 367px;"
+          >
+            <div style="box-sizing: border-box; font-size: 0; text-align: center; ">
+              <div style="display: inline-block; font-size: 9px; font-family: Helvetica; color: #FF3333; line-height: 1.2; pointer-events: all; white-space: normal; word-wrap: normal; ">8</div>
+            </div>
+          </div>
+        </foreignObject>
+        <text x="399" y="211" fill="#FF3333" font-family="Helvetica" font-size="9px" text-anchor="middle">8</text>
+      </switch>
+    </g>
+    <rect x="352" y="136" width="66" height="20" fill="none" stroke="none" pointer-events="all"/>
+    <g transform="translate(-0.5 -0.5)">
+      <switch>
+        <foreignObject style="overflow: visible; text-align: left;" pointer-events="none" width="100%" height="100%" requiredFeatures="http://www.w3.org/TR/SVG11/feature#Extensibility">
+          <div
+            xmlns="http://www.w3.org/1999/xhtml"
+            style="display: flex; align-items: unsafe center; justify-content: unsafe center; width: 64px; height: 1px; padding-top: 146px; margin-left: 353px;"
+          >
+            <div style="box-sizing: border-box; font-size: 0; text-align: center; ">
+              <div style="display: inline-block; font-size: 9px; font-family: Helvetica; color: #FF3333; line-height: 1.2; pointer-events: all; white-space: normal; word-wrap: normal; ">7</div>
+            </div>
+          </div>
+        </foreignObject>
+        <text x="385" y="149" fill="#FF3333" font-family="Helvetica" font-size="9px" text-anchor="middle">7</text>
+      </switch>
+    </g>
+    <rect x="327" y="87" width="66" height="20" fill="none" stroke="none" pointer-events="all"/>
+    <g transform="translate(-0.5 -0.5)">
+      <switch>
+        <foreignObject style="overflow: visible; text-align: left;" pointer-events="none" width="100%" height="100%" requiredFeatures="http://www.w3.org/TR/SVG11/feature#Extensibility">
+          <div xmlns="http://www.w3.org/1999/xhtml" style="display: flex; align-items: unsafe center; justify-content: unsafe center; width: 64px; height: 1px; padding-top: 97px; margin-left: 328px;">
+            <div style="box-sizing: border-box; font-size: 0; text-align: center; ">
+              <div style="display: inline-block; font-size: 9px; font-family: Helvetica; color: #FF3333; line-height: 1.2; pointer-events: all; white-space: normal; word-wrap: normal; ">6</div>
+            </div>
+          </div>
+        </foreignObject>
+        <text x="360" y="100" fill="#FF3333" font-family="Helvetica" font-size="9px" text-anchor="middle">6</text>
+      </switch>
+    </g>
+    <rect x="320" y="115" width="66" height="20" fill="none" stroke="none" pointer-events="all"/>
+    <g transform="translate(-0.5 -0.5)">
+      <switch>
+        <foreignObject style="overflow: visible; text-align: left;" pointer-events="none" width="100%" height="100%" requiredFeatures="http://www.w3.org/TR/SVG11/feature#Extensibility">
+          <div
+            xmlns="http://www.w3.org/1999/xhtml"
+            style="display: flex; align-items: unsafe center; justify-content: unsafe center; width: 64px; height: 1px; padding-top: 125px; margin-left: 321px;"
+          >
+            <div style="box-sizing: border-box; font-size: 0; text-align: center; ">
+              <div style="display: inline-block; font-size: 9px; font-family: Helvetica; color: #0829FF; line-height: 1.2; pointer-events: all; white-space: normal; word-wrap: normal; ">6</div>
+            </div>
+          </div>
+        </foreignObject>
+        <text x="353" y="128" fill="#0829FF" font-family="Helvetica" font-size="9px" text-anchor="middle">6</text>
+      </switch>
+    </g>
+    <rect x="189" y="9" width="66" height="20" fill="none" stroke="none" pointer-events="all"/>
+    <g transform="translate(-0.5 -0.5)">
+      <switch>
+        <foreignObject style="overflow: visible; text-align: left;" pointer-events="none" width="100%" height="100%" requiredFeatures="http://www.w3.org/TR/SVG11/feature#Extensibility">
+          <div xmlns="http://www.w3.org/1999/xhtml" style="display: flex; align-items: unsafe center; justify-content: unsafe center; width: 64px; height: 1px; padding-top: 19px; margin-left: 190px;">
+            <div style="box-sizing: border-box; font-size: 0; text-align: center; ">
+              <div
+                style="display: inline-block; font-size: 9px; font-family: Helvetica; color: #FF3333; line-height: 1.2; pointer-events: all; font-weight: bold; white-space: normal; word-wrap: normal; "
+              >
+                nearest
+                <br style="font-size: 9px"/>
+                index
+              </div>
+            </div>
+          </div>
+        </foreignObject>
+        <text x="222" y="22" fill="#FF3333" font-family="Helvetica" font-size="9px" text-anchor="middle" font-weight="bold">nearest...</text>
+      </switch>
+    </g>
+    <rect x="231" y="67" width="66" height="20" fill="none" stroke="none" pointer-events="all"/>
+    <g transform="translate(-0.5 -0.5)">
+      <switch>
+        <foreignObject style="overflow: visible; text-align: left;" pointer-events="none" width="100%" height="100%" requiredFeatures="http://www.w3.org/TR/SVG11/feature#Extensibility">
+          <div xmlns="http://www.w3.org/1999/xhtml" style="display: flex; align-items: unsafe center; justify-content: unsafe center; width: 64px; height: 1px; padding-top: 77px; margin-left: 232px;">
+            <div style="box-sizing: border-box; font-size: 0; text-align: center; ">
+              <div
+                style="display: inline-block; font-size: 9px; font-family: Helvetica; color: #0829FF; line-height: 1.2; pointer-events: all; font-weight: bold; white-space: normal; word-wrap: normal; "
+              >
+                nearest
+                <br style="font-size: 9px"/>
+                segment index
+              </div>
+            </div>
+          </div>
+        </foreignObject>
+        <text x="264" y="80" fill="#0829FF" font-family="Helvetica" font-size="9px" text-anchor="middle" font-weight="bold">nearest...</text>
+      </switch>
+    </g>
+    <rect x="33" y="191" width="66" height="20" fill="none" stroke="none" pointer-events="all"/>
+    <g transform="translate(-0.5 -0.5)">
+      <switch>
+        <foreignObject style="overflow: visible; text-align: left;" pointer-events="none" width="100%" height="100%" requiredFeatures="http://www.w3.org/TR/SVG11/feature#Extensibility">
+          <div xmlns="http://www.w3.org/1999/xhtml" style="display: flex; align-items: unsafe center; justify-content: unsafe center; width: 64px; height: 1px; padding-top: 201px; margin-left: 34px;">
+            <div style="box-sizing: border-box; font-size: 0; text-align: center; ">
+              <div style="display: inline-block; font-size: 9px; font-family: Helvetica; color: #FF3333; line-height: 1.2; pointer-events: all; white-space: normal; word-wrap: normal; ">0</div>
+            </div>
+          </div>
+        </foreignObject>
+        <text x="66" y="204" fill="#FF3333" font-family="Helvetica" font-size="9px" text-anchor="middle">0</text>
+      </switch>
+    </g>
+    <rect x="57" y="163" width="66" height="20" fill="none" stroke="none" pointer-events="all"/>
+    <g transform="translate(-0.5 -0.5)">
+      <switch>
+        <foreignObject style="overflow: visible; text-align: left;" pointer-events="none" width="100%" height="100%" requiredFeatures="http://www.w3.org/TR/SVG11/feature#Extensibility">
+          <div xmlns="http://www.w3.org/1999/xhtml" style="display: flex; align-items: unsafe center; justify-content: unsafe center; width: 64px; height: 1px; padding-top: 173px; margin-left: 58px;">
+            <div style="box-sizing: border-box; font-size: 0; text-align: center; ">
+              <div style="display: inline-block; font-size: 9px; font-family: Helvetica; color: #0829FF; line-height: 1.2; pointer-events: all; white-space: normal; word-wrap: normal; ">0</div>
+            </div>
+          </div>
+        </foreignObject>
+        <text x="90" y="176" fill="#0829FF" font-family="Helvetica" font-size="9px" text-anchor="middle">0</text>
+      </switch>
+    </g>
+    <rect x="173" y="49" width="66" height="20" fill="none" stroke="none" pointer-events="all"/>
+    <g transform="translate(-0.5 -0.5)">
+      <switch>
+        <foreignObject style="overflow: visible; text-align: left;" pointer-events="none" width="100%" height="100%" requiredFeatures="http://www.w3.org/TR/SVG11/feature#Extensibility">
+          <div xmlns="http://www.w3.org/1999/xhtml" style="display: flex; align-items: unsafe center; justify-content: unsafe center; width: 64px; height: 1px; padding-top: 59px; margin-left: 174px;">
+            <div style="box-sizing: border-box; font-size: 0; text-align: center; ">
+              <div style="display: inline-block; font-size: 9px; font-family: Helvetica; color: #0829FF; line-height: 1.2; pointer-events: all; white-space: normal; word-wrap: normal; ">3</div>
+            </div>
+          </div>
+        </foreignObject>
+        <text x="206" y="62" fill="#0829FF" font-family="Helvetica" font-size="9px" text-anchor="middle">3</text>
+      </switch>
+    </g>
+    <rect x="118" y="66" width="66" height="20" fill="none" stroke="none" pointer-events="all"/>
+    <g transform="translate(-0.5 -0.5)">
+      <switch>
+        <foreignObject style="overflow: visible; text-align: left;" pointer-events="none" width="100%" height="100%" requiredFeatures="http://www.w3.org/TR/SVG11/feature#Extensibility">
+          <div xmlns="http://www.w3.org/1999/xhtml" style="display: flex; align-items: unsafe center; justify-content: unsafe center; width: 64px; height: 1px; padding-top: 76px; margin-left: 119px;">
+            <div style="box-sizing: border-box; font-size: 0; text-align: center; ">
+              <div style="display: inline-block; font-size: 9px; font-family: Helvetica; color: #0829FF; line-height: 1.2; pointer-events: all; white-space: normal; word-wrap: normal; ">2</div>
+            </div>
+          </div>
+        </foreignObject>
+        <text x="151" y="79" fill="#0829FF" font-family="Helvetica" font-size="9px" text-anchor="middle">2</text>
+      </switch>
+    </g>
+    <rect x="284" y="76" width="66" height="20" fill="none" stroke="none" pointer-events="all"/>
+    <g transform="translate(-0.5 -0.5)">
+      <switch>
+        <foreignObject style="overflow: visible; text-align: left;" pointer-events="none" width="100%" height="100%" requiredFeatures="http://www.w3.org/TR/SVG11/feature#Extensibility">
+          <div xmlns="http://www.w3.org/1999/xhtml" style="display: flex; align-items: unsafe center; justify-content: unsafe center; width: 64px; height: 1px; padding-top: 86px; margin-left: 285px;">
+            <div style="box-sizing: border-box; font-size: 0; text-align: center; ">
+              <div style="display: inline-block; font-size: 9px; font-family: Helvetica; color: #0829FF; line-height: 1.2; pointer-events: all; white-space: normal; word-wrap: normal; ">5</div>
+            </div>
+          </div>
+        </foreignObject>
+        <text x="317" y="89" fill="#0829FF" font-family="Helvetica" font-size="9px" text-anchor="middle">5</text>
+      </switch>
+    </g>
+    <rect x="140" y="33" width="66" height="20" fill="none" stroke="none" pointer-events="all"/>
+    <g transform="translate(-0.5 -0.5)">
+      <switch>
+        <foreignObject style="overflow: visible; text-align: left;" pointer-events="none" width="100%" height="100%" requiredFeatures="http://www.w3.org/TR/SVG11/feature#Extensibility">
+          <div xmlns="http://www.w3.org/1999/xhtml" style="display: flex; align-items: unsafe center; justify-content: unsafe center; width: 64px; height: 1px; padding-top: 43px; margin-left: 141px;">
+            <div style="box-sizing: border-box; font-size: 0; text-align: center; ">
+              <div style="display: inline-block; font-size: 9px; font-family: Helvetica; color: #FF3333; line-height: 1.2; pointer-events: all; white-space: normal; word-wrap: normal; ">3</div>
+            </div>
+          </div>
+        </foreignObject>
+        <text x="173" y="46" fill="#FF3333" font-family="Helvetica" font-size="9px" text-anchor="middle">3</text>
+      </switch>
+    </g>
+    <rect x="201" y="24" width="66" height="20" fill="none" stroke="none" pointer-events="all"/>
+    <g transform="translate(-0.5 -0.5)">
+      <switch>
+        <foreignObject style="overflow: visible; text-align: left;" pointer-events="none" width="100%" height="100%" requiredFeatures="http://www.w3.org/TR/SVG11/feature#Extensibility">
+          <div xmlns="http://www.w3.org/1999/xhtml" style="display: flex; align-items: unsafe center; justify-content: unsafe center; width: 64px; height: 1px; padding-top: 34px; margin-left: 202px;">
+            <div style="box-sizing: border-box; font-size: 0; text-align: center; ">
+              <div
+                style="display: inline-block; font-size: 9px; font-family: Helvetica; color: #FF3333; line-height: 1.2; pointer-events: all; font-weight: bold; white-space: normal; word-wrap: normal; "
+              >
+                4
+              </div>
+            </div>
+          </div>
+        </foreignObject>
+        <text x="234" y="37" fill="#FF3333" font-family="Helvetica" font-size="9px" text-anchor="middle" font-weight="bold">4</text>
+      </switch>
+    </g>
+    <rect x="275" y="41" width="66" height="20" fill="none" stroke="none" pointer-events="all"/>
+    <g transform="translate(-0.5 -0.5)">
+      <switch>
+        <foreignObject style="overflow: visible; text-align: left;" pointer-events="none" width="100%" height="100%" requiredFeatures="http://www.w3.org/TR/SVG11/feature#Extensibility">
+          <div xmlns="http://www.w3.org/1999/xhtml" style="display: flex; align-items: unsafe center; justify-content: unsafe center; width: 64px; height: 1px; padding-top: 51px; margin-left: 276px;">
+            <div style="box-sizing: border-box; font-size: 0; text-align: center; ">
+              <div style="display: inline-block; font-size: 9px; font-family: Helvetica; color: #FF3333; line-height: 1.2; pointer-events: all; white-space: normal; word-wrap: normal; ">5</div>
+            </div>
+          </div>
+        </foreignObject>
+        <text x="308" y="54" fill="#FF3333" font-family="Helvetica" font-size="9px" text-anchor="middle">5</text>
+      </switch>
+    </g>
+    <rect x="231" y="52" width="66" height="20" fill="none" stroke="none" pointer-events="all"/>
+    <g transform="translate(-0.5 -0.5)">
+      <switch>
+        <foreignObject style="overflow: visible; text-align: left;" pointer-events="none" width="100%" height="100%" requiredFeatures="http://www.w3.org/TR/SVG11/feature#Extensibility">
+          <div xmlns="http://www.w3.org/1999/xhtml" style="display: flex; align-items: unsafe center; justify-content: unsafe center; width: 64px; height: 1px; padding-top: 62px; margin-left: 232px;">
+            <div style="box-sizing: border-box; font-size: 0; text-align: center; ">
+              <div
+                style="display: inline-block; font-size: 9px; font-family: Helvetica; color: #0829FF; line-height: 1.2; pointer-events: all; font-weight: bold; white-space: normal; word-wrap: normal; "
+              >
+                4
+              </div>
+            </div>
+          </div>
+        </foreignObject>
+        <text x="264" y="65" fill="#0829FF" font-family="Helvetica" font-size="9px" text-anchor="middle" font-weight="bold">4</text>
+      </switch>
+    </g>
+  </g>
+  <switch>
+    <g requiredFeatures="http://www.w3.org/TR/SVG11/feature#Extensibility"/>
+    <a transform="translate(0,-5)" xlink:href="https://desk.draw.io/support/solutions/articles/16000042487" target="_blank">
+      <text text-anchor="middle" font-size="10px" x="50%" y="100%">Viewer does not support full SVG 1.1</text>
+    </a>
+  </switch>
+</svg>

slave/src/universe/common/autoware_motion_utils/package.xml

@@ -0,0 +1,45 @@
+<?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_motion_utils</name>
+  <version>0.1.0</version>
+  <description>The autoware_motion_utils package</description>
+  <maintainer email="satoshi.ota@tier4.jp">Satoshi Ota</maintainer>
+  <maintainer email="takayuki.murooka@tier4.jp">Takayuki Murooka</maintainer>
+  <!-- reviewer-->
+  <maintainer email="fumiya.watanabe@tier4.jp">Fumiya Watanabe</maintainer>
+  <maintainer email="kosuke.takeuchi@tier4.jp">Kosuke Takeuchi</maintainer>
+  <maintainer email="taiki.tanaka@tier4.jp">Taiki Tanaka</maintainer>
+  <maintainer email="takamasa.horibe@tier4.jp">Takamasa Horibe</maintainer>
+  <maintainer email="tomoya.kimura@tier4.jp">Tomoya Kimura</maintainer>
+  <maintainer email="mamoru.sobue@tier4.jp">Mamoru Sobue</maintainer>
+  <license>Apache License 2.0</license>
+
+  <author email="takayuki.murooka@tier4.jp">Takayuki Murooka</author>
+  <author email="satoshi.ota@tier4.jp">Satoshi Ota</author>
+
+  <buildtool_depend>ament_cmake_auto</buildtool_depend>
+  <buildtool_depend>autoware_cmake</buildtool_depend>
+
+  <depend>autoware_adapi_v1_msgs</depend>
+  <depend>autoware_interpolation</depend>
+  <depend>autoware_planning_msgs</depend>
+  <depend>autoware_universe_utils</depend>
+  <depend>autoware_vehicle_msgs</depend>
+  <depend>builtin_interfaces</depend>
+  <depend>geometry_msgs</depend>
+  <depend>libboost-dev</depend>
+  <depend>rclcpp</depend>
+  <depend>tf2</depend>
+  <depend>tf2_geometry_msgs</depend>
+  <depend>tier4_planning_msgs</depend>
+  <depend>visualization_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>

slave/src/universe/common/autoware_motion_utils/src/distance/distance.cpp

@@ -0,0 +1,272 @@
+// 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/motion_utils/distance/distance.hpp"
+
+namespace autoware::motion_utils
+{
+namespace
+{
+bool validCheckDecelPlan(
+  const double v_end, const double a_end, const double v_target, const double a_target,
+  const double v_margin, const double a_margin)
+{
+  const double v_min = v_target - std::abs(v_margin);
+  const double v_max = v_target + std::abs(v_margin);
+  const double a_min = a_target - std::abs(a_margin);
+  const double a_max = a_target + std::abs(a_margin);
+
+  if (v_end < v_min || v_max < v_end) {
+    return false;
+  }
+  if (a_end < a_min || a_max < a_end) {
+    return false;
+  }
+
+  return true;
+}
+
+/**
+ * @brief update traveling distance, velocity and acceleration under constant jerk.
+ * @param (x) current traveling distance [m/s]
+ * @param (v) current velocity [m/s]
+ * @param (a) current acceleration [m/ss]
+ * @param (j) target jerk [m/sss]
+ * @param (t) time [s]
+ * @return updated traveling distance, velocity and acceleration
+ */
+std::tuple<double, double, double> update(
+  const double x, const double v, const double a, const double j, const double t)
+{
+  const double a_new = a + j * t;
+  const double v_new = v + a * t + 0.5 * j * t * t;
+  const double x_new = x + v * t + 0.5 * a * t * t + (1.0 / 6.0) * j * t * t * t;
+
+  return {x_new, v_new, a_new};
+}
+
+/**
+ * @brief calculate distance until velocity is reached target velocity (TYPE: TRAPEZOID
+ * ACCELERATION PROFILE). this type of profile has ZERO JERK time.
+ *
+ * [ACCELERATION PROFILE]
+ *  a  ^
+ *     |
+ *  a0 *
+ *     |*
+ * ----+-*-------------------*------> t
+ *     |  *                 *
+ *     |   *               *
+ *     | a1 ***************
+ *     |
+ *
+ * [JERK PROFILE]
+ *  j  ^
+ *     |
+ *     |               ja ****
+ *     |                  *
+ * ----+----***************---------> t
+ *     |    *
+ *     |    *
+ *  jd ******
+ *     |
+ *
+ * @param (v0) current velocity [m/s]
+ * @param (a0) current acceleration [m/ss]
+ * @param (vt) target velocity [m/s]
+ * @param (am) minimum deceleration [m/ss]
+ * @param (ja) maximum jerk [m/sss]
+ * @param (jd) minimum jerk [m/sss]
+ * @param (t_during_min_acc) duration of constant deceleration [s]
+ * @return moving distance until velocity is reached vt [m]
+ */
+std::optional<double> calcDecelDistPlanType1(
+  const double v0, const double vt, const double a0, const double am, const double ja,
+  const double jd, const double t_during_min_acc)
+{
+  constexpr double epsilon = 1e-3;
+
+  // negative jerk time
+  const double j1 = am < a0 ? jd : ja;
+  const double t1 = epsilon < (am - a0) / j1 ? (am - a0) / j1 : 0.0;
+  const auto [x1, v1, a1] = update(0.0, v0, a0, j1, t1);
+
+  // zero jerk time
+  const double t2 = epsilon < t_during_min_acc ? t_during_min_acc : 0.0;
+  const auto [x2, v2, a2] = update(x1, v1, a1, 0.0, t2);
+
+  // positive jerk time
+  const double t3 = epsilon < (0.0 - am) / ja ? (0.0 - am) / ja : 0.0;
+  const auto [x3, v3, a3] = update(x2, v2, a2, ja, t3);
+
+  const double a_target = 0.0;
+  const double v_margin = 0.3;  // [m/s]
+  const double a_margin = 0.1;  // [m/s^2]
+
+  if (!validCheckDecelPlan(v3, a3, vt, a_target, v_margin, a_margin)) {
+    return {};
+  }
+
+  return x3;
+}
+
+/**
+ * @brief calculate distance until velocity is reached target velocity (TYPE: TRIANGLE
+ * ACCELERATION PROFILE), This type of profile do NOT have ZERO JERK time.
+ *
+ * [ACCELERATION PROFILE]
+ *  a  ^
+ *     |
+ *  a0 *
+ *     |*
+ * ----+-*-----*--------------------> t
+ *     |  *   *
+ *     |   * *
+ *     | a1 *
+ *     |
+ *
+ * [JERK PROFILE]
+ *  j  ^
+ *     |
+ *     | ja ****
+ *     |    *
+ * ----+----*-----------------------> t
+ *     |    *
+ *     |    *
+ *  jd ******
+ *     |
+ *
+ * @param (v0) current velocity [m/s]
+ * @param (vt) target velocity [m/s]
+ * @param (a0) current acceleration [m/ss]
+ * @param (am) minimum deceleration [m/ss]
+ * @param (ja) maximum jerk [m/sss]
+ * @param (jd) minimum jerk [m/sss]
+ * @return moving distance until velocity is reached vt [m]
+ */
+std::optional<double> calcDecelDistPlanType2(
+  const double v0, const double vt, const double a0, const double ja, const double jd)
+{
+  constexpr double epsilon = 1e-3;
+
+  const double a1_square = (vt - v0 - 0.5 * (0.0 - a0) / jd * a0) * (2.0 * ja * jd / (ja - jd));
+  const double a1 = -std::sqrt(a1_square);
+
+  // negative jerk time
+  const double t1 = epsilon < (a1 - a0) / jd ? (a1 - a0) / jd : 0.0;
+  const auto [x1, v1, no_use_a1] = update(0.0, v0, a0, jd, t1);
+
+  // positive jerk time
+  const double t2 = epsilon < (0.0 - a1) / ja ? (0.0 - a1) / ja : 0.0;
+  const auto [x2, v2, a2] = update(x1, v1, a1, ja, t2);
+
+  const double a_target = 0.0;
+  const double v_margin = 0.3;
+  const double a_margin = 0.1;
+
+  if (!validCheckDecelPlan(v2, a2, vt, a_target, v_margin, a_margin)) {
+    return {};
+  }
+
+  return x2;
+}
+
+/**
+ * @brief calculate distance until velocity is reached target velocity (TYPE: LINEAR ACCELERATION
+ * PROFILE). This type of profile has only positive jerk time.
+ *
+ * [ACCELERATION PROFILE]
+ *  a  ^
+ *     |
+ * ----+----*-----------------------> t
+ *     |   *
+ *     |  *
+ *     | *
+ *     |*
+ *  a0 *
+ *     |
+ *
+ * [JERK PROFILE]
+ *  j  ^
+ *     |
+ *  ja ******
+ *     |    *
+ *     |    *
+ * ----+----*-----------------------> t
+ *     |
+ *
+ * @param (v0) current velocity [m/s]
+ * @param (vt) target velocity [m/s]
+ * @param (a0) current acceleration [m/ss]
+ * @param (ja) maximum jerk [m/sss]
+ * @return moving distance until velocity is reached vt [m]
+ */
+std::optional<double> calcDecelDistPlanType3(
+  const double v0, const double vt, const double a0, const double ja)
+{
+  constexpr double epsilon = 1e-3;
+
+  // positive jerk time
+  const double t_acc = (0.0 - a0) / ja;
+  const double t1 = epsilon < t_acc ? t_acc : 0.0;
+  const auto [x1, v1, a1] = update(0.0, v0, a0, ja, t1);
+
+  const double a_target = 0.0;
+  const double v_margin = 0.3;
+  const double a_margin = 0.1;
+
+  if (!validCheckDecelPlan(v1, a1, vt, a_target, v_margin, a_margin)) {
+    return {};
+  }
+
+  return x1;
+}
+}  // namespace
+
+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)
+{
+  if (current_vel < target_vel) {
+    return {};
+  }
+
+  constexpr double epsilon = 1e-3;
+  const double jerk_before_min_acc = acc_min < current_acc ? jerk_dec : jerk_acc;
+  const double t_before_min_acc = (acc_min - current_acc) / jerk_before_min_acc;
+  const double jerk_after_min_acc = jerk_acc;
+  const double t_after_min_acc = (0.0 - acc_min) / jerk_after_min_acc;
+
+  const double t_during_min_acc =
+    (target_vel - current_vel - current_acc * t_before_min_acc -
+     0.5 * jerk_before_min_acc * std::pow(t_before_min_acc, 2) - acc_min * t_after_min_acc -
+     0.5 * jerk_after_min_acc * std::pow(t_after_min_acc, 2)) /
+    acc_min;
+
+  // check if it is possible to decelerate to the target velocity
+  // by simply bringing the current acceleration to zero.
+  const auto is_decel_needed =
+    0.5 * (0.0 - current_acc) / jerk_acc * current_acc > target_vel - current_vel;
+
+  if (t_during_min_acc > epsilon) {
+    return calcDecelDistPlanType1(
+      current_vel, target_vel, current_acc, acc_min, jerk_acc, jerk_dec, t_during_min_acc);
+  }
+  if (is_decel_needed || current_acc > epsilon) {
+    return calcDecelDistPlanType2(current_vel, target_vel, current_acc, jerk_acc, jerk_dec);
+  }
+
+  return calcDecelDistPlanType3(current_vel, target_vel, current_acc, jerk_acc);
+}
+}  // namespace autoware::motion_utils

slave/src/universe/common/autoware_motion_utils/src/factor/velocity_factor_interface.cpp

@@ -0,0 +1,49 @@
+// Copyright 2023-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/factor/velocity_factor_interface.hpp>
+#include <autoware/motion_utils/trajectory/trajectory.hpp>
+
+#include <autoware_planning_msgs/msg/path_point.hpp>
+#include <autoware_planning_msgs/msg/trajectory_point.hpp>
+#include <tier4_planning_msgs/msg/path_point_with_lane_id.hpp>
+
+namespace autoware::motion_utils
+{
+template <class PointType>
+void VelocityFactorInterface::set(
+  const std::vector<PointType> & points, const Pose & curr_pose, const Pose & stop_pose,
+  const VelocityFactorStatus status, const std::string & detail)
+{
+  const auto & curr_point = curr_pose.position;
+  const auto & stop_point = stop_pose.position;
+  velocity_factor_.behavior = behavior_;
+  velocity_factor_.pose = stop_pose;
+  velocity_factor_.distance =
+    static_cast<float>(autoware::motion_utils::calcSignedArcLength(points, curr_point, stop_point));
+  velocity_factor_.status = status;
+  velocity_factor_.detail = detail;
+}
+
+template void VelocityFactorInterface::set<tier4_planning_msgs::msg::PathPointWithLaneId>(
+  const std::vector<tier4_planning_msgs::msg::PathPointWithLaneId> &, const Pose &, const Pose &,
+  const VelocityFactorStatus, const std::string &);
+template void VelocityFactorInterface::set<autoware_planning_msgs::msg::PathPoint>(
+  const std::vector<autoware_planning_msgs::msg::PathPoint> &, const Pose &, const Pose &,
+  const VelocityFactorStatus, const std::string &);
+template void VelocityFactorInterface::set<autoware_planning_msgs::msg::TrajectoryPoint>(
+  const std::vector<autoware_planning_msgs::msg::TrajectoryPoint> &, const Pose &, const Pose &,
+  const VelocityFactorStatus, const std::string &);
+
+}  // namespace autoware::motion_utils

slave/src/universe/common/autoware_motion_utils/src/marker/marker_helper.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/motion_utils/marker/marker_helper.hpp"
+
+#include "autoware/universe_utils/ros/marker_helper.hpp"
+
+#include <autoware/universe_utils/geometry/geometry.hpp>
+
+#include <visualization_msgs/msg/marker_array.hpp>
+
+using autoware::universe_utils::createDefaultMarker;
+using autoware::universe_utils::createDeletedDefaultMarker;
+using autoware::universe_utils::createMarkerColor;
+using autoware::universe_utils::createMarkerScale;
+using visualization_msgs::msg::MarkerArray;
+
+namespace
+{
+
+inline visualization_msgs::msg::MarkerArray createVirtualWallMarkerArray(
+  const geometry_msgs::msg::Pose & vehicle_front_pose, const std::string & module_name,
+  const std::string & ns_prefix, const rclcpp::Time & now, const int32_t id,
+  const std_msgs::msg::ColorRGBA & color)
+{
+  visualization_msgs::msg::MarkerArray marker_array;
+
+  // Virtual Wall
+  {
+    auto marker = createDefaultMarker(
+      "map", now, ns_prefix + "virtual_wall", id, visualization_msgs::msg::Marker::CUBE,
+      createMarkerScale(0.1, 5.0, 2.0), color);
+
+    marker.pose = vehicle_front_pose;
+    marker.pose.position.z += 1.0;
+
+    marker_array.markers.push_back(marker);
+  }
+
+  // Factor Text
+  {
+    auto marker = createDefaultMarker(
+      "map", now, ns_prefix + "factor_text", id, visualization_msgs::msg::Marker::TEXT_VIEW_FACING,
+      createMarkerScale(0.0, 0.0, 1.0), createMarkerColor(1.0, 1.0, 1.0, 1.0));
+
+    marker.pose = vehicle_front_pose;
+    marker.pose.position.z += 2.0;
+    marker.text = module_name;
+
+    marker_array.markers.push_back(marker);
+  }
+
+  return marker_array;
+}
+
+inline visualization_msgs::msg::MarkerArray createDeletedVirtualWallMarkerArray(
+  const std::string & ns_prefix, const rclcpp::Time & now, const int32_t id)
+{
+  visualization_msgs::msg::MarkerArray marker_array;
+
+  // Virtual Wall
+  {
+    auto marker = createDeletedDefaultMarker(now, ns_prefix + "virtual_wall", id);
+    marker_array.markers.push_back(marker);
+  }
+
+  // Factor Text
+  {
+    auto marker = createDeletedDefaultMarker(now, ns_prefix + "factor_text", id);
+    marker_array.markers.push_back(marker);
+  }
+
+  return marker_array;
+}
+}  // namespace
+
+namespace autoware::motion_utils
+{
+visualization_msgs::msg::MarkerArray createStopVirtualWallMarker(
+  const geometry_msgs::msg::Pose & pose, const std::string & module_name, const rclcpp::Time & now,
+  const int32_t id, const double longitudinal_offset, const std::string & ns_prefix,
+  const bool is_driving_forward)
+{
+  const auto pose_with_offset = autoware::universe_utils::calcOffsetPose(
+    pose, longitudinal_offset * (is_driving_forward ? 1.0 : -1.0), 0.0, 0.0);
+  return createVirtualWallMarkerArray(
+    pose_with_offset, module_name, ns_prefix + "stop_", now, id,
+    createMarkerColor(1.0, 0.0, 0.0, 0.5));
+}
+
+visualization_msgs::msg::MarkerArray createSlowDownVirtualWallMarker(
+  const geometry_msgs::msg::Pose & pose, const std::string & module_name, const rclcpp::Time & now,
+  const int32_t id, const double longitudinal_offset, const std::string & ns_prefix,
+  const bool is_driving_forward)
+{
+  const auto pose_with_offset = autoware::universe_utils::calcOffsetPose(
+    pose, longitudinal_offset * (is_driving_forward ? 1.0 : -1.0), 0.0, 0.0);
+  return createVirtualWallMarkerArray(
+    pose_with_offset, module_name, ns_prefix + "slow_down_", now, id,
+    createMarkerColor(1.0, 1.0, 0.0, 0.5));
+}
+
+visualization_msgs::msg::MarkerArray createDeadLineVirtualWallMarker(
+  const geometry_msgs::msg::Pose & pose, const std::string & module_name, const rclcpp::Time & now,
+  const int32_t id, const double longitudinal_offset, const std::string & ns_prefix,
+  const bool is_driving_forward)
+{
+  const auto pose_with_offset = autoware::universe_utils::calcOffsetPose(
+    pose, longitudinal_offset * (is_driving_forward ? 1.0 : -1.0), 0.0, 0.0);
+  return createVirtualWallMarkerArray(
+    pose_with_offset, module_name, ns_prefix + "dead_line_", now, id,
+    createMarkerColor(0.0, 1.0, 0.0, 0.5));
+}
+
+visualization_msgs::msg::MarkerArray createDeletedStopVirtualWallMarker(
+  const rclcpp::Time & now, const int32_t id)
+{
+  return createDeletedVirtualWallMarkerArray("stop_", now, id);
+}
+
+visualization_msgs::msg::MarkerArray createDeletedSlowDownVirtualWallMarker(
+  const rclcpp::Time & now, const int32_t id)
+{
+  return createDeletedVirtualWallMarkerArray("slow_down_", now, id);
+}
+}  // namespace autoware::motion_utils

slave/src/universe/common/autoware_motion_utils/src/marker/virtual_wall_marker_creator.cpp

@@ -0,0 +1,88 @@
+// 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/motion_utils/marker/virtual_wall_marker_creator.hpp"
+
+#include "autoware/motion_utils/marker/marker_helper.hpp"
+
+namespace autoware::motion_utils
+{
+
+void VirtualWallMarkerCreator::cleanup()
+{
+  for (auto it = marker_count_per_namespace_.begin(); it != marker_count_per_namespace_.end();) {
+    const auto & marker_count = it->second;
+    const auto is_unused_namespace = marker_count.previous == 0 && marker_count.current == 0;
+    if (is_unused_namespace)
+      it = marker_count_per_namespace_.erase(it);
+    else
+      ++it;
+  }
+  virtual_walls_.clear();
+}
+
+void VirtualWallMarkerCreator::add_virtual_wall(const VirtualWall & virtual_wall)
+{
+  virtual_walls_.push_back(virtual_wall);
+}
+void VirtualWallMarkerCreator::add_virtual_walls(const VirtualWalls & walls)
+{
+  virtual_walls_.insert(virtual_walls_.end(), walls.begin(), walls.end());
+}
+
+visualization_msgs::msg::MarkerArray VirtualWallMarkerCreator::create_markers(
+  const rclcpp::Time & now)
+{
+  visualization_msgs::msg::MarkerArray marker_array;
+  // update marker counts
+  for (auto & [ns, count] : marker_count_per_namespace_) {
+    count.previous = count.current;
+    count.current = 0UL;
+  }
+  // convert to markers
+  create_wall_function create_fn;
+  for (const auto & virtual_wall : virtual_walls_) {
+    switch (virtual_wall.style) {
+      case stop:
+        create_fn = autoware::motion_utils::createStopVirtualWallMarker;
+        break;
+      case slowdown:
+        create_fn = autoware::motion_utils::createSlowDownVirtualWallMarker;
+        break;
+      case deadline:
+        create_fn = autoware::motion_utils::createDeadLineVirtualWallMarker;
+        break;
+    }
+    auto markers = create_fn(
+      virtual_wall.pose, virtual_wall.text, now, 0, virtual_wall.longitudinal_offset,
+      virtual_wall.ns, virtual_wall.is_driving_forward);
+    for (auto & marker : markers.markers) {
+      marker.id = static_cast<int>(marker_count_per_namespace_[marker.ns].current++);
+      marker_array.markers.push_back(marker);
+    }
+  }
+  // create delete markers
+  visualization_msgs::msg::Marker marker;
+  marker.action = visualization_msgs::msg::Marker::DELETE;
+  for (const auto & [ns, count] : marker_count_per_namespace_) {
+    for (marker.id = static_cast<int>(count.current); marker.id < static_cast<int>(count.previous);
+         ++marker.id) {
+      marker.ns = ns;
+      marker_array.markers.push_back(marker);
+    }
+  }
+  cleanup();
+  return marker_array;
+}
+}  // namespace autoware::motion_utils

slave/src/universe/common/autoware_motion_utils/src/resample/resample.cpp

@@ -0,0 +1,753 @@
+// 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/motion_utils/resample/resample.hpp"
+
+#include "autoware/interpolation/linear_interpolation.hpp"
+#include "autoware/interpolation/spline_interpolation.hpp"
+#include "autoware/interpolation/zero_order_hold.hpp"
+#include "autoware/motion_utils/resample/resample_utils.hpp"
+#include "autoware/motion_utils/trajectory/trajectory.hpp"
+#include "autoware/universe_utils/geometry/geometry.hpp"
+
+#include <cstdlib>
+
+namespace autoware::motion_utils
+{
+std::vector<geometry_msgs::msg::Point> resamplePointVector(
+  const std::vector<geometry_msgs::msg::Point> & points,
+  const std::vector<double> & resampled_arclength, const bool use_akima_spline_for_xy,
+  const bool use_lerp_for_z)
+{
+  // validate arguments
+  if (!resample_utils::validate_arguments(points, resampled_arclength)) {
+    return points;
+  }
+
+  // Input Path Information
+  std::vector<double> input_arclength;
+  std::vector<double> x;
+  std::vector<double> y;
+  std::vector<double> z;
+  input_arclength.reserve(points.size());
+  x.reserve(points.size());
+  y.reserve(points.size());
+  z.reserve(points.size());
+
+  input_arclength.push_back(0.0);
+  x.push_back(points.front().x);
+  y.push_back(points.front().y);
+  z.push_back(points.front().z);
+  for (size_t i = 1; i < points.size(); ++i) {
+    const auto & prev_pt = points.at(i - 1);
+    const auto & curr_pt = points.at(i);
+    const double ds = autoware::universe_utils::calcDistance2d(prev_pt, curr_pt);
+    input_arclength.push_back(ds + input_arclength.back());
+    x.push_back(curr_pt.x);
+    y.push_back(curr_pt.y);
+    z.push_back(curr_pt.z);
+  }
+
+  // Interpolate
+  const auto lerp = [&](const auto & input) {
+    return autoware::interpolation::lerp(input_arclength, input, resampled_arclength);
+  };
+  const auto spline = [&](const auto & input) {
+    return autoware::interpolation::spline(input_arclength, input, resampled_arclength);
+  };
+  const auto spline_by_akima = [&](const auto & input) {
+    return autoware::interpolation::splineByAkima(input_arclength, input, resampled_arclength);
+  };
+
+  const auto interpolated_x = use_akima_spline_for_xy ? lerp(x) : spline_by_akima(x);
+  const auto interpolated_y = use_akima_spline_for_xy ? lerp(y) : spline_by_akima(y);
+  const auto interpolated_z = use_lerp_for_z ? lerp(z) : spline(z);
+
+  std::vector<geometry_msgs::msg::Point> resampled_points;
+  resampled_points.resize(interpolated_x.size());
+
+  // Insert Position
+  for (size_t i = 0; i < resampled_points.size(); ++i) {
+    geometry_msgs::msg::Point point;
+    point.x = interpolated_x.at(i);
+    point.y = interpolated_y.at(i);
+    point.z = interpolated_z.at(i);
+    resampled_points.at(i) = point;
+  }
+
+  return resampled_points;
+}
+
+std::vector<geometry_msgs::msg::Point> resamplePointVector(
+  const std::vector<geometry_msgs::msg::Point> & points, const double resample_interval,
+  const bool use_akima_spline_for_xy, const bool use_lerp_for_z)
+{
+  const double input_length = autoware::motion_utils::calcArcLength(points);
+
+  std::vector<double> resampling_arclength;
+  for (double s = 0.0; s < input_length; s += resample_interval) {
+    resampling_arclength.push_back(s);
+  }
+  if (resampling_arclength.empty()) {
+    std::cerr << "[autoware_motion_utils]: resampling arclength is empty" << std::endl;
+    return points;
+  }
+
+  // Insert terminal point
+  if (input_length - resampling_arclength.back() < autoware::motion_utils::overlap_threshold) {
+    resampling_arclength.back() = input_length;
+  } else {
+    resampling_arclength.push_back(input_length);
+  }
+
+  return resamplePointVector(points, resampling_arclength, use_akima_spline_for_xy, use_lerp_for_z);
+}
+
+std::vector<geometry_msgs::msg::Pose> resamplePoseVector(
+  const std::vector<geometry_msgs::msg::Pose> & points_raw,
+  const std::vector<double> & resampled_arclength, const bool use_akima_spline_for_xy,
+  const bool use_lerp_for_z)
+{
+  // Remove overlap points for resampling
+  const auto points = autoware::motion_utils::removeOverlapPoints(points_raw);
+
+  // validate arguments
+  if (!resample_utils::validate_arguments(points, resampled_arclength)) {
+    return points_raw;
+  }
+
+  std::vector<geometry_msgs::msg::Point> position(points.size());
+  for (size_t i = 0; i < points.size(); ++i) {
+    position.at(i) = points.at(i).position;
+  }
+  const auto resampled_position =
+    resamplePointVector(position, resampled_arclength, use_akima_spline_for_xy, use_lerp_for_z);
+
+  std::vector<geometry_msgs::msg::Pose> resampled_points(resampled_position.size());
+
+  // Insert Position
+  for (size_t i = 0; i < resampled_position.size(); ++i) {
+    geometry_msgs::msg::Pose pose;
+    pose.position.x = resampled_position.at(i).x;
+    pose.position.y = resampled_position.at(i).y;
+    pose.position.z = resampled_position.at(i).z;
+    resampled_points.at(i) = pose;
+  }
+
+  const bool is_driving_forward =
+    autoware::universe_utils::isDrivingForward(points.at(0), points.at(1));
+  autoware::motion_utils::insertOrientation(resampled_points, is_driving_forward);
+
+  // Initial orientation is depend on the initial value of the resampled_arclength
+  // when backward driving
+  if (!is_driving_forward && resampled_arclength.front() < 1e-3) {
+    resampled_points.at(0).orientation = points.at(0).orientation;
+  }
+
+  return resampled_points;
+}
+
+std::vector<geometry_msgs::msg::Pose> resamplePoseVector(
+  const std::vector<geometry_msgs::msg::Pose> & points, const double resample_interval,
+  const bool use_akima_spline_for_xy, const bool use_lerp_for_z)
+{
+  const double input_length = autoware::motion_utils::calcArcLength(points);
+
+  std::vector<double> resampling_arclength;
+  for (double s = 0.0; s < input_length; s += resample_interval) {
+    resampling_arclength.push_back(s);
+  }
+  if (resampling_arclength.empty()) {
+    std::cerr << "[autoware_motion_utils]: resampling arclength is empty" << std::endl;
+    return points;
+  }
+
+  // Insert terminal point
+  if (input_length - resampling_arclength.back() < autoware::motion_utils::overlap_threshold) {
+    resampling_arclength.back() = input_length;
+  } else {
+    resampling_arclength.push_back(input_length);
+  }
+
+  return resamplePoseVector(points, resampling_arclength, use_akima_spline_for_xy, use_lerp_for_z);
+}
+
+tier4_planning_msgs::msg::PathWithLaneId resamplePath(
+  const tier4_planning_msgs::msg::PathWithLaneId & input_path,
+  const std::vector<double> & resampled_arclength, const bool use_akima_spline_for_xy,
+  const bool use_lerp_for_z, const bool use_zero_order_hold_for_v)
+{
+  auto resampling_arclength = resampled_arclength;
+
+  // Add resampling_arclength to insert input points which have multiple lane_ids
+  for (size_t i = 0; i < input_path.points.size(); ++i) {
+    if (input_path.points.at(i).lane_ids.size() < 2) {
+      continue;
+    }
+
+    const double distance_to_resampling_point = calcSignedArcLength(input_path.points, 0, i);
+    for (size_t j = 1; j < resampling_arclength.size(); ++j) {
+      if (
+        resampling_arclength.at(j - 1) <= distance_to_resampling_point &&
+        distance_to_resampling_point < resampling_arclength.at(j)) {
+        const double dist_to_prev_point =
+          std::fabs(distance_to_resampling_point - resampling_arclength.at(j - 1));
+        const double dist_to_following_point =
+          std::fabs(resampling_arclength.at(j) - distance_to_resampling_point);
+        if (dist_to_prev_point < autoware::motion_utils::overlap_threshold) {
+          resampling_arclength.at(j - 1) = distance_to_resampling_point;
+        } else if (dist_to_following_point < autoware::motion_utils::overlap_threshold) {
+          resampling_arclength.at(j) = distance_to_resampling_point;
+        } else {
+          resampling_arclength.insert(
+            resampling_arclength.begin() + j, distance_to_resampling_point);
+        }
+        break;
+      }
+    }
+  }
+
+  // validate arguments
+  if (!resample_utils::validate_arguments(input_path.points, resampling_arclength)) {
+    return input_path;
+  }
+
+  // For LaneIds, is_final
+  //
+  // ------|----|----|----|----|----|----|-------> resampled
+  //      [0]  [1]  [2]  [3]  [4]  [5]  [6]
+  //
+  // ------|----------------|----------|---------> base
+  //      [0]             [1]        [2]
+  //
+  // resampled[0~3] = base[0]
+  // resampled[4~5] = base[1]
+  // resampled[6] = base[2]
+
+  // Input Path Information
+  std::vector<double> input_arclength;
+  std::vector<geometry_msgs::msg::Pose> input_pose;
+  std::vector<double> v_lon;
+  std::vector<double> v_lat;
+  std::vector<double> heading_rate;
+  std::vector<bool> is_final;
+  std::vector<std::vector<int64_t>> lane_ids;
+  input_arclength.reserve(input_path.points.size());
+  input_pose.reserve(input_path.points.size());
+  v_lon.reserve(input_path.points.size());
+  v_lat.reserve(input_path.points.size());
+  heading_rate.reserve(input_path.points.size());
+  is_final.reserve(input_path.points.size());
+  lane_ids.reserve(input_path.points.size());
+
+  input_arclength.push_back(0.0);
+  input_pose.push_back(input_path.points.front().point.pose);
+  v_lon.push_back(input_path.points.front().point.longitudinal_velocity_mps);
+  v_lat.push_back(input_path.points.front().point.lateral_velocity_mps);
+  heading_rate.push_back(input_path.points.front().point.heading_rate_rps);
+  is_final.push_back(input_path.points.front().point.is_final);
+  lane_ids.push_back(input_path.points.front().lane_ids);
+  for (size_t i = 1; i < input_path.points.size(); ++i) {
+    const auto & prev_pt = input_path.points.at(i - 1).point;
+    const auto & curr_pt = input_path.points.at(i).point;
+    const double ds =
+      autoware::universe_utils::calcDistance2d(prev_pt.pose.position, curr_pt.pose.position);
+    input_arclength.push_back(ds + input_arclength.back());
+    input_pose.push_back(curr_pt.pose);
+    v_lon.push_back(curr_pt.longitudinal_velocity_mps);
+    v_lat.push_back(curr_pt.lateral_velocity_mps);
+    heading_rate.push_back(curr_pt.heading_rate_rps);
+    is_final.push_back(curr_pt.is_final);
+    lane_ids.push_back(input_path.points.at(i).lane_ids);
+  }
+
+  if (input_arclength.back() < resampling_arclength.back()) {
+    std::cerr << "[autoware_motion_utils]: resampled path length is longer than input path length"
+              << std::endl;
+    return input_path;
+  }
+
+  // Interpolate
+  const auto lerp = [&](const auto & input) {
+    return autoware::interpolation::lerp(input_arclength, input, resampling_arclength);
+  };
+
+  auto closest_segment_indices =
+    autoware::interpolation::calc_closest_segment_indices(input_arclength, resampling_arclength);
+
+  const auto zoh = [&](const auto & input) {
+    return autoware::interpolation::zero_order_hold(
+      input_arclength, input, closest_segment_indices);
+  };
+
+  const auto interpolated_pose =
+    resamplePoseVector(input_pose, resampling_arclength, use_akima_spline_for_xy, use_lerp_for_z);
+  const auto interpolated_v_lon = use_zero_order_hold_for_v ? zoh(v_lon) : lerp(v_lon);
+  const auto interpolated_v_lat = use_zero_order_hold_for_v ? zoh(v_lat) : lerp(v_lat);
+  const auto interpolated_heading_rate = lerp(heading_rate);
+  const auto interpolated_is_final = zoh(is_final);
+
+  // interpolate lane_ids
+  std::vector<std::vector<int64_t>> interpolated_lane_ids;
+  interpolated_lane_ids.resize(resampling_arclength.size());
+  constexpr double epsilon = 1e-6;
+  for (size_t i = 0; i < resampling_arclength.size(); ++i) {
+    const size_t seg_idx = std::min(closest_segment_indices.at(i), input_path.points.size() - 2);
+    const auto & prev_lane_ids = input_path.points.at(seg_idx).lane_ids;
+    const auto & next_lane_ids = input_path.points.at(seg_idx + 1).lane_ids;
+
+    if (std::abs(input_arclength.at(seg_idx) - resampling_arclength.at(i)) <= epsilon) {
+      interpolated_lane_ids.at(i).insert(
+        interpolated_lane_ids.at(i).end(), prev_lane_ids.begin(), prev_lane_ids.end());
+    } else if (std::abs(input_arclength.at(seg_idx + 1) - resampling_arclength.at(i)) <= epsilon) {
+      interpolated_lane_ids.at(i).insert(
+        interpolated_lane_ids.at(i).end(), next_lane_ids.begin(), next_lane_ids.end());
+    } else {
+      // extract lane_ids those prev_lane_ids and next_lane_ids have in common
+      for (const auto target_lane_id : prev_lane_ids) {
+        if (
+          std::find(next_lane_ids.begin(), next_lane_ids.end(), target_lane_id) !=
+          next_lane_ids.end()) {
+          interpolated_lane_ids.at(i).push_back(target_lane_id);
+        }
+      }
+      // If there are no common lane_ids, the prev_lane_ids is assigned.
+      if (interpolated_lane_ids.at(i).empty()) {
+        interpolated_lane_ids.at(i).insert(
+          interpolated_lane_ids.at(i).end(), prev_lane_ids.begin(), prev_lane_ids.end());
+      }
+    }
+  }
+
+  if (interpolated_pose.size() != resampling_arclength.size()) {
+    std::cerr
+      << "[autoware_motion_utils]: Resampled pose size is different from resampled arclength"
+      << std::endl;
+    return input_path;
+  }
+
+  tier4_planning_msgs::msg::PathWithLaneId resampled_path;
+  resampled_path.header = input_path.header;
+  resampled_path.left_bound = input_path.left_bound;
+  resampled_path.right_bound = input_path.right_bound;
+  resampled_path.points.resize(interpolated_pose.size());
+  for (size_t i = 0; i < resampled_path.points.size(); ++i) {
+    autoware_planning_msgs::msg::PathPoint path_point;
+    path_point.pose = interpolated_pose.at(i);
+    path_point.longitudinal_velocity_mps = interpolated_v_lon.at(i);
+    path_point.lateral_velocity_mps = interpolated_v_lat.at(i);
+    path_point.heading_rate_rps = interpolated_heading_rate.at(i);
+    path_point.is_final = interpolated_is_final.at(i);
+    resampled_path.points.at(i).point = path_point;
+    resampled_path.points.at(i).lane_ids = interpolated_lane_ids.at(i);
+  }
+
+  return resampled_path;
+}
+
+tier4_planning_msgs::msg::PathWithLaneId resamplePath(
+  const tier4_planning_msgs::msg::PathWithLaneId & input_path, const double resample_interval,
+  const bool use_akima_spline_for_xy, const bool use_lerp_for_z,
+  const bool use_zero_order_hold_for_v, const bool resample_input_path_stop_point)
+{
+  // validate arguments
+  if (!resample_utils::validate_arguments(input_path.points, resample_interval)) {
+    return input_path;
+  }
+
+  // transform input_path
+  std::vector<autoware_planning_msgs::msg::PathPoint> transformed_input_path(
+    input_path.points.size());
+  for (size_t i = 0; i < input_path.points.size(); ++i) {
+    transformed_input_path.at(i) = input_path.points.at(i).point;
+  }
+  // compute path length
+  const double input_path_len = autoware::motion_utils::calcArcLength(transformed_input_path);
+
+  std::vector<double> resampling_arclength;
+  for (double s = 0.0; s < input_path_len; s += resample_interval) {
+    resampling_arclength.push_back(s);
+  }
+  if (resampling_arclength.empty()) {
+    std::cerr << "[autoware_motion_utils]: resampling arclength is empty" << std::endl;
+    return input_path;
+  }
+
+  // Insert terminal point
+  if (input_path_len - resampling_arclength.back() < autoware::motion_utils::overlap_threshold) {
+    resampling_arclength.back() = input_path_len;
+  } else {
+    resampling_arclength.push_back(input_path_len);
+  }
+
+  // Insert stop point
+  if (resample_input_path_stop_point) {
+    const auto distance_to_stop_point =
+      autoware::motion_utils::calcDistanceToForwardStopPoint(transformed_input_path, 0);
+    if (distance_to_stop_point && !resampling_arclength.empty()) {
+      for (size_t i = 1; i < resampling_arclength.size(); ++i) {
+        if (
+          resampling_arclength.at(i - 1) <= *distance_to_stop_point &&
+          *distance_to_stop_point < resampling_arclength.at(i)) {
+          const double dist_to_prev_point =
+            std::fabs(*distance_to_stop_point - resampling_arclength.at(i - 1));
+          const double dist_to_following_point =
+            std::fabs(resampling_arclength.at(i) - *distance_to_stop_point);
+          if (dist_to_prev_point < autoware::motion_utils::overlap_threshold) {
+            resampling_arclength.at(i - 1) = *distance_to_stop_point;
+          } else if (dist_to_following_point < autoware::motion_utils::overlap_threshold) {
+            resampling_arclength.at(i) = *distance_to_stop_point;
+          } else {
+            resampling_arclength.insert(resampling_arclength.begin() + i, *distance_to_stop_point);
+          }
+          break;
+        }
+      }
+    }
+  }
+
+  return resamplePath(
+    input_path, resampling_arclength, use_akima_spline_for_xy, use_lerp_for_z,
+    use_zero_order_hold_for_v);
+}
+
+autoware_planning_msgs::msg::Path resamplePath(
+  const autoware_planning_msgs::msg::Path & input_path,
+  const std::vector<double> & resampled_arclength, const bool use_akima_spline_for_xy,
+  const bool use_lerp_for_z, const bool use_zero_order_hold_for_v)
+{
+  // validate arguments
+  if (!resample_utils::validate_arguments(input_path.points, resampled_arclength)) {
+    return input_path;
+  }
+
+  // Input Path Information
+  std::vector<double> input_arclength;
+  std::vector<geometry_msgs::msg::Pose> input_pose;
+  std::vector<double> v_lon;
+  std::vector<double> v_lat;
+  std::vector<double> heading_rate;
+  input_arclength.reserve(input_path.points.size());
+  input_pose.reserve(input_path.points.size());
+  v_lon.reserve(input_path.points.size());
+  v_lat.reserve(input_path.points.size());
+  heading_rate.reserve(input_path.points.size());
+
+  input_arclength.push_back(0.0);
+  input_pose.push_back(input_path.points.front().pose);
+  v_lon.push_back(input_path.points.front().longitudinal_velocity_mps);
+  v_lat.push_back(input_path.points.front().lateral_velocity_mps);
+  heading_rate.push_back(input_path.points.front().heading_rate_rps);
+  for (size_t i = 1; i < input_path.points.size(); ++i) {
+    const auto & prev_pt = input_path.points.at(i - 1);
+    const auto & curr_pt = input_path.points.at(i);
+    const double ds =
+      autoware::universe_utils::calcDistance2d(prev_pt.pose.position, curr_pt.pose.position);
+    input_arclength.push_back(ds + input_arclength.back());
+    input_pose.push_back(curr_pt.pose);
+    v_lon.push_back(curr_pt.longitudinal_velocity_mps);
+    v_lat.push_back(curr_pt.lateral_velocity_mps);
+    heading_rate.push_back(curr_pt.heading_rate_rps);
+  }
+
+  // Interpolate
+  const auto lerp = [&](const auto & input) {
+    return autoware::interpolation::lerp(input_arclength, input, resampled_arclength);
+  };
+
+  std::vector<size_t> closest_segment_indices;
+  if (use_zero_order_hold_for_v) {
+    closest_segment_indices =
+      autoware::interpolation::calc_closest_segment_indices(input_arclength, resampled_arclength);
+  }
+  const auto zoh = [&](const auto & input) {
+    return autoware::interpolation::zero_order_hold(
+      input_arclength, input, closest_segment_indices);
+  };
+
+  const auto interpolated_pose =
+    resamplePoseVector(input_pose, resampled_arclength, use_akima_spline_for_xy, use_lerp_for_z);
+  const auto interpolated_v_lon = use_zero_order_hold_for_v ? zoh(v_lon) : lerp(v_lon);
+  const auto interpolated_v_lat = use_zero_order_hold_for_v ? zoh(v_lat) : lerp(v_lat);
+  const auto interpolated_heading_rate = lerp(heading_rate);
+
+  if (interpolated_pose.size() != resampled_arclength.size()) {
+    std::cerr
+      << "[autoware_motion_utils]: Resampled pose size is different from resampled arclength"
+      << std::endl;
+    return input_path;
+  }
+
+  autoware_planning_msgs::msg::Path resampled_path;
+  resampled_path.header = input_path.header;
+  resampled_path.left_bound = input_path.left_bound;
+  resampled_path.right_bound = input_path.right_bound;
+  resampled_path.points.resize(interpolated_pose.size());
+  for (size_t i = 0; i < resampled_path.points.size(); ++i) {
+    autoware_planning_msgs::msg::PathPoint path_point;
+    path_point.pose = interpolated_pose.at(i);
+    path_point.longitudinal_velocity_mps = interpolated_v_lon.at(i);
+    path_point.lateral_velocity_mps = interpolated_v_lat.at(i);
+    path_point.heading_rate_rps = interpolated_heading_rate.at(i);
+    resampled_path.points.at(i) = path_point;
+  }
+
+  return resampled_path;
+}
+
+autoware_planning_msgs::msg::Path resamplePath(
+  const autoware_planning_msgs::msg::Path & input_path, const double resample_interval,
+  const bool use_akima_spline_for_xy, const bool use_lerp_for_z,
+  const bool use_zero_order_hold_for_twist, const bool resample_input_path_stop_point)
+{
+  // validate arguments
+  if (!resample_utils::validate_arguments(input_path.points, resample_interval)) {
+    return input_path;
+  }
+
+  const double input_path_len = autoware::motion_utils::calcArcLength(input_path.points);
+
+  std::vector<double> resampling_arclength;
+  for (double s = 0.0; s < input_path_len; s += resample_interval) {
+    resampling_arclength.push_back(s);
+  }
+  if (resampling_arclength.empty()) {
+    std::cerr << "[autoware_motion_utils]: resampling arclength is empty" << std::endl;
+    return input_path;
+  }
+
+  // Insert terminal point
+  if (input_path_len - resampling_arclength.back() < autoware::motion_utils::overlap_threshold) {
+    resampling_arclength.back() = input_path_len;
+  } else {
+    resampling_arclength.push_back(input_path_len);
+  }
+
+  // Insert stop point
+  if (resample_input_path_stop_point) {
+    const auto distance_to_stop_point =
+      autoware::motion_utils::calcDistanceToForwardStopPoint(input_path.points, 0);
+    if (distance_to_stop_point && !resampling_arclength.empty()) {
+      for (size_t i = 1; i < resampling_arclength.size(); ++i) {
+        if (
+          resampling_arclength.at(i - 1) <= *distance_to_stop_point &&
+          *distance_to_stop_point < resampling_arclength.at(i)) {
+          const double dist_to_prev_point =
+            std::fabs(*distance_to_stop_point - resampling_arclength.at(i - 1));
+          const double dist_to_following_point =
+            std::fabs(resampling_arclength.at(i) - *distance_to_stop_point);
+          if (dist_to_prev_point < autoware::motion_utils::overlap_threshold) {
+            resampling_arclength.at(i - 1) = *distance_to_stop_point;
+          } else if (dist_to_following_point < autoware::motion_utils::overlap_threshold) {
+            resampling_arclength.at(i) = *distance_to_stop_point;
+          } else {
+            resampling_arclength.insert(resampling_arclength.begin() + i, *distance_to_stop_point);
+          }
+          break;
+        }
+      }
+    }
+  }
+
+  return resamplePath(
+    input_path, resampling_arclength, use_akima_spline_for_xy, use_lerp_for_z,
+    use_zero_order_hold_for_twist);
+}
+
+autoware_planning_msgs::msg::Trajectory resampleTrajectory(
+  const autoware_planning_msgs::msg::Trajectory & input_trajectory,
+  const std::vector<double> & resampled_arclength, const bool use_akima_spline_for_xy,
+  const bool use_lerp_for_z, const bool use_zero_order_hold_for_twist)
+{
+  // validate arguments
+  if (!resample_utils::validate_arguments(input_trajectory.points, resampled_arclength)) {
+    return input_trajectory;
+  }
+
+  // Input Trajectory Information
+  std::vector<double> input_arclength;
+  std::vector<geometry_msgs::msg::Pose> input_pose;
+  std::vector<double> v_lon;
+  std::vector<double> v_lat;
+  std::vector<double> heading_rate;
+  std::vector<double> acceleration;
+  std::vector<double> front_wheel_angle;
+  std::vector<double> rear_wheel_angle;
+  std::vector<double> time_from_start;
+  input_arclength.reserve(input_trajectory.points.size());
+  input_pose.reserve(input_trajectory.points.size());
+  v_lon.reserve(input_trajectory.points.size());
+  v_lat.reserve(input_trajectory.points.size());
+  heading_rate.reserve(input_trajectory.points.size());
+  acceleration.reserve(input_trajectory.points.size());
+  front_wheel_angle.reserve(input_trajectory.points.size());
+  rear_wheel_angle.reserve(input_trajectory.points.size());
+  time_from_start.reserve(input_trajectory.points.size());
+
+  input_arclength.push_back(0.0);
+  input_pose.push_back(input_trajectory.points.front().pose);
+  v_lon.push_back(input_trajectory.points.front().longitudinal_velocity_mps);
+  v_lat.push_back(input_trajectory.points.front().lateral_velocity_mps);
+  heading_rate.push_back(input_trajectory.points.front().heading_rate_rps);
+  acceleration.push_back(input_trajectory.points.front().acceleration_mps2);
+  front_wheel_angle.push_back(input_trajectory.points.front().front_wheel_angle_rad);
+  rear_wheel_angle.push_back(input_trajectory.points.front().rear_wheel_angle_rad);
+  time_from_start.push_back(
+    rclcpp::Duration(input_trajectory.points.front().time_from_start).seconds());
+
+  for (size_t i = 1; i < input_trajectory.points.size(); ++i) {
+    const auto & prev_pt = input_trajectory.points.at(i - 1);
+    const auto & curr_pt = input_trajectory.points.at(i);
+    const double ds =
+      autoware::universe_utils::calcDistance2d(prev_pt.pose.position, curr_pt.pose.position);
+
+    input_arclength.push_back(ds + input_arclength.back());
+    input_pose.push_back(curr_pt.pose);
+    v_lon.push_back(curr_pt.longitudinal_velocity_mps);
+    v_lat.push_back(curr_pt.lateral_velocity_mps);
+    heading_rate.push_back(curr_pt.heading_rate_rps);
+    acceleration.push_back(curr_pt.acceleration_mps2);
+    front_wheel_angle.push_back(curr_pt.front_wheel_angle_rad);
+    rear_wheel_angle.push_back(curr_pt.rear_wheel_angle_rad);
+    time_from_start.push_back(rclcpp::Duration(curr_pt.time_from_start).seconds());
+  }
+
+  // Set Zero Velocity After Stop Point
+  // If the longitudinal velocity is zero, set the velocity to zero after that point.
+  bool stop_point_found_in_v_lon = false;
+  constexpr double epsilon = 1e-4;
+  for (size_t i = 0; i < v_lon.size(); ++i) {
+    if (std::abs(v_lon.at(i)) < epsilon) {
+      stop_point_found_in_v_lon = true;
+    }
+    if (stop_point_found_in_v_lon) {
+      v_lon.at(i) = 0.0;
+    }
+  }
+
+  // Interpolate
+  const auto lerp = [&](const auto & input) {
+    return autoware::interpolation::lerp(input_arclength, input, resampled_arclength);
+  };
+
+  std::vector<size_t> closest_segment_indices;
+  if (use_zero_order_hold_for_twist) {
+    closest_segment_indices =
+      autoware::interpolation::calc_closest_segment_indices(input_arclength, resampled_arclength);
+  }
+  const auto zoh = [&](const auto & input) {
+    return autoware::interpolation::zero_order_hold(
+      input_arclength, input, closest_segment_indices);
+  };
+
+  const auto interpolated_pose =
+    resamplePoseVector(input_pose, resampled_arclength, use_akima_spline_for_xy, use_lerp_for_z);
+  const auto interpolated_v_lon = use_zero_order_hold_for_twist ? zoh(v_lon) : lerp(v_lon);
+  const auto interpolated_v_lat = use_zero_order_hold_for_twist ? zoh(v_lat) : lerp(v_lat);
+  const auto interpolated_heading_rate = lerp(heading_rate);
+  const auto interpolated_acceleration =
+    use_zero_order_hold_for_twist ? zoh(acceleration) : lerp(acceleration);
+  const auto interpolated_front_wheel_angle = lerp(front_wheel_angle);
+  const auto interpolated_rear_wheel_angle = lerp(rear_wheel_angle);
+  const auto interpolated_time_from_start = lerp(time_from_start);
+
+  if (interpolated_pose.size() != resampled_arclength.size()) {
+    std::cerr
+      << "[autoware_motion_utils]: Resampled pose size is different from resampled arclength"
+      << std::endl;
+    return input_trajectory;
+  }
+
+  autoware_planning_msgs::msg::Trajectory resampled_trajectory;
+  resampled_trajectory.header = input_trajectory.header;
+  resampled_trajectory.points.resize(interpolated_pose.size());
+  for (size_t i = 0; i < resampled_trajectory.points.size(); ++i) {
+    autoware_planning_msgs::msg::TrajectoryPoint traj_point;
+    traj_point.pose = interpolated_pose.at(i);
+    traj_point.longitudinal_velocity_mps = interpolated_v_lon.at(i);
+    traj_point.lateral_velocity_mps = interpolated_v_lat.at(i);
+    traj_point.heading_rate_rps = interpolated_heading_rate.at(i);
+    traj_point.acceleration_mps2 = interpolated_acceleration.at(i);
+    traj_point.front_wheel_angle_rad = interpolated_front_wheel_angle.at(i);
+    traj_point.rear_wheel_angle_rad = interpolated_rear_wheel_angle.at(i);
+    traj_point.time_from_start = rclcpp::Duration::from_seconds(interpolated_time_from_start.at(i));
+    resampled_trajectory.points.at(i) = traj_point;
+  }
+
+  return resampled_trajectory;
+}
+
+autoware_planning_msgs::msg::Trajectory resampleTrajectory(
+  const autoware_planning_msgs::msg::Trajectory & input_trajectory, const double resample_interval,
+  const bool use_akima_spline_for_xy, const bool use_lerp_for_z,
+  const bool use_zero_order_hold_for_twist, const bool resample_input_trajectory_stop_point)
+{
+  // validate arguments
+  if (!resample_utils::validate_arguments(input_trajectory.points, resample_interval)) {
+    return input_trajectory;
+  }
+
+  const double input_trajectory_len =
+    autoware::motion_utils::calcArcLength(input_trajectory.points);
+
+  std::vector<double> resampling_arclength;
+  for (double s = 0.0; s < input_trajectory_len; s += resample_interval) {
+    resampling_arclength.push_back(s);
+  }
+  if (resampling_arclength.empty()) {
+    std::cerr << "[autoware_motion_utils]: resampling arclength is empty" << std::endl;
+    return input_trajectory;
+  }
+
+  // Insert terminal point
+  if (
+    input_trajectory_len - resampling_arclength.back() <
+    autoware::motion_utils::overlap_threshold) {
+    resampling_arclength.back() = input_trajectory_len;
+  } else {
+    resampling_arclength.push_back(input_trajectory_len);
+  }
+
+  // Insert stop point
+  if (resample_input_trajectory_stop_point) {
+    const auto distance_to_stop_point =
+      autoware::motion_utils::calcDistanceToForwardStopPoint(input_trajectory.points, 0);
+    if (distance_to_stop_point && !resampling_arclength.empty()) {
+      for (size_t i = 1; i < resampling_arclength.size(); ++i) {
+        if (
+          resampling_arclength.at(i - 1) <= *distance_to_stop_point &&
+          *distance_to_stop_point < resampling_arclength.at(i)) {
+          const double dist_to_prev_point =
+            std::fabs(*distance_to_stop_point - resampling_arclength.at(i - 1));
+          const double dist_to_following_point =
+            std::fabs(resampling_arclength.at(i) - *distance_to_stop_point);
+          if (dist_to_prev_point < autoware::motion_utils::overlap_threshold) {
+            resampling_arclength.at(i - 1) = *distance_to_stop_point;
+          } else if (dist_to_following_point < autoware::motion_utils::overlap_threshold) {
+            resampling_arclength.at(i) = *distance_to_stop_point;
+          } else {
+            resampling_arclength.insert(resampling_arclength.begin() + i, *distance_to_stop_point);
+          }
+          break;
+        }
+      }
+    }
+  }
+
+  return resampleTrajectory(
+    input_trajectory, resampling_arclength, use_akima_spline_for_xy, use_lerp_for_z,
+    use_zero_order_hold_for_twist);
+}
+
+}  // namespace autoware::motion_utils