From fddfaf481b3ee3798d38acbbcbc3a38c661b8346 Mon Sep 17 00:00:00 2001
From: Mamoru Sobue <mamoru.sobue@tier4.jp>
Date: Mon, 10 Mar 2025 21:34:22 +0900
Subject: [PATCH] doc(lanelet2_utils): fix invalid drawio link and update image

Signed-off-by: Mamoru Sobue <mamoru.sobue@tier4.jp>
---
 .../trajectory/detail/interpolated_array.hpp  | 26 ++++++----
 .../trajectory/interpolator/akima_spline.hpp  | 20 +++++--
 .../trajectory/interpolator/cubic_spline.hpp  | 21 ++++++--
 .../detail/interpolator_common_interface.hpp  | 52 ++++++++++---------
 .../detail/interpolator_mixin.hpp             | 16 +++++-
 .../detail/nearest_neighbor_common_impl.hpp   | 22 ++++++--
 .../detail/stairstep_common_impl.hpp          | 19 ++++++-
 .../trajectory/interpolator/interpolator.hpp  | 12 ++---
 .../trajectory/interpolator/linear.hpp        | 21 ++++++--
 .../interpolator/nearest_neighbor.hpp         |  4 +-
 .../interpolator/spherical_linear.hpp         | 17 ++++--
 .../trajectory/interpolator/stairstep.hpp     |  4 +-
 .../autoware/trajectory/path_point.hpp        | 15 +++---
 .../trajectory/path_point_with_lane_id.hpp    | 11 ++--
 .../include/autoware/trajectory/point.hpp     | 18 +++----
 .../include/autoware/trajectory/pose.hpp      |  6 +--
 .../autoware/trajectory/trajectory_point.hpp  | 21 ++++----
 .../src/interpolator/akima_spline.cpp         | 19 +++++--
 .../src/interpolator/cubic_spline.cpp         | 19 +++++--
 .../src/interpolator/linear.cpp               | 17 ++++--
 .../src/interpolator/spherical_linear.cpp     | 13 ++++-
 common/autoware_trajectory/src/path_point.cpp | 12 ++---
 .../src/path_point_with_lane_id.cpp           |  8 +--
 common/autoware_trajectory/src/point.cpp      | 44 ++++++++--------
 common/autoware_trajectory/src/pose.cpp       | 10 ++--
 .../src/trajectory_point.cpp                  | 18 +++----
 26 files changed, 295 insertions(+), 170 deletions(-)

diff --git a/common/autoware_trajectory/include/autoware/trajectory/detail/interpolated_array.hpp b/common/autoware_trajectory/include/autoware/trajectory/detail/interpolated_array.hpp
index 06f1185620..02e01a4815 100644
--- a/common/autoware_trajectory/include/autoware/trajectory/detail/interpolated_array.hpp
+++ b/common/autoware_trajectory/include/autoware/trajectory/detail/interpolated_array.hpp
@@ -71,6 +71,13 @@ class InterpolatedArray
     return interpolator_->build(bases_, values_);
   }
 
+  bool build(std::vector<double> && bases, std::vector<T> && values)
+  {
+    bases_ = std::move(bases);
+    values_ = std::move(values);
+    return interpolator_->build(bases_, values_);
+  }
+
   /**
    * @brief Move constructor.
    * @param other The InterpolatedArray to move from.
@@ -99,22 +106,22 @@ class InterpolatedArray
    * @brief Get the start value of the base.
    * @return The start value.
    */
-  [[nodiscard]] double start() const { return bases_.front(); }
+  double start() const { return bases_.front(); }
 
   /**
    * @brief Get the end value of the base.
    * @return The end value.
    */
-  [[nodiscard]] double end() const { return bases_.at(bases_.size() - 1); }
+  double end() const { return bases_.at(bases_.size() - 1); }
 
   class Segment
   {
     friend class InterpolatedArray;
 
-    double start_;
-    double end_;
+    const double start_;
+    const double end_;
     InterpolatedArray<T> & parent_;
-    Segment(InterpolatedArray<T> & parent, double start, double end)
+    Segment(InterpolatedArray<T> & parent, const double start, const double end)
     : start_(start), end_(end), parent_(parent)
     {
     }
@@ -125,7 +132,7 @@ class InterpolatedArray
       std::vector<double> & bases = parent_.bases_;
       std::vector<T> & values = parent_.values_;
 
-      auto insert_if_not_present = [&](double val) -> size_t {
+      auto insert_if_not_present = [&](const double val) -> size_t {
         auto it = std::lower_bound(bases.begin(), bases.end(), val);
         size_t index = std::distance(bases.begin(), it);
 
@@ -200,16 +207,13 @@ class InterpolatedArray
    * @param x The position to compute the value at.
    * @return The interpolated value.
    */
-  [[nodiscard]] T compute(const double & x) const { return interpolator_->compute(x); }
+  T compute(const double x) const { return interpolator_->compute(x); }
 
   /**
    * @brief Get the underlying data of the array.
    * @return A pair containing the axis and values.
    */
-  [[nodiscard]] std::pair<std::vector<double>, std::vector<T>> get_data() const
-  {
-    return {bases_, values_};
-  }
+  std::pair<std::vector<double>, std::vector<T>> get_data() const { return {bases_, values_}; }
 };
 
 }  // namespace autoware::trajectory::detail
diff --git a/common/autoware_trajectory/include/autoware/trajectory/interpolator/akima_spline.hpp b/common/autoware_trajectory/include/autoware/trajectory/interpolator/akima_spline.hpp
index e3484b0205..1cd0a9f863 100644
--- a/common/autoware_trajectory/include/autoware/trajectory/interpolator/akima_spline.hpp
+++ b/common/autoware_trajectory/include/autoware/trajectory/interpolator/akima_spline.hpp
@@ -52,7 +52,17 @@ class AkimaSpline : public detail::InterpolatorMixin<AkimaSpline, double>
    * @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;
+  [[nodiscard]] bool build_impl(
+    const std::vector<double> & bases, const std::vector<double> & values) override;
+
+  /**
+   * @brief Build the interpolator with the given values.
+   *
+   * @param bases The bases values.
+   * @param values The values to interpolate.
+   */
+  [[nodiscard]] bool build_impl(
+    std::vector<double> && bases, std::vector<double> && values) override;
 
   /**
    * @brief Compute the interpolated value at the given point.
@@ -60,7 +70,7 @@ class AkimaSpline : public detail::InterpolatorMixin<AkimaSpline, double>
    * @param s The point at which to compute the interpolated value.
    * @return The interpolated value.
    */
-  [[nodiscard]] double compute_impl(const double & s) const override;
+  double compute_impl(const double s) const override;
 
   /**
    * @brief Compute the first derivative at the given point.
@@ -68,7 +78,7 @@ class AkimaSpline : public detail::InterpolatorMixin<AkimaSpline, double>
    * @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;
+  double compute_first_derivative_impl(const double s) const override;
 
   /**
    * @brief Compute the second derivative at the given point.
@@ -76,7 +86,7 @@ class AkimaSpline : public detail::InterpolatorMixin<AkimaSpline, double>
    * @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;
+  double compute_second_derivative_impl(const double s) const override;
 
 public:
   AkimaSpline() = default;
@@ -86,7 +96,7 @@ class AkimaSpline : public detail::InterpolatorMixin<AkimaSpline, double>
    *
    * @return The minimum number of required points.
    */
-  [[nodiscard]] size_t minimum_required_points() const override { return 5; }
+  size_t minimum_required_points() const override { return 5; }
 };
 
 }  // namespace autoware::trajectory::interpolator
