From 2e5c9e0d2d6e08d03e2a299bc3a3e9c55105e57d Mon Sep 17 00:00:00 2001 From: danielsanchezaran Date: Fri, 7 Mar 2025 16:26:15 +0900 Subject: [PATCH] feat(autoware_trajectory): add trajectory point (#233) * add TrajectoryPoint class to templates Signed-off-by: Daniel Sanchez * add tests Signed-off-by: Daniel Sanchez * add method to_point for TrajectoryPoint type Signed-off-by: Daniel * change name of test to avoid name collision Signed-off-by: Daniel * add missing items Signed-off-by: Y.Hisaki * rename example name for clarity Signed-off-by: Y.Hisaki --------- Signed-off-by: Daniel Sanchez Signed-off-by: Daniel Signed-off-by: Y.Hisaki Co-authored-by: Y.Hisaki --- ..._path_point.cpp => example_path_point.cpp} | 0 ...trajectory_point.cpp => example_point.cpp} | 0 ...e_trajectory_pose.cpp => example_pose.cpp} | 0 .../autoware/trajectory/detail/types.hpp | 4 + .../autoware/trajectory/trajectory_point.hpp | 221 ++++++++++++++++++ .../autoware_trajectory/src/detail/types.cpp | 12 + .../src/trajectory_point.cpp | 157 +++++++++++++ ..._trajectory_container_trajectory_point.cpp | 173 ++++++++++++++ 8 files changed, 567 insertions(+) rename common/autoware_trajectory/examples/{example_trajectory_path_point.cpp => example_path_point.cpp} (100%) rename common/autoware_trajectory/examples/{example_trajectory_point.cpp => example_point.cpp} (100%) rename common/autoware_trajectory/examples/{example_trajectory_pose.cpp => example_pose.cpp} (100%) create mode 100644 common/autoware_trajectory/include/autoware/trajectory/trajectory_point.hpp create mode 100644 common/autoware_trajectory/src/trajectory_point.cpp create mode 100644 common/autoware_trajectory/test/test_trajectory_container_trajectory_point.cpp diff --git a/common/autoware_trajectory/examples/example_trajectory_path_point.cpp b/common/autoware_trajectory/examples/example_path_point.cpp similarity index 100% rename from common/autoware_trajectory/examples/example_trajectory_path_point.cpp rename to common/autoware_trajectory/examples/example_path_point.cpp diff --git a/common/autoware_trajectory/examples/example_trajectory_point.cpp b/common/autoware_trajectory/examples/example_point.cpp similarity index 100% rename from common/autoware_trajectory/examples/example_trajectory_point.cpp rename to common/autoware_trajectory/examples/example_point.cpp diff --git a/common/autoware_trajectory/examples/example_trajectory_pose.cpp b/common/autoware_trajectory/examples/example_pose.cpp similarity index 100% rename from common/autoware_trajectory/examples/example_trajectory_pose.cpp rename to common/autoware_trajectory/examples/example_pose.cpp diff --git a/common/autoware_trajectory/include/autoware/trajectory/detail/types.hpp b/common/autoware_trajectory/include/autoware/trajectory/detail/types.hpp index f4150b7e37..dd64a7d3eb 100644 --- a/common/autoware_trajectory/include/autoware/trajectory/detail/types.hpp +++ b/common/autoware_trajectory/include/autoware/trajectory/detail/types.hpp @@ -20,7 +20,9 @@ #include #include +#include #include +#include #include #include @@ -63,12 +65,14 @@ struct ImmutablePoint3d : ImmutablePoint2d 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(autoware_planning_msgs::msg::TrajectoryPoint & p); MutablePoint3d to_point(autoware_internal_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 autoware_planning_msgs::msg::TrajectoryPoint & p); ImmutablePoint3d to_point(const autoware_internal_planning_msgs::msg::PathPointWithLaneId & p); ImmutablePoint2d to_point(const lanelet::BasicPoint2d & p); diff --git a/common/autoware_trajectory/include/autoware/trajectory/trajectory_point.hpp b/common/autoware_trajectory/include/autoware/trajectory/trajectory_point.hpp new file mode 100644 index 0000000000..e507c933b8 --- /dev/null +++ b/common/autoware_trajectory/include/autoware/trajectory/trajectory_point.hpp @@ -0,0 +1,221 @@ +// 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__TRAJECTORY_POINT_HPP_ +#define AUTOWARE__TRAJECTORY__TRAJECTORY_POINT_HPP_ + +#include "autoware/trajectory/detail/interpolated_array.hpp" +#include "autoware/trajectory/pose.hpp" + +#include + +#include +#include +#include + +namespace autoware::trajectory +{ +template <> +class Trajectory +: public Trajectory +{ + using BaseClass = Trajectory; + using PointType = autoware_planning_msgs::msg::TrajectoryPoint; + + 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}; + std::shared_ptr> + acceleration_mps2_; //!< Longitudinal acceleration in m/s^2} Warning, this is not used + std::shared_ptr> + front_wheel_angle_rad_; //!< Front wheel angle in rad} Warning, this is not used + std::shared_ptr> + rear_wheel_angle_rad_; //!< Rear wheel angle in rad} Warning, this is not used + +public: + 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_; } + + detail::InterpolatedArray & acceleration_mps2() { return *acceleration_mps2_; } + + detail::InterpolatedArray & front_wheel_angle_rad() { return *front_wheel_angle_rad_; } + + detail::InterpolatedArray & rear_wheel_angle_rad() { return *rear_wheel_angle_rad_; } + + [[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_; + } + + [[nodiscard]] const detail::InterpolatedArray & acceleration_mps2() const + { + return *acceleration_mps2_; + } + + [[nodiscard]] const detail::InterpolatedArray & front_wheel_angle_rad() const + { + return *front_wheel_angle_rad_; + } + + [[nodiscard]] const detail::InterpolatedArray & rear_wheel_angle_rad() const + { + return *rear_wheel_angle_rad_; + } + + /** + * @brief Build the trajectory from the points + * @param points Vector of points + * @return True if the build is successful + */ + bool build(const std::vector & points); + + /** + * @brief Compute the point on the trajectory at a given s value + * @param s Arc length + * @return Point on the trajectory + */ + [[nodiscard]] PointType compute(double s) const; + + /** + * @brief Restore the trajectory points + * @param min_points Minimum number of points + * @return Vector of points + */ + [[nodiscard]] std::vector restore(const size_t & min_points = 4) const; + + class Builder + { + private: + std::unique_ptr trajectory_; + + public: + Builder() : trajectory_(std::make_unique()) {} + + template + Builder & set_xy_interpolator(Args &&... args) + { + trajectory_->x_interpolator_ = + std::make_shared(std::forward(args)...); + trajectory_->y_interpolator_ = + std::make_shared(std::forward(args)...); + return *this; + } + + template + Builder & set_z_interpolator(Args &&... args) + { + trajectory_->z_interpolator_ = + std::make_shared(std::forward(args)...); + return *this; + } + + template + Builder & set_orientation_interpolator(Args &&... args) + { + trajectory_->orientation_interpolator_ = + std::make_shared(std::forward(args)...); + return *this; + } + + template + Builder & set_longitudinal_velocity_interpolator(Args &&... args) + { + trajectory_->longitudinal_velocity_mps_ = std::make_shared>( + std::make_shared(std::forward(args)...)); + return *this; + } + + template + Builder & set_lateral_velocity_interpolator(Args &&... args) + { + trajectory_->lateral_velocity_mps_ = std::make_shared>( + std::make_shared(std::forward(args)...)); + return *this; + } + + template + Builder & set_heading_rate_interpolator(Args &&... args) + { + trajectory_->heading_rate_rps_ = std::make_shared>( + std::make_shared(std::forward(args)...)); + return *this; + } + + template + Builder & set_acceleration_interpolator(Args &&... args) + { + trajectory_->acceleration_mps2_ = std::make_shared>( + std::make_shared(std::forward(args)...)); + return *this; + } + + template + Builder & set_front_wheel_angle_interpolator(Args &&... args) + { + trajectory_->front_wheel_angle_rad_ = std::make_shared>( + std::make_shared(std::forward(args)...)); + return *this; + } + + template + Builder & set_rear_wheel_angle_interpolator(Args &&... args) + { + trajectory_->rear_wheel_angle_rad_ = std::make_shared>( + std::make_shared(std::forward(args)...)); + return *this; + } + + std::optional build(const std::vector & points) + { + if (trajectory_->build(points)) { + auto result = std::make_optional(std::move(*trajectory_)); + trajectory_.reset(); + return result; + } + return std::nullopt; + } + }; +}; + +} // namespace autoware::trajectory + +#endif // AUTOWARE__TRAJECTORY__TRAJECTORY_POINT_HPP_ diff --git a/common/autoware_trajectory/src/detail/types.cpp b/common/autoware_trajectory/src/detail/types.cpp index 9c3174f608..dec4205f25 100644 --- a/common/autoware_trajectory/src/detail/types.cpp +++ b/common/autoware_trajectory/src/detail/types.cpp @@ -14,6 +14,8 @@ #include "autoware/trajectory/detail/types.hpp" +#include + namespace autoware::trajectory::detail { MutablePoint3d to_point(geometry_msgs::msg::Point & p) @@ -31,6 +33,11 @@ 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(autoware_planning_msgs::msg::TrajectoryPoint & p) +{ + return {p.pose.position.x, p.pose.position.y, p.pose.position.z}; +} + MutablePoint3d to_point(autoware_internal_planning_msgs::msg::PathPointWithLaneId & p) { return {p.point.pose.position.x, p.point.pose.position.y, p.point.pose.position.z}; @@ -56,6 +63,11 @@ 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 autoware_planning_msgs::msg::TrajectoryPoint & p) +{ + return {p.pose.position.x, p.pose.position.y, p.pose.position.z}; +} + ImmutablePoint3d to_point(const autoware_internal_planning_msgs::msg::PathPointWithLaneId & p) { return {p.point.pose.position.x, p.point.pose.position.y, p.point.pose.position.z}; diff --git a/common/autoware_trajectory/src/trajectory_point.cpp b/common/autoware_trajectory/src/trajectory_point.cpp new file mode 100644 index 0000000000..6cb49735db --- /dev/null +++ b/common/autoware_trajectory/src/trajectory_point.cpp @@ -0,0 +1,157 @@ +// 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/trajectory_point.hpp" + +#include "autoware/trajectory/detail/helpers.hpp" +#include "autoware/trajectory/detail/interpolated_array.hpp" +#include "autoware/trajectory/forward.hpp" +#include "autoware/trajectory/interpolator/stairstep.hpp" +#include "autoware/trajectory/pose.hpp" + +#include + +#include +#include + +namespace autoware::trajectory +{ + +using PointType = autoware_planning_msgs::msg::TrajectoryPoint; + +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>())), + acceleration_mps2_(std::make_shared>( + std::make_shared>())), + front_wheel_angle_rad_(std::make_shared>( + std::make_shared>())), + rear_wheel_angle_rad_(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_)), + acceleration_mps2_(std::make_shared>(*rhs.acceleration_mps2_)), + front_wheel_angle_rad_( + std::make_shared>(*rhs.front_wheel_angle_rad_)), + rear_wheel_angle_rad_( + std::make_shared>(*rhs.rear_wheel_angle_rad_)) +{ +} + +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_; + *acceleration_mps2_ = *rhs.acceleration_mps2_; + *front_wheel_angle_rad_ = *rhs.front_wheel_angle_rad_; + *rear_wheel_angle_rad_ = *rhs.rear_wheel_angle_rad_; + } + return *this; +} + +bool Trajectory::build(const std::vector & points) +{ + std::vector poses; + std::vector longitudinal_velocity_mps_values; + std::vector lateral_velocity_mps_values; + std::vector heading_rate_rps_values; + std::vector acceleration_mps2_values; + std::vector front_wheel_angle_rad_values; + std::vector rear_wheel_angle_rad_values; + + for (const auto & point : points) { + poses.emplace_back(point.pose); + longitudinal_velocity_mps_values.emplace_back(point.longitudinal_velocity_mps); + lateral_velocity_mps_values.emplace_back(point.lateral_velocity_mps); + heading_rate_rps_values.emplace_back(point.heading_rate_rps); + acceleration_mps2_values.emplace_back(point.acceleration_mps2); + front_wheel_angle_rad_values.emplace_back(point.front_wheel_angle_rad); + rear_wheel_angle_rad_values.emplace_back(point.rear_wheel_angle_rad); + } + + bool is_valid = true; + + is_valid &= Trajectory::build(poses); + is_valid &= this->longitudinal_velocity_mps().build(bases_, longitudinal_velocity_mps_values); + is_valid &= this->lateral_velocity_mps().build(bases_, lateral_velocity_mps_values); + is_valid &= this->heading_rate_rps().build(bases_, heading_rate_rps_values); + is_valid &= this->acceleration_mps2().build(bases_, acceleration_mps2_values); + is_valid &= this->front_wheel_angle_rad().build(bases_, front_wheel_angle_rad_values); + is_valid &= this->rear_wheel_angle_rad().build(bases_, rear_wheel_angle_rad_values); + + 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->acceleration_mps2()), + get_bases(this->front_wheel_angle_rad()), get_bases(this->rear_wheel_angle_rad())); + + 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.acceleration_mps2 = static_cast(this->acceleration_mps2().compute(s)); + result.front_wheel_angle_rad = static_cast(this->front_wheel_angle_rad().compute(s)); + result.rear_wheel_angle_rad = static_cast(this->rear_wheel_angle_rad().compute(s)); + return result; +} + +std::vector Trajectory::restore(const size_t & min_points) const +{ + 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) { + points.emplace_back(compute(s)); + } + return points; +} + +} // namespace autoware::trajectory diff --git a/common/autoware_trajectory/test/test_trajectory_container_trajectory_point.cpp b/common/autoware_trajectory/test/test_trajectory_container_trajectory_point.cpp new file mode 100644 index 0000000000..d4eacd4833 --- /dev/null +++ b/common/autoware_trajectory/test/test_trajectory_container_trajectory_point.cpp @@ -0,0 +1,173 @@ +// 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/trajectory_point.hpp" +#include "autoware/trajectory/utils/closest.hpp" +#include "autoware/trajectory/utils/crossed.hpp" +#include "autoware/trajectory/utils/curvature_utils.hpp" +#include "lanelet2_core/primitives/LineString.h" + +#include + +#include + +using Trajectory = autoware::trajectory::Trajectory; + +autoware_planning_msgs::msg::TrajectoryPoint trajectory_point(double x, double y) +{ + autoware_planning_msgs::msg::TrajectoryPoint point; + point.pose.position.x = x; + point.pose.position.y = y; + return point; +} + +TEST(TrajectoryCreatorTestForTrajectoryPoint, create) +{ + { + std::vector points{trajectory_point(0.00, 0.00)}; + auto trajectory = Trajectory::Builder{}.build(points); + ASSERT_TRUE(!trajectory); + } + { + std::vector points{ + trajectory_point(0.00, 0.00), trajectory_point(0.81, 1.68), trajectory_point(1.65, 2.98), + trajectory_point(3.30, 4.01)}; + auto trajectory = Trajectory::Builder{}.build(points); + ASSERT_TRUE(trajectory); + } +} + +class TrajectoryTestForTrajectoryPoint : public ::testing::Test +{ +public: + std::optional trajectory; + + void SetUp() override + { + std::vector points{ + trajectory_point(0.00, 0.00), trajectory_point(0.81, 1.68), trajectory_point(1.65, 2.98), + trajectory_point(3.30, 4.01), trajectory_point(4.70, 4.52), trajectory_point(6.49, 5.20), + trajectory_point(8.11, 6.07), trajectory_point(8.76, 7.23), trajectory_point(9.36, 8.74), + trajectory_point(10.0, 10.0)}; + + trajectory = Trajectory::Builder{}.build(points); + ASSERT_TRUE(trajectory); + } +}; + +TEST_F(TrajectoryTestForTrajectoryPoint, compute) +{ + double length = trajectory->length(); + + trajectory->longitudinal_velocity_mps() + .range(trajectory->length() / 3.0, trajectory->length()) + .set(10.0); + auto point = trajectory->compute(length / 2.0); + + EXPECT_LT(0, point.pose.position.x); + EXPECT_LT(point.pose.position.x, 10); + + EXPECT_LT(0, point.pose.position.y); + EXPECT_LT(point.pose.position.y, 10); +} + +TEST_F(TrajectoryTestForTrajectoryPoint, manipulate_velocity) +{ + 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); + auto point2 = trajectory->compute(trajectory->length() / 2.0); + auto point3 = trajectory->compute(trajectory->length()); + + EXPECT_EQ(10.0, point1.longitudinal_velocity_mps); + EXPECT_EQ(5.0, point2.longitudinal_velocity_mps); + EXPECT_EQ(10.0, point3.longitudinal_velocity_mps); +} + +TEST_F(TrajectoryTestForTrajectoryPoint, direction) +{ + double dir = trajectory->azimuth(0.0); + EXPECT_LT(0, dir); + EXPECT_LT(dir, M_PI / 2); +} + +TEST_F(TrajectoryTestForTrajectoryPoint, curvature) +{ + double curvature_val = trajectory->curvature(0.0); + EXPECT_LT(-1.0, curvature_val); + EXPECT_LT(curvature_val, 1.0); +} + +TEST_F(TrajectoryTestForTrajectoryPoint, restore) +{ + using autoware::trajectory::Trajectory; + trajectory->longitudinal_velocity_mps().range(4.0, trajectory->length()).set(5.0); + auto points = trajectory->restore(0); + EXPECT_EQ(11, points.size()); +} + +TEST_F(TrajectoryTestForTrajectoryPoint, crossed) +{ + 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(TrajectoryTestForTrajectoryPoint, closest) +{ + geometry_msgs::msg::Pose pose; + pose.position.x = 5.0; + pose.position.y = 5.0; + + auto closest_pose = trajectory->compute(autoware::trajectory::closest(*trajectory, pose)); + + double distance = std::hypot( + closest_pose.pose.position.x - pose.position.x, closest_pose.pose.position.y - pose.position.y); + + EXPECT_LT(distance, 3.0); +} + +TEST_F(TrajectoryTestForTrajectoryPoint, crop) +{ + double length = trajectory->length(); + + auto start_point_expect = trajectory->compute(length / 3.0); + auto end_point_expect = trajectory->compute(length / 3.0 + 1.0); + + trajectory->crop(length / 3.0, 1.0); + + EXPECT_EQ(trajectory->length(), 1.0); + + auto start_point_actual = trajectory->compute(0.0); + auto end_point_actual = trajectory->compute(trajectory->length()); + + EXPECT_EQ(start_point_expect.pose.position.x, start_point_actual.pose.position.x); + EXPECT_EQ(start_point_expect.pose.position.y, start_point_actual.pose.position.y); + + EXPECT_EQ(end_point_expect.pose.position.x, end_point_actual.pose.position.x); + EXPECT_EQ(end_point_expect.pose.position.y, end_point_actual.pose.position.y); +} + +TEST_F(TrajectoryTestForTrajectoryPoint, max_curvature) +{ + double max_curvature = autoware::trajectory::max_curvature(*trajectory); + EXPECT_LT(0, max_curvature); +}