Skip to content

Commit 3eafb9b

Browse files
go-sakayorigo-sakayori
authored
test(bpp_common): add test for parking departure utils (#9055)
* add test for parking departure utils Signed-off-by: Go Sakayori <gsakayori@gmail.com> * fix Signed-off-by: Go Sakayori <gsakayori@gmail.com> * fix typo Signed-off-by: Go Sakayori <gsakayori@gmail.com> * use EXPECT_DOUBLE_EQ instead of EXPECT_NEAR Signed-off-by: Go Sakayori <go.sakayori@tier4.jp> --------- Signed-off-by: Go Sakayori <gsakayori@gmail.com> Signed-off-by: Go Sakayori <go.sakayori@tier4.jp>
1 parent 9d78e00 commit 3eafb9b

File tree

2 files changed

+323
-0
lines changed

2 files changed

+323
-0
lines changed

‎planning/behavior_path_planner/autoware_behavior_path_planner_common/CMakeLists.txt

+8
Original file line numberDiff line numberDiff line change
@@ -58,6 +58,14 @@ if(BUILD_TESTING)
5858
${PROJECT_NAME}
5959
)
6060

61+
ament_add_ros_isolated_gmock(test_${PROJECT_NAME}_parking_departure
62+
test/test_parking_departure_utils.cpp
63+
)
64+
65+
target_link_libraries(test_${PROJECT_NAME}_parking_departure
66+
${PROJECT_NAME}
67+
)
68+
6169
ament_add_ros_isolated_gmock(test_${PROJECT_NAME}_turn_signal
6270
test/test_turn_signal.cpp
6371
)

‎planning/behavior_path_planner/autoware_behavior_path_planner_common/test/test_parking_departure_utils.cpp