diff --git a/common/autoware_trajectory/include/autoware/trajectory/interpolator/cubic_spline.hpp b/common/autoware_trajectory/include/autoware/trajectory/interpolator/cubic_spline.hpp
index 51694b15f8..8a17647261 100644
--- a/common/autoware_trajectory/include/autoware/trajectory/interpolator/cubic_spline.hpp
+++ b/common/autoware_trajectory/include/autoware/trajectory/interpolator/cubic_spline.hpp
@@ -54,7 +54,18 @@ class CubicSpline : public detail::InterpolatorMixin<CubicSpline, double>
    * @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;
+  [[nodiscard]] bool build_impl(
+    const std::vector<double> & bases, const std::vector<double> & values) override;
+
+  /**
+   * @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.
+   */
+  [[nodiscard]] bool build_impl(
+    std::vector<double> && bases, std::vector<double> && values) override;
 
   /**
    * @brief Compute the interpolated value at the given point.
@@ -62,7 +73,7 @@ class CubicSpline : public detail::InterpolatorMixin<CubicSpline, double>
    * @param s The point at which to compute the interpolated value.
    * @return The interpolated value.
    */
-  [[nodiscard]] double compute_impl(const double & s) const override;
+  double compute_impl(const double s) const override;
 
   /**
    * @brief Compute the first derivative at the given point.
@@ -70,7 +81,7 @@ class CubicSpline : public detail::InterpolatorMixin<CubicSpline, double>
    * @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;
+  double compute_first_derivative_impl(const double s) const override;
 
   /**
    * @brief Compute the second derivative at the given point.
@@ -78,7 +89,7 @@ class CubicSpline : public detail::InterpolatorMixin<CubicSpline, double>
    * @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;
+  double compute_second_derivative_impl(const double s) const override;
 
 public:
   CubicSpline() = default;
@@ -88,7 +99,7 @@ class CubicSpline : public detail::InterpolatorMixin<CubicSpline, double>
    *
    * @return The minimum number of required points.
    */
-  [[nodiscard]] size_t minimum_required_points() const override { return 4; }
+  size_t minimum_required_points() const override { return 4; }
 };
 
 }  // namespace autoware::trajectory::interpolator
