refactor(autoware_trajectory): refactor autoware_trajectory (#9356)

Signed-off-by: Y.Hisaki <yhisaki31@gmail.com>

common/autoware_trajectory/include/autoware/trajectory/detail/interpolated_array.hpp

@@ -194,13 +194,16 @@ class InterpolatedArray
    * @param x The position to compute the value at.
    * @return The interpolated value.
    */
-  T compute(const double & x) const { return interpolator_->compute(x); }
+  [[nodiscard]] 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.
    */
-  std::pair<std::vector<double>, std::vector<T>> get_data() const { return {bases_, values_}; }
+  [[nodiscard]] std::pair<std::vector<double>, std::vector<T>> get_data() const
+  {
+    return {bases_, values_};
+  }
 };
 
 }  // namespace autoware::trajectory::detail

common/autoware_trajectory/include/autoware/trajectory/interpolator/detail/interpolator_common_interface.hpp

@@ -12,10 +12,8 @@
 // See the License for the specific language governing permissions and
 // limitations under the License.
 
-// clang-format off
-#ifndef AUTOWARE__TRAJECTORY__INTERPOLATOR__DETAIL__INTERPOLATOR_COMMON_INTERFACE_HPP_  // NOLINT
-#define AUTOWARE__TRAJECTORY__INTERPOLATOR__DETAIL__INTERPOLATOR_COMMON_INTERFACE_HPP_  // NOLINT
-// clang-format on
+#ifndef AUTOWARE__TRAJECTORY__INTERPOLATOR__DETAIL__INTERPOLATOR_COMMON_INTERFACE_HPP_
+#define AUTOWARE__TRAJECTORY__INTERPOLATOR__DETAIL__INTERPOLATOR_COMMON_INTERFACE_HPP_
 
 #include <Eigen/Dense>
 #include <rclcpp/logging.hpp>
@@ -74,17 +72,33 @@ class InterpolatorCommonInterface
    * 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.
+   * @return The input value, clamped to the range of the interpolator.
    */
-  void validate_compute_input(const double & s) const
+  [[nodiscard]] double 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());
     }
+    return std::clamp(s, start(), end());
   }
 
+  /**
+   * @brief Get the index of the interval containing the input value.
+   *
+   * This method determines the index of the interval in the bases array that contains the given
+   * value. It assumes that the bases array is sorted in ascending order.
+   *
+   * If `end_inclusive` is true and the input value matches the end of the bases array,
+   * the method returns the index of the second-to-last interval.
+   *
+   * @param s The input value for which to find the interval index.
+   * @param end_inclusive Whether to include the end value in the last interval. Defaults to true.
+   * @return The index of the interval containing the input value.
+   *
+   * @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
   {
     if (end_inclusive && s == end()) {
@@ -96,6 +110,13 @@ class InterpolatorCommonInterface
   }
 
 public:
+  InterpolatorCommonInterface() = default;
+  virtual ~InterpolatorCommonInterface() = default;
+  InterpolatorCommonInterface(const InterpolatorCommonInterface & other) = default;
+  InterpolatorCommonInterface & operator=(const InterpolatorCommonInterface & other) = default;
+  InterpolatorCommonInterface(InterpolatorCommonInterface && other) noexcept = default;
+  InterpolatorCommonInterface & operator=(InterpolatorCommonInterface && other) noexcept = default;
+
   /**
    * @brief Build the interpolator with the given bases and values.
    *
@@ -132,12 +153,10 @@ class InterpolatorCommonInterface
    */
   [[nodiscard]] T compute(const double & s) const
   {
-    validate_compute_input(s);
-    return compute_impl(s);
+    double clamped_s = validate_compute_input(s);
+    return compute_impl(clamped_s);
   }
 };
 }  // namespace autoware::trajectory::interpolator::detail
 
