Skip to content

Commit f256c0b

Browse files
go-sakayorigo-sakayori
authored
test(bpp_common): add test for footprint (#9056)
add test for footprint Signed-off-by: Go Sakayori <gsakayori@gmail.com>
1 parent 76c1a6d commit f256c0b

File tree

5 files changed

+141
-4
lines changed

5 files changed

+141
-4
lines changed

‎planning/behavior_path_planner/autoware_behavior_path_planner_common/CMakeLists.txt

+3-2
Original file line numberDiff line numberDiff line change
@@ -33,11 +33,12 @@ target_include_directories(${PROJECT_NAME} SYSTEM PUBLIC
3333
)
3434

3535
if(BUILD_TESTING)
36-
ament_add_ros_isolated_gmock(test_${PROJECT_NAME}_utilities
36+
ament_add_ros_isolated_gmock(test_${PROJECT_NAME}_drivable_area_expansion
3737
test/test_drivable_area_expansion.cpp
38+
test/test_footprints.cpp
3839
)
3940

40-
target_link_libraries(test_${PROJECT_NAME}_utilities
41+
target_link_libraries(test_${PROJECT_NAME}_drivable_area_expansion
4142
${PROJECT_NAME}
4243
)
4344

‎planning/behavior_path_planner/autoware_behavior_path_planner_common/include/autoware/behavior_path_planner_common/utils/drivable_area_expansion/footprints.hpp

+1-1
Original file line numberDiff line numberDiff line change
@@ -36,7 +36,7 @@ Polygon2d translate_polygon(const Polygon2d & polygon, const double x, const dou
3636
/// @param[in] pose the origin pose of the footprint
3737
/// @param[in] base_footprint the base axis-aligned footprint
3838
/// @return footprint polygon
39-
Polygon2d create_footprint(const geometry_msgs::msg::Pose & pose, const Polygon2d base_footprint);
39+
Polygon2d create_footprint(const geometry_msgs::msg::Pose & pose, const Polygon2d & base_footprint);
4040

4141
/// @brief create footprints of the predicted paths of an object
4242
/// @param [in] objects objects from which to create polygons

‎planning/behavior_path_planner/autoware_behavior_path_planner_common/package.xml

+2
Original file line numberDiff line numberDiff line change
@@ -63,9 +63,11 @@
6363
<depend>tier4_planning_msgs</depend>
6464
<depend>traffic_light_utils</depend>
6565
<depend>visualization_msgs</depend>
66+
6667
<test_depend>ament_cmake_ros</test_depend>
6768
<test_depend>ament_lint_auto</test_depend>
6869
<test_depend>autoware_lint_common</test_depend>
70+
<test_depend>autoware_test_utils</test_depend>
6971

7072
<export>
7173
<build_type>ament_cmake</build_type>

‎planning/behavior_path_planner/autoware_behavior_path_planner_common/src/utils/drivable_area_expansion/footprints.cpp

+1-1
Original file line numberDiff line numberDiff line change
@@ -33,7 +33,7 @@ Polygon2d translate_polygon(const Polygon2d & polygon, const double x, const dou
3333
return translated_polygon;
3434
}
3535

36-
Polygon2d create_footprint(const geometry_msgs::msg::Pose & pose, const Polygon2d base_footprint)
36+
Polygon2d create_footprint(const geometry_msgs::msg::Pose & pose, const Polygon2d & base_footprint)
3737
{
3838
const auto angle = tf2::getYaw(pose.orientation);
3939
return translate_polygon(

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

+134
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,134 @@
1+
// Copyright 2024 TIER IV, Inc.
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/drivable_area_expansion/footprints.hpp"
16+
17+
#include <autoware_test_utils/autoware_test_utils.hpp>
18+
19+
#include <gtest/gtest.h>
20+
21+
using autoware::test_utils::createPose;
22+
using autoware::universe_utils::Point2d;
23+
using autoware::universe_utils::Polygon2d;
24+
25+
constexpr auto epsilon = 1e-6;
26+
27+
TEST(FootprintTest, translate_polygon)
28+
{
29+
using autoware::behavior_path_planner::drivable_area_expansion::translate_polygon;
30+
31+
Polygon2d polygon;
32+
polygon.outer() = {
33+
Point2d{0.0, 0.0}, Point2d{1.0, 0.0}, Point2d{1.0, 1.0}, Point2d{0.0, 1.0}, Point2d{0.0, 0.0}};
34+
35+
Polygon2d translated_polygon = translate_polygon(polygon, 1.0, 2.0);
36+
37+
ASSERT_EQ(translated_polygon.outer().size(), 5);
38+
EXPECT_NEAR(translated_polygon.outer().at(0).x(), 1.0, epsilon);
39+
EXPECT_NEAR(translated_polygon.outer().at(0).y(), 2.0, epsilon);
40+
EXPECT_NEAR(translated_polygon.outer().at(1).x(), 2.0, epsilon);
41+
EXPECT_NEAR(translated_polygon.outer().at(1).y(), 2.0, epsilon);
42+
EXPECT_NEAR(translated_polygon.outer().at(2).x(), 2.0, epsilon);
43+
EXPECT_NEAR(translated_polygon.outer().at(2).y(), 3.0, epsilon);
44+
EXPECT_NEAR(translated_polygon.outer().at(3).x(), 1.0, epsilon);
45+
EXPECT_NEAR(translated_polygon.outer().at(3).y(), 3.0, epsilon);
46+
EXPECT_NEAR(translated_polygon.outer().at(4).x(), 1.0, epsilon);
47+
EXPECT_NEAR(translated_polygon.outer().at(4).y(), 2.0, epsilon);
48+
}
49+
50+
TEST(FootprintTest, create_footprint)
51+
{
52+
using autoware::behavior_path_planner::drivable_area_expansion::create_footprint;
53+
54+
Polygon2d base_footprint;
55+
base_footprint.outer() = {
56+
Point2d{1.0, 1.0}, Point2d{2.0, 1.0}, Point2d{2.0, 2.0}, Point2d{1.0, 2.0}, Point2d{1.0, 1.0}};
57+
58+
// Condition: without rotation
59+
auto pose = createPose(1.0, 2.0, 0.0, 0.0, 0.0, 0.0);
60+
auto footprint = create_footprint(pose, base_footprint);
61+
ASSERT_EQ(footprint.outer().size(), 5);
62+
EXPECT_NEAR(footprint.outer().at(0).x(), 2.0, epsilon);
63+
EXPECT_NEAR(footprint.outer().at(0).y(), 3.0, epsilon);
64+
EXPECT_NEAR(footprint.outer().at(1).x(), 3.0, epsilon);
65+
EXPECT_NEAR(footprint.outer().at(1).y(), 3.0, epsilon);
66+
EXPECT_NEAR(footprint.outer().at(2).x(), 3.0, epsilon);
67+
EXPECT_NEAR(footprint.outer().at(2).y(), 4.0, epsilon);
68+
EXPECT_NEAR(footprint.outer().at(3).x(), 2.0, epsilon);
69+
EXPECT_NEAR(footprint.outer().at(3).y(), 4.0, epsilon);
70+
EXPECT_NEAR(footprint.outer().at(4).x(), 2.0, epsilon);
71+
EXPECT_NEAR(footprint.outer().at(4).y(), 3.0, epsilon);
72+
73+
// Condition: with rotation
74+
pose = createPose(1.0, 2.0, 0.0, 0.0, 0.0, M_PI_2);
75+
footprint = create_footprint(pose, base_footprint);
76+
ASSERT_EQ(footprint.outer().size(), 5);
77+
EXPECT_NEAR(footprint.outer().at(0).x(), 0.0, epsilon);
78+
EXPECT_NEAR(footprint.outer().at(0).y(), 3.0, epsilon);
79+
EXPECT_NEAR(footprint.outer().at(1).x(), 0.0, epsilon);
80+
EXPECT_NEAR(footprint.outer().at(1).y(), 4.0, epsilon);
81+
EXPECT_NEAR(footprint.outer().at(2).x(), -1.0, epsilon);
82+
EXPECT_NEAR(footprint.outer().at(2).y(), 4.0, epsilon);
83+
EXPECT_NEAR(footprint.outer().at(3).x(), -1.0, epsilon);
84+
EXPECT_NEAR(footprint.outer().at(3).y(), 3.0, epsilon);
85+
EXPECT_NEAR(footprint.outer().at(4).x(), 0.0, epsilon);
86+
EXPECT_NEAR(footprint.outer().at(4).y(), 3.0, epsilon);
87+
}
88+
89+
TEST(FootprintTest, create_object_footprints)
90+
{
91+
using autoware::behavior_path_planner::drivable_area_expansion::create_object_footprints;
92+
93+
autoware_perception_msgs::msg::PredictedObjects objects;
94+
autoware_perception_msgs::msg::PredictedObject object;
95+
object.shape.dimensions.x = 4.0;
96+
object.shape.dimensions.y = 2.0;
97+
98+
// Add a predicted path
99+
autoware_perception_msgs::msg::PredictedPath path;
100+
auto pose = createPose(0.0, 0.0, 0.0, 0.0, 0.0, 0.0);
101+
path.path.push_back(pose);
102+
object.kinematics.predicted_paths.push_back(path);
103+
104+
objects.objects.push_back(object);
105+
106+
autoware::behavior_path_planner::drivable_area_expansion::DrivableAreaExpansionParameters params;
107+
params.avoid_dynamic_objects = false;
108+
params.dynamic_objects_extra_front_offset = 0.5;
109+
params.dynamic_objects_extra_rear_offset = 0.5;
110+
params.dynamic_objects_extra_left_offset = 0.5;
111+
params.dynamic_objects_extra_right_offset = 0.5;
112+
113+
// Condition: doesn't avoid dynamic objects
114+
auto footprints = create_object_footprints(objects, params);
115+
EXPECT_TRUE(footprints.empty());
116+
117+
// Condition: single object and single point path
118+
params.avoid_dynamic_objects = true;
119+
footprints = create_object_footprints(objects, params);
120+
121+
ASSERT_EQ(footprints.size(), 1);
122+
ASSERT_EQ(footprints.front().outer().size(), 5);
123+
124+
EXPECT_NEAR(footprints.front().outer().at(0).x(), 2.5, epsilon);
125+
EXPECT_NEAR(footprints.front().outer().at(0).y(), 1.5, epsilon);
126+
EXPECT_NEAR(footprints.front().outer().at(1).x(), 2.5, epsilon);
127+
EXPECT_NEAR(footprints.front().outer().at(1).y(), -1.5, epsilon);
128+
EXPECT_NEAR(footprints.front().outer().at(2).x(), -2.5, epsilon);
129+
EXPECT_NEAR(footprints.front().outer().at(2).y(), -1.5, epsilon);
130+
EXPECT_NEAR(footprints.front().outer().at(3).x(), -2.5, epsilon);
131+
EXPECT_NEAR(footprints.front().outer().at(3).y(), 1.5, epsilon);
132+
EXPECT_NEAR(footprints.front().outer().at(4).x(), 2.5, epsilon);
133+
EXPECT_NEAR(footprints.front().outer().at(4).y(), 1.5, epsilon);
134+
}

0 commit comments

Comments
 (0)