diff --git a/common/autoware_trajectory/include/autoware/trajectory/detail/types.hpp b/common/autoware_trajectory/include/autoware/trajectory/detail/types.hpp index dd64a7d3eb..833d0ff564 100644 --- a/common/autoware_trajectory/include/autoware/trajectory/detail/types.hpp +++ b/common/autoware_trajectory/include/autoware/trajectory/detail/types.hpp @@ -17,8 +17,6 @@ #include "lanelet2_core/primitives/Point.h" -#include - #include #include #include diff --git a/common/autoware_trajectory/include/autoware/trajectory/detail/interpolated_array.hpp b/common/autoware_trajectory/include/autoware/trajectory/interpolated_array.hpp similarity index 91% rename from common/autoware_trajectory/include/autoware/trajectory/detail/interpolated_array.hpp rename to common/autoware_trajectory/include/autoware/trajectory/interpolated_array.hpp index 02e01a4815..2920d7ef5b 100644 --- a/common/autoware_trajectory/include/autoware/trajectory/detail/interpolated_array.hpp +++ b/common/autoware_trajectory/include/autoware/trajectory/interpolated_array.hpp @@ -12,13 +12,12 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef AUTOWARE__TRAJECTORY__DETAIL__INTERPOLATED_ARRAY_HPP_ -#define AUTOWARE__TRAJECTORY__DETAIL__INTERPOLATED_ARRAY_HPP_ +#ifndef AUTOWARE__TRAJECTORY__INTERPOLATED_ARRAY_HPP_ +#define AUTOWARE__TRAJECTORY__INTERPOLATED_ARRAY_HPP_ #include "autoware/trajectory/detail/logging.hpp" #include "autoware/trajectory/interpolator/interpolator.hpp" -#include #include #include @@ -64,16 +63,18 @@ class InterpolatedArray InterpolatedArray(InterpolatedArray && other) = default; - bool build(const std::vector & bases, const std::vector & values) + interpolator::InterpolationResult build( + const std::vector & bases, const std::vector & values) { bases_ = bases; values_ = values; return interpolator_->build(bases_, values_); } - bool build(std::vector && bases, std::vector && values) + interpolator::InterpolationResult build( + const std::vector & bases, std::vector && values) { - bases_ = std::move(bases); + bases_ = bases; values_ = std::move(values); return interpolator_->build(bases_, values_); } @@ -160,7 +161,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 +195,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. @@ -218,4 +219,4 @@ class InterpolatedArray } // namespace autoware::trajectory::detail -#endif // AUTOWARE__TRAJECTORY__DETAIL__INTERPOLATED_ARRAY_HPP_ +#endif // AUTOWARE__TRAJECTORY__INTERPOLATED_ARRAY_HPP_ 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 1cd0a9f863..cc511d5d9c 100644 --- a/common/autoware_trajectory/include/autoware/trajectory/interpolator/akima_spline.hpp +++ b/common/autoware_trajectory/include/autoware/trajectory/interpolator/akima_spline.hpp @@ -62,7 +62,7 @@ class AkimaSpline : public detail::InterpolatorMixin * @param values The values to interpolate. */ [[nodiscard]] bool build_impl( - std::vector && bases, std::vector && values) override; + const std::vector & bases, std::vector && values) override; /** * @brief Compute the interpolated value at the given point. 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 8a17647261..42f18003fa 100644 --- a/common/autoware_trajectory/include/autoware/trajectory/interpolator/cubic_spline.hpp +++ b/common/autoware_trajectory/include/autoware/trajectory/interpolator/cubic_spline.hpp @@ -65,7 +65,7 @@ class CubicSpline : public detail::InterpolatorMixin * @return True if the interpolator was built successfully, false otherwise. */ [[nodiscard]] bool build_impl( - std::vector && bases, std::vector && values) override; + const std::vector & bases, std::vector && values) override; /** * @brief Compute the interpolated value at the given point. 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 4d1db74369..3b9990c8d8 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 @@ -15,7 +15,8 @@ #ifndef AUTOWARE__TRAJECTORY__INTERPOLATOR__DETAIL__INTERPOLATOR_COMMON_INTERFACE_HPP_ #define AUTOWARE__TRAJECTORY__INTERPOLATOR__DETAIL__INTERPOLATOR_COMMON_INTERFACE_HPP_ -#include +#include "autoware/trajectory/interpolator/result.hpp" + #include #include @@ -76,7 +77,8 @@ class InterpolatorCommonInterface * @param bases The bases values. * @param values The values to interpolate. */ - [[nodiscard]] virtual bool build_impl(std::vector && bases, std::vector && values) = 0; + [[nodiscard]] virtual bool build_impl( + const std::vector & bases, std::vector && values) = 0; /** * @brief Validate the input to the compute method. @@ -141,15 +143,23 @@ class InterpolatorCommonInterface std::conjunction_v< std::is_same, std::vector>, std::is_same, std::vector>>, - 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(bases), std::forward(values))) { + return tl::unexpected( + InterpolationFailure{"failed to interpolate from given base and values"}); } - return build_impl(std::forward(bases), std::forward(values)); + return InterpolationSuccess{}; } /** 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 ebc7bbe400..987ef6704e 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 @@ -81,12 +81,14 @@ struct InterpolatorMixin : public InterpolatorInterface } template - [[nodiscard]] std::optional build(Args &&... args) + [[nodiscard]] tl::expected build( + Args &&... args) { auto interpolator = InterpolatorType(std::forward(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; } 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 a99f4112b0..4d8856374c 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 @@ -76,9 +76,10 @@ class NearestNeighborCommonImpl : public detail::InterpolatorMixin && bases, std::vector && values) override + [[nodiscard]] bool build_impl( + const std::vector & bases, std::vector && values) override { - this->bases_ = std::move(bases); + this->bases_ = bases; this->values_ = std::move(values); return true; } 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 a7fcb0e315..d8bdc11ba5 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 @@ -73,9 +73,10 @@ class StairstepCommonImpl : public detail::InterpolatorMixin, T> * @param bases The bases values. * @param values The values to interpolate. */ - [[nodiscard]] bool build_impl(std::vector && bases, std::vector && values) override + [[nodiscard]] bool build_impl( + const std::vector & bases, std::vector && values) override { - this->bases_ = std::move(bases); + this->bases_ = bases; this->values_ = std::move(values); return true; } diff --git a/common/autoware_trajectory/include/autoware/trajectory/interpolator/linear.hpp b/common/autoware_trajectory/include/autoware/trajectory/interpolator/linear.hpp index a0876c70fe..0f6c2eef36 100644 --- a/common/autoware_trajectory/include/autoware/trajectory/interpolator/linear.hpp +++ b/common/autoware_trajectory/include/autoware/trajectory/interpolator/linear.hpp @@ -52,7 +52,7 @@ class Linear : public detail::InterpolatorMixin * @return True if the interpolator was built successfully, false otherwise. */ [[nodiscard]] bool build_impl( - std::vector && bases, std::vector && values) override; + const std::vector & bases, std::vector && values) override; /** * @brief Compute the interpolated value at the given point. diff --git a/common/autoware_trajectory/include/autoware/trajectory/interpolator/result.hpp b/common/autoware_trajectory/include/autoware/trajectory/interpolator/result.hpp new file mode 100644 index 0000000000..2596bc737b --- /dev/null +++ b/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 + +#include + +namespace autoware::trajectory::interpolator +{ +struct InterpolationSuccess +{ +}; + +struct InterpolationFailure +{ + const std::string what; +}; + +InterpolationFailure operator+( + const InterpolationFailure & primary, const InterpolationFailure & nested); + +using InterpolationResult = tl::expected; + +} // namespace autoware::trajectory::interpolator +#endif // AUTOWARE__TRAJECTORY__INTERPOLATOR__RESULT_HPP_ 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 59d012ee37..3c2d42d8b6 100644 --- a/common/autoware_trajectory/include/autoware/trajectory/interpolator/spherical_linear.hpp +++ b/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 && bases, + const std::vector & bases, std::vector && quaternions) override; /** diff --git a/common/autoware_trajectory/include/autoware/trajectory/path_point.hpp b/common/autoware_trajectory/include/autoware/trajectory/path_point.hpp index 68ff1f0b11..e1b6c73d6e 100644 --- a/common/autoware_trajectory/include/autoware/trajectory/path_point.hpp +++ b/common/autoware_trajectory/include/autoware/trajectory/path_point.hpp @@ -15,7 +15,8 @@ #ifndef AUTOWARE__TRAJECTORY__PATH_POINT_HPP_ #define AUTOWARE__TRAJECTORY__PATH_POINT_HPP_ -#include "autoware/trajectory/detail/interpolated_array.hpp" +#include "autoware/trajectory/interpolated_array.hpp" +#include "autoware/trajectory/interpolator/result.hpp" #include "autoware/trajectory/pose.hpp" #include @@ -76,7 +77,7 @@ class Trajectory * @param points Vector of points * @return True if the build is successful */ - bool build(const std::vector & points); + interpolator::InterpolationResult build(const std::vector & points); /** * @brief Compute the point on the trajectory at a given s value @@ -150,14 +151,16 @@ class Trajectory return *this; } - std::optional build(const std::vector & points) + tl::expected build( + const std::vector & points) { - if (trajectory_->build(points)) { - auto result = std::make_optional(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()); } }; }; 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 6e0d4b7ce2..68f1a95a21 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 @@ -52,7 +52,7 @@ class Trajectory * @param points Vector of points * @return True if the build is successful */ - bool build(const std::vector & points); + interpolator::InterpolationResult build(const std::vector & points); std::vector get_internal_bases() const override; @@ -136,14 +136,16 @@ class Trajectory return *this; } - std::optional build(const std::vector & points) + tl::expected build( + const std::vector & points) { - if (trajectory_->build(points)) { - auto result = std::make_optional(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()); } }; }; diff --git a/common/autoware_trajectory/include/autoware/trajectory/point.hpp b/common/autoware_trajectory/include/autoware/trajectory/point.hpp index 647063bd98..074ec1c026 100644 --- a/common/autoware_trajectory/include/autoware/trajectory/point.hpp +++ b/common/autoware_trajectory/include/autoware/trajectory/point.hpp @@ -18,13 +18,10 @@ #include "autoware/trajectory/forward.hpp" #include "autoware/trajectory/interpolator/interpolator.hpp" -#include - #include #include #include -#include #include #include @@ -90,7 +87,7 @@ class Trajectory * @param points Vector of points * @return True if the build is successful */ - bool build(const std::vector & points); + interpolator::InterpolationResult build(const std::vector & points); /** * @brief Get the azimuth angle at a given s value @@ -148,14 +145,16 @@ class Trajectory return *this; } - std::optional build(const std::vector & points) + tl::expected build( + const std::vector & points) { - if (trajectory_->build(points)) { - auto result = std::make_optional(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()); } }; }; diff --git a/common/autoware_trajectory/include/autoware/trajectory/pose.hpp b/common/autoware_trajectory/include/autoware/trajectory/pose.hpp index 4eacde3e57..088cf3472a 100644 --- a/common/autoware_trajectory/include/autoware/trajectory/pose.hpp +++ b/common/autoware_trajectory/include/autoware/trajectory/pose.hpp @@ -52,7 +52,7 @@ class Trajectory : public Trajectory & point_trajectory); - bool build(const std::vector & points); + interpolator::InterpolationResult build(const std::vector & points); /** * @brief Get the internal bases(arc lengths) of the trajectory @@ -112,14 +112,16 @@ class Trajectory : public Trajectory build(const std::vector & points) + tl::expected build( + const std::vector & points) { - if (trajectory_->build(points)) { - auto result = std::make_optional(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()); } }; }; diff --git a/common/autoware_trajectory/include/autoware/trajectory/trajectory_point.hpp b/common/autoware_trajectory/include/autoware/trajectory/trajectory_point.hpp index c62afbb433..20a3c44fb1 100644 --- a/common/autoware_trajectory/include/autoware/trajectory/trajectory_point.hpp +++ b/common/autoware_trajectory/include/autoware/trajectory/trajectory_point.hpp @@ -15,7 +15,7 @@ #ifndef AUTOWARE__TRAJECTORY__TRAJECTORY_POINT_HPP_ #define AUTOWARE__TRAJECTORY__TRAJECTORY_POINT_HPP_ -#include "autoware/trajectory/detail/interpolated_array.hpp" +#include "autoware/trajectory/interpolated_array.hpp" #include "autoware/trajectory/pose.hpp" #include @@ -103,7 +103,7 @@ class Trajectory * @param points Vector of points * @return True if the build is successful */ - bool build(const std::vector & points); + interpolator::InterpolationResult build(const std::vector & points); /** * @brief Compute the point on the trajectory at a given s value @@ -201,14 +201,16 @@ class Trajectory return *this; } - std::optional build(const std::vector & points) + tl::expected build( + const std::vector & points) { - if (trajectory_->build(points)) { - auto result = std::make_optional(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()); } }; }; diff --git a/common/autoware_trajectory/include/autoware/trajectory/utils/find_intervals.hpp b/common/autoware_trajectory/include/autoware/trajectory/utils/find_intervals.hpp index ad816d0cd7..d7bafcc8ec 100644 --- a/common/autoware_trajectory/include/autoware/trajectory/utils/find_intervals.hpp +++ b/common/autoware_trajectory/include/autoware/trajectory/utils/find_intervals.hpp @@ -18,8 +18,6 @@ #include "autoware/trajectory/detail/types.hpp" #include "autoware/trajectory/forward.hpp" -#include - #include #include namespace autoware::trajectory diff --git a/common/autoware_trajectory/package.xml b/common/autoware_trajectory/package.xml index a0ee4d8c81..6f5f8ef68a 100644 --- a/common/autoware_trajectory/package.xml +++ b/common/autoware_trajectory/package.xml @@ -20,6 +20,7 @@ lanelet2_core rclcpp tf2 + tl_expected ament_cmake_ros ament_lint_auto diff --git a/common/autoware_trajectory/src/interpolator/akima_spline.cpp b/common/autoware_trajectory/src/interpolator/akima_spline.cpp index f4db3517d1..a30a376953 100644 --- a/common/autoware_trajectory/src/interpolator/akima_spline.cpp +++ b/common/autoware_trajectory/src/interpolator/akima_spline.cpp @@ -69,12 +69,11 @@ bool AkimaSpline::build_impl(const std::vector & bases, const std::vecto return true; } -bool AkimaSpline::build_impl(std::vector && bases, std::vector && values) +bool AkimaSpline::build_impl(const std::vector & bases, std::vector && values) { - this->bases_ = std::move(bases); + this->bases_ = bases; compute_parameters( - Eigen::Map( - this->bases_.data(), static_cast(this->bases_.size())), + Eigen::Map(bases.data(), static_cast(bases.size())), Eigen::Map(values.data(), static_cast(values.size()))); return true; } diff --git a/common/autoware_trajectory/src/interpolator/cubic_spline.cpp b/common/autoware_trajectory/src/interpolator/cubic_spline.cpp index c45d65e634..0e75b59181 100644 --- a/common/autoware_trajectory/src/interpolator/cubic_spline.cpp +++ b/common/autoware_trajectory/src/interpolator/cubic_spline.cpp @@ -70,9 +70,9 @@ bool CubicSpline::build_impl(const std::vector & bases, const std::vecto return true; } -bool CubicSpline::build_impl(std::vector && bases, std::vector && values) +bool CubicSpline::build_impl(const std::vector & bases, std::vector && values) { - this->bases_ = std::move(bases); + this->bases_ = bases; compute_parameters( Eigen::Map( this->bases_.data(), static_cast(this->bases_.size())), diff --git a/common/autoware_trajectory/src/interpolator/linear.cpp b/common/autoware_trajectory/src/interpolator/linear.cpp index 1162bf9f36..e4d54c4e7f 100644 --- a/common/autoware_trajectory/src/interpolator/linear.cpp +++ b/common/autoware_trajectory/src/interpolator/linear.cpp @@ -30,9 +30,9 @@ bool Linear::build_impl(const std::vector & bases, const std::vector && bases, std::vector && values) +bool Linear::build_impl(const std::vector & bases, std::vector && values) { - this->bases_ = std::move(bases); + this->bases_ = bases; this->values_ = Eigen::Map(values.data(), static_cast(values.size())); return true; diff --git a/common/autoware_trajectory/src/interpolator/result.cpp b/common/autoware_trajectory/src/interpolator/result.cpp new file mode 100644 index 0000000000..54a8f9606f --- /dev/null +++ b/common/autoware_trajectory/src/interpolator/result.cpp @@ -0,0 +1,31 @@ +// 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. + +#include "autoware/trajectory/interpolator/result.hpp" + +#include +#include + +namespace autoware::trajectory::interpolator +{ + +InterpolationFailure operator+( + const InterpolationFailure & primary, const InterpolationFailure & nested) +{ + std::stringstream ss; + ss << primary.what << "." << std::endl << "\tReason: " << nested.what << std::endl; + return InterpolationFailure{ss.str()}; +} + +} // namespace autoware::trajectory::interpolator diff --git a/common/autoware_trajectory/src/interpolator/spherical_linear.cpp b/common/autoware_trajectory/src/interpolator/spherical_linear.cpp index b8f3bc2387..5f532ec0ba 100644 --- a/common/autoware_trajectory/src/interpolator/spherical_linear.cpp +++ b/common/autoware_trajectory/src/interpolator/spherical_linear.cpp @@ -32,9 +32,9 @@ bool SphericalLinear::build_impl( } bool SphericalLinear::build_impl( - std::vector && bases, std::vector && quaternions) + const std::vector & bases, std::vector && quaternions) { - this->bases_ = std::move(bases); + this->bases_ = bases; this->quaternions_ = std::move(quaternions); return true; } diff --git a/common/autoware_trajectory/src/path_point.cpp b/common/autoware_trajectory/src/path_point.cpp index b45c8d7109..d19b7fcfc6 100644 --- a/common/autoware_trajectory/src/path_point.cpp +++ b/common/autoware_trajectory/src/path_point.cpp @@ -15,7 +15,6 @@ #include "autoware/trajectory/path_point.hpp" #include "autoware/trajectory/detail/helpers.hpp" -#include "autoware/trajectory/detail/interpolated_array.hpp" #include "autoware/trajectory/forward.hpp" #include "autoware/trajectory/interpolator/stairstep.hpp" #include "autoware/trajectory/pose.hpp" @@ -23,8 +22,8 @@ #include #include +#include #include - namespace autoware::trajectory { @@ -61,7 +60,8 @@ Trajectory & Trajectory::operator=(const Trajectory & rhs) return *this; } -bool Trajectory::build(const std::vector & points) +interpolator::InterpolationResult Trajectory::build( + const std::vector & points) { std::vector poses; std::vector longitudinal_velocity_mps_values; @@ -75,14 +75,30 @@ bool Trajectory::build(const std::vector & points) heading_rate_rps_values.emplace_back(point.heading_rate_rps); } - bool is_valid = true; - - is_valid &= Trajectory::build(poses); - is_valid &= this->longitudinal_velocity_mps().build(bases_, longitudinal_velocity_mps_values); - is_valid &= this->lateral_velocity_mps().build(bases_, lateral_velocity_mps_values); - is_valid &= this->heading_rate_rps().build(bases_, heading_rate_rps_values); + if (const auto result = Trajectory::build(poses); !result) { + return tl::unexpected( + interpolator::InterpolationFailure{"failed to interpolate PathPoint::pose"} + result.error()); + } + if (const auto result = this->longitudinal_velocity_mps().build( + bases_, std::move(longitudinal_velocity_mps_values)); + !result) { + return tl::unexpected(interpolator::InterpolationFailure{ + "failed to interpolate PathPoint::longitudinal_velocity_mps"}); + } + if (const auto result = + this->lateral_velocity_mps().build(bases_, std::move(lateral_velocity_mps_values)); + !result) { + return tl::unexpected( + interpolator::InterpolationFailure{"failed to interpolate PathPoint::lateral_velocity_mps"}); + } + if (const auto result = + this->heading_rate_rps().build(bases_, std::move(heading_rate_rps_values)); + !result) { + return tl::unexpected( + interpolator::InterpolationFailure{"failed to interpolate PathPoint::heading_rate_rps"}); + } - return is_valid; + return interpolator::InterpolationSuccess{}; } std::vector Trajectory::get_internal_bases() const 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 27e4f8949c..2c478e1f5a 100644 --- a/common/autoware_trajectory/src/path_point_with_lane_id.cpp +++ b/common/autoware_trajectory/src/path_point_with_lane_id.cpp @@ -18,6 +18,7 @@ #include "autoware/trajectory/interpolator/stairstep.hpp" #include +#include #include namespace autoware::trajectory @@ -40,7 +41,8 @@ Trajectory & Trajectory::operator=(const Trajectory & rhs) return *this; } -bool Trajectory::build(const std::vector & points) +interpolator::InterpolationResult Trajectory::build( + const std::vector & points) { std::vector path_points; std::vector> lane_ids_values; @@ -49,10 +51,18 @@ bool Trajectory::build(const std::vector & points) path_points.emplace_back(point.point); lane_ids_values.emplace_back(point.lane_ids); } - bool is_valid = true; - is_valid &= BaseClass::build(path_points); - is_valid &= lane_ids().build(bases_, lane_ids_values); - return is_valid; + + if (const auto result = BaseClass::build(path_points); !result) { + return tl::unexpected( + interpolator::InterpolationFailure{"failed to interpolate PathPointWithLaneId::point"} + + result.error()); + } + if (const auto result = lane_ids().build(bases_, std::move(lane_ids_values)); !result) { + return tl::unexpected( + interpolator::InterpolationFailure{"failed to interpolate PathPointWithLaneId::lane_id"}); + } + + return interpolator::InterpolationSuccess{}; } std::vector Trajectory::get_internal_bases() const diff --git a/common/autoware_trajectory/src/point.cpp b/common/autoware_trajectory/src/point.cpp index 626e142dbf..37baa97076 100644 --- a/common/autoware_trajectory/src/point.cpp +++ b/common/autoware_trajectory/src/point.cpp @@ -25,6 +25,7 @@ #include #include #include +#include #include namespace autoware::trajectory @@ -62,7 +63,8 @@ Trajectory & Trajectory::operator=(const Trajectory & rhs) return *this; } -bool Trajectory::build(const std::vector & points) +interpolator::InterpolationResult Trajectory::build( + const std::vector & points) { std::vector xs; std::vector ys; @@ -86,12 +88,19 @@ bool Trajectory::build(const std::vector & points) start_ = bases_.front(); end_ = bases_.back(); - bool is_valid = true; - is_valid &= x_interpolator_->build(bases_, xs); - is_valid &= y_interpolator_->build(bases_, ys); - is_valid &= z_interpolator_->build(bases_, zs); - - return is_valid; + if (const auto result = x_interpolator_->build(bases_, std::move(xs)); !result) { + return tl::unexpected( + interpolator::InterpolationFailure{"failed to interpolate Point::x"} + result.error()); + } + if (const auto result = y_interpolator_->build(bases_, std::move(ys)); !result) { + return tl::unexpected( + interpolator::InterpolationFailure{"failed to interpolate Point::y"} + result.error()); + } + if (const auto result = z_interpolator_->build(bases_, std::move(zs)); !result) { + return tl::unexpected( + interpolator::InterpolationFailure{"failed to interpolate Point::z"} + result.error()); + } + return interpolator::InterpolationSuccess{}; } double Trajectory::clamp(const double s, bool show_warning) const diff --git a/common/autoware_trajectory/src/pose.cpp b/common/autoware_trajectory/src/pose.cpp index d89862c464..cedbe9d112 100644 --- a/common/autoware_trajectory/src/pose.cpp +++ b/common/autoware_trajectory/src/pose.cpp @@ -21,6 +21,7 @@ #include #include +#include #include namespace autoware::trajectory @@ -61,7 +62,7 @@ Trajectory::Trajectory(const Trajectory & for (size_t i = 0; i < bases_.size(); ++i) { orientations[i].w = 1.0; } - bool success = orientation_interpolator_->build(bases_, orientations); + const auto success = orientation_interpolator_->build(bases_, std::move(orientations)); if (!success) { throw std::runtime_error( @@ -72,7 +73,8 @@ Trajectory::Trajectory(const Trajectory & align_orientation_with_trajectory_direction(); } -bool Trajectory::build(const std::vector & points) +interpolator::InterpolationResult Trajectory::build( + const std::vector & points) { std::vector path_points; std::vector orientations; @@ -83,10 +85,17 @@ bool Trajectory::build(const std::vector & points) orientations.emplace_back(point.orientation); } - bool is_valid = true; - is_valid &= BaseClass::build(path_points); - is_valid &= orientation_interpolator_->build(bases_, orientations); - return is_valid; + if (const auto result = BaseClass::build(path_points); !result) { + return tl::unexpected( + interpolator::InterpolationFailure{"failed to interpolate Pose::points"} + result.error()); + } + if (const auto result = orientation_interpolator_->build(bases_, std::move(orientations)); + !result) { + return tl::unexpected( + interpolator::InterpolationFailure{"failed to interpolate Pose::orientation"} + + result.error()); + } + return interpolator::InterpolationSuccess{}; } std::vector Trajectory::get_internal_bases() const @@ -157,7 +166,7 @@ void Trajectory::align_orientation_with_trajectory_direction() aligned_orientations.emplace_back(aligned_orientation); } - const bool success = orientation_interpolator_->build(bases_, aligned_orientations); + const auto success = orientation_interpolator_->build(bases_, std::move(aligned_orientations)); if (!success) { throw std::runtime_error( "Failed to build orientation interpolator."); // This exception should not be thrown. diff --git a/common/autoware_trajectory/src/trajectory_point.cpp b/common/autoware_trajectory/src/trajectory_point.cpp index 87658624c0..31af13d805 100644 --- a/common/autoware_trajectory/src/trajectory_point.cpp +++ b/common/autoware_trajectory/src/trajectory_point.cpp @@ -15,7 +15,6 @@ #include "autoware/trajectory/trajectory_point.hpp" #include "autoware/trajectory/detail/helpers.hpp" -#include "autoware/trajectory/detail/interpolated_array.hpp" #include "autoware/trajectory/forward.hpp" #include "autoware/trajectory/interpolator/stairstep.hpp" #include "autoware/trajectory/pose.hpp" @@ -23,6 +22,7 @@ #include #include +#include #include namespace autoware::trajectory @@ -75,7 +75,8 @@ Trajectory & Trajectory::operator=(const Trajectory & rhs) return *this; } -bool Trajectory::build(const std::vector & points) +interpolator::InterpolationResult Trajectory::build( + const std::vector & points) { std::vector poses; std::vector longitudinal_velocity_mps_values; @@ -95,17 +96,49 @@ bool Trajectory::build(const std::vector & points) rear_wheel_angle_rad_values.emplace_back(point.rear_wheel_angle_rad); } - bool is_valid = true; - - is_valid &= Trajectory::build(poses); - is_valid &= this->longitudinal_velocity_mps().build(bases_, longitudinal_velocity_mps_values); - is_valid &= this->lateral_velocity_mps().build(bases_, lateral_velocity_mps_values); - is_valid &= this->heading_rate_rps().build(bases_, heading_rate_rps_values); - is_valid &= this->acceleration_mps2().build(bases_, acceleration_mps2_values); - is_valid &= this->front_wheel_angle_rad().build(bases_, front_wheel_angle_rad_values); - is_valid &= this->rear_wheel_angle_rad().build(bases_, rear_wheel_angle_rad_values); + if (const auto result = Trajectory::build(poses); !result) { + return tl::unexpected( + interpolator::InterpolationFailure{"failed to interpolate TrajectoryPoint::pose"} + + result.error()); + } + if (const auto result = this->longitudinal_velocity_mps().build( + bases_, std::move(longitudinal_velocity_mps_values)); + !result) { + return tl::unexpected(interpolator::InterpolationFailure{ + "failed to interpolate TrajectoryPoint::longitudinal_velocity_mps"}); + } + if (const auto result = + this->lateral_velocity_mps().build(bases_, std::move(lateral_velocity_mps_values)); + !result) { + return tl::unexpected(interpolator::InterpolationFailure{ + "failed to interpolate TrajectoryPoint::lateral_velocity_mps"}); + } + if (const auto result = + this->heading_rate_rps().build(bases_, std::move(heading_rate_rps_values)); + !result) { + return tl::unexpected(interpolator::InterpolationFailure{ + "failed to interpolate TrajectoryPoint::heading_rate_rps"}); + } + if (const auto result = + this->acceleration_mps2().build(bases_, std::move(acceleration_mps2_values)); + !result) { + return tl::unexpected(interpolator::InterpolationFailure{ + "failed to interpolate TrajectoryPoint::acceleration_mps2"}); + } + if (const auto result = + this->front_wheel_angle_rad().build(bases_, std::move(front_wheel_angle_rad_values)); + !result) { + return tl::unexpected(interpolator::InterpolationFailure{ + "failed to interpolate TrajectoryPoint::front_wheel_angle_rad"}); + } + if (const auto result = + this->rear_wheel_angle_rad().build(bases_, std::move(rear_wheel_angle_rad_values)); + !result) { + return tl::unexpected(interpolator::InterpolationFailure{ + "failed to interpolate TrajectoryPoint::rear_wheel_angle_rad"}); + } - return is_valid; + return interpolator::InterpolationSuccess{}; } std::vector Trajectory::get_internal_bases() const diff --git a/common/autoware_trajectory/test/test_interpolator.cpp b/common/autoware_trajectory/test/test_interpolator.cpp index d00d48f807..292ba2afb5 100644 --- a/common/autoware_trajectory/test/test_interpolator.cpp +++ b/common/autoware_trajectory/test/test_interpolator.cpp @@ -61,7 +61,7 @@ TYPED_TEST_SUITE(TestInterpolator, Interpolators, ); TYPED_TEST(TestInterpolator, compute) { this->interpolator = - typename TypeParam::Builder().set_bases(this->bases).set_values(this->values).build(); + typename TypeParam::Builder().set_bases(this->bases).set_values(this->values).build().value(); for (size_t i = 0; i < this->bases.size(); ++i) { EXPECT_NEAR(this->values[i], this->interpolator->compute(this->bases[i]), 1e-6); } diff --git a/common/autoware_trajectory/test/test_trajectory_container.cpp b/common/autoware_trajectory/test/test_trajectory_container.cpp index ce7962a525..6f7b710d0b 100644 --- a/common/autoware_trajectory/test/test_trajectory_container.cpp +++ b/common/autoware_trajectory/test/test_trajectory_container.cpp @@ -66,8 +66,9 @@ class TrajectoryTest : public ::testing::Test path_point_with_lane_id(8.11, 6.07, 1), path_point_with_lane_id(8.76, 7.23, 1), path_point_with_lane_id(9.36, 8.74, 1), path_point_with_lane_id(10.0, 10.0, 1)}; - trajectory = Trajectory::Builder{}.build(points); - ASSERT_TRUE(trajectory); + const auto result = Trajectory::Builder{}.build(points); + ASSERT_TRUE(result); + trajectory.emplace(result.value()); } }; diff --git a/common/autoware_trajectory/test/test_trajectory_container_trajectory_point.cpp b/common/autoware_trajectory/test/test_trajectory_container_trajectory_point.cpp index d4eacd4833..01ce382f7e 100644 --- a/common/autoware_trajectory/test/test_trajectory_container_trajectory_point.cpp +++ b/common/autoware_trajectory/test/test_trajectory_container_trajectory_point.cpp @@ -60,8 +60,9 @@ class TrajectoryTestForTrajectoryPoint : public ::testing::Test trajectory_point(8.11, 6.07), trajectory_point(8.76, 7.23), trajectory_point(9.36, 8.74), trajectory_point(10.0, 10.0)}; - trajectory = Trajectory::Builder{}.build(points); - ASSERT_TRUE(trajectory); + const auto result = Trajectory::Builder{}.build(points); + ASSERT_TRUE(result); + trajectory.emplace(result.value()); } };