-// clang-format off
-#endif  // AUTOWARE__TRAJECTORY__INTERPOLATOR__DETAIL__INTERPOLATOR_COMMON_INTERFACE_HPP_  // NOLINT
-// clang-format on
+#endif  // AUTOWARE__TRAJECTORY__INTERPOLATOR__DETAIL__INTERPOLATOR_COMMON_INTERFACE_HPP_

common/autoware_trajectory/include/autoware/trajectory/interpolator/detail/interpolator_mixin.hpp

@@ -12,10 +12,8 @@
 // See the License for the specific language governing permissions and
 // limitations under the License.
 
-// clang-format off
-#ifndef AUTOWARE__TRAJECTORY__INTERPOLATOR__DETAIL__INTERPOLATOR_MIXIN_HPP_  // NOLINT
-#define AUTOWARE__TRAJECTORY__INTERPOLATOR__DETAIL__INTERPOLATOR_MIXIN_HPP_  // NOLINT
-// clang-format on
+#ifndef AUTOWARE__TRAJECTORY__INTERPOLATOR__DETAIL__INTERPOLATOR_MIXIN_HPP_
+#define AUTOWARE__TRAJECTORY__INTERPOLATOR__DETAIL__INTERPOLATOR_MIXIN_HPP_
 
 #include "autoware/trajectory/interpolator/interpolator.hpp"
 
@@ -40,7 +38,7 @@ namespace autoware::trajectory::interpolator::detail
 template <class InterpolatorType, class T>
 struct InterpolatorMixin : public InterpolatorInterface<T>
 {
-  std::shared_ptr<InterpolatorInterface<T>> clone() const override
+  [[nodiscard]] std::shared_ptr<InterpolatorInterface<T>> clone() const override
   {
     return std::make_shared<InterpolatorType>(static_cast<const InterpolatorType &>(*this));
   }
@@ -85,6 +83,4 @@ struct InterpolatorMixin : public InterpolatorInterface<T>
 
 }  // namespace autoware::trajectory::interpolator::detail
 
-// clang-format off
-#endif  // AUTOWARE__TRAJECTORY__INTERPOLATOR__DETAIL__INTERPOLATOR_MIXIN_HPP_  // NOLINT
-// clang-format on
+#endif  // AUTOWARE__TRAJECTORY__INTERPOLATOR__DETAIL__INTERPOLATOR_MIXIN_HPP_

common/autoware_trajectory/include/autoware/trajectory/interpolator/detail/nearest_neighbor_common_impl.hpp

@@ -12,10 +12,8 @@
 // See the License for the specific language governing permissions and
 // limitations under the License.
 
-// clang-format off
-#ifndef AUTOWARE__TRAJECTORY__INTERPOLATOR__DETAIL__NEAREST_NEIGHBOR_COMMON_IMPL_HPP_  // NOLINT
-#define AUTOWARE__TRAJECTORY__INTERPOLATOR__DETAIL__NEAREST_NEIGHBOR_COMMON_IMPL_HPP_  // NOLINT
-// clang-format on
+#ifndef AUTOWARE__TRAJECTORY__INTERPOLATOR__DETAIL__NEAREST_NEIGHBOR_COMMON_IMPL_HPP_
+#define AUTOWARE__TRAJECTORY__INTERPOLATOR__DETAIL__NEAREST_NEIGHBOR_COMMON_IMPL_HPP_
 
 #include "autoware/trajectory/interpolator/detail/interpolator_mixin.hpp"
 
@@ -80,6 +78,5 @@ class NearestNeighborCommonImpl : public detail::InterpolatorMixin<NearestNeighb
 
 }  // namespace detail
 }  // namespace autoware::trajectory::interpolator
-// clang-format off
-#endif  // AUTOWARE__TRAJECTORY__INTERPOLATOR__DETAIL__NEAREST_NEIGHBOR_COMMON_IMPL_HPP_  // NOLINT
-// clang-format on
+
+#endif  // AUTOWARE__TRAJECTORY__INTERPOLATOR__DETAIL__NEAREST_NEIGHBOR_COMMON_IMPL_HPP_