diff --git a/common/autoware_trajectory/include/autoware/trajectory/interpolator/detail/interpolator_common_interface.hpp b/common/autoware_trajectory/include/autoware/trajectory/interpolator/detail/interpolator_common_interface.hpp
index df18c65e05..e566b97878 100644
--- a/common/autoware_trajectory/include/autoware/trajectory/interpolator/detail/interpolator_common_interface.hpp
+++ b/common/autoware_trajectory/include/autoware/trajectory/interpolator/detail/interpolator_common_interface.hpp
@@ -35,17 +35,16 @@ class InterpolatorCommonInterface
 {
 protected:
   std::vector<double> bases_;  ///< bases values for the interpolation.
-  bool is_built_{false};       ///< flag indicating whether the interpolator has been built.
 
   /**
    * @brief Get the start of the interpolation range.
    */
-  [[nodiscard]] double start() const { return bases_.front(); }
+  double start() const { return bases_.front(); }
 
   /**
    * @brief Get the end of the interpolation range.
    */
-  [[nodiscard]] double end() const { return bases_.back(); }
+  double end() const { return bases_.back(); }
 
   /**
    * @brief Compute the interpolated value at the given point.
@@ -55,7 +54,7 @@ class InterpolatorCommonInterface
    * @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;
+  virtual T compute_impl(const double s) const = 0;
 
   /**
    * @brief Build the interpolator with the given values.
@@ -65,7 +64,18 @@ class InterpolatorCommonInterface
    * @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;
+  [[nodiscard]] virtual bool build_impl(
+    const std::vector<double> & bases, const std::vector<T> & values) = 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.
+   */
+  [[nodiscard]] virtual bool build_impl(std::vector<double> && bases, std::vector<T> && values) = 0;
 
   /**
    * @brief Validate the input to the compute method.
@@ -75,7 +85,7 @@ class InterpolatorCommonInterface
    * @param s The input value.
    * @return The input value, clamped to the range of the interpolator.
    */
-  [[nodiscard]] double validate_compute_input(const double & s) const
+double validate_compute_input(const double s) const
   {
     if (s < start() || s > end()) {
       RCLCPP_WARN(
@@ -100,7 +110,7 @@ class InterpolatorCommonInterface
    *
    * @throw std::out_of_range if the input value is outside the range of the bases array.
    */
-  [[nodiscard]] int32_t get_index(const double & s, bool end_inclusive = true) const
+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;
@@ -125,7 +135,12 @@ class InterpolatorCommonInterface
    * @param values The values to interpolate.
    * @return True if the interpolator was built successfully, false otherwise.
    */
-  [[nodiscard]] bool build(const std::vector<double> & bases, const std::vector<T> & values)
+  template <typename BaseVectorT, typename ValueVectorT>
+  [[nodiscard]] auto build(BaseVectorT && bases, ValueVectorT && values) -> std::enable_if_t<
+    std::conjunction_v<
+      std::is_same<std::decay_t<BaseVectorT>, std::vector<double>>,
+      std::is_same<std::decay_t<ValueVectorT>, std::vector<T>>>,
+    bool>
   {
     if (bases.size() != values.size()) {
       return false;
@@ -133,18 +148,11 @@ class InterpolatorCommonInterface
     if (bases.size() < minimum_required_points()) {
       return false;
     }
-    build_impl(bases, values);
-    is_built_ = true;
-    return true;
+    return build_impl(
+      std::forward<std::decay_t<BaseVectorT>>(bases),
+      std::forward<std::decay_t<ValueVectorT>>(values));
   }
 
-  /**
-   * @brief Check if the interpolator has been built.
-   *
-   * @return True if the interpolator has been built, false otherwise.
-   */
-  [[nodiscard]] bool is_built() const { return is_built_; }
-
   /**
    * @brief Get the minimum number of required points for the interpolator.
    *
@@ -152,7 +160,7 @@ class InterpolatorCommonInterface
    *
    * @return The minimum number of required points.
    */
-  [[nodiscard]] virtual size_t minimum_required_points() const = 0;
+  virtual size_t minimum_required_points() const = 0;
 
   /**
    * @brief Compute the interpolated value at the given point.
@@ -161,12 +169,8 @@ class InterpolatorCommonInterface
    * @return The interpolated value.
    * @throw std::runtime_error if the interpolator has not been built.
    */
-  [[nodiscard]] T compute(const double & s) const
+  T compute(const double s) const
   {
-    if (!is_built_) {
-      throw std::runtime_error(
-        "Interpolator has not been built.");  // This Exception should not be thrown.
-    }
     const double clamped_s = validate_compute_input(s);
     return compute_impl(clamped_s);
   }
diff --git a/common/autoware_trajectory/include/autoware/trajectory/interpolator/detail/interpolator_mixin.hpp b/common/autoware_trajectory/include/autoware/trajectory/interpolator/detail/interpolator_mixin.hpp
index 98e46e5467..ebc7bbe400 100644
--- a/common/autoware_trajectory/include/autoware/trajectory/interpolator/detail/interpolator_mixin.hpp
+++ b/common/autoware_trajectory/include/autoware/trajectory/interpolator/detail/interpolator_mixin.hpp
@@ -38,7 +38,7 @@ namespace autoware::trajectory::interpolator::detail
 template <class InterpolatorType, class T>
 struct InterpolatorMixin : public InterpolatorInterface<T>
 {
-  [[nodiscard]] std::shared_ptr<InterpolatorInterface<T>> clone() const override
+  std::shared_ptr<InterpolatorInterface<T>> clone() const override
   {
     return std::make_shared<InterpolatorType>(static_cast<const InterpolatorType &>(*this));
   }
@@ -62,17 +62,29 @@ struct InterpolatorMixin : public InterpolatorInterface<T>
       return *this;
     }
 
+    [[nodiscard]] Builder & set_bases(std::vector<double> && bases)
+    {
+      bases_ = std::move(bases);
+      return *this;
+    }
+
     [[nodiscard]] Builder & set_values(const std::vector<T> & values)
     {
       values_ = values;
       return *this;
     }
 
+    [[nodiscard]] Builder & set_values(std::vector<T> && values)
+    {
+      values_ = std::move(values);
+      return *this;
+    }
+
     template <typename... Args>
     [[nodiscard]] std::optional<InterpolatorType> build(Args &&... args)
     {
       auto interpolator = InterpolatorType(std::forward<Args>(args)...);
-      const bool success = interpolator.build(bases_, values_);
+      const bool success = interpolator.build(std::move(bases_), std::move(values_));
       if (!success) {
         return std::nullopt;
       }
diff --git a/common/autoware_trajectory/include/autoware/trajectory/interpolator/detail/nearest_neighbor_common_impl.hpp b/common/autoware_trajectory/include/autoware/trajectory/interpolator/detail/nearest_neighbor_common_impl.hpp
index f02ed5fa8a..6327d12427 100644
--- a/common/autoware_trajectory/include/autoware/trajectory/interpolator/detail/nearest_neighbor_common_impl.hpp
+++ b/common/autoware_trajectory/include/autoware/trajectory/interpolator/detail/nearest_neighbor_common_impl.hpp
@@ -46,7 +46,7 @@ class NearestNeighborCommonImpl : public detail::InterpolatorMixin<NearestNeighb
    * @param s The point at which to compute the interpolated value.
    * @return The interpolated value.
    */
-  [[nodiscard]] T compute_impl(const double & s) const override
+  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]))
@@ -61,10 +61,26 @@ class NearestNeighborCommonImpl : public detail::InterpolatorMixin<NearestNeighb
    * @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
+  [[nodiscard]] bool build_impl(
+    const std::vector<double> & bases, const std::vector<T> & values) override
   {
     this->bases_ = bases;
     this->values_ = values;
+    return true;
+  }
+
+  /**
+   * @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.
+   */
+  [[nodiscard]] bool build_impl(std::vector<double> && bases, std::vector<T> && values) override
+  {
+    this->bases_ = std::move(bases);
+    this->values_ = std::move(values);
+    return true;
   }
 
 public:
@@ -73,7 +89,7 @@ class NearestNeighborCommonImpl : public detail::InterpolatorMixin<NearestNeighb
    *
    * @return The minimum number of required points.
    */
-  [[nodiscard]] size_t minimum_required_points() const override { return 1; }
+  size_t minimum_required_points() const override { return 1; }
 };
 
 }  // namespace detail
diff --git a/common/autoware_trajectory/include/autoware/trajectory/interpolator/detail/stairstep_common_impl.hpp b/common/autoware_trajectory/include/autoware/trajectory/interpolator/detail/stairstep_common_impl.hpp
index 5b6a6b6942..f2b1c4bc85 100644
--- a/common/autoware_trajectory/include/autoware/trajectory/interpolator/detail/stairstep_common_impl.hpp
+++ b/common/autoware_trajectory/include/autoware/trajectory/interpolator/detail/stairstep_common_impl.hpp
@@ -47,7 +47,7 @@ class StairstepCommonImpl : public detail::InterpolatorMixin<Stairstep<T>, T>
    * @param s The point at which to compute the interpolated value.
    * @return The interpolated value.
    */
-  [[nodiscard]] T compute_impl(const double & s) const override
+  T compute_impl(const double s) const override
   {
     const int32_t idx = this->get_index(s, false);
     return this->values_.at(idx);
@@ -58,10 +58,25 @@ class StairstepCommonImpl : public detail::InterpolatorMixin<Stairstep<T>, T>
    * @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
+  [[nodiscard]] bool build_impl(
+    const std::vector<double> & bases, const std::vector<T> & values) override
   {
     this->bases_ = bases;
     this->values_ = values;
+    return true;
+  }
+
+  /**
+   * @brief Build the interpolator with the given values.
+   *
+   * @param bases The bases values.
+   * @param values The values to interpolate.
+   */
+  [[nodiscard]] bool build_impl(std::vector<double> && bases, std::vector<T> && values) override
+  {
+    this->bases_ = std::move(bases);
+    this->values_ = std::move(values);
+    return true;
   }
 
 public:
diff --git a/common/autoware_trajectory/include/autoware/trajectory/interpolator/interpolator.hpp b/common/autoware_trajectory/include/autoware/trajectory/interpolator/interpolator.hpp
index 1f207822e2..ebba82cd46 100644
--- a/common/autoware_trajectory/include/autoware/trajectory/interpolator/interpolator.hpp
+++ b/common/autoware_trajectory/include/autoware/trajectory/interpolator/interpolator.hpp
@@ -32,7 +32,7 @@ template <typename T>
 class InterpolatorInterface : public detail::InterpolatorCommonInterface<T>
 {
 public:
-  [[nodiscard]] virtual std::shared_ptr<InterpolatorInterface<T>> clone() const = 0;
+  virtual std::shared_ptr<InterpolatorInterface<T>> clone() const = 0;
 };
 
 /**
@@ -52,7 +52,7 @@ class InterpolatorInterface<double> : public detail::InterpolatorCommonInterface
    * @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;
+  virtual double compute_first_derivative_impl(const double s) const = 0;
 
   /**
    * @brief Compute the second derivative at the given point.
@@ -62,7 +62,7 @@ class InterpolatorInterface<double> : public detail::InterpolatorCommonInterface
    * @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;
+  virtual double compute_second_derivative_impl(const double s) const = 0;
 
 public:
   /**
@@ -71,7 +71,7 @@ class InterpolatorInterface<double> : public detail::InterpolatorCommonInterface
    * @param s The point at which to compute the first derivative.
    * @return The first derivative.
    */
-  [[nodiscard]] double compute_first_derivative(const double & s) const
+  double compute_first_derivative(const double s) const
   {
     const double clamped_s = this->validate_compute_input(s);
     return compute_first_derivative_impl(clamped_s);
@@ -83,13 +83,13 @@ class InterpolatorInterface<double> : public detail::InterpolatorCommonInterface
    * @param s The point at which to compute the second derivative.
    * @return The second derivative.
    */
-  [[nodiscard]] double compute_second_derivative(const double & s) const
+  double compute_second_derivative(const double s) const
   {
     const double clamped_s = this->validate_compute_input(s);
     return compute_second_derivative_impl(clamped_s);
   }
 
-  [[nodiscard]] virtual std::shared_ptr<InterpolatorInterface<double>> clone() const = 0;
+  virtual std::shared_ptr<InterpolatorInterface<double>> clone() const = 0;
 };
 
 }  // namespace autoware::trajectory::interpolator
diff --git a/common/autoware_trajectory/include/autoware/trajectory/interpolator/linear.hpp b/common/autoware_trajectory/include/autoware/trajectory/interpolator/linear.hpp
index bef2cde70f..a0876c70fe 100644
--- a/common/autoware_trajectory/include/autoware/trajectory/interpolator/linear.hpp
+++ b/common/autoware_trajectory/include/autoware/trajectory/interpolator/linear.hpp
@@ -41,7 +41,18 @@ class Linear : public detail::InterpolatorMixin<Linear, double>
    * @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;
+  [[nodiscard]] bool build_impl(
+    const std::vector<double> & bases, const std::vector<double> & values) override;
+
+  /**
+   * @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.
+   */
+  [[nodiscard]] bool build_impl(
+    std::vector<double> && bases, std::vector<double> && values) override;
 
   /**
    * @brief Compute the interpolated value at the given point.
@@ -49,7 +60,7 @@ class Linear : public detail::InterpolatorMixin<Linear, double>
    * @param s The point at which to compute the interpolated value.
    * @return The interpolated value.
    */
-  [[nodiscard]] double compute_impl(const double & s) const override;
+  double compute_impl(const double s) const override;
 
   /**
    * @brief Compute the first derivative at the given point.
@@ -57,7 +68,7 @@ class Linear : public detail::InterpolatorMixin<Linear, double>
    * @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;
+  double compute_first_derivative_impl(const double s) const override;
 
   /**
    * @brief Compute the second derivative at the given point.
@@ -65,7 +76,7 @@ class Linear : public detail::InterpolatorMixin<Linear, double>
    * @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;
+  double compute_second_derivative_impl(const double) const override;
 
 public:
   /**
@@ -78,7 +89,7 @@ class Linear : public detail::InterpolatorMixin<Linear, double>
    *
    * @return The minimum number of required points.
    */
-  [[nodiscard]] size_t minimum_required_points() const override;
+  size_t minimum_required_points() const override;
 };
 
 }  // namespace autoware::trajectory::interpolator