+315
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,315 @@
1+
// Copyright 2024 Tier IV, Inc. All rights reserved.
2+
//
3+
// Licensed under the Apache License, Version 2.0 (the "License");
4+
// you may not use this file except in compliance with the License.
5+
// You may obtain a copy of the License at
6+
//
7+
// http://www.apache.org/licenses/LICENSE-2.0
8+
//
9+
// Unless required by applicable law or agreed to in writing, software
10+
// distributed under the License is distributed on an "AS IS" BASIS,
11+
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
12+
// See the License for the specific language governing permissions and
13+
// limitations under the License.
14+
15+
#include "autoware/behavior_path_planner_common/utils/parking_departure/utils.hpp"
16+
#include "autoware/behavior_path_planner_common/utils/path_safety_checker/path_safety_checker_parameters.hpp"
17+
18+
#include <ament_index_cpp/get_package_share_directory.hpp>
19+
#include <autoware_test_utils/autoware_test_utils.hpp>
20+
21+
#include <gmock/gmock.h>
22+
#include <gtest/gtest.h>
23+
#include <lanelet2_core/Forward.h>
24+
25+
#include <cstddef>
26+
27+
constexpr double epsilon = 1e-6;
28+
29+
using autoware::behavior_path_planner::PlannerData;
30+
using autoware_planning_msgs::msg::Trajectory;
31+
using tier4_planning_msgs::msg::PathPointWithLaneId;
32+
using tier4_planning_msgs::msg::PathWithLaneId;
33+
34+
using autoware::test_utils::generateTrajectory;
35+
36+
PathWithLaneId trajectory_to_path_with_lane_id(const Trajectory & trajectory)
37+
{
38+
PathWithLaneId path_with_lane_id;
39+
PathPointWithLaneId path_point_with_lane_id;
40+
for (const auto & point : trajectory.points) {
41+
path_point_with_lane_id.point.pose = point.pose;
42+
path_point_with_lane_id.point.lateral_velocity_mps = point.lateral_velocity_mps;
43+
path_point_with_lane_id.point.longitudinal_velocity_mps = point.longitudinal_velocity_mps;
44+
path_point_with_lane_id.point.heading_rate_rps = point.heading_rate_rps;
45+
path_with_lane_id.points.push_back(path_point_with_lane_id);
46+
}
47+
return path_with_lane_id;
48+
}
49+
50+
TEST(BehaviorPathPlanningParkingDepartureUtil, calcFeasibleDecelDistance)
51+
{
52+
using autoware::behavior_path_planner::utils::parking_departure::calcFeasibleDecelDistance;
53+
54+
auto data = std::make_shared<PlannerData>();
55+
double velocity = 2.0;
56+
double acceleration = 1.0;
57+
double acceleration_limit = 2.0;
58+
double jerk_limit = 1.0;
59+
double target_velocity = 3.0;
60+
61+
auto odometry = std::make_shared<nav_msgs::msg::Odometry>();
62+
odometry->pose.pose = autoware::test_utils::createPose(0.0, 0.0, 0.0, 0.0, 0.0, 0.0);
63+
odometry->twist.twist.linear.x = velocity;
64+
auto accel_with_covariance =
65+
std::make_shared<autoware::behavior_path_planner::AccelWithCovarianceStamped>();
66+
accel_with_covariance->accel.accel.linear.x = acceleration;
67+
data->self_odometry = odometry;
68+
data->self_acceleration = accel_with_covariance;
69+
auto planner_data = std::static_pointer_cast<const PlannerData>(data);
70+
71+
// condition: current velocity is slower than target velocity
72+
auto distance =
73+
calcFeasibleDecelDistance(planner_data, acceleration_limit, jerk_limit, target_velocity);
74+
ASSERT_TRUE(distance.has_value());
75+
EXPECT_DOUBLE_EQ(distance.value(), 0.0);
76+
77+
// condition: calculates deceleration distance
78+
velocity = 5.0;
79+
odometry->twist.twist.linear.x = velocity;
80+
data->self_odometry = odometry;
81+
planner_data = std::static_pointer_cast<const PlannerData>(data);
82+
distance =
83+
calcFeasibleDecelDistance(planner_data, acceleration_limit, jerk_limit, target_velocity);
84+
ASSERT_TRUE(distance.has_value());
85+
EXPECT_NEAR(distance.value(), 18.7730133, epsilon);
86+
87+
// condition: not valid condition
88+
velocity = 0.3;
89+
target_velocity = 0.0;
90+
acceleration = -1.5;
91+
jerk_limit = 0.25;
92+
acceleration_limit = 2.0;
93+
odometry->twist.twist.linear.x = velocity;
94+
accel_with_covariance->accel.accel.linear.x = acceleration;
95+
data->self_odometry = odometry;
96+
data->self_acceleration = accel_with_covariance;
97+
planner_data = std::static_pointer_cast<const PlannerData>(data);
98+
distance =
99+
calcFeasibleDecelDistance(planner_data, acceleration_limit, jerk_limit, target_velocity);
100+
ASSERT_FALSE(distance.has_value());
101+
}
102+
103+
TEST(BehaviorPathPlanningParkingDepartureUtil, modifyVelocityByDirection)
104+
{
105+
using autoware::behavior_path_planner::utils::parking_departure::modifyVelocityByDirection;
106+
107+
const double velocity = 3.0;
108+
const double target_velocity = 2.0;
109+
const double acceleration = 2.0;
110+
111+
std::vector<PathWithLaneId> paths;
112+
auto short_path = trajectory_to_path_with_lane_id(generateTrajectory<Trajectory>(1, 1.0));
113+
auto long_path =
114+
trajectory_to_path_with_lane_id(generateTrajectory<Trajectory>(10, 1.0, -velocity));
115+
auto reverse_path =
116+
trajectory_to_path_with_lane_id(generateTrajectory<Trajectory>(10, -1.0, velocity, -M_PI));
117+
118+
paths.push_back(short_path);
119+
paths.push_back(long_path);
120+
paths.push_back(reverse_path);
121+
122+
std::vector<std::pair<double, double>> terminal_vel_acc_pairs;
123+
terminal_vel_acc_pairs.emplace_back(0.5, 0.5);
124+
terminal_vel_acc_pairs.emplace_back(1.0, 1.0);
125+
terminal_vel_acc_pairs.emplace_back(1.5, 1.5);
126+
127+
modifyVelocityByDirection(paths, terminal_vel_acc_pairs, target_velocity, acceleration);
128+
129+
// condition: number of point less than 2
130+
EXPECT_DOUBLE_EQ(terminal_vel_acc_pairs.at(0).first, 0.0);
131+
EXPECT_DOUBLE_EQ(terminal_vel_acc_pairs.at(0).second, 0.0);
132+
133+
// condition: forward driving
134+
EXPECT_DOUBLE_EQ(terminal_vel_acc_pairs.at(1).first, target_velocity);
135+
EXPECT_DOUBLE_EQ(terminal_vel_acc_pairs.at(1).second, acceleration);
136+
for (const auto & point : paths.at(1).points) {
137+
if (point == paths.at(1).points.back())
138+
EXPECT_DOUBLE_EQ(point.point.longitudinal_velocity_mps, 0.0);
139+
else
140+
EXPECT_DOUBLE_EQ(point.point.longitudinal_velocity_mps, velocity);
141+
}
142+
143+
// condition: reverse driving
144+
EXPECT_DOUBLE_EQ(terminal_vel_acc_pairs.at(2).first, -target_velocity);
145+
EXPECT_DOUBLE_EQ(terminal_vel_acc_pairs.at(2).second, -acceleration);
146+
for (const auto & point : paths.at(2).points) {
147+
if (point == paths.at(2).points.back())
148+
EXPECT_DOUBLE_EQ(point.point.longitudinal_velocity_mps, 0.0);
149+
else
150+
EXPECT_DOUBLE_EQ(point.point.longitudinal_velocity_mps, -velocity);
151+
}
152+
}
153+
154+
TEST(BehaviorPathPlanningParkingDepartureUtil, updatePathProperty)
155+
{
156+
using autoware::behavior_path_planner::utils::parking_departure::EgoPredictedPathParams;
157+
using autoware::behavior_path_planner::utils::parking_departure::updatePathProperty;
158+
159+
auto params = std::make_shared<EgoPredictedPathParams>();
160+
params->min_acceleration = 1.0;
161+
params->acceleration = 1.5;
162+
params->max_velocity = 0.0;
163+
auto pair_terminal_velocity_and_accel = std::make_pair(3.0, 2.0);
164+
165+
updatePathProperty(params, pair_terminal_velocity_and_accel);
166+
EXPECT_DOUBLE_EQ(params->max_velocity, 3.0);
167+
EXPECT_DOUBLE_EQ(params->acceleration, 2.0);
168+
}
169+
170+
TEST(BehaviorPathPlanningParkingDepartureUtil, initializeCollisionCheckDebugMap)
171+
{
172+
using autoware::behavior_path_planner::utils::parking_departure::initializeCollisionCheckDebugMap;
173+
174+
autoware::behavior_path_planner::CollisionCheckDebugMap debug_map;
175+
auto uuid1 = autoware::universe_utils::toBoostUUID(autoware::universe_utils::generateUUID());
176+
autoware::behavior_path_planner::utils::path_safety_checker::CollisionCheckDebug debug_info;
177+
debug_map[uuid1] = debug_info;
178+
179+
initializeCollisionCheckDebugMap(debug_map);
180+
ASSERT_TRUE(debug_map.empty());
181+
}
182+
183+
TEST(BehaviorPathPlanningParkingDepartureUtil, getPairsTerminalVelocityAndAccel)
184+
{
185+
using autoware::behavior_path_planner::utils::parking_departure::getPairsTerminalVelocityAndAccel;
186+
187+
std::vector<std::pair<double, double>> pairs_terminal_velocity_and_accel;
188+
pairs_terminal_velocity_and_accel.emplace_back(2.0, 1.0);
189+
pairs_terminal_velocity_and_accel.emplace_back(0.05, -1.0);
190+
191+
// condition: current path idx exceeds pairs size
192+
auto pair = getPairsTerminalVelocityAndAccel(pairs_terminal_velocity_and_accel, 2);
193+
EXPECT_DOUBLE_EQ(pair.first, 0.0);
194+
EXPECT_DOUBLE_EQ(pair.first, 0.0);
195+
196+
// condition: get current idx pair
197+
pair = getPairsTerminalVelocityAndAccel(pairs_terminal_velocity_and_accel, 1);
198+
EXPECT_DOUBLE_EQ(pair.first, 0.05);
199+
EXPECT_DOUBLE_EQ(pair.second, -1.0);
200+
}
201+
202+
TEST(BehaviorPathPlanningParkingDepartureUtil, generateFeasibleStopPath)
203+
{
204+
using autoware::behavior_path_planner::utils::parking_departure::generateFeasibleStopPath;
205+
206+
auto data = std::make_shared<PlannerData>();
207+
double velocity = 0.3;
208+
double acceleration = -1.5;
209+
double maximum_jerk = 0.25;
210+
double maximum_deceleration = 2.0;
211+
212+
auto odometry = std::make_shared<nav_msgs::msg::Odometry>();
213+
odometry->pose.pose = autoware::test_utils::createPose(0.0, 0.0, 0.0, 0.0, 0.0, 0.0);
214+
odometry->twist.twist.linear.x = velocity;
215+
auto accel_with_covariance =
216+
std::make_shared<autoware::behavior_path_planner::AccelWithCovarianceStamped>();
217+
accel_with_covariance->accel.accel.linear.x = acceleration;
218+
data->self_odometry = odometry;
219+
data->self_acceleration = accel_with_covariance;
220+
auto planner_data = std::static_pointer_cast<const PlannerData>(data);
221+
222+
std::optional<geometry_msgs::msg::Pose> stop_pose;
223+
224+
// condition: empty path
225+
PathWithLaneId path;
226+
auto stop_path =
227+
generateFeasibleStopPath(path, planner_data, stop_pose, maximum_deceleration, maximum_jerk);
228+
EXPECT_FALSE(stop_path.has_value());
229+
230+
// condition: not valid condition for stop distance
231+
path = trajectory_to_path_with_lane_id(generateTrajectory<Trajectory>(10, 1.0, velocity));
232+
stop_path =
233+
generateFeasibleStopPath(path, planner_data, stop_pose, maximum_deceleration, maximum_jerk);
234+
EXPECT_FALSE(stop_path.has_value());
235+
236+
// condition: not valid condition for stop index
237+
velocity = 5.0;
238+
acceleration = 1.0;
239+
odometry->twist.twist.linear.x = velocity;
240+
accel_with_covariance->accel.accel.linear.x = acceleration;
241+
data->self_odometry = odometry;
242+
data->self_acceleration = accel_with_covariance;
243+
planner_data = std::static_pointer_cast<const PlannerData>(data);
244+
path = trajectory_to_path_with_lane_id(generateTrajectory<Trajectory>(10, 1.0, velocity));
245+
stop_path =
246+
generateFeasibleStopPath(path, planner_data, stop_pose, maximum_deceleration, maximum_jerk);
247+
EXPECT_FALSE(stop_path.has_value());
248+
249+
// condition: valid condition
250+
maximum_jerk = 5.0;
251+
maximum_deceleration = -3.0;
252+
stop_path =
253+
generateFeasibleStopPath(path, planner_data, stop_pose, maximum_deceleration, maximum_jerk);
254+
size_t i = 0;
255+
ASSERT_TRUE(stop_path.has_value());
256+
for (const auto & point : stop_path->points) {
257+
if (i < 7)
258+
EXPECT_DOUBLE_EQ(point.point.longitudinal_velocity_mps, velocity);
259+
else
260+
EXPECT_DOUBLE_EQ(point.point.longitudinal_velocity_mps, 0.0);
261+
i++;
262+
}
263+
}
264+
265+
TEST(BehaviorPathPlanningParkingDepartureUtil, calcEndArcLength)
266+
{
267+
using autoware::behavior_path_planner::utils::parking_departure::calcEndArcLength;
268+
269+
lanelet::LineString3d left_bound;
270+
lanelet::LineString3d right_bound;
271+
272+
left_bound.push_back(lanelet::Point3d{lanelet::InvalId, -1, -1});
273+
left_bound.push_back(lanelet::Point3d{lanelet::InvalId, 0, -1});
274+
left_bound.push_back(lanelet::Point3d{lanelet::InvalId, 1, -1});
275+
right_bound.push_back(lanelet::Point3d{lanelet::InvalId, -1, 1});
276+
right_bound.push_back(lanelet::Point3d{lanelet::InvalId, 0, 1});
277+
right_bound.push_back(lanelet::Point3d{lanelet::InvalId, 1, 1});
278+
lanelet::Lanelet lanelet{lanelet::InvalId, left_bound, right_bound};
279+
280+
lanelet::LineString3d centerline;
281+
centerline.push_back(lanelet::Point3d{lanelet::InvalId, -1, 0});
282+
centerline.push_back(lanelet::Point3d{lanelet::InvalId, 0, 0});
283+
centerline.push_back(lanelet::Point3d{lanelet::InvalId, 1, 0});
284+
lanelet.setCenterline(centerline);
285+
286+
lanelet::ConstLanelets road_lanes = {lanelet};
287+
288+
double s_start = 0.2;
289+
double forward_path_length = 0.1;
290+
auto goal_pose = autoware::test_utils::createPose(5.0, 5.0, 0.0, 0.0, 0.0, 0.0);
291+
292+
// condition: goal pose not in lanelets
293+
auto end_arc = calcEndArcLength(s_start, forward_path_length, road_lanes, goal_pose);
294+
EXPECT_DOUBLE_EQ(end_arc.first, s_start + forward_path_length);
295+
EXPECT_FALSE(end_arc.second);
296+
297+
// condition: goal pose behind start
298+
goal_pose.position.x = -0.9;
299+
goal_pose.position.y = 0.0;
300+
end_arc = calcEndArcLength(s_start, forward_path_length, road_lanes, goal_pose);
301+
EXPECT_DOUBLE_EQ(end_arc.first, s_start + forward_path_length);
302+
EXPECT_FALSE(end_arc.second);
303+
304+
// condition: goal pose beyond start
305+
goal_pose.position.x = 0.0;
306+
end_arc = calcEndArcLength(s_start, forward_path_length, road_lanes, goal_pose);
307+
EXPECT_DOUBLE_EQ(end_arc.first, s_start + forward_path_length);
308+
EXPECT_FALSE(end_arc.second);
309+
310+
// condition: path end is goal
311+
goal_pose.position.x = -0.75;
312+
end_arc = calcEndArcLength(s_start, forward_path_length, road_lanes, goal_pose);
313+
EXPECT_DOUBLE_EQ(end_arc.first, 0.25);
314+
EXPECT_TRUE(end_arc.second);
315+
}

0 commit comments

Comments
 (0)