common/autoware_trajectory/include/autoware/trajectory/interpolator/detail/stairstep_common_impl.hpp

@@ -12,10 +12,8 @@
 // See the License for the specific language governing permissions and
 // limitations under the License.
 
-// clang-format off
-#ifndef AUTOWARE__TRAJECTORY__INTERPOLATOR__DETAIL__STAIRSTEP_COMMON_IMPL_HPP_  // NOLINT
-#define AUTOWARE__TRAJECTORY__INTERPOLATOR__DETAIL__STAIRSTEP_COMMON_IMPL_HPP_  // NOLINT
-// clang-format on
+#ifndef AUTOWARE__TRAJECTORY__INTERPOLATOR__DETAIL__STAIRSTEP_COMMON_IMPL_HPP_
+#define AUTOWARE__TRAJECTORY__INTERPOLATOR__DETAIL__STAIRSTEP_COMMON_IMPL_HPP_
 
 #include "autoware/trajectory/interpolator/detail/interpolator_mixin.hpp"
 
@@ -80,6 +78,4 @@ class StairstepCommonImpl : public detail::InterpolatorMixin<Stairstep<T>, T>
 }  // namespace detail
 }  // namespace autoware::trajectory::interpolator
 
-// clang-format off
-#endif  // AUTOWARE__TRAJECTORY__INTERPOLATOR__DETAIL__STAIRSTEP_COMMON_IMPL_HPP_  // NOLINT
-// clang-format on
+#endif  // AUTOWARE__TRAJECTORY__INTERPOLATOR__DETAIL__STAIRSTEP_COMMON_IMPL_HPP_

common/autoware_trajectory/include/autoware/trajectory/interpolator/interpolator.hpp

@@ -73,8 +73,8 @@ class InterpolatorInterface<double> : public detail::InterpolatorCommonInterface
    */
   [[nodiscard]] double compute_first_derivative(const double & s) const
   {
-    this->validate_compute_input(s);
-    return compute_first_derivative_impl(s);
+    double clamped_s = this->validate_compute_input(s);
+    return compute_first_derivative_impl(clamped_s);
   }
 
   /**
@@ -85,8 +85,8 @@ class InterpolatorInterface<double> : public detail::InterpolatorCommonInterface
    */
   [[nodiscard]] double compute_second_derivative(const double & s) const
   {
-    this->validate_compute_input(s);
-    return compute_second_derivative_impl(s);
+    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;

common/autoware_trajectory/include/autoware/trajectory/path_point_with_lane_id.hpp

@@ -12,10 +12,8 @@
 // See the License for the specific language governing permissions and
 // limitations under the License.
 
-// clang-format off
-#ifndef AUTOWARE__TRAJECTORY__PATH_POINT_WITH_LANE_ID_HPP_  // NOLINT
-#define AUTOWARE__TRAJECTORY__PATH_POINT_WITH_LANE_ID_HPP_  // NOLINT
-// clang-format on
+#ifndef AUTOWARE__TRAJECTORY__PATH_POINT_WITH_LANE_ID_HPP_
+#define AUTOWARE__TRAJECTORY__PATH_POINT_WITH_LANE_ID_HPP_
 
 #include "autoware/trajectory/path_point.hpp"
 #include "autoware/trajectory/point.hpp"
@@ -149,6 +147,4 @@ class Trajectory<tier4_planning_msgs::msg::PathPointWithLaneId>
 };
 }  // namespace autoware::trajectory
 
-// clang-format off
-#endif  // AUTOWARE__TRAJECTORY__PATH_POINT_WITH_LANE_ID_HPP_  // NOLINT
-// clang-format on
+#endif  // AUTOWARE__TRAJECTORY__PATH_POINT_WITH_LANE_ID_HPP_

common/autoware_trajectory/src/path_point_with_lane_id.cpp

@@ -16,6 +16,8 @@
 
 #include "autoware/trajectory/detail/utils.hpp"
 
+#include <vector>
+
 namespace autoware::trajectory
 {