diff --git a/common/autoware_trajectory/include/autoware/trajectory/interpolator/nearest_neighbor.hpp b/common/autoware_trajectory/include/autoware/trajectory/interpolator/nearest_neighbor.hpp
index 959c8cfb3a..b1b404033b 100644
--- a/common/autoware_trajectory/include/autoware/trajectory/interpolator/nearest_neighbor.hpp
+++ b/common/autoware_trajectory/include/autoware/trajectory/interpolator/nearest_neighbor.hpp
@@ -59,7 +59,7 @@ class NearestNeighbor<double> : public detail::NearestNeighborCommonImpl<double>
    * @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; }
+  double compute_first_derivative_impl(const double) const override { return 0.0; }
 
   /**
    * @brief Compute the second derivative at the given point.
@@ -67,7 +67,7 @@ class NearestNeighbor<double> : public detail::NearestNeighborCommonImpl<double>
    * @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; }
+  double compute_second_derivative_impl(const double) const override { return 0.0; }
 
 public:
   NearestNeighbor() = default;
diff --git a/common/autoware_trajectory/include/autoware/trajectory/interpolator/spherical_linear.hpp b/common/autoware_trajectory/include/autoware/trajectory/interpolator/spherical_linear.hpp
index c443daf114..59d012ee37 100644
--- a/common/autoware_trajectory/include/autoware/trajectory/interpolator/spherical_linear.hpp
+++ b/common/autoware_trajectory/include/autoware/trajectory/interpolator/spherical_linear.hpp
@@ -42,17 +42,28 @@ class SphericalLinear
    * @param values The values to interpolate.
    * @return True if the interpolator was built successfully, false otherwise.
    */
-  void build_impl(
+  [[nodiscard]] bool build_impl(
     const std::vector<double> & bases,
     const std::vector<geometry_msgs::msg::Quaternion> & quaternions) override;
 
+  /**
+   * @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.
+   */
+  [[nodiscard]] bool build_impl(
+    std::vector<double> && bases,
+    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;
+  geometry_msgs::msg::Quaternion compute_impl(const double s) const override;
 
 public:
   /**
@@ -65,7 +76,7 @@ class SphericalLinear
    *
    * @return The minimum number of required points.
    */
-  [[nodiscard]] size_t minimum_required_points() const override;
+  size_t minimum_required_points() const override;
 };
 
 }  // namespace autoware::trajectory::interpolator
diff --git a/common/autoware_trajectory/include/autoware/trajectory/interpolator/stairstep.hpp b/common/autoware_trajectory/include/autoware/trajectory/interpolator/stairstep.hpp
index ad7ac8c7e5..d6af957014 100644
--- a/common/autoware_trajectory/include/autoware/trajectory/interpolator/stairstep.hpp
+++ b/common/autoware_trajectory/include/autoware/trajectory/interpolator/stairstep.hpp
@@ -59,7 +59,7 @@ class Stairstep<double> : public detail::StairstepCommonImpl<double>
    * @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; }
+  double compute_first_derivative_impl(const double) const override { return 0.0; }
 
   /**
    * @brief Compute the second derivative at the given point.
@@ -67,7 +67,7 @@ class Stairstep<double> : public detail::StairstepCommonImpl<double>
    * @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; }
+  double compute_second_derivative_impl(const double) const override { return 0.0; }
 
 public:
   Stairstep() = default;
diff --git a/common/autoware_trajectory/include/autoware/trajectory/path_point.hpp b/common/autoware_trajectory/include/autoware/trajectory/path_point.hpp
index d7295570b5..68ff1f0b11 100644
--- a/common/autoware_trajectory/include/autoware/trajectory/path_point.hpp
+++ b/common/autoware_trajectory/include/autoware/trajectory/path_point.hpp
@@ -48,7 +48,7 @@ class Trajectory<autoware_planning_msgs::msg::PathPoint>
   Trajectory & operator=(const Trajectory & rhs);
   Trajectory & operator=(Trajectory && rhs) = default;
 
-  [[nodiscard]] std::vector<double> get_internal_bases() const override;
+  std::vector<double> get_internal_bases() const override;
 
   detail::InterpolatedArray<double> & longitudinal_velocity_mps()
   {
@@ -59,20 +59,17 @@ class Trajectory<autoware_planning_msgs::msg::PathPoint>
 
   detail::InterpolatedArray<double> & heading_rate_rps() { return *heading_rate_rps_; }
 
-  [[nodiscard]] const detail::InterpolatedArray<double> & longitudinal_velocity_mps() const
+  const detail::InterpolatedArray<double> & longitudinal_velocity_mps() const
   {
     return *longitudinal_velocity_mps_;
   }
 
-  [[nodiscard]] const detail::InterpolatedArray<double> & lateral_velocity_mps() const
+  const detail::InterpolatedArray<double> & lateral_velocity_mps() const
   {
     return *lateral_velocity_mps_;
   }
 
-  [[nodiscard]] const detail::InterpolatedArray<double> & heading_rate_rps() const
-  {
-    return *heading_rate_rps_;
-  }
+  const detail::InterpolatedArray<double> & heading_rate_rps() const { return *heading_rate_rps_; }
 
   /**
    * @brief Build the trajectory from the points
@@ -86,14 +83,14 @@ class Trajectory<autoware_planning_msgs::msg::PathPoint>
    * @param s Arc length
    * @return Point on the trajectory
    */
