From 9cd2ce995d9f383c9ffd67d20be84021b143b7ff Mon Sep 17 00:00:00 2001 From: "Y.Hisaki" Date: Tue, 28 Jan 2025 18:02:18 +0900 Subject: [PATCH 1/5] feat(autoware_trajectory): update autoware_trajectory interfaces Signed-off-by: Y.Hisaki --- .../examples/example_find_intervals.cpp | 107 +++++++ .../examples/example_shift.cpp | 187 ++++++++++++ .../example_trajectory_path_point.cpp | 24 +- .../examples/example_trajectory_point.cpp | 29 +- .../examples/example_trajectory_pose.cpp | 1 - .../detail/{utils.hpp => helpers.hpp} | 49 ++- .../trajectory/detail/interpolated_array.hpp | 30 +- .../autoware/trajectory/detail/logging.hpp | 30 ++ .../autoware/trajectory/detail/types.hpp | 77 +++++ .../include/autoware/trajectory/forward.hpp | 26 ++ .../detail/interpolator_common_interface.hpp | 16 +- .../autoware/trajectory/path_point.hpp | 63 ++-- .../trajectory/path_point_with_lane_id.hpp | 39 +-- .../include/autoware/trajectory/point.hpp | 163 +--------- .../include/autoware/trajectory/pose.hpp | 24 +- .../autoware/trajectory/utils/closest.hpp | 100 +++++++ .../autoware/trajectory/utils/crop.hpp | 33 +++ .../autoware/trajectory/utils/crossed.hpp | 118 ++++++++ .../trajectory/utils/find_intervals.hpp | 76 +++++ .../autoware/trajectory/utils/shift.hpp | 119 ++++++++ .../src/detail/logging.cpp | 32 ++ .../autoware_trajectory/src/detail/types.cpp | 68 +++++ .../autoware_trajectory/src/detail/util.cpp | 55 +--- common/autoware_trajectory/src/path_point.cpp | 87 ++++-- .../src/path_point_with_lane_id.cpp | 57 ++-- common/autoware_trajectory/src/point.cpp | 52 +++- common/autoware_trajectory/src/pose.cpp | 44 ++- .../autoware_trajectory/src/utils/closest.cpp | 64 ++++ .../autoware_trajectory/src/utils/crossed.cpp | 85 ++++++ .../src/utils/find_intervals.cpp | 42 +++ .../autoware_trajectory/src/utils/shift.cpp | 279 ++++++++++++++++++ .../autoware_trajectory/test/test_helpers.cpp | 55 ++++ .../test/test_trajectory_container.cpp | 73 +++-- .../src/scene.cpp | 16 +- .../src/scene.hpp | 6 +- 35 files changed, 1914 insertions(+), 412 deletions(-) create mode 100644 common/autoware_trajectory/examples/example_find_intervals.cpp create mode 100644 common/autoware_trajectory/examples/example_shift.cpp rename common/autoware_trajectory/include/autoware/trajectory/detail/{utils.hpp => helpers.hpp} (60%) create mode 100644 common/autoware_trajectory/include/autoware/trajectory/detail/logging.hpp create mode 100644 common/autoware_trajectory/include/autoware/trajectory/detail/types.hpp create mode 100644 common/autoware_trajectory/include/autoware/trajectory/forward.hpp create mode 100644 common/autoware_trajectory/include/autoware/trajectory/utils/closest.hpp create mode 100644 common/autoware_trajectory/include/autoware/trajectory/utils/crop.hpp create mode 100644 common/autoware_trajectory/include/autoware/trajectory/utils/crossed.hpp create mode 100644 common/autoware_trajectory/include/autoware/trajectory/utils/find_intervals.hpp create mode 100644 common/autoware_trajectory/include/autoware/trajectory/utils/shift.hpp create mode 100644 common/autoware_trajectory/src/detail/logging.cpp create mode 100644 common/autoware_trajectory/src/detail/types.cpp create mode 100644 common/autoware_trajectory/src/utils/closest.cpp create mode 100644 common/autoware_trajectory/src/utils/crossed.cpp create mode 100644 common/autoware_trajectory/src/utils/find_intervals.cpp create mode 100644 common/autoware_trajectory/src/utils/shift.cpp create mode 100644 common/autoware_trajectory/test/test_helpers.cpp diff --git a/common/autoware_trajectory/examples/example_find_intervals.cpp b/common/autoware_trajectory/examples/example_find_intervals.cpp new file mode 100644 index 0000000000000..1148b1b062490 --- /dev/null +++ b/common/autoware_trajectory/examples/example_find_intervals.cpp @@ -0,0 +1,107 @@ +// Copyright 2020 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/path_point_with_lane_id.hpp" +#include "autoware/trajectory/utils/find_intervals.hpp" + +#include + +#include + +using Trajectory = autoware::trajectory::Trajectory; + +tier4_planning_msgs::msg::PathPointWithLaneId path_point_with_lane_id( + double x, double y, uint8_t lane_id) +{ + tier4_planning_msgs::msg::PathPointWithLaneId point; + point.point.pose.position.x = x; + point.point.pose.position.y = y; + point.lane_ids.emplace_back(lane_id); + return point; +} + +int main() +{ + pybind11::scoped_interpreter guard{}; + auto plt = matplotlibcpp17::pyplot::import(); + + std::vector points{ + path_point_with_lane_id(0.41, 0.69, 0), path_point_with_lane_id(0.66, 1.09, 0), + path_point_with_lane_id(0.93, 1.41, 0), path_point_with_lane_id(1.26, 1.71, 0), + path_point_with_lane_id(1.62, 1.90, 0), path_point_with_lane_id(1.96, 1.98, 0), + path_point_with_lane_id(2.48, 1.96, 1), path_point_with_lane_id(3.02, 1.87, 1), + path_point_with_lane_id(3.56, 1.82, 1), path_point_with_lane_id(4.14, 2.02, 1), + path_point_with_lane_id(4.56, 2.36, 1), path_point_with_lane_id(4.89, 2.72, 1), + path_point_with_lane_id(5.27, 3.15, 1), path_point_with_lane_id(5.71, 3.69, 1), + path_point_with_lane_id(6.09, 4.02, 0), path_point_with_lane_id(6.54, 4.16, 0), + path_point_with_lane_id(6.79, 3.92, 0), path_point_with_lane_id(7.11, 3.60, 0), + path_point_with_lane_id(7.42, 3.01, 0)}; + + auto trajectory = Trajectory::Builder{}.build(points); + + if (!trajectory) { + return 1; + } + + const auto intervals = autoware::trajectory::find_intervals( + *trajectory, [](const tier4_planning_msgs::msg::PathPointWithLaneId & point) { + return point.lane_ids[0] == 1; + }); + + std::vector x_id0; + std::vector y_id0; + std::vector x_id1; + std::vector y_id1; + std::vector x_all; + std::vector y_all; + + for (const auto & point : points) { + if (point.lane_ids[0] == 0) { + x_id0.push_back(point.point.pose.position.x); + y_id0.push_back(point.point.pose.position.y); + } else { + x_id1.push_back(point.point.pose.position.x); + y_id1.push_back(point.point.pose.position.y); + } + } + + for (double s = 0.0; s < trajectory->length(); s += 0.01) { + auto p = trajectory->compute(s); + x_all.push_back(p.point.pose.position.x); + y_all.push_back(p.point.pose.position.y); + } + + plt.plot(Args(x_all, y_all), Kwargs("color"_a = "blue")); + plt.scatter(Args(x_id0, y_id0), Kwargs("color"_a = "blue", "label"_a = "lane_id = 0")); + plt.scatter(Args(x_id1, y_id1), Kwargs("color"_a = "red", "label"_a = "lane_id = 1")); + + std::vector x_cropped; + std::vector y_cropped; + + trajectory->crop(intervals[0].start, intervals[0].end - intervals[0].start); + + for (double s = 0.0; s < trajectory->length(); s += 0.01) { + auto p = trajectory->compute(s); + x_cropped.push_back(p.point.pose.position.x); + y_cropped.push_back(p.point.pose.position.y); + } + + plt.plot( + Args(x_cropped, y_cropped), + Kwargs("color"_a = "red", "label"_a = "interval between lane_id = 1")); + + plt.legend(); + plt.show(); + + return 0; +} diff --git a/common/autoware_trajectory/examples/example_shift.cpp b/common/autoware_trajectory/examples/example_shift.cpp new file mode 100644 index 0000000000000..c4f4a30dd439d --- /dev/null +++ b/common/autoware_trajectory/examples/example_shift.cpp @@ -0,0 +1,187 @@ +// 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/point.hpp" +#include "autoware/trajectory/utils/shift.hpp" + +#include + +#include +#include + +geometry_msgs::msg::Point point(double x, double y) +{ + geometry_msgs::msg::Point p; + p.x = x; + p.y = y; + return p; +} + +int main() +{ + pybind11::scoped_interpreter guard{}; + auto plt = matplotlibcpp17::pyplot::import(); + + geometry_msgs::msg::Point p; + (void)(p); + + std::vector points = { + point(0.49, 0.59), point(0.61, 1.22), point(0.86, 1.93), point(1.20, 2.56), point(1.51, 3.17), + point(1.85, 3.76), point(2.14, 4.26), point(2.60, 4.56), point(3.07, 4.55), point(3.61, 4.30), + point(3.95, 4.01), point(4.29, 3.68), point(4.90, 3.25), point(5.54, 3.10), point(6.24, 3.18), + point(6.88, 3.54), point(7.51, 4.25), point(7.85, 4.93), point(8.03, 5.73), point(8.16, 6.52), + point(8.31, 7.28), point(8.45, 7.93), point(8.68, 8.45), point(8.96, 8.96), point(9.32, 9.36)}; + + auto trajectory = + autoware::trajectory::Trajectory::Builder{}.build(points); + + if (!trajectory) { + return 1; + } + + std::cout << "length: " << trajectory->length() << std::endl; + + { + std::vector x; + std::vector y; + for (double s = 0.0; s < trajectory->length(); s += 0.01) { + auto p = trajectory->compute(s); + x.push_back(p.x); + y.push_back(p.y); + } + plt.plot(Args(x, y), Kwargs("label"_a = "original")); + + x.clear(); + y.clear(); + + autoware::trajectory::ShiftInterval shift_interval; + shift_interval.end = -1.0; + shift_interval.length = 0.5; + + auto shifted_trajectory = autoware::trajectory::shift(*trajectory, shift_interval); + + for (double s = 0.0; s < shifted_trajectory.length(); s += 0.01) { + auto p = shifted_trajectory.compute(s); + x.push_back(p.x); + y.push_back(p.y); + } + + plt.axis(Args("equal")); + plt.plot(Args(x, y), Kwargs("label"_a = "shifted")); + plt.legend(); + plt.show(); + } + + { + std::vector x; + std::vector y; + for (double s = 0.0; s < trajectory->length(); s += 0.01) { + auto p = trajectory->compute(s); + x.push_back(p.x); + y.push_back(p.y); + } + plt.plot(Args(x, y), Kwargs("label"_a = "original")); + + x.clear(); + y.clear(); + + autoware::trajectory::ShiftInterval shift_interval; + shift_interval.start = trajectory->length() / 4.0; + shift_interval.end = trajectory->length() * 3.0 / 4.0; + shift_interval.length = 0.5; + auto shifted_trajectory = autoware::trajectory::shift(*trajectory, shift_interval); + + for (double s = 0.0; s < shifted_trajectory.length(); s += 0.01) { + auto p = shifted_trajectory.compute(s); + x.push_back(p.x); + y.push_back(p.y); + } + + plt.axis(Args("equal")); + plt.plot(Args(x, y), Kwargs("label"_a = "shifted")); + plt.legend(); + plt.show(); + } + + { + std::vector x; + std::vector y; + for (double s = 0.0; s < trajectory->length(); s += 0.01) { + auto p = trajectory->compute(s); + x.push_back(p.x); + y.push_back(p.y); + } + plt.plot(Args(x, y), Kwargs("label"_a = "original")); + + x.clear(); + y.clear(); + + autoware::trajectory::ShiftInterval shift_interval; + shift_interval.start = trajectory->length() * 3.0 / 4.0; + shift_interval.end = trajectory->length() / 4.0; + shift_interval.length = 0.5; + auto shifted_trajectory = autoware::trajectory::shift(*trajectory, shift_interval); + + for (double s = 0.0; s < shifted_trajectory.length(); s += 0.01) { + auto p = shifted_trajectory.compute(s); + x.push_back(p.x); + y.push_back(p.y); + } + + plt.axis(Args("equal")); + plt.plot(Args(x, y), Kwargs("label"_a = "shifted")); + plt.legend(); + plt.show(); + } + + { + std::vector x; + std::vector y; + for (double s = 0.0; s < trajectory->length(); s += 0.01) { + auto p = trajectory->compute(s); + x.push_back(p.x); + y.push_back(p.y); + } + plt.plot(Args(x, y), Kwargs("label"_a = "original")); + + x.clear(); + y.clear(); + + autoware::trajectory::ShiftInterval shift_interval1; + shift_interval1.start = trajectory->length() / 4.0; + shift_interval1.end = trajectory->length() * 2.0 / 4.0; + shift_interval1.length = 0.5; + + autoware::trajectory::ShiftInterval shift_interval2; + shift_interval2.start = trajectory->length() * 2.0 / 4.0; + shift_interval2.end = trajectory->length() * 3.0 / 4.0; + shift_interval2.length = -0.5; + + auto shifted_trajectory = + autoware::trajectory::shift(*trajectory, {shift_interval1, shift_interval2}); + + for (double s = 0.0; s < shifted_trajectory.length(); s += 0.01) { + auto p = shifted_trajectory.compute(s); + x.push_back(p.x); + y.push_back(p.y); + } + + plt.axis(Args("equal")); + plt.plot(Args(x, y), Kwargs("label"_a = "shifted")); + plt.legend(); + plt.show(); + } + + return 0; +} diff --git a/common/autoware_trajectory/examples/example_trajectory_path_point.cpp b/common/autoware_trajectory/examples/example_trajectory_path_point.cpp index fb1197e53ab67..0d7317f09789e 100644 --- a/common/autoware_trajectory/examples/example_trajectory_path_point.cpp +++ b/common/autoware_trajectory/examples/example_trajectory_path_point.cpp @@ -13,10 +13,15 @@ // limitations under the License. #include "autoware/trajectory/path_point.hpp" +#include "autoware/trajectory/utils/crossed.hpp" +#include "lanelet2_core/primitives/LineString.h" #include #include +#include + +#include #include #include @@ -69,25 +74,24 @@ int main() trajectory->align_orientation_with_trajectory_direction(); - geometry_msgs::msg::Point p1; - geometry_msgs::msg::Point p2; - p1.x = 7.5; - p1.y = 8.6; - p2.x = 10.2; - p2.y = 7.7; + lanelet::LineString2d line_string; + line_string.push_back(lanelet::Point3d(lanelet::InvalId, 7.5, 8.6, 0.0)); + line_string.push_back(lanelet::Point3d(lanelet::InvalId, 10.2, 7.7, 0.0)); - auto s = trajectory->crossed(p1, p2); - auto crossed = trajectory->compute(s.value()); + auto s = autoware::trajectory::crossed(*trajectory, line_string); + auto crossed = trajectory->compute(s.at(0)); plt.plot( - Args(std::vector{p1.x, p2.x}, std::vector{p1.y, p2.y}), + Args( + std::vector{line_string[0].x(), line_string[1].x()}, + std::vector{line_string[0].y(), line_string[1].y()}), Kwargs("color"_a = "purple")); plt.scatter( Args(crossed.pose.position.x, crossed.pose.position.y), Kwargs("label"_a = "Crossed on trajectory", "color"_a = "purple")); - trajectory->longitudinal_velocity_mps.range(s.value(), trajectory->length()).set(0.0); + trajectory->longitudinal_velocity_mps().range(s.at(0), trajectory->length()).set(0.0); std::vector x; std::vector y; diff --git a/common/autoware_trajectory/examples/example_trajectory_point.cpp b/common/autoware_trajectory/examples/example_trajectory_point.cpp index 014f27ffc9a3a..3aead351085ee 100644 --- a/common/autoware_trajectory/examples/example_trajectory_point.cpp +++ b/common/autoware_trajectory/examples/example_trajectory_point.cpp @@ -14,6 +14,9 @@ #include "autoware/trajectory/interpolator/cubic_spline.hpp" #include "autoware/trajectory/point.hpp" +#include "autoware/trajectory/utils/closest.hpp" +#include "autoware/trajectory/utils/crossed.hpp" +#include "lanelet2_core/primitives/LineString.h" #include @@ -86,7 +89,8 @@ int main() p.x = 5.37; p.y = 6.0; - auto s = trajectory->closest(p); + double s = autoware::trajectory::closest(*trajectory, p); + auto closest = trajectory->compute(s); plt.scatter(Args(p.x, p.y), Kwargs("color"_a = "green")); @@ -98,18 +102,21 @@ int main() Kwargs("color"_a = "green")); } { - geometry_msgs::msg::Point p1; - geometry_msgs::msg::Point p2; - p1.x = 6.97; - p1.y = 6.36; - p2.x = 9.23; - p2.y = 5.92; - - auto s = trajectory->crossed(p1, p2); - auto crossed = trajectory->compute(s.value()); + lanelet::LineString2d line_string; + line_string.push_back(lanelet::Point3d(lanelet::InvalId, 6.97, 6.36, 0.0)); + line_string.push_back(lanelet::Point3d(lanelet::InvalId, 9.23, 5.92, 0.0)); + + auto s = autoware::trajectory::crossed(*trajectory, line_string); + if (s.empty()) { + std::cerr << "Failed to find a crossing point" << std::endl; + return 1; + } + auto crossed = trajectory->compute(s.at(0)); plt.plot( - Args(std::vector{p1.x, p2.x}, std::vector{p1.y, p2.y}), + Args( + std::vector{line_string[0].x(), line_string[1].x()}, + std::vector{line_string[0].y(), line_string[1].y()}), Kwargs("color"_a = "purple")); plt.scatter( diff --git a/common/autoware_trajectory/examples/example_trajectory_pose.cpp b/common/autoware_trajectory/examples/example_trajectory_pose.cpp index a6ca1310fdc42..baa26abedecc1 100644 --- a/common/autoware_trajectory/examples/example_trajectory_pose.cpp +++ b/common/autoware_trajectory/examples/example_trajectory_pose.cpp @@ -45,7 +45,6 @@ int main() pose(8.31, 7.28), pose(8.45, 7.93), pose(8.68, 8.45), pose(8.96, 8.96), pose(9.32, 9.36)}; using autoware::trajectory::Trajectory; - using autoware::trajectory::interpolator::CubicSpline; auto trajectory = Trajectory::Builder{}.build(poses); diff --git a/common/autoware_trajectory/include/autoware/trajectory/detail/utils.hpp b/common/autoware_trajectory/include/autoware/trajectory/detail/helpers.hpp similarity index 60% rename from common/autoware_trajectory/include/autoware/trajectory/detail/utils.hpp rename to common/autoware_trajectory/include/autoware/trajectory/detail/helpers.hpp index 3346d4ce8104f..55981326c4db4 100644 --- a/common/autoware_trajectory/include/autoware/trajectory/detail/utils.hpp +++ b/common/autoware_trajectory/include/autoware/trajectory/detail/helpers.hpp @@ -12,36 +12,17 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef AUTOWARE__TRAJECTORY__DETAIL__UTILS_HPP_ -#define AUTOWARE__TRAJECTORY__DETAIL__UTILS_HPP_ - -#include "lanelet2_core/primitives/Point.h" - -#include - -#include -#include -#include -#include +#ifndef AUTOWARE__TRAJECTORY__DETAIL__HELPERS_HPP_ +#define AUTOWARE__TRAJECTORY__DETAIL__HELPERS_HPP_ +#include #include #include namespace autoware::trajectory::detail { -/** - * @brief Convert various point types to geometry_msgs::msg::Point. - * @param p The input point to be converted. - * @return geometry_msgs::msg::Point The converted point. - */ -geometry_msgs::msg::Point to_point(const geometry_msgs::msg::Point & p); -geometry_msgs::msg::Point to_point(const geometry_msgs::msg::Pose & p); -geometry_msgs::msg::Point to_point(const Eigen::Vector2d & p); -geometry_msgs::msg::Point to_point(const autoware_planning_msgs::msg::PathPoint & p); -geometry_msgs::msg::Point to_point(const tier4_planning_msgs::msg::PathPointWithLaneId & p); -geometry_msgs::msg::Point to_point(const lanelet::BasicPoint2d & p); -geometry_msgs::msg::Point to_point(const lanelet::ConstPoint3d & p); - +inline namespace helpers +{ /** * @brief Merge multiple vectors into one, keeping only unique elements. * @tparam Vectors Variadic template parameter for vector types. @@ -65,11 +46,27 @@ std::vector merge_vectors(const Vectors &... vectors) return {unique_elements.begin(), unique_elements.end()}; } +/** + * @brief Ensures the vector has at least a specified number of points by linearly interpolating + * values. + * + * @param x Input vector of double values. + * @param min_points Minimum number of points required. + * @return A vector with at least `min_points` elements. + * + * @note If `x.size() >= min_points`, the input vector is returned as-is. + * + * @code + * std::vector input = {1.0, 4.0, 6.0}; + * auto result = fill_bases(input, 6); + * // result: {1.0, 2.0, 3.0, 4.0, 5.0, 6.0} + * @endcode + */ std::vector fill_bases(const std::vector & x, const size_t & min_points); std::vector crop_bases( const std::vector & x, const double & start, const double & end); - +} // namespace helpers } // namespace autoware::trajectory::detail -#endif // AUTOWARE__TRAJECTORY__DETAIL__UTILS_HPP_ +#endif // AUTOWARE__TRAJECTORY__DETAIL__HELPERS_HPP_ 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 1ab22d94605dc..fcea47e703f38 100644 --- a/common/autoware_trajectory/include/autoware/trajectory/detail/interpolated_array.hpp +++ b/common/autoware_trajectory/include/autoware/trajectory/detail/interpolated_array.hpp @@ -12,11 +12,10 @@ // See the License for the specific language governing permissions and // limitations under the License. -// clang-format off -#ifndef AUTOWARE__TRAJECTORY__DETAIL__INTERPOLATED_ARRAY_HPP_ // NOLINT -#define AUTOWARE__TRAJECTORY__DETAIL__INTERPOLATED_ARRAY_HPP_ // NOLINT -// clang-format on +#ifndef AUTOWARE__TRAJECTORY__DETAIL__INTERPOLATED_ARRAY_HPP_ +#define AUTOWARE__TRAJECTORY__DETAIL__INTERPOLATED_ARRAY_HPP_ +#include "autoware/trajectory/detail/logging.hpp" #include "autoware/trajectory/interpolator/interpolator.hpp" #include @@ -152,9 +151,11 @@ class InterpolatedArray // Set the values in the specified range std::fill(values.begin() + start_index, values.begin() + end_index + 1, value); - parent_.interpolator_->build(bases, values); - - // return *this; + bool success = parent_.interpolator_->build(bases, values); + if (!success) { + throw std::runtime_error( + "Failed to build interpolator."); // This Exception should not be thrown. + } } }; @@ -168,9 +169,8 @@ class InterpolatedArray { if (start < this->start() || end > this->end()) { RCLCPP_WARN( - rclcpp::get_logger("InterpolatedArray"), - "The range [%f, %f] is out of the array range [%f, %f]", start, end, this->start(), - this->end()); + get_logger(), "The range [%f, %f] is out of the array range [%f, %f]", start, end, + this->start(), this->end()); start = std::max(start, this->start()); end = std::min(end, this->end()); } @@ -185,7 +185,11 @@ class InterpolatedArray InterpolatedArray & operator=(const T & value) { std::fill(values_.begin(), values_.end(), value); - interpolator_->build(bases_, values_); + bool success = interpolator_->build(bases_, values_); + if (!success) { + throw std::runtime_error( + "Failed to build interpolator."); // This Exception should not be thrown. + } return *this; } @@ -208,6 +212,4 @@ class InterpolatedArray } // namespace autoware::trajectory::detail -// clang-format off -#endif // AUTOWARE__TRAJECTORY__DETAIL__INTERPOLATED_ARRAY_HPP_ // NOLINT -// clang-format on +#endif // AUTOWARE__TRAJECTORY__DETAIL__INTERPOLATED_ARRAY_HPP_ diff --git a/common/autoware_trajectory/include/autoware/trajectory/detail/logging.hpp b/common/autoware_trajectory/include/autoware/trajectory/detail/logging.hpp new file mode 100644 index 0000000000000..2ad8d66a4c46d --- /dev/null +++ b/common/autoware_trajectory/include/autoware/trajectory/detail/logging.hpp @@ -0,0 +1,30 @@ +// 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__DETAIL__LOGGING_HPP_ +#define AUTOWARE__TRAJECTORY__DETAIL__LOGGING_HPP_ + +#include +#include + +namespace autoware::trajectory +{ + +rclcpp::Logger & get_logger(); + +rclcpp::Clock & get_clock(); + +} // namespace autoware::trajectory + +#endif // AUTOWARE__TRAJECTORY__DETAIL__LOGGING_HPP_ diff --git a/common/autoware_trajectory/include/autoware/trajectory/detail/types.hpp b/common/autoware_trajectory/include/autoware/trajectory/detail/types.hpp new file mode 100644 index 0000000000000..f21d2fe73ec61 --- /dev/null +++ b/common/autoware_trajectory/include/autoware/trajectory/detail/types.hpp @@ -0,0 +1,77 @@ +// Copyright 2024 TIER IV, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef AUTOWARE__TRAJECTORY__DETAIL__TYPES_HPP_ +#define AUTOWARE__TRAJECTORY__DETAIL__TYPES_HPP_ + +#include "lanelet2_core/primitives/Point.h" + +#include + +#include +#include +#include +#include + +namespace autoware::trajectory::detail +{ + +struct MutablePoint2d +{ + MutablePoint2d(double & x, double & y) : x(x), y(y) {} + double & x; + double & y; +}; +struct MutablePoint3d : MutablePoint2d +{ + MutablePoint3d(double & x, double & y, double & z) : MutablePoint2d{x, y}, z(z) {} + double & z; +}; + +struct ImmutablePoint2d +{ + ImmutablePoint2d(const double & x, const double & y) : x(x), y(y) {} + const double x; + const double y; +}; + +struct ImmutablePoint3d : ImmutablePoint2d +{ + ImmutablePoint3d(const double & x, const double & y, const double & z) + : ImmutablePoint2d{x, y}, z(z) + { + } + const double z; +}; + +/** + * @brief Convert various point types to geometry_msgs::msg::Point. + * @param p The input point to be converted. + * @return geometry_msgs::msg::Point The converted point. + */ +MutablePoint3d to_point(geometry_msgs::msg::Point & p); +MutablePoint3d to_point(geometry_msgs::msg::Pose & p); +MutablePoint3d to_point(autoware_planning_msgs::msg::PathPoint & p); +MutablePoint3d to_point(tier4_planning_msgs::msg::PathPointWithLaneId & p); +MutablePoint2d to_point(lanelet::BasicPoint2d & p); + +ImmutablePoint3d to_point(const geometry_msgs::msg::Point & p); +ImmutablePoint3d to_point(const geometry_msgs::msg::Pose & p); +ImmutablePoint3d to_point(const autoware_planning_msgs::msg::PathPoint & p); +ImmutablePoint3d to_point(const tier4_planning_msgs::msg::PathPointWithLaneId & p); +ImmutablePoint2d to_point(const lanelet::BasicPoint2d & p); + +} // namespace autoware::trajectory::detail + +#endif // AUTOWARE__TRAJECTORY__DETAIL__TYPES_HPP_ diff --git a/common/autoware_trajectory/include/autoware/trajectory/forward.hpp b/common/autoware_trajectory/include/autoware/trajectory/forward.hpp new file mode 100644 index 0000000000000..1bd9d1e4705e2 --- /dev/null +++ b/common/autoware_trajectory/include/autoware/trajectory/forward.hpp @@ -0,0 +1,26 @@ +// Copyright 2024 TIER IV, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef AUTOWARE__TRAJECTORY__FORWARD_HPP_ +#define AUTOWARE__TRAJECTORY__FORWARD_HPP_ + +namespace autoware::trajectory +{ + +template +class Trajectory; + +} // namespace autoware::trajectory + +#endif // AUTOWARE__TRAJECTORY__FORWARD_HPP_ 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 eeca7775a7ff3..5c8a233c9121c 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,6 +35,7 @@ class InterpolatorCommonInterface { protected: std::vector 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. @@ -124,7 +125,7 @@ class InterpolatorCommonInterface * @param values The values to interpolate. * @return True if the interpolator was built successfully, false otherwise. */ - bool build(const std::vector & bases, const std::vector & values) + [[nodiscard]] bool build(const std::vector & bases, const std::vector & values) { if (bases.size() != values.size()) { return false; @@ -133,9 +134,17 @@ class InterpolatorCommonInterface return false; } build_impl(bases, values); + is_built_ = true; return true; } + /** + * @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. * @@ -150,9 +159,14 @@ class InterpolatorCommonInterface * * @param s The point at which to compute the interpolated value. * @return The interpolated value. + * @throw std::runtime_error if the interpolator has not been built. */ [[nodiscard]] T compute(const double & s) const { + if (!is_built_) { + throw std::runtime_error( + "Interpolator has not been built."); // This Exception should not be thrown. + } double clamped_s = validate_compute_input(s); return compute_impl(clamped_s); } diff --git a/common/autoware_trajectory/include/autoware/trajectory/path_point.hpp b/common/autoware_trajectory/include/autoware/trajectory/path_point.hpp index eb4ffd6f46207..d7295570b53b9 100644 --- a/common/autoware_trajectory/include/autoware/trajectory/path_point.hpp +++ b/common/autoware_trajectory/include/autoware/trajectory/path_point.hpp @@ -16,9 +16,10 @@ #define AUTOWARE__TRAJECTORY__PATH_POINT_HPP_ #include "autoware/trajectory/detail/interpolated_array.hpp" -#include "autoware/trajectory/interpolator/stairstep.hpp" #include "autoware/trajectory/pose.hpp" +#include + #include #include #include @@ -32,11 +33,46 @@ class Trajectory using BaseClass = Trajectory; using PointType = autoware_planning_msgs::msg::PathPoint; + std::shared_ptr> + longitudinal_velocity_mps_; //!< Longitudinal velocity in m/s + std::shared_ptr> + lateral_velocity_mps_; //!< Lateral velocity in m/s + std::shared_ptr> + heading_rate_rps_; //!< Heading rate in rad/s}; + public: - detail::InterpolatedArray longitudinal_velocity_mps{ - nullptr}; //!< Longitudinal velocity in m/s - detail::InterpolatedArray lateral_velocity_mps{nullptr}; //!< Lateral velocity in m/s - detail::InterpolatedArray heading_rate_rps{nullptr}; //!< Heading rate in rad/s}; + Trajectory(); + ~Trajectory() override = default; + Trajectory(const Trajectory & rhs); + Trajectory(Trajectory && rhs) = default; + Trajectory & operator=(const Trajectory & rhs); + Trajectory & operator=(Trajectory && rhs) = default; + + [[nodiscard]] std::vector get_internal_bases() const override; + + detail::InterpolatedArray & longitudinal_velocity_mps() + { + return *longitudinal_velocity_mps_; + } + + detail::InterpolatedArray & lateral_velocity_mps() { return *lateral_velocity_mps_; } + + detail::InterpolatedArray & heading_rate_rps() { return *heading_rate_rps_; } + + [[nodiscard]] const detail::InterpolatedArray & longitudinal_velocity_mps() const + { + return *longitudinal_velocity_mps_; + } + + [[nodiscard]] const detail::InterpolatedArray & lateral_velocity_mps() const + { + return *lateral_velocity_mps_; + } + + [[nodiscard]] const detail::InterpolatedArray & heading_rate_rps() const + { + return *heading_rate_rps_; + } /** * @brief Build the trajectory from the points @@ -65,16 +101,7 @@ class Trajectory std::unique_ptr trajectory_; public: - Builder() - { - trajectory_ = std::make_unique(); - set_xy_interpolator(); - set_z_interpolator(); - set_orientation_interpolator(); - set_longitudinal_velocity_interpolator>(); - set_lateral_velocity_interpolator>(); - set_heading_rate_interpolator>(); - } + Builder() : trajectory_(std::make_unique()) {} template Builder & set_xy_interpolator(Args &&... args) @@ -105,7 +132,7 @@ class Trajectory template Builder & set_longitudinal_velocity_interpolator(Args &&... args) { - trajectory_->longitudinal_velocity_mps = detail::InterpolatedArray( + trajectory_->longitudinal_velocity_mps_ = std::make_shared>( std::make_shared(std::forward(args)...)); return *this; } @@ -113,7 +140,7 @@ class Trajectory template Builder & set_lateral_velocity_interpolator(Args &&... args) { - trajectory_->lateral_velocity_mps = detail::InterpolatedArray( + trajectory_->lateral_velocity_mps_ = std::make_shared>( std::make_shared(std::forward(args)...)); return *this; } @@ -121,7 +148,7 @@ class Trajectory template Builder & set_heading_rate_interpolator(Args &&... args) { - trajectory_->heading_rate_rps = detail::InterpolatedArray( + trajectory_->heading_rate_rps_ = std::make_shared>( std::make_shared(std::forward(args)...)); return *this; } 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 0ed92a10a7ea4..2e2f84dcca2d5 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 @@ -16,7 +16,6 @@ #define AUTOWARE__TRAJECTORY__PATH_POINT_WITH_LANE_ID_HPP_ #include "autoware/trajectory/path_point.hpp" -#include "autoware/trajectory/point.hpp" #include @@ -34,8 +33,22 @@ class Trajectory using PointType = tier4_planning_msgs::msg::PathPointWithLaneId; using LaneIdType = std::vector; + std::shared_ptr> lane_ids_; //!< Lane ID + public: - detail::InterpolatedArray lane_ids{nullptr}; //!< Lane ID + Trajectory(); + ~Trajectory() override = default; + Trajectory(const Trajectory & rhs) = default; + Trajectory(Trajectory && rhs) = default; + Trajectory & operator=(const Trajectory & rhs); + Trajectory & operator=(Trajectory && rhs) = default; + + detail::InterpolatedArray & lane_ids() { return *lane_ids_; } + + [[nodiscard]] const detail::InterpolatedArray & lane_ids() const + { + return *lane_ids_; + } /** * @brief Build the trajectory from the points @@ -44,6 +57,8 @@ class Trajectory */ bool build(const std::vector & points); + [[nodiscard]] std::vector get_internal_bases() const override; + /** * @brief Compute the point on the trajectory at a given s value * @param s Arc length @@ -64,17 +79,7 @@ class Trajectory std::unique_ptr trajectory_; public: - Builder() - { - trajectory_ = std::make_unique(); - set_xy_interpolator(); - set_z_interpolator(); - set_orientation_interpolator(); - set_longitudinal_velocity_interpolator>(); - set_lateral_velocity_interpolator>(); - set_heading_rate_interpolator>(); - set_lane_ids_interpolator>(); - } + Builder() : trajectory_(std::make_unique()) {} template Builder & set_xy_interpolator(Args &&... args) @@ -105,7 +110,7 @@ class Trajectory template Builder & set_longitudinal_velocity_interpolator(Args &&... args) { - trajectory_->longitudinal_velocity_mps = detail::InterpolatedArray( + trajectory_->longitudinal_velocity_mps_ = std::make_shared>( std::make_shared(std::forward(args)...)); return *this; } @@ -113,7 +118,7 @@ class Trajectory template Builder & set_lateral_velocity_interpolator(Args &&... args) { - trajectory_->lateral_velocity_mps = detail::InterpolatedArray( + trajectory_->lateral_velocity_mps_ = std::make_shared>( std::make_shared(std::forward(args)...)); return *this; } @@ -121,7 +126,7 @@ class Trajectory template Builder & set_heading_rate_interpolator(Args &&... args) { - trajectory_->heading_rate_rps = detail::InterpolatedArray( + trajectory_->heading_rate_rps_ = std::make_shared>( std::make_shared(std::forward(args)...)); return *this; } @@ -129,7 +134,7 @@ class Trajectory template Builder & set_lane_ids_interpolator(Args &&... args) { - trajectory_->lane_ids = detail::InterpolatedArray( + trajectory_->lane_ids_ = std::make_shared>( std::make_shared(std::forward(args)...)); return *this; } diff --git a/common/autoware_trajectory/include/autoware/trajectory/point.hpp b/common/autoware_trajectory/include/autoware/trajectory/point.hpp index 3e763e2428f1b..af47c404f7351 100644 --- a/common/autoware_trajectory/include/autoware/trajectory/point.hpp +++ b/common/autoware_trajectory/include/autoware/trajectory/point.hpp @@ -15,18 +15,14 @@ #ifndef AUTOWARE__TRAJECTORY__POINT_HPP_ #define AUTOWARE__TRAJECTORY__POINT_HPP_ -#include "autoware/trajectory/detail/utils.hpp" -#include "autoware/trajectory/interpolator/cubic_spline.hpp" +#include "autoware/trajectory/forward.hpp" #include "autoware/trajectory/interpolator/interpolator.hpp" -#include "autoware/trajectory/interpolator/linear.hpp" #include #include -#include #include -#include #include #include #include @@ -34,10 +30,6 @@ namespace autoware::trajectory { - -template -class Trajectory; - /** * @brief Trajectory class for geometry_msgs::msg::Point */ @@ -58,8 +50,6 @@ class Trajectory double start_{0.0}, end_{0.0}; //!< Start and end of the arc length of the trajectory - using ConstraintFunction = std::function; - /** * @brief Validate the arc length is within the trajectory * @param s Arc length @@ -67,6 +57,18 @@ class Trajectory [[nodiscard]] double clamp(const double & s, bool show_warning = false) const; public: + Trajectory(); + virtual ~Trajectory() = default; + Trajectory(const Trajectory & rhs); + Trajectory(Trajectory && rhs) = default; + Trajectory & operator=(const Trajectory & rhs); + Trajectory & operator=(Trajectory && rhs) = default; + + /** + * @brief Get the internal bases(arc lengths) of the trajectory + * @return Vector of bases(arc lengths) + */ + [[nodiscard]] virtual std::vector get_internal_bases() const; /** * @brief Get the length of the trajectory * @return Length of the trajectory @@ -108,137 +110,6 @@ class Trajectory */ [[nodiscard]] double curvature(double s) const; - /** - * @brief Find the closest point with constraint - * @tparam InputPointType Type of input point - * @param p Input point - * @param constraints Constraint function - * @return Optional arc length of the closest point - */ - template - [[nodiscard]] std::optional closest_with_constraint( - const InputPointType & p, const ConstraintFunction & constraints) const - { - using trajectory::detail::to_point; - Eigen::Vector2d point(to_point(p).x, to_point(p).y); - std::vector distances_from_segments; - std::vector lengths_from_start_points; - - auto axis = detail::crop_bases(bases_, start_, end_); - - for (size_t i = 1; i < axis.size(); ++i) { - Eigen::Vector2d p0; - Eigen::Vector2d p1; - p0 << x_interpolator_->compute(axis.at(i - 1)), y_interpolator_->compute(axis.at(i - 1)); - p1 << x_interpolator_->compute(axis.at(i)), y_interpolator_->compute(axis.at(i)); - Eigen::Vector2d v = p1 - p0; - Eigen::Vector2d w = point - p0; - double c1 = w.dot(v); - double c2 = v.dot(v); - double length_from_start_point = NAN; - double distance_from_segment = NAN; - if (c1 <= 0) { - length_from_start_point = axis.at(i - 1); - distance_from_segment = (point - p0).norm(); - } else if (c2 <= c1) { - length_from_start_point = axis.at(i); - distance_from_segment = (point - p1).norm(); - } else { - length_from_start_point = axis.at(i - 1) + c1 / c2 * (p1 - p0).norm(); - distance_from_segment = (point - (p0 + (c1 / c2) * v)).norm(); - } - if (constraints(length_from_start_point)) { - distances_from_segments.push_back(distance_from_segment); - lengths_from_start_points.push_back(length_from_start_point); - } - } - if (distances_from_segments.empty()) { - return std::nullopt; - } - - auto min_it = std::min_element(distances_from_segments.begin(), distances_from_segments.end()); - - return lengths_from_start_points[std::distance(distances_from_segments.begin(), min_it)] - - start_; - } - - /** - * @brief Find the closest point - * @tparam InputPointType Type of input point - * @param p Input point - * @return Arc length of the closest point - */ - template - [[nodiscard]] double closest(const InputPointType & p) const - { - auto s = closest_with_constraint(p, [](const double &) { return true; }); - return *s; - } - /** - * @brief Find the crossing point with constraint - * @tparam InputPointType Type of input point - * @param start Start point - * @param end End point - * @param constraints Constraint function - * @return Optional arc length of the crossing point - */ - template - [[nodiscard]] std::optional crossed_with_constraint( - const InputPointType & start, const InputPointType & end, - const ConstraintFunction & constraints) const - { - using trajectory::detail::to_point; - Eigen::Vector2d line_start(to_point(start).x, to_point(start).y); - Eigen::Vector2d line_end(to_point(end).x, to_point(end).y); - Eigen::Vector2d line_dir = line_end - line_start; - - auto axis = detail::crop_bases(bases_, start_, end_); - - for (size_t i = 1; i < axis.size(); ++i) { - Eigen::Vector2d p0; - Eigen::Vector2d p1; - p0 << x_interpolator_->compute(axis.at(i - 1)), y_interpolator_->compute(axis.at(i - 1)); - p1 << x_interpolator_->compute(axis.at(i)), y_interpolator_->compute(axis.at(i)); - - Eigen::Vector2d segment_dir = p1 - p0; - - double det = segment_dir.x() * line_dir.y() - segment_dir.y() * line_dir.x(); - - if (std::abs(det) < 1e-10) { - continue; - } - - Eigen::Vector2d p0_to_line_start = line_start - p0; - - double t = (p0_to_line_start.x() * line_dir.y() - p0_to_line_start.y() * line_dir.x()) / det; - double u = - (p0_to_line_start.x() * segment_dir.y() - p0_to_line_start.y() * segment_dir.x()) / det; - - if (t >= 0.0 && t <= 1.0 && u >= 0.0 && u <= 1.0) { - double intersection_s = axis.at(i - 1) + t * (axis.at(i) - axis.at(i - 1)); - if (constraints(intersection_s)) { - return intersection_s - start_; - } - } - } - - return std::nullopt; - } - - /** - * @brief Find the crossing point - * @tparam InputPointType Type of input point - * @param start Start point - * @param end End point - * @return Optional arc length of the crossing point - */ - template - [[nodiscard]] std::optional crossed( - const InputPointType & start, const InputPointType & end) const - { - return crossed_with_constraint(start, end, [](const double &) { return true; }); - } - /** * @brief Restore the trajectory points * @param min_points Minimum number of points @@ -254,13 +125,7 @@ class Trajectory std::unique_ptr trajectory_; public: - Builder() - { - trajectory_ = std::make_unique(); - // Default interpolators - set_xy_interpolator(); - set_z_interpolator(); - } + Builder() : trajectory_(std::make_unique()) {} template Builder & set_xy_interpolator(Args &&... args) diff --git a/common/autoware_trajectory/include/autoware/trajectory/pose.hpp b/common/autoware_trajectory/include/autoware/trajectory/pose.hpp index 8acc7b48dc8e4..911db11bf8dfc 100644 --- a/common/autoware_trajectory/include/autoware/trajectory/pose.hpp +++ b/common/autoware_trajectory/include/autoware/trajectory/pose.hpp @@ -15,11 +15,10 @@ #ifndef AUTOWARE__TRAJECTORY__POSE_HPP_ #define AUTOWARE__TRAJECTORY__POSE_HPP_ -#include "autoware/trajectory/interpolator/cubic_spline.hpp" #include "autoware/trajectory/interpolator/interpolator.hpp" -#include "autoware/trajectory/interpolator/spherical_linear.hpp" #include "autoware/trajectory/point.hpp" +#include #include #include @@ -43,8 +42,21 @@ class Trajectory : public Trajectory & points); + /** + * @brief Get the internal bases(arc lengths) of the trajectory + * @return Vector of bases(arc lengths) + */ + [[nodiscard]] std::vector get_internal_bases() const override; + /** * @brief Compute the pose on the trajectory at a given s value * @param s Arc length @@ -69,13 +81,7 @@ class Trajectory : public Trajectory trajectory_; public: - Builder() - { - trajectory_ = std::make_unique(); - set_xy_interpolator(); - set_z_interpolator(); - set_orientation_interpolator(); - } + Builder() : trajectory_(std::make_unique()) {} template Builder & set_xy_interpolator(Args &&... args) diff --git a/common/autoware_trajectory/include/autoware/trajectory/utils/closest.hpp b/common/autoware_trajectory/include/autoware/trajectory/utils/closest.hpp new file mode 100644 index 0000000000000..842e743271318 --- /dev/null +++ b/common/autoware_trajectory/include/autoware/trajectory/utils/closest.hpp @@ -0,0 +1,100 @@ +// Copyright 2024 TIER IV, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef AUTOWARE__TRAJECTORY__UTILS__CLOSEST_HPP_ +#define AUTOWARE__TRAJECTORY__UTILS__CLOSEST_HPP_ + +#include "autoware/trajectory/detail/types.hpp" +#include "autoware/trajectory/forward.hpp" + +#include + +#include +#include +#include + +namespace autoware::trajectory +{ +namespace detail::impl +{ +/** + * @brief Internal implementation to find the closest point on a trajectory to a given point with + * constraints. + * @param trajectory_compute A function that computes a 2D point on the trajectory for a given + * parameter `s`. + * @param bases A vector of double values representing the sequence of bases for the trajectory. + * @param point The 2D point to which the closest point on the trajectory is to be found. + * @param constraint A function that evaluates whether a given parameter `s` satisfies the + * constraint. + * @return An optional double value representing the parameter `s` of the closest point on the + * trajectory that satisfies the constraint, or `std::nullopt` if no such point exists. + */ +std::optional closest_with_constraint_impl( + const std::function & trajectory_compute, + const std::vector & bases, const Eigen::Vector2d & point, + const std::function & constraint); +} // namespace detail::impl + +/** + * @brief Finds the closest point on a trajectory to a given point where the given constraint is + * satisfied. + * @tparam TrajectoryPointType The type of points in the trajectory. + * @tparam ArgPointType The type of the input point. + * @tparam Constraint A callable type that evaluates a constraint on a trajectory point. + * @param trajectory The trajectory to evaluate. + * @param point The point to which the closest point on the trajectory is to be found. + * @param constraint The constraint to apply to each point in the trajectory. + * @return An optional double value representing the parameter `s` of the closest point on the + * trajectory that satisfies the constraint, or `std::nullopt` if no such point exists. + */ +template +std::optional closest_with_constraint( + const trajectory::Trajectory & trajectory, const ArgPointType & point, + const Constraint & constraint) +{ + using autoware::trajectory::detail::to_point; + + return detail::impl::closest_with_constraint_impl( + [&trajectory](const double & s) { + TrajectoryPointType point = trajectory.compute(s); + Eigen::Vector2d result; + result << to_point(point).x, to_point(point).y; + return result; + }, + trajectory.get_internal_bases(), {to_point(point).x, to_point(point).y}, + [&constraint, &trajectory](const double & s) { return constraint(trajectory.compute(s)); }); +} + +/** + * @brief Finds the closest point on a trajectory to a given point. + * @tparam TrajectoryPointType The type of points in the trajectory. + * @tparam ArgPointType The type of the input point. + * @param trajectory The trajectory to evaluate. + * @param point The point to which the closest point on the trajectory is to be found. + * @return The parameter `s` of the closest point on the trajectory. + */ +template +double closest( + const trajectory::Trajectory & trajectory, const ArgPointType & point) +{ + std::optional s = + *closest_with_constraint(trajectory, point, [](const TrajectoryPointType &) { return true; }); + if (!s) { + throw std::runtime_error("No closest point found."); // This Exception should not be thrown. + } + return *s; +} +} // namespace autoware::trajectory + +#endif // AUTOWARE__TRAJECTORY__UTILS__CLOSEST_HPP_ diff --git a/common/autoware_trajectory/include/autoware/trajectory/utils/crop.hpp b/common/autoware_trajectory/include/autoware/trajectory/utils/crop.hpp new file mode 100644 index 0000000000000..1ce75c67d8128 --- /dev/null +++ b/common/autoware_trajectory/include/autoware/trajectory/utils/crop.hpp @@ -0,0 +1,33 @@ +// 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__UTILS__CROP_HPP_ +#define AUTOWARE__TRAJECTORY__UTILS__CROP_HPP_ + +#include "autoware/trajectory/forward.hpp" + +namespace autoware::trajectory +{ + +template +trajectory::Trajectory crop( + trajectory::Trajectory trajectory, const double & start, const double & end) +{ + trajectory.crop(start, end); + return trajectory; +} + +} // namespace autoware::trajectory + +#endif // AUTOWARE__TRAJECTORY__UTILS__CROP_HPP_ diff --git a/common/autoware_trajectory/include/autoware/trajectory/utils/crossed.hpp b/common/autoware_trajectory/include/autoware/trajectory/utils/crossed.hpp new file mode 100644 index 0000000000000..cb3406218800e --- /dev/null +++ b/common/autoware_trajectory/include/autoware/trajectory/utils/crossed.hpp @@ -0,0 +1,118 @@ +// Copyright 2024 TIER IV, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef AUTOWARE__TRAJECTORY__UTILS__CROSSED_HPP_ +#define AUTOWARE__TRAJECTORY__UTILS__CROSSED_HPP_ +#include "autoware/trajectory/detail/types.hpp" +#include "autoware/trajectory/forward.hpp" + +#include + +#include +#include + +namespace autoware::trajectory +{ +namespace detail::impl +{ + +/** + * @brief Internal implementation to find intersections between a trajectory and a linestring with + * constraints. + * @param trajectory_compute A function that computes a 2D point on the trajectory for a given + * parameter `s`. + * @param bases A vector of double values representing the sequence of bases for the trajectory. + * @param linestring A vector of pairs representing the linestring as a sequence of 2D line + * segments. + * @param constraint A function that evaluates whether a given parameter `s` satisfies the + * constraint. + * @return A vector of double values representing the parameters `s` where the trajectory intersects + * the linestring and satisfies the constraint. + */ +std::vector crossed_with_constraint_impl( + const std::function & trajectory_compute, + const std::vector & bases, // + const std::vector> & linestring, + const std::function & constraint); +} // namespace detail::impl + +/** + * @brief Finds intersections between a trajectory and a linestring where the given constraint is + * satisfied. + * @tparam TrajectoryPointType The type of points in the trajectory. + * @tparam LineStringType The type of the linestring. + * @tparam Constraint A callable type that evaluates a constraint on a trajectory point. + * @param trajectory The trajectory to evaluate. + * @param linestring The linestring to intersect with the trajectory. + * @param constraint The constraint to apply to each point in the trajectory. + * @return A vector of double values representing the parameters `s` where the trajectory intersects + * the linestring and satisfies the constraint. + */ +template +[[nodiscard]] std::vector crossed_with_constraint( + const trajectory::Trajectory & trajectory, const LineStringType & linestring, + const Constraint & constraint) +{ + using autoware::trajectory::detail::to_point; + + std::function trajectory_compute = + [&trajectory](const double & s) { + TrajectoryPointType point = trajectory.compute(s); + Eigen::Vector2d result; + result << to_point(point).x, to_point(point).y; + return result; + }; + + std::vector> linestring_eigen; + + if (linestring.end() - linestring.begin() < 2) { + return {}; + } + + auto point_it = linestring.begin(); + auto point_it_next = linestring.begin() + 1; + + for (; point_it_next != linestring.end(); ++point_it, ++point_it_next) { + Eigen::Vector2d start; + Eigen::Vector2d end; + start << point_it->x(), point_it->y(); + end << point_it_next->x(), point_it_next->y(); + linestring_eigen.emplace_back(start, end); + } + + return detail::impl::crossed_with_constraint_impl( + trajectory_compute, trajectory.get_internal_bases(), linestring_eigen, + [&constraint, &trajectory](const double & s) { return constraint(trajectory.compute(s)); }); +} + +/** + * @brief Finds intersections between a trajectory and a linestring. + * @tparam TrajectoryPointType The type of points in the trajectory. + * @tparam LineStringType The type of the linestring. + * @param trajectory The trajectory to evaluate. + * @param linestring The linestring to intersect with the trajectory. + * @return A vector of double values representing the parameters `s` where the trajectory intersects + * the linestring. + */ +template +[[nodiscard]] std::vector crossed( + const trajectory::Trajectory & trajectory, const LineStringType & linestring) +{ + return crossed_with_constraint( + trajectory, linestring, [](const TrajectoryPointType &) { return true; }); +} + +} // namespace autoware::trajectory + +#endif // AUTOWARE__TRAJECTORY__UTILS__CROSSED_HPP_ diff --git a/common/autoware_trajectory/include/autoware/trajectory/utils/find_intervals.hpp b/common/autoware_trajectory/include/autoware/trajectory/utils/find_intervals.hpp new file mode 100644 index 0000000000000..ad816d0cd7917 --- /dev/null +++ b/common/autoware_trajectory/include/autoware/trajectory/utils/find_intervals.hpp @@ -0,0 +1,76 @@ +// Copyright 2024 TIER IV, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef AUTOWARE__TRAJECTORY__UTILS__FIND_INTERVALS_HPP_ +#define AUTOWARE__TRAJECTORY__UTILS__FIND_INTERVALS_HPP_ + +#include "autoware/trajectory/detail/types.hpp" +#include "autoware/trajectory/forward.hpp" + +#include + +#include +#include +namespace autoware::trajectory +{ + +/** + * @struct Interval + * @brief Represents an interval with a start and end value. + */ +struct Interval +{ + double start; ///< Start value of the interval. + double end; ///< End value of the interval. +}; + +namespace detail::impl +{ + +/** + * @brief Internal implementation to find intervals in a sequence of bases that satisfy a + * constraint. + * @param bases A vector of double values representing the sequence of bases. + * @param constraint A function that evaluates whether a given base satisfies the constraint. + * @return A vector of Interval objects representing the intervals where the constraint is + * satisfied. + */ +std::vector find_intervals_impl( + const std::vector & bases, const std::function & constraint); + +} // namespace detail::impl + +/** + * @brief Finds intervals in a trajectory where the given constraint is satisfied. + * @tparam TrajectoryPointType The type of points in the trajectory. + * @tparam Constraint A callable type that evaluates a constraint on a trajectory point. + * @param trajectory The trajectory to evaluate. + * @param constraint The constraint to apply to each point in the trajectory. + * @return A vector of Interval objects representing the intervals where the constraint is + * satisfied. + */ +template +std::vector find_intervals( + const Trajectory & trajectory, const Constraint & constraint) +{ + using autoware::trajectory::detail::to_point; + + return detail::impl::find_intervals_impl( + trajectory.get_internal_bases(), + [&constraint, &trajectory](const double & s) { return constraint(trajectory.compute(s)); }); +} + +} // namespace autoware::trajectory + +#endif // AUTOWARE__TRAJECTORY__UTILS__FIND_INTERVALS_HPP_ diff --git a/common/autoware_trajectory/include/autoware/trajectory/utils/shift.hpp b/common/autoware_trajectory/include/autoware/trajectory/utils/shift.hpp new file mode 100644 index 0000000000000..d1457d8ba3a0d --- /dev/null +++ b/common/autoware_trajectory/include/autoware/trajectory/utils/shift.hpp @@ -0,0 +1,119 @@ +// Copyright 2024 TIER IV, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef AUTOWARE__TRAJECTORY__UTILS__SHIFT_HPP_ +#define AUTOWARE__TRAJECTORY__UTILS__SHIFT_HPP_ + +#include "autoware/trajectory/detail/types.hpp" +#include "autoware/trajectory/forward.hpp" + +#include + +namespace autoware::trajectory +{ + +/** + * @struct ShiftInterval + * @brief Represents an interval for shifting a trajectory. + */ +struct ShiftInterval +{ + double start{0.0}; ///< Start position of the shift interval. + double end{0.0}; ///< End position of the shift interval. + double length{0.0}; ///< Length of the shift to be applied. +}; + +/** + * @struct ShiftParameters + * @brief Represents parameters for shifting a trajectory. + */ +struct ShiftParameters +{ + double velocity{0.0}; ///< Velocity parameter for the shift. + double longitudinal_acc{0.0}; ///< Longitudinal acceleration parameter for the shift. + double lateral_acc_limit{-1.0}; ///< Lateral acceleration limit for the shift. +}; + +namespace detail::impl +{ + +/** + * @brief Internal implementation to apply a shift to a trajectory. + * @param bases A vector of double values representing the sequence of bases for the trajectory. + * @param shift_lengths A pointer to a vector of double values representing the shift lengths to be + * applied. + * @param shift_interval The interval over which the shift is applied. + * @param shift_parameters The parameters for the shift. + */ +void shift_impl( + const std::vector & bases, std::vector * shift_lengths, + const ShiftInterval & shift_interval, const ShiftParameters & shift_parameters); + +} // namespace detail::impl + +/** + * @brief Shifts a trajectory based on the provided shift intervals and parameters. + * @tparam PointType The type of points in the trajectory. + * @param reference_trajectory The reference trajectory to be shifted. + * @param shift_intervals A vector of ShiftInterval objects representing the intervals for shifting. + * @param shift_parameters The parameters for the shift. + * @return The shifted trajectory. + */ +template +trajectory::Trajectory shift( + const trajectory::Trajectory & reference_trajectory, + const std::vector & shift_intervals, const ShiftParameters & shift_parameters = {}) +{ + auto bases = reference_trajectory.get_internal_bases(); + std::vector shift_lengths(bases.size(), 0.0); + for (const auto & shift_interval : shift_intervals) { + detail::impl::shift_impl(bases, &shift_lengths, shift_interval, shift_parameters); + } + // Apply shift. + std::vector shifted_points; + for (size_t i = 0; i < bases.size(); ++i) { + shifted_points.emplace_back(reference_trajectory.compute(bases.at(i))); + double azimuth = reference_trajectory.azimuth(bases.at(i)); + double shift_length = shift_lengths.at(i); + detail::to_point(shifted_points.back()).x += std::sin(azimuth) * shift_length; + detail::to_point(shifted_points.back()).y -= std::cos(azimuth) * shift_length; + } + auto shifted_trajectory = reference_trajectory; + bool valid = shifted_trajectory.build(shifted_points); + if (!valid) { + throw std::runtime_error( + "Failed to build shifted trajectory"); // This Exception should not be thrown. + } + return shifted_trajectory; +} + +/** + * @brief Shifts a trajectory based on a single shift interval and parameters. + * @tparam PointType The type of points in the trajectory. + * @param reference_trajectory The reference trajectory to be shifted. + * @param shift_interval The interval for shifting. + * @param shift_parameters The parameters for the shift. + * @return The shifted trajectory. + */ +template +trajectory::Trajectory shift( + const trajectory::Trajectory & reference_trajectory, + const ShiftInterval & shift_interval, const ShiftParameters & shift_parameters = {}) +{ + return shift(reference_trajectory, std::vector{shift_interval}, shift_parameters); +} + +} // namespace autoware::trajectory + +#endif // AUTOWARE__TRAJECTORY__UTILS__SHIFT_HPP_ diff --git a/common/autoware_trajectory/src/detail/logging.cpp b/common/autoware_trajectory/src/detail/logging.cpp new file mode 100644 index 0000000000000..97816ee83b0ef --- /dev/null +++ b/common/autoware_trajectory/src/detail/logging.cpp @@ -0,0 +1,32 @@ +// 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/detail/logging.hpp" + +namespace autoware::trajectory +{ + +rclcpp::Logger & get_logger() +{ + static rclcpp::Logger logger = rclcpp::get_logger("autoware_trajectory"); + return logger; +} + +rclcpp::Clock & get_clock() +{ + static rclcpp::Clock clock(RCL_ROS_TIME); + return clock; +} + +} // namespace autoware::trajectory diff --git a/common/autoware_trajectory/src/detail/types.cpp b/common/autoware_trajectory/src/detail/types.cpp new file mode 100644 index 0000000000000..9c7867fffd1c1 --- /dev/null +++ b/common/autoware_trajectory/src/detail/types.cpp @@ -0,0 +1,68 @@ +// Copyright 2024 TIER IV, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include "autoware/trajectory/detail/types.hpp" + +namespace autoware::trajectory::detail +{ +MutablePoint3d to_point(geometry_msgs::msg::Point & p) +{ + return {p.x, p.y, p.z}; +} + +MutablePoint3d to_point(geometry_msgs::msg::Pose & p) +{ + return {p.position.x, p.position.y, p.position.z}; +} + +MutablePoint3d to_point(autoware_planning_msgs::msg::PathPoint & p) +{ + return {p.pose.position.x, p.pose.position.y, p.pose.position.z}; +} + +MutablePoint3d to_point(tier4_planning_msgs::msg::PathPointWithLaneId & p) +{ + return {p.point.pose.position.x, p.point.pose.position.y, p.point.pose.position.z}; +} + +MutablePoint2d to_point(lanelet::BasicPoint2d & p) +{ + return {p.x(), p.y()}; +} + +ImmutablePoint3d to_point(const geometry_msgs::msg::Point & p) +{ + return {p.x, p.y, p.z}; +} + +ImmutablePoint3d to_point(const geometry_msgs::msg::Pose & p) +{ + return {p.position.x, p.position.y, p.position.z}; +} + +ImmutablePoint3d to_point(const autoware_planning_msgs::msg::PathPoint & p) +{ + return {p.pose.position.x, p.pose.position.y, p.pose.position.z}; +} + +ImmutablePoint3d to_point(const tier4_planning_msgs::msg::PathPointWithLaneId & p) +{ + return {p.point.pose.position.x, p.point.pose.position.y, p.point.pose.position.z}; +} + +ImmutablePoint2d to_point(const lanelet::BasicPoint2d & p) +{ + return {p.x(), p.y()}; +} +} // namespace autoware::trajectory::detail diff --git a/common/autoware_trajectory/src/detail/util.cpp b/common/autoware_trajectory/src/detail/util.cpp index 4c9649ef1f3ab..f491740b15994 100644 --- a/common/autoware_trajectory/src/detail/util.cpp +++ b/common/autoware_trajectory/src/detail/util.cpp @@ -12,64 +12,15 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "autoware/trajectory/detail/utils.hpp" +#include "autoware/trajectory/detail/helpers.hpp" #include #include namespace autoware::trajectory::detail { - -geometry_msgs::msg::Point to_point(const geometry_msgs::msg::Point & p) -{ - return p; -} - -geometry_msgs::msg::Point to_point(const geometry_msgs::msg::Pose & p) -{ - return p.position; -} - -geometry_msgs::msg::Point to_point(const Eigen::Vector2d & p) -{ - geometry_msgs::msg::Point point; - point.x = p(0); - point.y = p(1); - return point; -} - -geometry_msgs::msg::Point to_point(const autoware_planning_msgs::msg::PathPoint & p) -{ - geometry_msgs::msg::Point point; - point.x = p.pose.position.x; - point.y = p.pose.position.y; - return point; -} - -geometry_msgs::msg::Point to_point(const tier4_planning_msgs::msg::PathPointWithLaneId & p) -{ - geometry_msgs::msg::Point point; - point.x = p.point.pose.position.x; - point.y = p.point.pose.position.y; - return point; -} - -geometry_msgs::msg::Point to_point(const lanelet::BasicPoint2d & p) +inline namespace helpers { - geometry_msgs::msg::Point point; - point.x = p.x(); - point.y = p.y(); - return point; -} - -geometry_msgs::msg::Point to_point(const lanelet::ConstPoint3d & p) -{ - geometry_msgs::msg::Point point; - point.x = p.x(); - point.y = p.y(); - return point; -} - std::vector fill_bases(const std::vector & x, const size_t & min_points) { const auto original_size = x.size(); @@ -128,5 +79,5 @@ std::vector crop_bases( return result; } - +} // namespace helpers } // namespace autoware::trajectory::detail diff --git a/common/autoware_trajectory/src/path_point.cpp b/common/autoware_trajectory/src/path_point.cpp index 0806bcead53f3..d264d1d054d87 100644 --- a/common/autoware_trajectory/src/path_point.cpp +++ b/common/autoware_trajectory/src/path_point.cpp @@ -14,10 +14,15 @@ #include "autoware/trajectory/path_point.hpp" -#include "autoware/trajectory/detail/utils.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" #include +#include #include namespace autoware::trajectory @@ -25,6 +30,37 @@ namespace autoware::trajectory using PointType = autoware_planning_msgs::msg::PathPoint; +Trajectory::Trajectory() +: longitudinal_velocity_mps_(std::make_shared>( + std::make_shared>())), + lateral_velocity_mps_(std::make_shared>( + std::make_shared>())), + heading_rate_rps_(std::make_shared>( + std::make_shared>())) +{ +} + +Trajectory::Trajectory(const Trajectory & rhs) +: BaseClass(rhs), + longitudinal_velocity_mps_( + std::make_shared>(*rhs.longitudinal_velocity_mps_)), + lateral_velocity_mps_( + std::make_shared>(*rhs.lateral_velocity_mps_)), + heading_rate_rps_(std::make_shared>(*rhs.heading_rate_rps_)) +{ +} + +Trajectory & Trajectory::operator=(const Trajectory & rhs) +{ + if (this != &rhs) { + BaseClass::operator=(rhs); + *longitudinal_velocity_mps_ = *rhs.longitudinal_velocity_mps_; + *lateral_velocity_mps_ = *rhs.lateral_velocity_mps_; + *heading_rate_rps_ = *rhs.heading_rate_rps_; + } + return *this; +} + bool Trajectory::build(const std::vector & points) { std::vector poses; @@ -42,49 +78,52 @@ bool Trajectory::build(const std::vector & points) 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->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); return is_valid; } +std::vector Trajectory::get_internal_bases() const +{ + auto get_bases = [](const auto & interpolated_array) { + auto [bases, values] = interpolated_array.get_data(); + return bases; + }; + + auto bases = detail::merge_vectors( + bases_, get_bases(this->longitudinal_velocity_mps()), get_bases(this->lateral_velocity_mps()), + get_bases(this->heading_rate_rps())); + + bases = detail::crop_bases(bases, start_, end_); + std::transform( + bases.begin(), bases.end(), bases.begin(), [this](const double & s) { return s - start_; }); + return bases; +} + PointType Trajectory::compute(double s) const { PointType result; result.pose = Trajectory::compute(s); s = clamp(s); - result.longitudinal_velocity_mps = static_cast(this->longitudinal_velocity_mps.compute(s)); - result.lateral_velocity_mps = static_cast(this->lateral_velocity_mps.compute(s)); - result.heading_rate_rps = static_cast(this->heading_rate_rps.compute(s)); + result.longitudinal_velocity_mps = + static_cast(this->longitudinal_velocity_mps().compute(s)); + result.lateral_velocity_mps = static_cast(this->lateral_velocity_mps().compute(s)); + result.heading_rate_rps = static_cast(this->heading_rate_rps().compute(s)); return result; } std::vector Trajectory::restore(const size_t & min_points) const { - auto get_bases = [](const auto & interpolated_array) { - auto [bases, values] = interpolated_array.get_data(); - return bases; - }; - - auto bases = detail::merge_vectors( - bases_, get_bases(this->longitudinal_velocity_mps), get_bases(this->lateral_velocity_mps), - get_bases(this->heading_rate_rps)); - - bases = detail::crop_bases(bases, start_, end_); + std::vector bases = get_internal_bases(); bases = detail::fill_bases(bases, min_points); std::vector points; points.reserve(bases.size()); for (const auto & s : bases) { - PointType p; - p.pose = Trajectory::compute(s - start_); - p.longitudinal_velocity_mps = static_cast(this->longitudinal_velocity_mps.compute(s)); - p.lateral_velocity_mps = static_cast(this->lateral_velocity_mps.compute(s)); - p.heading_rate_rps = static_cast(this->heading_rate_rps.compute(s)); - points.emplace_back(p); + points.emplace_back(compute(s)); } - return 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 c3d7441fef405..27ade116f3330 100644 --- a/common/autoware_trajectory/src/path_point_with_lane_id.cpp +++ b/common/autoware_trajectory/src/path_point_with_lane_id.cpp @@ -14,8 +14,10 @@ #include "autoware/trajectory/path_point_with_lane_id.hpp" -#include "autoware/trajectory/detail/utils.hpp" +#include "autoware/trajectory/detail/helpers.hpp" +#include "autoware/trajectory/interpolator/stairstep.hpp" +#include #include namespace autoware::trajectory @@ -23,6 +25,21 @@ namespace autoware::trajectory using PointType = tier4_planning_msgs::msg::PathPointWithLaneId; +Trajectory::Trajectory() +: lane_ids_(std::make_shared>( + std::make_shared>())) +{ +} + +Trajectory & Trajectory::operator=(const Trajectory & rhs) +{ + if (this != &rhs) { + BaseClass::operator=(rhs); + lane_ids_ = std::make_shared>(this->lane_ids()); + } + return *this; +} + bool Trajectory::build(const std::vector & points) { std::vector path_points; @@ -34,40 +51,46 @@ bool Trajectory::build(const std::vector & points) } bool is_valid = true; is_valid &= BaseClass::build(path_points); - is_valid &= lane_ids.build(bases_, lane_ids_values); + is_valid &= lane_ids().build(bases_, lane_ids_values); return is_valid; } +std::vector Trajectory::get_internal_bases() const +{ + auto get_bases = [](const auto & interpolated_array) { + auto [bases, values] = interpolated_array.get_data(); + return bases; + }; + + auto bases = detail::merge_vectors( + bases_, get_bases(this->longitudinal_velocity_mps()), get_bases(this->lateral_velocity_mps()), + get_bases(this->heading_rate_rps()), get_bases(this->lane_ids())); + + bases = detail::crop_bases(bases, start_, end_); + + std::transform( + bases.begin(), bases.end(), bases.begin(), [this](const double & s) { return s - start_; }); + return bases; +} + PointType Trajectory::compute(double s) const { PointType result; result.point = BaseClass::compute(s); s = clamp(s); - result.lane_ids = lane_ids.compute(s); + result.lane_ids = lane_ids().compute(s); return result; } std::vector Trajectory::restore(const size_t & min_points) const { - auto get_bases = [](const auto & interpolated_array) { - auto [bases, values] = interpolated_array.get_data(); - return bases; - }; - - auto bases = detail::merge_vectors( - bases_, get_bases(this->longitudinal_velocity_mps), get_bases(this->lateral_velocity_mps), - get_bases(this->heading_rate_rps), get_bases(this->lane_ids)); - - bases = detail::crop_bases(bases, start_, end_); + auto bases = get_internal_bases(); bases = detail::fill_bases(bases, min_points); std::vector points; points.reserve(bases.size()); for (const auto & s : bases) { - PointType p; - p.point = BaseClass::compute(s - start_); - p.lane_ids = lane_ids.compute(s); - points.emplace_back(p); + points.emplace_back(compute(s)); } return points; } diff --git a/common/autoware_trajectory/src/point.cpp b/common/autoware_trajectory/src/point.cpp index 40c6c357e9ba5..017c5691ce44b 100644 --- a/common/autoware_trajectory/src/point.cpp +++ b/common/autoware_trajectory/src/point.cpp @@ -14,7 +14,9 @@ #include "autoware/trajectory/point.hpp" -#include "autoware/trajectory/detail/utils.hpp" +#include "autoware/trajectory/detail/helpers.hpp" +#include "autoware/trajectory/interpolator/cubic_spline.hpp" +#include "autoware/trajectory/interpolator/linear.hpp" #include #include @@ -22,6 +24,7 @@ #include #include #include +#include #include namespace autoware::trajectory @@ -29,12 +32,43 @@ namespace autoware::trajectory using PointType = geometry_msgs::msg::Point; +Trajectory::Trajectory() +: x_interpolator_(std::make_shared()), + y_interpolator_(std::make_shared()), + z_interpolator_(std::make_shared()) +{ +} + +Trajectory::Trajectory(const Trajectory & rhs) +: x_interpolator_(rhs.x_interpolator_->clone()), + y_interpolator_(rhs.y_interpolator_->clone()), + z_interpolator_(rhs.z_interpolator_->clone()), + bases_(rhs.bases_), + start_(rhs.start_), + end_(rhs.end_) +{ +} + +Trajectory & Trajectory::operator=(const Trajectory & rhs) +{ + if (this != &rhs) { + x_interpolator_ = rhs.x_interpolator_->clone(); + y_interpolator_ = rhs.y_interpolator_->clone(); + z_interpolator_ = rhs.z_interpolator_->clone(); + bases_ = rhs.bases_; + start_ = rhs.start_; + end_ = rhs.end_; + } + return *this; +} + bool Trajectory::build(const std::vector & points) { std::vector xs; std::vector ys; std::vector zs; + bases_.clear(); bases_.emplace_back(0.0); xs.emplace_back(points[0].x); ys.emplace_back(points[0].y); @@ -70,6 +104,14 @@ double Trajectory::clamp(const double & s, bool show_warning) const return std::clamp(s, 0.0, length()) + start_; } +std::vector Trajectory::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_; }); + return bases; +} + double Trajectory::length() const { return end_ - start_; @@ -112,16 +154,12 @@ double Trajectory::curvature(double s) const std::vector Trajectory::restore(const size_t & min_points) const { - auto bases = detail::crop_bases(bases_, start_, end_); + auto bases = get_internal_bases(); bases = detail::fill_bases(bases, min_points); std::vector points; points.reserve(bases.size()); for (const auto & s : bases) { - PointType p; - p.x = x_interpolator_->compute(s); - p.y = y_interpolator_->compute(s); - p.z = z_interpolator_->compute(s); - points.emplace_back(p); + points.emplace_back(compute(s)); } return points; } diff --git a/common/autoware_trajectory/src/pose.cpp b/common/autoware_trajectory/src/pose.cpp index 3906ee17fa2eb..1d3891a95f6c6 100644 --- a/common/autoware_trajectory/src/pose.cpp +++ b/common/autoware_trajectory/src/pose.cpp @@ -14,6 +14,9 @@ #include "autoware/trajectory/pose.hpp" +#include "autoware/trajectory/detail/helpers.hpp" +#include "autoware/trajectory/interpolator/spherical_linear.hpp" + #include #include @@ -21,9 +24,27 @@ namespace autoware::trajectory { - using PointType = geometry_msgs::msg::Pose; +Trajectory::Trajectory() +: orientation_interpolator_(std::make_shared()) +{ +} + +Trajectory::Trajectory(const Trajectory & rhs) +: BaseClass(rhs), orientation_interpolator_(rhs.orientation_interpolator_->clone()) +{ +} + +Trajectory & Trajectory::operator=(const Trajectory & rhs) +{ + if (this != &rhs) { + BaseClass::operator=(rhs); + orientation_interpolator_ = rhs.orientation_interpolator_->clone(); + } + return *this; +} + bool Trajectory::build(const std::vector & points) { std::vector path_points; @@ -41,6 +62,14 @@ bool Trajectory::build(const std::vector & points) return is_valid; } +std::vector Trajectory::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_; }); + return bases; +} + PointType Trajectory::compute(double s) const { PointType result; @@ -81,20 +110,21 @@ void Trajectory::align_orientation_with_trajectory_direction() aligned_orientations.emplace_back(aligned_orientation); } - orientation_interpolator_->build(bases_, aligned_orientations); + bool success = orientation_interpolator_->build(bases_, aligned_orientations); + if (!success) { + throw std::runtime_error( + "Failed to build orientation interpolator."); // This Exception should not be thrown. + } } std::vector Trajectory::restore(const size_t & min_points) const { - auto bases = detail::crop_bases(bases_, start_, end_); + auto bases = get_internal_bases(); bases = detail::fill_bases(bases, min_points); std::vector points; points.reserve(bases.size()); for (const auto & s : bases) { - PointType p; - p.position = BaseClass::compute(s - start_); - p.orientation = orientation_interpolator_->compute(s); - points.emplace_back(p); + points.emplace_back(compute(s)); } return points; } diff --git a/common/autoware_trajectory/src/utils/closest.cpp b/common/autoware_trajectory/src/utils/closest.cpp new file mode 100644 index 0000000000000..d51fb26a9d188 --- /dev/null +++ b/common/autoware_trajectory/src/utils/closest.cpp @@ -0,0 +1,64 @@ +// Copyright 2024 TIER IV, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include "autoware/trajectory/utils/closest.hpp" + +#include +#include + +namespace autoware::trajectory::detail::impl +{ +std::optional closest_with_constraint_impl( + const std::function & trajectory_compute, + const std::vector & bases, const Eigen::Vector2d & point, + const std::function & constraint) +{ + using trajectory::detail::to_point; + std::vector distances_from_segments; + std::vector lengths_from_start_points; + + for (size_t i = 1; i < bases.size(); ++i) { + Eigen::Vector2d p0 = trajectory_compute(bases.at(i - 1)); + Eigen::Vector2d p1 = trajectory_compute(bases.at(i)); + + Eigen::Vector2d v = p1 - p0; + Eigen::Vector2d w = point - p0; + double c1 = w.dot(v); + double c2 = v.dot(v); + double length_from_start_point = NAN; + double distance_from_segment = NAN; + if (c1 <= 0) { + length_from_start_point = bases.at(i - 1); + distance_from_segment = (point - p0).norm(); + } else if (c2 <= c1) { + length_from_start_point = bases.at(i); + distance_from_segment = (point - p1).norm(); + } else { + length_from_start_point = bases.at(i - 1) + c1 / c2 * (p1 - p0).norm(); + distance_from_segment = (point - (p0 + (c1 / c2) * v)).norm(); + } + if (constraint(length_from_start_point)) { + distances_from_segments.push_back(distance_from_segment); + lengths_from_start_points.push_back(length_from_start_point); + } + } + if (distances_from_segments.empty()) { + return std::nullopt; + } + + auto min_it = std::min_element(distances_from_segments.begin(), distances_from_segments.end()); + + return lengths_from_start_points[std::distance(distances_from_segments.begin(), min_it)]; +} +} // namespace autoware::trajectory::detail::impl diff --git a/common/autoware_trajectory/src/utils/crossed.cpp b/common/autoware_trajectory/src/utils/crossed.cpp new file mode 100644 index 0000000000000..44419f1598b2c --- /dev/null +++ b/common/autoware_trajectory/src/utils/crossed.cpp @@ -0,0 +1,85 @@ +// Copyright 2024 TIER IV, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include "autoware/trajectory/utils/crossed.hpp" + +#include +#include + +namespace autoware::trajectory::detail::impl +{ + +std::optional crossed_with_constraint_impl( + const std::function & trajectory_compute, + const std::vector & bases, // + const Eigen::Vector2d & line_start, const Eigen::Vector2d & line_end, + const std::function & constraint) +{ + Eigen::Vector2d line_dir = line_end - line_start; + + for (size_t i = 1; i < bases.size(); ++i) { + Eigen::Vector2d p0 = trajectory_compute(bases.at(i - 1)); + Eigen::Vector2d p1 = trajectory_compute(bases.at(i)); + + Eigen::Vector2d segment_dir = p1 - p0; + + double det = segment_dir.x() * line_dir.y() - segment_dir.y() * line_dir.x(); + + if (std::abs(det) < 1e-10) { + continue; + } + + Eigen::Vector2d p0_to_line_start = line_start - p0; + + double t = (p0_to_line_start.x() * line_dir.y() - p0_to_line_start.y() * line_dir.x()) / det; + double u = + (p0_to_line_start.x() * segment_dir.y() - p0_to_line_start.y() * segment_dir.x()) / det; + + if (t >= 0.0 && t <= 1.0 && u >= 0.0 && u <= 1.0) { + double intersection = bases.at(i - 1) + t * (bases.at(i) - bases.at(i - 1)); + if (constraint(intersection)) { + return intersection; + } + } + } + + return std::nullopt; +} + +std::vector crossed_with_constraint_impl( + const std::function & trajectory_compute, + const std::vector & bases, // + const std::vector> & linestring, + const std::function & constraint) +{ + using trajectory::detail::to_point; + + std::vector intersections; + + for (const auto & line : linestring) { + const Eigen::Vector2d & line_start = line.first; + const Eigen::Vector2d & line_end = line.second; + + std::optional intersection = + crossed_with_constraint_impl(trajectory_compute, bases, line_start, line_end, constraint); + + if (intersection) { + intersections.push_back(*intersection); + } + } + + return intersections; +} + +} // namespace autoware::trajectory::detail::impl diff --git a/common/autoware_trajectory/src/utils/find_intervals.cpp b/common/autoware_trajectory/src/utils/find_intervals.cpp new file mode 100644 index 0000000000000..4899f07f9cccd --- /dev/null +++ b/common/autoware_trajectory/src/utils/find_intervals.cpp @@ -0,0 +1,42 @@ +// Copyright 2024 TIER IV, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include "autoware/trajectory/utils/find_intervals.hpp" + +#include +#include +#include + +namespace autoware::trajectory::detail::impl +{ + +std::vector find_intervals_impl( + const std::vector & bases, const std::function & constraint) +{ + std::vector intervals; + + std::optional start = std::nullopt; + for (size_t i = 0; i < bases.size(); ++i) { + if (!start && constraint(bases.at(i))) { + start = bases.at(i); // Start a new interval + } else if (start && (!constraint(bases.at(i)) || i == bases.size() - 1)) { + // End the current interval if the constraint fails or it's the last element + intervals.emplace_back(Interval{start.value(), bases.at(i - !constraint(bases.at(i)))}); + start = std::nullopt; // Reset the start + } + } + return intervals; +} + +} // namespace autoware::trajectory::detail::impl diff --git a/common/autoware_trajectory/src/utils/shift.cpp b/common/autoware_trajectory/src/utils/shift.cpp new file mode 100644 index 0000000000000..c17abfbbfb4ee --- /dev/null +++ b/common/autoware_trajectory/src/utils/shift.cpp @@ -0,0 +1,279 @@ +// Copyright 2024 TIER IV, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include "autoware/trajectory/utils/shift.hpp" + +#include "autoware/trajectory/detail/logging.hpp" +#include "autoware/trajectory/interpolator/cubic_spline.hpp" + +#include +#include +#include +#include + +namespace autoware::trajectory::detail::impl +{ + +// This function calculates base longitudinal and lateral lengths +// when acceleration limit is not considered (simple division approach). +std::pair, std::vector> get_base_lengths_without_accel_limit( + const double arc_length, const double shift_length) +{ + // Alias for clarity + const double total_arc_length = arc_length; + const double total_shift_length = shift_length; + + // Prepare base longitudinal positions + std::vector base_longitudinal = { + 0.0, 0.25 * total_arc_length, 0.75 * total_arc_length, total_arc_length}; + + // Prepare base lateral positions + std::vector base_lateral = { + 0.0, 1.0 / 12.0 * total_shift_length, 11.0 / 12.0 * total_shift_length, total_shift_length}; + + return {base_longitudinal, base_lateral}; +} + +// This function calculates base longitudinal and lateral lengths +// when acceleration limit is not considered, but velocity and acceleration are known. +std::pair, std::vector> get_base_lengths_without_accel_limit( + const double arc_length, const double shift_length, const double velocity, + const double longitudinal_acc, const double total_time) +{ + // Aliases for clarity + const double total_arc_length = arc_length; + const double total_shift_length = shift_length; + const double v0 = velocity; // initial velocity + const double a = longitudinal_acc; // longitudinal acceleration + const double t = total_time / 4.0; // quarter of total time + + // Calculate first segment in longitudinal direction + // s1 = v0 * t + 1/2 * a * t^2 (but capped by total_arc_length) + const double segment_s1 = std::min(v0 * t + 0.5 * a * t * t, total_arc_length); + + // Velocity at the end of first segment + const double v1 = v0 + a * t; + + // Calculate second segment in longitudinal direction + // s2 = s1 + 2 * v1 * t + 2 * a * t^2 (but capped by total_arc_length) + const double segment_s2 = std::min(segment_s1 + 2.0 * v1 * t + 2.0 * a * t * t, total_arc_length); + + // Prepare base longitudinal positions + std::vector base_longitudinal = {0.0, segment_s1, segment_s2, total_arc_length}; + + // Prepare base lateral positions (simple division approach as original) + std::vector base_lateral = { + 0.0, 1.0 / 12.0 * total_shift_length, 11.0 / 12.0 * total_shift_length, total_shift_length}; + + return {base_longitudinal, base_lateral}; +} +std::pair, std::vector> calc_base_lengths( + const double & arc_length, const double & shift_length, const ShiftParameters & shift_parameters) +{ + // Threshold for treating acceleration as zero + const double acc_threshold = 1.0e-4; + + // Extract initial velocity and clamp negative acceleration to zero + const double initial_vel = std::abs(shift_parameters.velocity); + const double used_lon_acc = + (shift_parameters.longitudinal_acc > acc_threshold) ? shift_parameters.longitudinal_acc : 0.0; + + // If there is no need to consider acceleration limit + if (initial_vel < 1.0e-5 && used_lon_acc < acc_threshold) { + RCLCPP_DEBUG( + get_logger(), + "Velocity is effectively zero. " + "No lateral acceleration limit will be applied."); + return get_base_lengths_without_accel_limit(arc_length, shift_length); + } + + // Prepare main parameters + const double target_arclength = arc_length; + const double target_shift_abs = std::abs(shift_length); + + // Calculate total time (total_time) to travel 'target_arclength' + // If used_lon_acc is valid (> 0), use time from kinematic formula. Otherwise, use s/v + const double total_time = + (used_lon_acc > acc_threshold) + ? (-initial_vel + + std::sqrt(initial_vel * initial_vel + 2.0 * used_lon_acc * target_arclength)) / + used_lon_acc + : (target_arclength / initial_vel); + + // Calculate the maximum lateral acceleration if we do not add further constraints + const double max_lateral_acc = 8.0 * target_shift_abs / (total_time * total_time); + + // If the max_lateral_acc is already below the limit, no need to reduce it + if (max_lateral_acc < shift_parameters.lateral_acc_limit) { + RCLCPP_DEBUG_THROTTLE( + get_logger(), get_clock(), 3000, "No need to consider lateral acc limit. max: %f, limit: %f", + max_lateral_acc, shift_parameters.lateral_acc_limit); + return get_base_lengths_without_accel_limit( + target_arclength, shift_length, initial_vel, used_lon_acc, total_time); + } + + // Compute intermediate times (jerk_time / accel_time) and lateral jerk + const double jerk_time = + total_time / 2.0 - 2.0 * target_shift_abs / (shift_parameters.lateral_acc_limit * total_time); + const double accel_time = + 4.0 * target_shift_abs / (shift_parameters.lateral_acc_limit * total_time) - total_time / 2.0; + const double lateral_jerk = + (2.0 * shift_parameters.lateral_acc_limit * shift_parameters.lateral_acc_limit * total_time) / + (shift_parameters.lateral_acc_limit * total_time * total_time - 4.0 * target_shift_abs); + + // If computed times or jerk are invalid (negative or too small), skip the acc limit + if (jerk_time < 0.0 || accel_time < 0.0 || lateral_jerk < 0.0 || (jerk_time / total_time) < 0.1) { + RCLCPP_WARN_THROTTLE( + get_logger(), get_clock(), 3000, + "The specified lateral acceleration limit appears too restrictive. " + "No feasible jerk_time or accel_time was found. " + "Possible reasons: (1) shift_length is too large, (2) lateral_acc_limit is too low, " + "or (3) system kinematics are outside valid range.\n" + "Details:\n" + " - jerk_time: %.4f\n" + " - accel_time: %.4f\n" + " - lateral_jerk: %.4f\n" + " - computed_lateral_acc: %.4f\n" + " - lateral_acc_limit: %.4f\n\n" + "Suggestions:\n" + " - Increase the lateral_acc_limit.\n" + " - Decrease shift_length if possible.\n" + " - Re-check velocity and acceleration settings for consistency.", + jerk_time, accel_time, lateral_jerk, max_lateral_acc, shift_parameters.lateral_acc_limit); + return get_base_lengths_without_accel_limit(target_arclength, shift_length); + } + + // Precompute powers for jerk_time and accel_time + const double jerk_time3 = jerk_time * jerk_time * jerk_time; + const double accel_time2_jerk = accel_time * accel_time * jerk_time; + const double accel_time_jerk2 = accel_time * jerk_time * jerk_time; + + // ------------------------------------------------------ + // Calculate longitudinal base points + // ------------------------------------------------------ + // Segment s1 + const double segment_s1 = std::min( + jerk_time * initial_vel + 0.5 * used_lon_acc * jerk_time * jerk_time, target_arclength); + const double v1 = initial_vel + used_lon_acc * jerk_time; + + // Segment s2 + const double segment_s2 = std::min( + segment_s1 + accel_time * v1 + 0.5 * used_lon_acc * accel_time * accel_time, target_arclength); + const double v2 = v1 + used_lon_acc * accel_time; + + // Segment s3 = s4 + const double segment_s3 = std::min( + segment_s2 + jerk_time * v2 + 0.5 * used_lon_acc * jerk_time * jerk_time, target_arclength); + const double v3 = v2 + used_lon_acc * jerk_time; + + // Segment s5 + const double segment_s5 = std::min( + segment_s3 + jerk_time * v3 + 0.5 * used_lon_acc * jerk_time * jerk_time, target_arclength); + const double v5 = v3 + used_lon_acc * jerk_time; + + // Segment s6 + const double segment_s6 = std::min( + segment_s5 + accel_time * v5 + 0.5 * used_lon_acc * accel_time * accel_time, target_arclength); + const double v6 = v5 + used_lon_acc * accel_time; + + // Segment s7 + const double segment_s7 = std::min( + segment_s6 + jerk_time * v6 + 0.5 * used_lon_acc * jerk_time * jerk_time, target_arclength); + + // ------------------------------------------------------ + // Calculate lateral base points + // ------------------------------------------------------ + // sign determines the direction of shift + const double shift_sign = (shift_length > 0.0) ? 1.0 : -1.0; + + // Shift amounts at each segment + const double shift_l1 = shift_sign * (1.0 / 6.0 * lateral_jerk * jerk_time3); + const double shift_l2 = + shift_sign * (1.0 / 6.0 * lateral_jerk * jerk_time3 + 0.5 * lateral_jerk * accel_time_jerk2 + + 0.5 * lateral_jerk * accel_time2_jerk); + const double shift_l3 = + shift_sign * (lateral_jerk * jerk_time3 + 1.5 * lateral_jerk * accel_time_jerk2 + + 0.5 * lateral_jerk * accel_time2_jerk); // = shift_l4 + const double shift_l5 = + shift_sign * (11.0 / 6.0 * lateral_jerk * jerk_time3 + 2.5 * lateral_jerk * accel_time_jerk2 + + 0.5 * lateral_jerk * accel_time2_jerk); + const double shift_l6 = + shift_sign * (11.0 / 6.0 * lateral_jerk * jerk_time3 + 3.0 * lateral_jerk * accel_time_jerk2 + + lateral_jerk * accel_time2_jerk); + const double shift_l7 = + shift_sign * (2.0 * lateral_jerk * jerk_time3 + 3.0 * lateral_jerk * accel_time_jerk2 + + lateral_jerk * accel_time2_jerk); + + // Construct the output vectors + std::vector base_lon = {0.0, segment_s1, segment_s2, segment_s3, + segment_s5, segment_s6, segment_s7}; + std::vector base_lat = {0.0, shift_l1, shift_l2, shift_l3, shift_l5, shift_l6, shift_l7}; + + return {base_lon, base_lat}; +} + +void shift_impl( + const std::vector & bases, std::vector * shift_lengths, + const ShiftInterval & shift_interval, const ShiftParameters & shift_parameters) +{ + // lateral shift + if (shift_interval.end <= 0.0) { + for (size_t i = 0; i < bases.size(); ++i) { + shift_lengths->at(i) += shift_interval.length; + } + return; + } + + double shift_arc_length = std::abs(shift_interval.end - shift_interval.start); + bool shift_direction = shift_interval.end > shift_interval.start; + // Calculate base lengths + auto [base_lon, base_lat] = calc_base_lengths( + shift_arc_length, // + shift_interval.length, // + shift_parameters); + + auto cubic_spline = + interpolator::CubicSpline::Builder{}.set_bases(base_lon).set_values(base_lat).build(); + + if (!cubic_spline) { + throw std::runtime_error( + "Failed to build cubic spline for shift calculation."); // This Exception should not be + // thrown. + } + for (size_t i = 0; i < bases.size(); ++i) { + // Calculate the shift length at the current base + const double s = bases.at(i); + if (shift_direction) { + if (s < shift_interval.start) { + continue; + } + if (s <= shift_interval.end) { + shift_lengths->at(i) += cubic_spline->compute(s - shift_interval.start); + } else { + shift_lengths->at(i) += cubic_spline->compute(shift_arc_length); + } + } else { + if (s > shift_interval.start) { + continue; + } + if (s >= shift_interval.end) { + shift_lengths->at(i) += cubic_spline->compute(shift_interval.start - s); + } else { + shift_lengths->at(i) += cubic_spline->compute(shift_arc_length); + } + } + } +} + +} // namespace autoware::trajectory::detail::impl diff --git a/common/autoware_trajectory/test/test_helpers.cpp b/common/autoware_trajectory/test/test_helpers.cpp new file mode 100644 index 0000000000000..c5ab9ae6b4758 --- /dev/null +++ b/common/autoware_trajectory/test/test_helpers.cpp @@ -0,0 +1,55 @@ +// Copyright 2024 TIER IV, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include "autoware/trajectory/detail/helpers.hpp" + +#include + +#include + +TEST(TestHelpers, fill_bases) +{ + using autoware::trajectory::detail::fill_bases; + + std::vector x = {0.0, 1.0, 2.0, 3.0}; + size_t min_points = 9; + std::vector expected = {0.0, 1.0 / 3, 2.0 / 3, 1.0, 4.0 / 3, 5.0 / 3, 2.0, 2.5, 3.0}; + + auto result = fill_bases(x, min_points); + + EXPECT_EQ(result.size(), min_points); + + for (size_t i = 0; i < min_points; ++i) { + EXPECT_NEAR(result[i], expected[i], 1e-6); + } +} + +TEST(TestHelpers, crop_bases) +{ + using autoware::trajectory::detail::crop_bases; + + std::vector x = {0.0, 1.0, 2.0, 3.0, 4.0}; + double start = 1.5; + double end = 3.5; + + std::vector expected = {1.5, 2.0, 3.0, 3.5}; + + auto result = crop_bases(x, start, end); + + EXPECT_EQ(result.size(), expected.size()); + + for (size_t i = 0; i < expected.size(); ++i) { + EXPECT_NEAR(result[i], expected[i], 1e-6); + } +} diff --git a/common/autoware_trajectory/test/test_trajectory_container.cpp b/common/autoware_trajectory/test/test_trajectory_container.cpp index 6f6d52107b348..6ad90b1a70fd6 100644 --- a/common/autoware_trajectory/test/test_trajectory_container.cpp +++ b/common/autoware_trajectory/test/test_trajectory_container.cpp @@ -12,10 +12,13 @@ // See the License for the specific language governing permissions and // limitations under the License. #include "autoware/trajectory/path_point_with_lane_id.hpp" +#include "autoware/trajectory/utils/closest.hpp" +#include "autoware/trajectory/utils/crossed.hpp" +#include "autoware/trajectory/utils/find_intervals.hpp" +#include "lanelet2_core/primitives/LineString.h" #include -#include #include using Trajectory = autoware::trajectory::Trajectory; @@ -70,7 +73,8 @@ TEST_F(TrajectoryTest, compute) { double length = trajectory->length(); - trajectory->longitudinal_velocity_mps.range(trajectory->length() / 3.0, trajectory->length()) + trajectory->longitudinal_velocity_mps() + .range(trajectory->length() / 3.0, trajectory->length()) .set(10.0); auto point = trajectory->compute(length / 2.0); @@ -85,8 +89,8 @@ TEST_F(TrajectoryTest, compute) TEST_F(TrajectoryTest, manipulate_velocity) { - trajectory->longitudinal_velocity_mps = 10.0; - trajectory->longitudinal_velocity_mps + trajectory->longitudinal_velocity_mps() = 10.0; + trajectory->longitudinal_velocity_mps() .range(trajectory->length() / 3, 2.0 * trajectory->length() / 3) .set(5.0); auto point1 = trajectory->compute(0.0); @@ -115,43 +119,22 @@ TEST_F(TrajectoryTest, curvature) TEST_F(TrajectoryTest, restore) { using autoware::trajectory::Trajectory; - trajectory->longitudinal_velocity_mps.range(4.0, trajectory->length()).set(5.0); - { - auto points = static_cast &>(*trajectory).restore(0); - EXPECT_EQ(10, points.size()); - } - - { - auto points = static_cast &>(*trajectory).restore(0); - EXPECT_EQ(10, points.size()); - } - - { - auto points = - static_cast &>(*trajectory).restore(0); - EXPECT_EQ(11, points.size()); - } - - { - auto points = trajectory->restore(0); - EXPECT_EQ(11, points.size()); - } + trajectory->longitudinal_velocity_mps().range(4.0, trajectory->length()).set(5.0); + auto points = trajectory->restore(0); + EXPECT_EQ(11, points.size()); } TEST_F(TrajectoryTest, crossed) { - geometry_msgs::msg::Pose pose1; - pose1.position.x = 0.0; - pose1.position.y = 10.0; - geometry_msgs::msg::Pose pose2; - pose2.position.x = 10.0; - pose2.position.y = 0.0; - - auto crossed_point = trajectory->crossed(pose1, pose2); - EXPECT_TRUE(crossed_point.has_value()); - - EXPECT_LT(0.0, *crossed_point); - EXPECT_LT(*crossed_point, trajectory->length()); + lanelet::LineString2d line_string; + line_string.push_back(lanelet::Point3d(lanelet::InvalId, 0.0, 10.0, 0.0)); + line_string.push_back(lanelet::Point3d(lanelet::InvalId, 10.0, 0.0, 0.0)); + + auto crossed_point = autoware::trajectory::crossed(*trajectory, line_string); + ASSERT_EQ(crossed_point.size(), 1); + + EXPECT_LT(0.0, crossed_point.at(0)); + EXPECT_LT(crossed_point.at(0), trajectory->length()); } TEST_F(TrajectoryTest, closest) @@ -160,9 +143,7 @@ TEST_F(TrajectoryTest, closest) pose.position.x = 5.0; pose.position.y = 5.0; - std::cerr << "Closest: " << trajectory->closest(pose) << std::endl; - - auto closest_pose = trajectory->compute(trajectory->closest(pose)); + auto closest_pose = trajectory->compute(autoware::trajectory::closest(*trajectory, pose)); double distance = std::hypot( closest_pose.point.pose.position.x - pose.position.x, @@ -193,3 +174,15 @@ TEST_F(TrajectoryTest, crop) EXPECT_EQ(end_point_expect.point.pose.position.y, end_point_actual.point.pose.position.y); EXPECT_EQ(end_point_expect.lane_ids[0], end_point_actual.lane_ids[0]); } + +TEST_F(TrajectoryTest, find_interval) +{ + auto intervals = autoware::trajectory::find_intervals( + *trajectory, [](const tier4_planning_msgs::msg::PathPointWithLaneId & point) { + return point.lane_ids[0] == 1; + }); + EXPECT_EQ(intervals.size(), 1); + EXPECT_LT(0, intervals[0].start); + EXPECT_LT(intervals[0].start, intervals[0].end); + EXPECT_NEAR(intervals[0].end, trajectory->length(), 0.1); +} diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_stop_line_module/src/scene.cpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_stop_line_module/src/scene.cpp index 2115a0a7eca6e..7b4f8af8c1dd8 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_stop_line_module/src/scene.cpp +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_stop_line_module/src/scene.cpp @@ -15,6 +15,8 @@ #include "scene.hpp" #include "autoware/behavior_velocity_planner_common/utilization/util.hpp" +#include "autoware/trajectory/utils/closest.hpp" +#include "autoware/trajectory/utils/crossed.hpp" #include @@ -30,8 +32,8 @@ namespace autoware::behavior_velocity_planner StopLineModule::StopLineModule( const int64_t module_id, lanelet::ConstLineString3d stop_line, const PlannerParam & planner_param, const rclcpp::Logger & logger, const rclcpp::Clock::SharedPtr clock, - const std::shared_ptr time_keeper, - const std::shared_ptr + const std::shared_ptr & time_keeper, + const std::shared_ptr & planning_factor_interface) : SceneModuleInterface(module_id, logger, clock, time_keeper, planning_factor_interface), stop_line_(std::move(stop_line)), @@ -58,7 +60,7 @@ bool StopLineModule::modifyPathVelocity(PathWithLaneId * path) return true; } - trajectory->longitudinal_velocity_mps.range(*stop_point, trajectory->length()).set(0.0); + trajectory->longitudinal_velocity_mps().range(*stop_point, trajectory->length()).set(0.0); path->points = trajectory->restore(); @@ -83,7 +85,7 @@ std::pair> StopLineModule::getEgoAndStopPoint( const Trajectory & trajectory, const geometry_msgs::msg::Pose & ego_pose, const State & state) const { - const double ego_s = trajectory.closest(ego_pose.position); + const double ego_s = autoware::trajectory::closest(trajectory, ego_pose); std::optional stop_point_s; switch (state) { @@ -94,16 +96,16 @@ std::pair> StopLineModule::getEgoAndStopPoint( // Calculate intersection with stop line const auto trajectory_stop_line_intersection = - trajectory.crossed(stop_line.front(), stop_line.back()); + autoware::trajectory::crossed(trajectory, stop_line); // If no collision found, do nothing - if (!trajectory_stop_line_intersection) { + if (trajectory_stop_line_intersection.size() == 0) { stop_point_s = std::nullopt; break; } stop_point_s = - *trajectory_stop_line_intersection - + trajectory_stop_line_intersection.at(0) - (base_link2front + planner_param_.stop_margin); // consider vehicle length and stop margin if (*stop_point_s < 0.0) { diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_stop_line_module/src/scene.hpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_stop_line_module/src/scene.hpp index d2c999a377ab4..fd49f4a576bd7 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_stop_line_module/src/scene.hpp +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_stop_line_module/src/scene.hpp @@ -64,13 +64,15 @@ class StopLineModule : public SceneModuleInterface * @param planner_param Planning parameters. * @param logger Logger for output messages. * @param clock Shared clock instance. + * @param time_keeper Time keeper for the module. + * @param planning_factor_interface Planning factor interface. */ StopLineModule( const int64_t module_id, lanelet::ConstLineString3d stop_line, const PlannerParam & planner_param, const rclcpp::Logger & logger, const rclcpp::Clock::SharedPtr clock, - const std::shared_ptr time_keeper, - const std::shared_ptr + const std::shared_ptr & time_keeper, + const std::shared_ptr & planning_factor_interface); bool modifyPathVelocity(PathWithLaneId * path) override; From 13abbd9cc066b96423324bb403fcba362fb9ff0a Mon Sep 17 00:00:00 2001 From: "Y.Hisaki" Date: Wed, 29 Jan 2025 12:16:01 +0900 Subject: [PATCH 2/5] add const Signed-off-by: Y.Hisaki --- .../detail/interpolator_common_interface.hpp | 2 +- .../detail/interpolator_mixin.hpp | 2 +- .../trajectory/interpolator/interpolator.hpp | 4 +-- .../autoware/trajectory/utils/shift.hpp | 2 +- .../autoware_trajectory/src/detail/util.cpp | 2 +- .../src/interpolator/spherical_linear.cpp | 4 +-- common/autoware_trajectory/src/point.cpp | 18 ++++++------- common/autoware_trajectory/src/pose.cpp | 26 ++++++++++--------- .../autoware_trajectory/src/utils/closest.cpp | 12 ++++----- .../autoware_trajectory/src/utils/crossed.cpp | 11 ++++---- .../autoware_trajectory/src/utils/shift.cpp | 19 +++++++------- 11 files changed, 53 insertions(+), 49 deletions(-) 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 5c8a233c9121c..df18c65e0520b 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 @@ -167,7 +167,7 @@ class InterpolatorCommonInterface throw std::runtime_error( "Interpolator has not been built."); // This Exception should not be thrown. } - double clamped_s = validate_compute_input(s); + 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 c9456ecf8a020..98e46e5467a89 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 @@ -72,7 +72,7 @@ struct InterpolatorMixin : public InterpolatorInterface [[nodiscard]] std::optional build(Args &&... args) { auto interpolator = InterpolatorType(std::forward(args)...); - bool success = interpolator.build(bases_, values_); + const bool success = interpolator.build(bases_, values_); if (!success) { return std::nullopt; } diff --git a/common/autoware_trajectory/include/autoware/trajectory/interpolator/interpolator.hpp b/common/autoware_trajectory/include/autoware/trajectory/interpolator/interpolator.hpp index 86dbbe8cdbde4..1f207822e23a0 100644 --- a/common/autoware_trajectory/include/autoware/trajectory/interpolator/interpolator.hpp +++ b/common/autoware_trajectory/include/autoware/trajectory/interpolator/interpolator.hpp @@ -73,7 +73,7 @@ class InterpolatorInterface : public detail::InterpolatorCommonInterface */ [[nodiscard]] double compute_first_derivative(const double & s) const { - double clamped_s = this->validate_compute_input(s); + const double clamped_s = this->validate_compute_input(s); return compute_first_derivative_impl(clamped_s); } @@ -85,7 +85,7 @@ class InterpolatorInterface : public detail::InterpolatorCommonInterface */ [[nodiscard]] double compute_second_derivative(const double & s) const { - double clamped_s = this->validate_compute_input(s); + const double clamped_s = this->validate_compute_input(s); return compute_second_derivative_impl(clamped_s); } diff --git a/common/autoware_trajectory/include/autoware/trajectory/utils/shift.hpp b/common/autoware_trajectory/include/autoware/trajectory/utils/shift.hpp index d1457d8ba3a0d..f709cd0e44e43 100644 --- a/common/autoware_trajectory/include/autoware/trajectory/utils/shift.hpp +++ b/common/autoware_trajectory/include/autoware/trajectory/utils/shift.hpp @@ -85,7 +85,7 @@ trajectory::Trajectory shift( for (size_t i = 0; i < bases.size(); ++i) { shifted_points.emplace_back(reference_trajectory.compute(bases.at(i))); double azimuth = reference_trajectory.azimuth(bases.at(i)); - double shift_length = shift_lengths.at(i); + const double shift_length = shift_lengths.at(i); detail::to_point(shifted_points.back()).x += std::sin(azimuth) * shift_length; detail::to_point(shifted_points.back()).y -= std::cos(azimuth) * shift_length; } diff --git a/common/autoware_trajectory/src/detail/util.cpp b/common/autoware_trajectory/src/detail/util.cpp index f491740b15994..58b54cb5ef76b 100644 --- a/common/autoware_trajectory/src/detail/util.cpp +++ b/common/autoware_trajectory/src/detail/util.cpp @@ -66,7 +66,7 @@ std::vector crop_bases( } // Copy all points within the range [start, end] - for (double i : x) { + for (const double i : x) { if (i >= start && i <= end) { result.push_back(i); } diff --git a/common/autoware_trajectory/src/interpolator/spherical_linear.cpp b/common/autoware_trajectory/src/interpolator/spherical_linear.cpp index ed3ba62031185..f255135ae898b 100644 --- a/common/autoware_trajectory/src/interpolator/spherical_linear.cpp +++ b/common/autoware_trajectory/src/interpolator/spherical_linear.cpp @@ -41,8 +41,8 @@ geometry_msgs::msg::Quaternion SphericalLinear::compute_impl(const double & s) c const double t = (s - x0) / (x1 - x0); // Convert quaternions to Eigen vectors for calculation - Eigen::Quaterniond q0(y0.w, y0.x, y0.y, y0.z); - Eigen::Quaterniond q1(y1.w, y1.x, y1.y, y1.z); + const Eigen::Quaterniond q0(y0.w, y0.x, y0.y, y0.z); + const Eigen::Quaterniond q1(y1.w, y1.x, y1.y, y1.z); // Perform Slerp Eigen::Quaterniond q_slerp = q0.slerp(t, q1); diff --git a/common/autoware_trajectory/src/point.cpp b/common/autoware_trajectory/src/point.cpp index 017c5691ce44b..405bc7e7a2733 100644 --- a/common/autoware_trajectory/src/point.cpp +++ b/common/autoware_trajectory/src/point.cpp @@ -75,8 +75,8 @@ bool Trajectory::build(const std::vector & points) zs.emplace_back(points[0].z); for (size_t i = 1; i < points.size(); ++i) { - Eigen::Vector2d p0(points[i - 1].x, points[i - 1].y); - Eigen::Vector2d p1(points[i].x, points[i].y); + const Eigen::Vector2d p0(points[i - 1].x, points[i - 1].y); + const Eigen::Vector2d p1(points[i].x, points[i].y); bases_.emplace_back(bases_.back() + (p1 - p0).norm()); xs.emplace_back(points[i].x); ys.emplace_back(points[i].y); @@ -130,25 +130,25 @@ PointType Trajectory::compute(double s) const double Trajectory::azimuth(double s) const { s = clamp(s, true); - double dx = x_interpolator_->compute_first_derivative(s); - double dy = y_interpolator_->compute_first_derivative(s); + const double dx = x_interpolator_->compute_first_derivative(s); + const double dy = y_interpolator_->compute_first_derivative(s); return std::atan2(dy, dx); } double Trajectory::elevation(double s) const { s = clamp(s, true); - double dz = z_interpolator_->compute_first_derivative(s); + const double dz = z_interpolator_->compute_first_derivative(s); return std::atan2(dz, 1.0); } double Trajectory::curvature(double s) const { s = clamp(s, true); - double dx = x_interpolator_->compute_first_derivative(s); - double ddx = x_interpolator_->compute_second_derivative(s); - double dy = y_interpolator_->compute_first_derivative(s); - double ddy = y_interpolator_->compute_second_derivative(s); + 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); return std::abs(dx * ddy - dy * ddx) / std::pow(dx * dx + dy * dy, 1.5); } diff --git a/common/autoware_trajectory/src/pose.cpp b/common/autoware_trajectory/src/pose.cpp index 1d3891a95f6c6..0dc3287769aea 100644 --- a/common/autoware_trajectory/src/pose.cpp +++ b/common/autoware_trajectory/src/pose.cpp @@ -83,24 +83,26 @@ void Trajectory::align_orientation_with_trajectory_direction() { std::vector aligned_orientations; for (const auto & s : bases_) { - double azimuth = this->azimuth(s); - double elevation = this->elevation(s); - geometry_msgs::msg::Quaternion current_orientation = orientation_interpolator_->compute(s); + const double azimuth = this->azimuth(s); + const double elevation = this->elevation(s); + const geometry_msgs::msg::Quaternion current_orientation = + orientation_interpolator_->compute(s); tf2::Quaternion current_orientation_tf2( current_orientation.x, current_orientation.y, current_orientation.z, current_orientation.w); current_orientation_tf2.normalize(); - tf2::Vector3 x_axis(1.0, 0.0, 0.0); - tf2::Vector3 current_x_axis = tf2::quatRotate(current_orientation_tf2, x_axis); + const tf2::Vector3 x_axis(1.0, 0.0, 0.0); + const tf2::Vector3 current_x_axis = tf2::quatRotate(current_orientation_tf2, x_axis); - tf2::Vector3 desired_x_axis( + const tf2::Vector3 desired_x_axis( std::cos(elevation) * std::cos(azimuth), std::cos(elevation) * std::sin(azimuth), std::sin(elevation)); - tf2::Vector3 rotation_axis = current_x_axis.cross(desired_x_axis).normalized(); - double dot_product = current_x_axis.dot(desired_x_axis); - double rotation_angle = std::acos(dot_product); + const tf2::Vector3 rotation_axis = current_x_axis.cross(desired_x_axis).normalized(); + const double dot_product = current_x_axis.dot(desired_x_axis); + const double rotation_angle = std::acos(dot_product); - tf2::Quaternion delta_q(rotation_axis, rotation_angle); - tf2::Quaternion aligned_orientation_tf2 = (delta_q * current_orientation_tf2).normalized(); + const tf2::Quaternion delta_q(rotation_axis, rotation_angle); + const tf2::Quaternion aligned_orientation_tf2 = + (delta_q * current_orientation_tf2).normalized(); geometry_msgs::msg::Quaternion aligned_orientation; aligned_orientation.x = aligned_orientation_tf2.x(); @@ -110,7 +112,7 @@ void Trajectory::align_orientation_with_trajectory_direction() aligned_orientations.emplace_back(aligned_orientation); } - bool success = orientation_interpolator_->build(bases_, aligned_orientations); + const bool success = orientation_interpolator_->build(bases_, 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/utils/closest.cpp b/common/autoware_trajectory/src/utils/closest.cpp index d51fb26a9d188..76537f7ac122f 100644 --- a/common/autoware_trajectory/src/utils/closest.cpp +++ b/common/autoware_trajectory/src/utils/closest.cpp @@ -29,13 +29,13 @@ std::optional closest_with_constraint_impl( std::vector lengths_from_start_points; for (size_t i = 1; i < bases.size(); ++i) { - Eigen::Vector2d p0 = trajectory_compute(bases.at(i - 1)); - Eigen::Vector2d p1 = trajectory_compute(bases.at(i)); + const Eigen::Vector2d p0 = trajectory_compute(bases.at(i - 1)); + const Eigen::Vector2d p1 = trajectory_compute(bases.at(i)); - Eigen::Vector2d v = p1 - p0; - Eigen::Vector2d w = point - p0; - double c1 = w.dot(v); - double c2 = v.dot(v); + const Eigen::Vector2d v = p1 - p0; + const Eigen::Vector2d w = point - p0; + const double c1 = w.dot(v); + const double c2 = v.dot(v); double length_from_start_point = NAN; double distance_from_segment = NAN; if (c1 <= 0) { diff --git a/common/autoware_trajectory/src/utils/crossed.cpp b/common/autoware_trajectory/src/utils/crossed.cpp index 44419f1598b2c..f0052ee9e6164 100644 --- a/common/autoware_trajectory/src/utils/crossed.cpp +++ b/common/autoware_trajectory/src/utils/crossed.cpp @@ -29,12 +29,12 @@ std::optional crossed_with_constraint_impl( Eigen::Vector2d line_dir = line_end - line_start; for (size_t i = 1; i < bases.size(); ++i) { - Eigen::Vector2d p0 = trajectory_compute(bases.at(i - 1)); - Eigen::Vector2d p1 = trajectory_compute(bases.at(i)); + const Eigen::Vector2d p0 = trajectory_compute(bases.at(i - 1)); + const Eigen::Vector2d p1 = trajectory_compute(bases.at(i)); Eigen::Vector2d segment_dir = p1 - p0; - double det = segment_dir.x() * line_dir.y() - segment_dir.y() * line_dir.x(); + const double det = segment_dir.x() * line_dir.y() - segment_dir.y() * line_dir.x(); if (std::abs(det) < 1e-10) { continue; @@ -42,8 +42,9 @@ std::optional crossed_with_constraint_impl( Eigen::Vector2d p0_to_line_start = line_start - p0; - double t = (p0_to_line_start.x() * line_dir.y() - p0_to_line_start.y() * line_dir.x()) / det; - double u = + const double t = + (p0_to_line_start.x() * line_dir.y() - p0_to_line_start.y() * line_dir.x()) / det; + const double u = (p0_to_line_start.x() * segment_dir.y() - p0_to_line_start.y() * segment_dir.x()) / det; if (t >= 0.0 && t <= 1.0 && u >= 0.0 && u <= 1.0) { diff --git a/common/autoware_trajectory/src/utils/shift.cpp b/common/autoware_trajectory/src/utils/shift.cpp index c17abfbbfb4ee..bc08606952ebb 100644 --- a/common/autoware_trajectory/src/utils/shift.cpp +++ b/common/autoware_trajectory/src/utils/shift.cpp @@ -35,11 +35,11 @@ std::pair, std::vector> get_base_lengths_without_acc const double total_shift_length = shift_length; // Prepare base longitudinal positions - std::vector base_longitudinal = { + const std::vector base_longitudinal = { 0.0, 0.25 * total_arc_length, 0.75 * total_arc_length, total_arc_length}; // Prepare base lateral positions - std::vector base_lateral = { + const std::vector base_lateral = { 0.0, 1.0 / 12.0 * total_shift_length, 11.0 / 12.0 * total_shift_length, total_shift_length}; return {base_longitudinal, base_lateral}; @@ -70,10 +70,10 @@ std::pair, std::vector> get_base_lengths_without_acc const double segment_s2 = std::min(segment_s1 + 2.0 * v1 * t + 2.0 * a * t * t, total_arc_length); // Prepare base longitudinal positions - std::vector base_longitudinal = {0.0, segment_s1, segment_s2, total_arc_length}; + const std::vector base_longitudinal = {0.0, segment_s1, segment_s2, total_arc_length}; // Prepare base lateral positions (simple division approach as original) - std::vector base_lateral = { + const std::vector base_lateral = { 0.0, 1.0 / 12.0 * total_shift_length, 11.0 / 12.0 * total_shift_length, total_shift_length}; return {base_longitudinal, base_lateral}; @@ -216,9 +216,10 @@ std::pair, std::vector> calc_base_lengths( lateral_jerk * accel_time2_jerk); // Construct the output vectors - std::vector base_lon = {0.0, segment_s1, segment_s2, segment_s3, - segment_s5, segment_s6, segment_s7}; - std::vector base_lat = {0.0, shift_l1, shift_l2, shift_l3, shift_l5, shift_l6, shift_l7}; + const std::vector base_lon = {0.0, segment_s1, segment_s2, segment_s3, + segment_s5, segment_s6, segment_s7}; + const std::vector base_lat = {0.0, shift_l1, shift_l2, shift_l3, + shift_l5, shift_l6, shift_l7}; return {base_lon, base_lat}; } @@ -235,8 +236,8 @@ void shift_impl( return; } - double shift_arc_length = std::abs(shift_interval.end - shift_interval.start); - bool shift_direction = shift_interval.end > shift_interval.start; + const double shift_arc_length = std::abs(shift_interval.end - shift_interval.start); + const bool shift_direction = shift_interval.end > shift_interval.start; // Calculate base lengths auto [base_lon, base_lat] = calc_base_lengths( shift_arc_length, // From 3c895e78e000081b2907e0c7449c76d65f64171b Mon Sep 17 00:00:00 2001 From: Yukinari Hisaki <42021302+yhisaki@users.noreply.github.com> Date: Wed, 29 Jan 2025 14:22:39 +0900 Subject: [PATCH 3/5] Update shift.hpp Signed-off-by: Y.Hisaki --- .../include/autoware/trajectory/utils/shift.hpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/common/autoware_trajectory/include/autoware/trajectory/utils/shift.hpp b/common/autoware_trajectory/include/autoware/trajectory/utils/shift.hpp index f709cd0e44e43..e561b2b1e99a7 100644 --- a/common/autoware_trajectory/include/autoware/trajectory/utils/shift.hpp +++ b/common/autoware_trajectory/include/autoware/trajectory/utils/shift.hpp @@ -90,7 +90,7 @@ trajectory::Trajectory shift( detail::to_point(shifted_points.back()).y -= std::cos(azimuth) * shift_length; } auto shifted_trajectory = reference_trajectory; - bool valid = shifted_trajectory.build(shifted_points); + const bool valid = shifted_trajectory.build(shifted_points); if (!valid) { throw std::runtime_error( "Failed to build shifted trajectory"); // This Exception should not be thrown. From d6faa1c8535999cb0d59a02b99954b7a3d6da550 Mon Sep 17 00:00:00 2001 From: "Y.Hisaki" Date: Wed, 29 Jan 2025 14:59:41 +0900 Subject: [PATCH 4/5] fix names Signed-off-by: Y.Hisaki --- common/autoware_trajectory/examples/example_shift.cpp | 10 +++++----- .../autoware/trajectory/detail/interpolated_array.hpp | 8 +++++--- .../include/autoware/trajectory/utils/shift.hpp | 6 +++--- common/autoware_trajectory/src/utils/shift.cpp | 6 +++--- 4 files changed, 16 insertions(+), 14 deletions(-) diff --git a/common/autoware_trajectory/examples/example_shift.cpp b/common/autoware_trajectory/examples/example_shift.cpp index c4f4a30dd439d..9d04696f78f53 100644 --- a/common/autoware_trajectory/examples/example_shift.cpp +++ b/common/autoware_trajectory/examples/example_shift.cpp @@ -67,7 +67,7 @@ int main() autoware::trajectory::ShiftInterval shift_interval; shift_interval.end = -1.0; - shift_interval.length = 0.5; + shift_interval.lateral_offset = 0.5; auto shifted_trajectory = autoware::trajectory::shift(*trajectory, shift_interval); @@ -99,7 +99,7 @@ int main() autoware::trajectory::ShiftInterval shift_interval; shift_interval.start = trajectory->length() / 4.0; shift_interval.end = trajectory->length() * 3.0 / 4.0; - shift_interval.length = 0.5; + shift_interval.lateral_offset = 0.5; auto shifted_trajectory = autoware::trajectory::shift(*trajectory, shift_interval); for (double s = 0.0; s < shifted_trajectory.length(); s += 0.01) { @@ -130,7 +130,7 @@ int main() autoware::trajectory::ShiftInterval shift_interval; shift_interval.start = trajectory->length() * 3.0 / 4.0; shift_interval.end = trajectory->length() / 4.0; - shift_interval.length = 0.5; + shift_interval.lateral_offset = 0.5; auto shifted_trajectory = autoware::trajectory::shift(*trajectory, shift_interval); for (double s = 0.0; s < shifted_trajectory.length(); s += 0.01) { @@ -161,12 +161,12 @@ int main() autoware::trajectory::ShiftInterval shift_interval1; shift_interval1.start = trajectory->length() / 4.0; shift_interval1.end = trajectory->length() * 2.0 / 4.0; - shift_interval1.length = 0.5; + shift_interval1.lateral_offset = 0.5; autoware::trajectory::ShiftInterval shift_interval2; shift_interval2.start = trajectory->length() * 2.0 / 4.0; shift_interval2.end = trajectory->length() * 3.0 / 4.0; - shift_interval2.length = -0.5; + shift_interval2.lateral_offset = -0.5; auto shifted_trajectory = autoware::trajectory::shift(*trajectory, {shift_interval1, shift_interval2}); 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 fcea47e703f38..06f1185620692 100644 --- a/common/autoware_trajectory/include/autoware/trajectory/detail/interpolated_array.hpp +++ b/common/autoware_trajectory/include/autoware/trajectory/detail/interpolated_array.hpp @@ -84,9 +84,11 @@ class InterpolatedArray */ InterpolatedArray & operator=(const InterpolatedArray & other) { - bases_ = other.bases_; - values_ = other.values_; - interpolator_ = other.interpolator_->clone(); + if (this != &other) { + bases_ = other.bases_; + values_ = other.values_; + interpolator_ = other.interpolator_->clone(); + } return *this; } diff --git a/common/autoware_trajectory/include/autoware/trajectory/utils/shift.hpp b/common/autoware_trajectory/include/autoware/trajectory/utils/shift.hpp index e561b2b1e99a7..1fa7f2401d31d 100644 --- a/common/autoware_trajectory/include/autoware/trajectory/utils/shift.hpp +++ b/common/autoware_trajectory/include/autoware/trajectory/utils/shift.hpp @@ -29,9 +29,9 @@ namespace autoware::trajectory */ struct ShiftInterval { - double start{0.0}; ///< Start position of the shift interval. - double end{0.0}; ///< End position of the shift interval. - double length{0.0}; ///< Length of the shift to be applied. + double start{0.0}; ///< Start position of the shift interval. + double end{0.0}; ///< End position of the shift interval. + double lateral_offset{0.0}; ///< Length of the shift to be applied. }; /** diff --git a/common/autoware_trajectory/src/utils/shift.cpp b/common/autoware_trajectory/src/utils/shift.cpp index bc08606952ebb..f12e8df11f1f0 100644 --- a/common/autoware_trajectory/src/utils/shift.cpp +++ b/common/autoware_trajectory/src/utils/shift.cpp @@ -231,7 +231,7 @@ void shift_impl( // lateral shift if (shift_interval.end <= 0.0) { for (size_t i = 0; i < bases.size(); ++i) { - shift_lengths->at(i) += shift_interval.length; + shift_lengths->at(i) += shift_interval.lateral_offset; } return; } @@ -240,8 +240,8 @@ void shift_impl( const bool shift_direction = shift_interval.end > shift_interval.start; // Calculate base lengths auto [base_lon, base_lat] = calc_base_lengths( - shift_arc_length, // - shift_interval.length, // + shift_arc_length, // + shift_interval.lateral_offset, // shift_parameters); auto cubic_spline = From eb030483e2bd835f003bb155164f2e3d81cb339f Mon Sep 17 00:00:00 2001 From: "Y.Hisaki" Date: Thu, 30 Jan 2025 15:54:34 +0900 Subject: [PATCH 5/5] fix copyright Signed-off-by: Y.Hisaki --- .../autoware_trajectory/examples/example_find_intervals.cpp | 3 ++- common/autoware_trajectory/src/detail/logging.cpp | 6 +++--- 2 files changed, 5 insertions(+), 4 deletions(-) diff --git a/common/autoware_trajectory/examples/example_find_intervals.cpp b/common/autoware_trajectory/examples/example_find_intervals.cpp index 1148b1b062490..58eb52093f161 100644 --- a/common/autoware_trajectory/examples/example_find_intervals.cpp +++ b/common/autoware_trajectory/examples/example_find_intervals.cpp @@ -1,4 +1,4 @@ -// Copyright 2020 Tier IV, Inc. +// 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. @@ -11,6 +11,7 @@ // 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/path_point_with_lane_id.hpp" #include "autoware/trajectory/utils/find_intervals.hpp" diff --git a/common/autoware_trajectory/src/detail/logging.cpp b/common/autoware_trajectory/src/detail/logging.cpp index 97816ee83b0ef..3656a38fcc50a 100644 --- a/common/autoware_trajectory/src/detail/logging.cpp +++ b/common/autoware_trajectory/src/detail/logging.cpp @@ -6,9 +6,9 @@ // // 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． +// 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.