feat(autoware_trajectory): use move semantics and return expected<T, E> for propagating failure reason (#254)

Signed-off-by: Mamoru Sobue <mamoru.sobue@tier4.jp>
Co-authored-by: Yukinari Hisaki <42021302+yhisaki@users.noreply.github.com>

Author: soblin

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

@@ -64,16 +64,18 @@ class InterpolatedArray
 
   InterpolatedArray(InterpolatedArray && other) = default;
 
-  bool build(const std::vector<double> & bases, const std::vector<T> & values)
+  interpolator::InterpolationResult build(
+    const std::vector<double> & bases, const std::vector<T> & values)
   {
     bases_ = bases;
     values_ = values;
     return interpolator_->build(bases_, values_);
   }
 
-  bool build(std::vector<double> && bases, std::vector<T> && values)
+  interpolator::InterpolationResult build(
+    const std::vector<double> & bases, std::vector<T> && values)
   {
-    bases_ = std::move(bases);
+    bases_ = bases;
     values_ = std::move(values);
     return interpolator_->build(bases_, values_);
   }
@@ -160,7 +162,7 @@ class InterpolatedArray
       // Set the values in the specified range
       std::fill(values.begin() + start_index, values.begin() + end_index + 1, value);
 
-      bool success = parent_.interpolator_->build(bases, values);
+      const auto success = parent_.interpolator_->build(bases, values);
       if (!success) {
         throw std::runtime_error(
           "Failed to build interpolator.");  // This Exception should not be thrown.
@@ -194,7 +196,7 @@ class InterpolatedArray
   InterpolatedArray & operator=(const T & value)
   {
     std::fill(values_.begin(), values_.end(), value);
-    bool success = interpolator_->build(bases_, values_);
+    const auto success = interpolator_->build(bases_, values_);
     if (!success) {
       throw std::runtime_error(
         "Failed to build interpolator.");  // This Exception should not be thrown.

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

@@ -62,7 +62,7 @@ class AkimaSpline : public detail::InterpolatorMixin<AkimaSpline, double>
    * @param values The values to interpolate.
    */
   [[nodiscard]] bool build_impl(
-    std::vector<double> && bases, std::vector<double> && values) override;
+    const std::vector<double> & bases, std::vector<double> && values) override;
 
   /**
    * @brief Compute the interpolated value at the given point.

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

@@ -65,7 +65,7 @@ class CubicSpline : public detail::InterpolatorMixin<CubicSpline, double>
    * @return True if the interpolator was built successfully, false otherwise.
    */
   [[nodiscard]] bool build_impl(
-    std::vector<double> && bases, std::vector<double> && values) override;
+    const std::vector<double> & bases, std::vector<double> && values) override;
 
   /**
    * @brief Compute the interpolated value at the given point.

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

@@ -15,7 +15,8 @@
 #ifndef AUTOWARE__TRAJECTORY__INTERPOLATOR__DETAIL__INTERPOLATOR_COMMON_INTERFACE_HPP_
 #define AUTOWARE__TRAJECTORY__INTERPOLATOR__DETAIL__INTERPOLATOR_COMMON_INTERFACE_HPP_
 
-#include <Eigen/Dense>
+#include "autoware/trajectory/interpolator/result.hpp"
+
 #include <rclcpp/logging.hpp>
 
 #include <utility>
@@ -76,7 +77,8 @@ class InterpolatorCommonInterface
    * @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;
+  [[nodiscard]] virtual bool build_impl(
+    const std::vector<double> & bases, std::vector<T> && values) = 0;
 
   /**
    * @brief Validate the input to the compute method.
@@ -141,15 +143,23 @@ class InterpolatorCommonInterface
     std::conjunction_v<
       std::is_same<std::decay_t<BaseVectorT>, std::vector<double>>,
       std::is_same<std::decay_t<ValueVectorT>, std::vector<T>>>,
-    bool>
+    InterpolationResult>
   {
     if (bases.size() != values.size()) {
-      return false;
+      return tl::unexpected(InterpolationFailure{
+        "base size " + std::to_string(bases.size()) + " and value size " +
+        std::to_string(values.size()) + " are different"});
+    }
+    if (const auto minimum_required = minimum_required_points(); bases.size() < minimum_required) {
+      return tl::unexpected(InterpolationFailure{
+        "base size " + std::to_string(bases.size()) + " is less than minimum required " +
+        std::to_string(minimum_required)});
     }
-    if (bases.size() < minimum_required_points()) {
-      return false;
+    if (!build_impl(std::forward<BaseVectorT>(bases), std::forward<ValueVectorT>(values))) {
+      return tl::unexpected(
+        InterpolationFailure{"failed to interpolate from given base and values"});
     }
-    return build_impl(std::forward<BaseVectorT>(bases), std::forward<ValueVectorT>(values));
+    return InterpolationSuccess{};
   }
 
   /**

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

@@ -81,12 +81,14 @@ struct InterpolatorMixin : public InterpolatorInterface<T>
     }
 
     template <typename... Args>
-    [[nodiscard]] std::optional<InterpolatorType> build(Args &&... args)
+    [[nodiscard]] tl::expected<InterpolatorType, interpolator::InterpolationFailure> build(
+      Args &&... args)
     {
       auto interpolator = InterpolatorType(std::forward<Args>(args)...);
-      const bool success = interpolator.build(std::move(bases_), std::move(values_));
+      const interpolator::InterpolationResult success =
+        interpolator.build(std::move(bases_), std::move(values_));
       if (!success) {
-        return std::nullopt;
+        return tl::unexpected(success.error());
       }
       return interpolator;
     }

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

@@ -76,9 +76,10 @@ class NearestNeighborCommonImpl : public detail::InterpolatorMixin<NearestNeighb
    * @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
+  [[nodiscard]] bool build_impl(
+    const std::vector<double> & bases, std::vector<T> && values) override
   {
-    this->bases_ = std::move(bases);
+    this->bases_ = bases;
     this->values_ = std::move(values);
     return true;
   }

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

@@ -73,9 +73,10 @@ class StairstepCommonImpl : public detail::InterpolatorMixin<Stairstep<T>, T>
    * @param bases The bases values.
    * @param values The values to interpolate.
    */
-  [[nodiscard]] bool build_impl(std::vector<double> && bases, std::vector<T> && values) override
+  [[nodiscard]] bool build_impl(
+    const std::vector<double> & bases, std::vector<T> && values) override
   {
-    this->bases_ = std::move(bases);
+    this->bases_ = bases;
     this->values_ = std::move(values);
     return true;
   }

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

@@ -52,7 +52,7 @@ class Linear : public detail::InterpolatorMixin<Linear, double>
    * @return True if the interpolator was built successfully, false otherwise.
    */
   [[nodiscard]] bool build_impl(
-    std::vector<double> && bases, std::vector<double> && values) override;
+    const std::vector<double> & bases, std::vector<double> && values) override;
 
   /**
    * @brief Compute the interpolated value at the given point.

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

@@ -0,0 +1,39 @@
+// Copyright 2025 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__TRAJECTORY__INTERPOLATOR__RESULT_HPP_
+#define AUTOWARE__TRAJECTORY__INTERPOLATOR__RESULT_HPP_
+
+#include <tl_expected/expected.hpp>
+
+#include <string>
+
+namespace autoware::trajectory::interpolator
+{
+struct InterpolationSuccess
+{
+};
+
+struct InterpolationFailure
+{
+  const std::string what;
+};
+
+InterpolationFailure operator+(
+  const InterpolationFailure & primary, const InterpolationFailure & nested);
+
+using InterpolationResult = tl::expected<InterpolationSuccess, InterpolationFailure>;
+
+}  // namespace autoware::trajectory::interpolator
+#endif  // AUTOWARE__TRAJECTORY__INTERPOLATOR__RESULT_HPP_

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

@@ -54,7 +54,7 @@ class SphericalLinear
    * @return True if the interpolator was built successfully, false otherwise.
    */
   [[nodiscard]] bool build_impl(
-    std::vector<double> && bases,
+    const std::vector<double> & bases,
     std::vector<geometry_msgs::msg::Quaternion> && quaternions) override;
 
   /**

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

@@ -16,6 +16,7 @@
 #define AUTOWARE__TRAJECTORY__PATH_POINT_HPP_
 
 #include "autoware/trajectory/detail/interpolated_array.hpp"
+#include "autoware/trajectory/interpolator/result.hpp"
 #include "autoware/trajectory/pose.hpp"
 
 #include <autoware_planning_msgs/msg/path_point.hpp>
@@ -76,7 +77,7 @@ class Trajectory<autoware_planning_msgs::msg::PathPoint>
    * @param points Vector of points
    * @return True if the build is successful
    */
-  bool build(const std::vector<PointType> & points);
+  interpolator::InterpolationResult build(const std::vector<PointType> & points);
 
   /**
    * @brief Compute the point on the trajectory at a given s value
@@ -150,14 +151,16 @@ class Trajectory<autoware_planning_msgs::msg::PathPoint>
       return *this;
     }
 
-    std::optional<Trajectory> build(const std::vector<PointType> & points)
+    tl::expected<Trajectory, interpolator::InterpolationFailure> build(
+      const std::vector<PointType> & points)
     {
-      if (trajectory_->build(points)) {
-        auto result = std::make_optional<Trajectory>(std::move(*trajectory_));
+      auto trajectory_result = trajectory_->build(points);
+      if (trajectory_result) {
+        auto result = Trajectory(std::move(*trajectory_));
         trajectory_.reset();
         return result;
       }
-      return std::nullopt;
+      return tl::unexpected(trajectory_result.error());
     }
   };
 };

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

@@ -52,7 +52,7 @@ class Trajectory<autoware_internal_planning_msgs::msg::PathPointWithLaneId>
    * @param points Vector of points
    * @return True if the build is successful
    */
-  bool build(const std::vector<PointType> & points);
+  interpolator::InterpolationResult build(const std::vector<PointType> & points);
 
   std::vector<double> get_internal_bases() const override;
 
@@ -136,14 +136,16 @@ class Trajectory<autoware_internal_planning_msgs::msg::PathPointWithLaneId>
       return *this;
     }
 