-  [[nodiscard]] PointType compute(double s) const;
+  PointType compute(const double s) const;
 
   /**
    * @brief Restore the trajectory points
    * @param min_points Minimum number of points
    * @return Vector of points
    */
-  [[nodiscard]] std::vector<PointType> restore(const size_t & min_points = 4) const;
+  std::vector<PointType> restore(const size_t min_points = 4) const;
 
   class Builder
   {
diff --git a/common/autoware_trajectory/include/autoware/trajectory/path_point_with_lane_id.hpp b/common/autoware_trajectory/include/autoware/trajectory/path_point_with_lane_id.hpp
index d70d8443a5..6e0d4b7ce2 100644
--- a/common/autoware_trajectory/include/autoware/trajectory/path_point_with_lane_id.hpp
+++ b/common/autoware_trajectory/include/autoware/trajectory/path_point_with_lane_id.hpp
@@ -45,10 +45,7 @@ class Trajectory<autoware_internal_planning_msgs::msg::PathPointWithLaneId>
 
   detail::InterpolatedArray<LaneIdType> & lane_ids() { return *lane_ids_; }
 
-  [[nodiscard]] const detail::InterpolatedArray<LaneIdType> & lane_ids() const
-  {
-    return *lane_ids_;
-  }
+  const detail::InterpolatedArray<LaneIdType> & lane_ids() const { return *lane_ids_; }
 
   /**
    * @brief Build the trajectory from the points
@@ -57,21 +54,21 @@ class Trajectory<autoware_internal_planning_msgs::msg::PathPointWithLaneId>
    */
   bool build(const std::vector<PointType> & points);
 
-  [[nodiscard]] std::vector<double> get_internal_bases() const override;
+  std::vector<double> get_internal_bases() const override;
 
   /**
    * @brief Compute the point on the trajectory at a given s value
    * @param s Arc length
    * @return Point on the trajectory
    */
-  [[nodiscard]] PointType compute(double s) const;
+  PointType compute(const double s) const;
 
   /**
    * @brief Restore the trajectory points
    * @param min_points Minimum number of points
    * @return Vector of points
    */
-  [[nodiscard]] std::vector<PointType> restore(const size_t & min_points = 4) const;
+  std::vector<PointType> restore(const size_t min_points = 4) const;
 
   class Builder
   {
diff --git a/common/autoware_trajectory/include/autoware/trajectory/point.hpp b/common/autoware_trajectory/include/autoware/trajectory/point.hpp
index cc69ffc2ae..647063bd98 100644
--- a/common/autoware_trajectory/include/autoware/trajectory/point.hpp
+++ b/common/autoware_trajectory/include/autoware/trajectory/point.hpp
@@ -57,7 +57,7 @@ class Trajectory<geometry_msgs::msg::Point>
    * @brief Validate the arc length is within the trajectory
    * @param s Arc length
    */
-  [[nodiscard]] double clamp(const double & s, bool show_warning = false) const;
+  double clamp(const double s, bool show_warning = false) const;
 
 public:
   Trajectory();
@@ -71,19 +71,19 @@ class Trajectory<geometry_msgs::msg::Point>
    * @brief Get the internal bases(arc lengths) of the trajectory
    * @return Vector of bases(arc lengths)
    */
-  [[nodiscard]] virtual std::vector<double> get_internal_bases() const;
+  virtual std::vector<double> get_internal_bases() const;
   /**
    * @brief Get the length of the trajectory
    * @return Length of the trajectory
    */
-  [[nodiscard]] double length() const;
+  double length() const;
 
   /**
    * @brief Compute the point on the trajectory at a given s value
    * @param s Arc length
    * @return Point on the trajectory
    */
-  [[nodiscard]] PointType compute(double s) const;
+  PointType compute(const double s) const;
 
   /**
    * @brief Build the trajectory from the points
@@ -97,30 +97,30 @@ class Trajectory<geometry_msgs::msg::Point>
    * @param s Arc length
    * @return Azimuth in radians
    */
-  [[nodiscard]] double azimuth(double s) const;
+  double azimuth(const double s) const;
 
   /**
    * @brief Get the elevation angle at a given s value
    * @param s Arc length
    * @return Elevation in radians
    */
-  [[nodiscard]] double elevation(double s) const;
+  double elevation(const double s) const;
 
   /**
    * @brief Get the curvature at a given s value
    * @param s Arc length
    * @return Curvature
    */
-  [[nodiscard]] double curvature(double s) const;
+  double curvature(const double s) const;
 
   /**
    * @brief Restore the trajectory points
    * @param min_points Minimum number of points
    * @return Vector of points
    */
-  [[nodiscard]] std::vector<PointType> restore(const size_t & min_points = 4) const;
+  std::vector<PointType> restore(const size_t min_points = 4) const;
 
-  void crop(const double & start, const double & length);
+  void crop(const double start, const double length);
 
   class Builder
   {
diff --git a/common/autoware_trajectory/include/autoware/trajectory/pose.hpp b/common/autoware_trajectory/include/autoware/trajectory/pose.hpp
index a7030e352b..4eacde3e57 100644
--- a/common/autoware_trajectory/include/autoware/trajectory/pose.hpp
+++ b/common/autoware_trajectory/include/autoware/trajectory/pose.hpp
@@ -58,20 +58,20 @@ class Trajectory<geometry_msgs::msg::Pose> : public Trajectory<geometry_msgs::ms
    * @brief Get the internal bases(arc lengths) of the trajectory
    * @return Vector of bases(arc lengths)
    */
-  [[nodiscard]] std::vector<double> get_internal_bases() const override;
+  std::vector<double> get_internal_bases() const override;
 
   /**
    * @brief Compute the pose on the trajectory at a given s value
    * @param s Arc length
    * @return Pose on the trajectory
    */
-  [[nodiscard]] PointType compute(double s) const;
+  PointType compute(const double s) const;
 
   /**
    * @brief Restore the trajectory poses
    * @return Vector of poses
    */
-  [[nodiscard]] std::vector<PointType> restore(const size_t & min_points = 4) const;
+  std::vector<PointType> restore(const size_t min_points = 4) const;
 
   /**
    * @brief Align the orientation with the direction
diff --git a/common/autoware_trajectory/include/autoware/trajectory/trajectory_point.hpp b/common/autoware_trajectory/include/autoware/trajectory/trajectory_point.hpp
index e507c933b8..c62afbb433 100644
--- a/common/autoware_trajectory/include/autoware/trajectory/trajectory_point.hpp
+++ b/common/autoware_trajectory/include/autoware/trajectory/trajectory_point.hpp
@@ -54,7 +54,7 @@ class Trajectory<autoware_planning_msgs::msg::TrajectoryPoint>
   Trajectory & operator=(const Trajectory & rhs);
   Trajectory & operator=(Trajectory && rhs) = default;
 
-  [[nodiscard]] std::vector<double> get_internal_bases() const override;
+  std::vector<double> get_internal_bases() const override;
 
   detail::InterpolatedArray<double> & longitudinal_velocity_mps()
   {
@@ -71,32 +71,29 @@ class Trajectory<autoware_planning_msgs::msg::TrajectoryPoint>
 
   detail::InterpolatedArray<double> & rear_wheel_angle_rad() { return *rear_wheel_angle_rad_; }
 
-  [[nodiscard]] const detail::InterpolatedArray<double> & longitudinal_velocity_mps() const
+  const detail::InterpolatedArray<double> & longitudinal_velocity_mps() const
   {
     return *longitudinal_velocity_mps_;
   }
 
-  [[nodiscard]] const detail::InterpolatedArray<double> & lateral_velocity_mps() const
+  const detail::InterpolatedArray<double> & lateral_velocity_mps() const
   {
     return *lateral_velocity_mps_;
   }
 
-  [[nodiscard]] const detail::InterpolatedArray<double> & heading_rate_rps() const
-  {
-    return *heading_rate_rps_;
-  }
+  const detail::InterpolatedArray<double> & heading_rate_rps() const { return *heading_rate_rps_; }
 
-  [[nodiscard]] const detail::InterpolatedArray<double> & acceleration_mps2() const
+  const detail::InterpolatedArray<double> & acceleration_mps2() const
   {
     return *acceleration_mps2_;
   }
 
-  [[nodiscard]] const detail::InterpolatedArray<double> & front_wheel_angle_rad() const
+  const detail::InterpolatedArray<double> & front_wheel_angle_rad() const
   {
     return *front_wheel_angle_rad_;
   }
 
-  [[nodiscard]] const detail::InterpolatedArray<double> & rear_wheel_angle_rad() const
+  const detail::InterpolatedArray<double> & rear_wheel_angle_rad() const
   {
     return *rear_wheel_angle_rad_;
   }
@@ -113,14 +110,14 @@ class Trajectory<autoware_planning_msgs::msg::TrajectoryPoint>
    * @param s Arc length
    * @return Point on the trajectory
    */
-  [[nodiscard]] PointType compute(double s) const;
+  PointType compute(const double s) const;
 
   /**
    * @brief Restore the trajectory points
    * @param min_points Minimum number of points
    * @return Vector of points
    */
-  [[nodiscard]] std::vector<PointType> restore(const size_t & min_points = 4) const;
+  std::vector<PointType> restore(const size_t min_points = 4) const;
 
   class Builder
   {
diff --git a/common/autoware_trajectory/src/interpolator/akima_spline.cpp b/common/autoware_trajectory/src/interpolator/akima_spline.cpp
index 766d01ca85..0289a7681f 100644
--- a/common/autoware_trajectory/src/interpolator/akima_spline.cpp
+++ b/common/autoware_trajectory/src/interpolator/akima_spline.cpp
@@ -60,29 +60,40 @@ void AkimaSpline::compute_parameters(
   }
 }
 
-void AkimaSpline::build_impl(const std::vector<double> & bases, const std::vector<double> & values)
+bool AkimaSpline::build_impl(const std::vector<double> & bases, const std::vector<double> & values)
 {
   this->bases_ = bases;
   compute_parameters(
     Eigen::Map<const Eigen::VectorXd>(bases.data(), static_cast<Eigen::Index>(bases.size())),
     Eigen::Map<const Eigen::VectorXd>(values.data(), static_cast<Eigen::Index>(values.size())));
+  return true;
 }
 
-double AkimaSpline::compute_impl(const double & s) const
+bool AkimaSpline::build_impl(std::vector<double> && bases, std::vector<double> && values)
+{
+  this->bases_ = std::move(bases);
+  compute_parameters(
+    Eigen::Map<const Eigen::VectorXd>(
+      this->bases_.data(), static_cast<Eigen::Index>(this->bases_.size())),
+    Eigen::Map<const Eigen::VectorXd>(values.data(), static_cast<Eigen::Index>(values.size())));
+  return true;
+}
+
+double AkimaSpline::compute_impl(const double s) const
 {
   const int32_t i = this->get_index(s);
   const double dx = s - this->bases_[i];
   return a_[i] + b_[i] * dx + c_[i] * dx * dx + d_[i] * dx * dx * dx;
 }
 
-double AkimaSpline::compute_first_derivative_impl(const double & s) const
+double AkimaSpline::compute_first_derivative_impl(const double s) const
 {
   const int32_t i = this->get_index(s);
   const double dx = s - this->bases_[i];
   return b_[i] + 2 * c_[i] * dx + 3 * d_[i] * dx * dx;
 }
 
-double AkimaSpline::compute_second_derivative_impl(const double & s) const
+double AkimaSpline::compute_second_derivative_impl(const double s) const
 {
   const int32_t i = this->get_index(s);
   const double dx = s - this->bases_[i];
diff --git a/common/autoware_trajectory/src/interpolator/cubic_spline.cpp b/common/autoware_trajectory/src/interpolator/cubic_spline.cpp
index 378233e799..c45d65e634 100644
--- a/common/autoware_trajectory/src/interpolator/cubic_spline.cpp
+++ b/common/autoware_trajectory/src/interpolator/cubic_spline.cpp
@@ -61,29 +61,40 @@ void CubicSpline::compute_parameters(
   }
 }
 
-void CubicSpline::build_impl(const std::vector<double> & bases, const std::vector<double> & values)
+bool CubicSpline::build_impl(const std::vector<double> & bases, const std::vector<double> & values)
 {
   this->bases_ = bases;
   compute_parameters(
     Eigen::Map<const Eigen::VectorXd>(bases.data(), static_cast<Eigen::Index>(bases.size())),
     Eigen::Map<const Eigen::VectorXd>(values.data(), static_cast<Eigen::Index>(values.size())));
+  return true;
 }
 
-double CubicSpline::compute_impl(const double & s) const
+bool CubicSpline::build_impl(std::vector<double> && bases, std::vector<double> && values)
+{
+  this->bases_ = std::move(bases);
+  compute_parameters(
+    Eigen::Map<const Eigen::VectorXd>(
+      this->bases_.data(), static_cast<Eigen::Index>(this->bases_.size())),
+    Eigen::Map<const Eigen::VectorXd>(values.data(), static_cast<Eigen::Index>(values.size())));
+  return true;
+}
+
+double CubicSpline::compute_impl(const double s) const
 {
   const int32_t i = this->get_index(s);
   const double dx = s - this->bases_.at(i);
   return a_(i) + b_(i) * dx + c_(i) * dx * dx + d_(i) * dx * dx * dx;
 }
 
-double CubicSpline::compute_first_derivative_impl(const double & s) const
+double CubicSpline::compute_first_derivative_impl(const double s) const
 {
   const int32_t i = this->get_index(s);
   const double dx = s - this->bases_.at(i);
   return b_(i) + 2 * c_(i) * dx + 3 * d_(i) * dx * dx;
 }
 
-double CubicSpline::compute_second_derivative_impl(const double & s) const
+double CubicSpline::compute_second_derivative_impl(const double s) const
 {
   const int32_t i = this->get_index(s);
   const double dx = s - this->bases_.at(i);
diff --git a/common/autoware_trajectory/src/interpolator/linear.cpp b/common/autoware_trajectory/src/interpolator/linear.cpp
index 7d75b75905..a0116c6419 100644
--- a/common/autoware_trajectory/src/interpolator/linear.cpp
+++ b/common/autoware_trajectory/src/interpolator/linear.cpp
@@ -21,14 +21,23 @@
 namespace autoware::trajectory::interpolator
 {
 
-void Linear::build_impl(const std::vector<double> & bases, const std::vector<double> & values)
+bool Linear::build_impl(const std::vector<double> & bases, const std::vector<double> & values)
 {
   this->bases_ = bases;
   this->values_ =
     Eigen::Map<const Eigen::VectorXd>(values.data(), static_cast<Eigen::Index>(values.size()));
+  return true;
 }
 
-double Linear::compute_impl(const double & s) const
+bool Linear::build_impl(std::vector<double> && bases, std::vector<double> && values)
+{
+  this->bases_ = std::move(bases);
+  this->values_ =
+    Eigen::Map<const Eigen::VectorXd>(values.data(), static_cast<Eigen::Index>(values.size()));
+  return true;
+}
+
+double Linear::compute_impl(const double s) const
 {
   const int32_t idx = this->get_index(s);
   const double x0 = this->bases_.at(idx);
@@ -38,7 +47,7 @@ double Linear::compute_impl(const double & s) const
   return y0 + (y1 - y0) * (s - x0) / (x1 - x0);
 }
 
-double Linear::compute_first_derivative_impl(const double & s) const
+double Linear::compute_first_derivative_impl(const double s) const
 {
   const int32_t idx = this->get_index(s);
   const double x0 = this->bases_.at(idx);
@@ -48,7 +57,7 @@ double Linear::compute_first_derivative_impl(const double & s) const
   return (y1 - y0) / (x1 - x0);
 }
 
-double Linear::compute_second_derivative_impl(const double &) const
+double Linear::compute_second_derivative_impl(const double) const
 {
   return 0.0;
 }
diff --git a/common/autoware_trajectory/src/interpolator/spherical_linear.cpp b/common/autoware_trajectory/src/interpolator/spherical_linear.cpp
index f255135ae8..3ccf75685f 100644
--- a/common/autoware_trajectory/src/interpolator/spherical_linear.cpp
+++ b/common/autoware_trajectory/src/interpolator/spherical_linear.cpp
@@ -21,15 +21,24 @@
 namespace autoware::trajectory::interpolator
 {
 
-void SphericalLinear::build_impl(
+bool SphericalLinear::build_impl(
   const std::vector<double> & bases,
   const std::vector<geometry_msgs::msg::Quaternion> & quaternions)
 {
   this->bases_ = bases;
   this->quaternions_ = quaternions;
+  return true;
 }
 
-geometry_msgs::msg::Quaternion SphericalLinear::compute_impl(const double & s) const
+bool SphericalLinear::build_impl(
+  std::vector<double> && bases, std::vector<geometry_msgs::msg::Quaternion> && quaternions)
+{
+  this->bases_ = std::move(bases);
+  this->quaternions_ = std::move(quaternions);
+  return true;
+}
+
+geometry_msgs::msg::Quaternion SphericalLinear::compute_impl(const double s) const
 {
   const int32_t idx = this->get_index(s);
   const double x0 = this->bases_.at(idx);
diff --git a/common/autoware_trajectory/src/path_point.cpp b/common/autoware_trajectory/src/path_point.cpp
index d264d1d054..b45c8d7109 100644
--- a/common/autoware_trajectory/src/path_point.cpp
+++ b/common/autoware_trajectory/src/path_point.cpp
@@ -102,19 +102,19 @@ std::vector<double> Trajectory<PointType>::get_internal_bases() const
   return bases;
 }
 
-PointType Trajectory<PointType>::compute(double s) const
+PointType Trajectory<PointType>::compute(const double s) const
 {
   PointType result;
   result.pose = Trajectory<geometry_msgs::msg::Pose>::compute(s);
-  s = clamp(s);
+  const auto s_clamp = clamp(s);
   result.longitudinal_velocity_mps =
-    static_cast<float>(this->longitudinal_velocity_mps().compute(s));
-  result.lateral_velocity_mps = static_cast<float>(this->lateral_velocity_mps().compute(s));
-  result.heading_rate_rps = static_cast<float>(this->heading_rate_rps().compute(s));
+    static_cast<float>(this->longitudinal_velocity_mps().compute(s_clamp));
+  result.lateral_velocity_mps = static_cast<float>(this->lateral_velocity_mps().compute(s_clamp));
+  result.heading_rate_rps = static_cast<float>(this->heading_rate_rps().compute(s_clamp));
   return result;
 }
 
-std::vector<PointType> Trajectory<PointType>::restore(const size_t & min_points) const
+std::vector<PointType> Trajectory<PointType>::restore(const size_t min_points) const
 {
   std::vector<double> bases = get_internal_bases();
   bases = detail::fill_bases(bases, min_points);
diff --git a/common/autoware_trajectory/src/path_point_with_lane_id.cpp b/common/autoware_trajectory/src/path_point_with_lane_id.cpp
index 96d3cfd91a..27e4f8949c 100644
--- a/common/autoware_trajectory/src/path_point_with_lane_id.cpp
+++ b/common/autoware_trajectory/src/path_point_with_lane_id.cpp
@@ -73,16 +73,16 @@ std::vector<double> Trajectory<PointType>::get_internal_bases() const
   return bases;
 }
 
-PointType Trajectory<PointType>::compute(double s) const
+PointType Trajectory<PointType>::compute(const double s) const
 {
   PointType result;
   result.point = BaseClass::compute(s);
-  s = clamp(s);
-  result.lane_ids = lane_ids().compute(s);
+  const auto s_clamp = clamp(s);
+  result.lane_ids = lane_ids().compute(s_clamp);
   return result;
 }
 
-std::vector<PointType> Trajectory<PointType>::restore(const size_t & min_points) const
+std::vector<PointType> Trajectory<PointType>::restore(const size_t min_points) const
 {
   auto bases = get_internal_bases();
   bases = detail::fill_bases(bases, min_points);
diff --git a/common/autoware_trajectory/src/point.cpp b/common/autoware_trajectory/src/point.cpp
index 405bc7e7a2..626e142dbf 100644
--- a/common/autoware_trajectory/src/point.cpp
+++ b/common/autoware_trajectory/src/point.cpp
@@ -94,7 +94,7 @@ bool Trajectory<PointType>::build(const std::vector<PointType> & points)
   return is_valid;
 }
 
-double Trajectory<PointType>::clamp(const double & s, bool show_warning) const
+double Trajectory<PointType>::clamp(const double s, bool show_warning) const
 {
   if ((s < 0 || s > length()) && show_warning) {
     RCLCPP_WARN(
@@ -108,7 +108,7 @@ std::vector<double> Trajectory<PointType>::get_internal_bases() const
 {
   auto bases = detail::crop_bases(bases_, start_, end_);
   std::transform(
-    bases.begin(), bases.end(), bases.begin(), [this](const double & s) { return s - start_; });
+    bases.begin(), bases.end(), bases.begin(), [this](const double s) { return s - start_; });
   return bases;
 }
 
@@ -117,42 +117,42 @@ double Trajectory<PointType>::length() const
   return end_ - start_;
 }
 
-PointType Trajectory<PointType>::compute(double s) const
+PointType Trajectory<PointType>::compute(const double s) const
 {
-  s = clamp(s, true);
+  const auto s_clamp = clamp(s, true);
   PointType result;
-  result.x = x_interpolator_->compute(s);
-  result.y = y_interpolator_->compute(s);
-  result.z = z_interpolator_->compute(s);
+  result.x = x_interpolator_->compute(s_clamp);
+  result.y = y_interpolator_->compute(s_clamp);
+  result.z = z_interpolator_->compute(s_clamp);
   return result;
 }
 
-double Trajectory<PointType>::azimuth(double s) const
+double Trajectory<PointType>::azimuth(const double s) const
 {
-  s = clamp(s, true);
-  const double dx = x_interpolator_->compute_first_derivative(s);
-  const double dy = y_interpolator_->compute_first_derivative(s);
+  const auto s_clamp = clamp(s, true);
+  const double dx = x_interpolator_->compute_first_derivative(s_clamp);
+  const double dy = y_interpolator_->compute_first_derivative(s_clamp);
   return std::atan2(dy, dx);
 }
 
-double Trajectory<PointType>::elevation(double s) const
+double Trajectory<PointType>::elevation(const double s) const
 {
-  s = clamp(s, true);
-  const double dz = z_interpolator_->compute_first_derivative(s);
+  const auto s_clamp = clamp(s, true);
+  const double dz = z_interpolator_->compute_first_derivative(s_clamp);
   return std::atan2(dz, 1.0);
 }
 
-double Trajectory<PointType>::curvature(double s) const
+double Trajectory<PointType>::curvature(const double s) const
 {
-  s = clamp(s, true);
-  const double dx = x_interpolator_->compute_first_derivative(s);
-  const double ddx = x_interpolator_->compute_second_derivative(s);
-  const double dy = y_interpolator_->compute_first_derivative(s);
-  const double ddy = y_interpolator_->compute_second_derivative(s);
+  const auto s_clamp = clamp(s, true);
+  const double dx = x_interpolator_->compute_first_derivative(s_clamp);
+  const double ddx = x_interpolator_->compute_second_derivative(s_clamp);
+  const double dy = y_interpolator_->compute_first_derivative(s_clamp);
+  const double ddy = y_interpolator_->compute_second_derivative(s_clamp);
   return std::abs(dx * ddy - dy * ddx) / std::pow(dx * dx + dy * dy, 1.5);
 }
 
-std::vector<PointType> Trajectory<PointType>::restore(const size_t & min_points) const
+std::vector<PointType> Trajectory<PointType>::restore(const size_t min_points) const
 {
   auto bases = get_internal_bases();
   bases = detail::fill_bases(bases, min_points);
@@ -164,7 +164,7 @@ std::vector<PointType> Trajectory<PointType>::restore(const size_t & min_points)
   return points;
 }
 
-void Trajectory<PointType>::crop(const double & start, const double & length)
+void Trajectory<PointType>::crop(const double start, const double length)
 {
   start_ = std::clamp(start_ + start, start_, end_);
   end_ = std::clamp(start_ + length, start_, end_);
diff --git a/common/autoware_trajectory/src/pose.cpp b/common/autoware_trajectory/src/pose.cpp
index 56f6975bc2..d89862c464 100644
--- a/common/autoware_trajectory/src/pose.cpp
+++ b/common/autoware_trajectory/src/pose.cpp
@@ -93,16 +93,16 @@ std::vector<double> Trajectory<PointType>::get_internal_bases() const
 {
   auto bases = detail::crop_bases(bases_, start_, end_);
   std::transform(
-    bases.begin(), bases.end(), bases.begin(), [this](const double & s) { return s - start_; });
+    bases.begin(), bases.end(), bases.begin(), [this](const double s) { return s - start_; });
   return bases;
 }
 
-PointType Trajectory<PointType>::compute(double s) const
+PointType Trajectory<PointType>::compute(const double s) const
 {
   PointType result;
   result.position = BaseClass::compute(s);
-  s = clamp(s);
-  result.orientation = orientation_interpolator_->compute(s);
+  const auto s_clamp = clamp(s);
+  result.orientation = orientation_interpolator_->compute(s_clamp);
   return result;
 }
 
@@ -164,7 +164,7 @@ void Trajectory<PointType>::align_orientation_with_trajectory_direction()
   }
 }
 
-std::vector<PointType> Trajectory<PointType>::restore(const size_t & min_points) const
+std::vector<PointType> Trajectory<PointType>::restore(const size_t min_points) const
 {
   auto bases = get_internal_bases();
   bases = detail::fill_bases(bases, min_points);
diff --git a/common/autoware_trajectory/src/trajectory_point.cpp b/common/autoware_trajectory/src/trajectory_point.cpp
index 6cb49735db..87658624c0 100644
--- a/common/autoware_trajectory/src/trajectory_point.cpp
+++ b/common/autoware_trajectory/src/trajectory_point.cpp
@@ -126,22 +126,22 @@ std::vector<double> Trajectory<PointType>::get_internal_bases() const
   return bases;
 }
 
-PointType Trajectory<PointType>::compute(double s) const
+PointType Trajectory<PointType>::compute(const double s) const
 {
   PointType result;
   result.pose = Trajectory<geometry_msgs::msg::Pose>::compute(s);
-  s = clamp(s);
+  const auto s_clamp = clamp(s);
   result.longitudinal_velocity_mps =
-    static_cast<float>(this->longitudinal_velocity_mps().compute(s));
-  result.lateral_velocity_mps = static_cast<float>(this->lateral_velocity_mps().compute(s));
-  result.heading_rate_rps = static_cast<float>(this->heading_rate_rps().compute(s));
-  result.acceleration_mps2 = static_cast<float>(this->acceleration_mps2().compute(s));
-  result.front_wheel_angle_rad = static_cast<float>(this->front_wheel_angle_rad().compute(s));
-  result.rear_wheel_angle_rad = static_cast<float>(this->rear_wheel_angle_rad().compute(s));
+    static_cast<float>(this->longitudinal_velocity_mps().compute(s_clamp));
+  result.lateral_velocity_mps = static_cast<float>(this->lateral_velocity_mps().compute(s_clamp));
+  result.heading_rate_rps = static_cast<float>(this->heading_rate_rps().compute(s_clamp));
+  result.acceleration_mps2 = static_cast<float>(this->acceleration_mps2().compute(s_clamp));
+  result.front_wheel_angle_rad = static_cast<float>(this->front_wheel_angle_rad().compute(s_clamp));
+  result.rear_wheel_angle_rad = static_cast<float>(this->rear_wheel_angle_rad().compute(s_clamp));
   return result;
 }
 
-std::vector<PointType> Trajectory<PointType>::restore(const size_t & min_points) const
+std::vector<PointType> Trajectory<PointType>::restore(const size_t min_points) const
 {
   std::vector<double> bases = get_internal_bases();
   bases = detail::fill_bases(bases, min_points);