From a1397be0dc7e90d4253f20eae217e1676415c736 Mon Sep 17 00:00:00 2001 From: Daniel Sanchez Date: Mon, 3 Mar 2025 18:05:08 +0900 Subject: [PATCH] add tests Signed-off-by: Daniel Sanchez --- ..._trajectory_container_trajectory_point.cpp | 173 ++++++++++++++++++ 1 file changed, 173 insertions(+) create mode 100644 common/autoware_trajectory/test/test_trajectory_container_trajectory_point.cpp 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..a40ebe69a6 --- /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(TrajectoryCreatorTest, 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 TrajectoryTest : 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(TrajectoryTest, 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(TrajectoryTest, 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(TrajectoryTest, direction) +{ + double dir = trajectory->azimuth(0.0); + EXPECT_LT(0, dir); + EXPECT_LT(dir, M_PI / 2); +} + +TEST_F(TrajectoryTest, curvature) +{ + double curvature_val = trajectory->curvature(0.0); + EXPECT_LT(-1.0, curvature_val); + EXPECT_LT(curvature_val, 1.0); +} + +TEST_F(TrajectoryTest, 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(TrajectoryTest, 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(TrajectoryTest, 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(TrajectoryTest, 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(TrajectoryTest, max_curvature) +{ + double max_curvature = autoware::trajectory::max_curvature(*trajectory); + EXPECT_LT(0, max_curvature); +}