-    std::optional<Trajectory> build(const std::vector<PointType> & points)
+    tl::expected<Trajectory, interpolator::InterpolationFailure> build(
+      const std::vector<PointType> & points)
     {
-      if (trajectory_->build(points)) {
-        auto result = std::make_optional<Trajectory>(std::move(*trajectory_));
+      auto trajectory_result = trajectory_->build(points);
+      if (trajectory_result) {
+        auto result = Trajectory(std::move(*trajectory_));
         trajectory_.reset();
         return result;
       }
-      return std::nullopt;
+      return tl::unexpected(trajectory_result.error());
     }
   };
 };

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

@@ -90,7 +90,7 @@ class Trajectory<geometry_msgs::msg::Point>
    * @param points Vector of points
    * @return True if the build is successful
    */
-  bool build(const std::vector<PointType> & points);
+  interpolator::InterpolationResult build(const std::vector<PointType> & points);
 
   /**
    * @brief Get the azimuth angle at a given s value
@@ -148,14 +148,16 @@ class Trajectory<geometry_msgs::msg::Point>
       return *this;
     }
 
-    std::optional<Trajectory> build(const std::vector<PointType> & points)
+    tl::expected<Trajectory, interpolator::InterpolationFailure> build(
+      const std::vector<PointType> & points)
     {
-      if (trajectory_->build(points)) {
-        auto result = std::make_optional<Trajectory>(std::move(*trajectory_));
+      auto trajectory_result = trajectory_->build(points);
+      if (trajectory_result) {
+        auto result = Trajectory(std::move(*trajectory_));
         trajectory_.reset();
         return result;
       }
-      return std::nullopt;
+      return tl::unexpected(trajectory_result.error());
     }
   };
 };

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

@@ -52,7 +52,7 @@ class Trajectory<geometry_msgs::msg::Pose> : public Trajectory<geometry_msgs::ms
   // enable making trajectory from point trajectory
   explicit Trajectory(const Trajectory<geometry_msgs::msg::Point> & point_trajectory);
 
-  bool build(const std::vector<PointType> & points);
+  interpolator::InterpolationResult build(const std::vector<PointType> & points);
 
   /**
    * @brief Get the internal bases(arc lengths) of the trajectory
@@ -112,14 +112,16 @@ class Trajectory<geometry_msgs::msg::Pose> : public Trajectory<geometry_msgs::ms
       return *this;
     }
 
-    std::optional<Trajectory> build(const std::vector<PointType> & points)
+    tl::expected<Trajectory, interpolator::InterpolationFailure> build(
+      const std::vector<PointType> & points)
     {
-      if (trajectory_->build(points)) {
-        auto result = std::make_optional<Trajectory>(std::move(*trajectory_));
+      auto trajectory_result = trajectory_->build(points);
+      if (trajectory_result) {
+        auto result = Trajectory(std::move(*trajectory_));
         trajectory_.reset();
         return result;
       }
-      return std::nullopt;
+      return tl::unexpected(trajectory_result.error());
     }
   };
 };

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

@@ -103,7 +103,7 @@ class Trajectory<autoware_planning_msgs::msg::TrajectoryPoint>
    * @param points Vector of points
    * @return True if the build is successful
    */
-  bool build(const std::vector<PointType> & points);
+  interpolator::InterpolationResult build(const std::vector<PointType> & points);
 
   /**
    * @brief Compute the point on the trajectory at a given s value
@@ -201,14 +201,16 @@ class Trajectory<autoware_planning_msgs::msg::TrajectoryPoint>
       return *this;
     }
 
-    std::optional<Trajectory> build(const std::vector<PointType> & points)
+    tl::expected<Trajectory, interpolator::InterpolationFailure> build(
+      const std::vector<PointType> & points)
     {
-      if (trajectory_->build(points)) {
-        auto result = std::make_optional<Trajectory>(std::move(*trajectory_));
+      auto trajectory_result = trajectory_->build(points);
+      if (trajectory_result) {
+        auto result = Trajectory(std::move(*trajectory_));
         trajectory_.reset();
         return result;
       }
-      return std::nullopt;
+      return tl::unexpected(trajectory_result.error());
     }
   };
 };

common/autoware_trajectory/package.xml

@@ -20,6 +20,7 @@
   <depend>lanelet2_core</depend>
   <depend>rclcpp</depend>
   <depend>tf2</depend>
+  <depend>tl_expected</depend>
 
   <test_depend>ament_cmake_ros</test_depend>
   <test_depend>ament_lint_auto</test_depend>

common/autoware_trajectory/src/interpolator/akima_spline.cpp

@@ -69,12 +69,11 @@ bool AkimaSpline::build_impl(const std::vector<double> & bases, const std::vecto
   return true;
 }
 
-bool AkimaSpline::build_impl(std::vector<double> && bases, std::vector<double> && values)
+bool AkimaSpline::build_impl(const std::vector<double> & bases, std::vector<double> && values)
 {
-  this->bases_ = std::move(bases);
+  this->bases_ = bases;
   compute_parameters(
-    Eigen::Map<const Eigen::VectorXd>(
-      this->bases_.data(), static_cast<Eigen::Index>(this->bases_.size())),
+    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;
 }