diff --git a/planning/behavior_path_planner/autoware_behavior_path_planner_common/CMakeLists.txt b/planning/behavior_path_planner/autoware_behavior_path_planner_common/CMakeLists.txt index 4a2891e116cc8..56b68304a9326 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_planner_common/CMakeLists.txt +++ b/planning/behavior_path_planner/autoware_behavior_path_planner_common/CMakeLists.txt @@ -33,11 +33,12 @@ target_include_directories(${PROJECT_NAME} SYSTEM PUBLIC ) if(BUILD_TESTING) - ament_add_ros_isolated_gmock(test_${PROJECT_NAME}_utilities + ament_add_ros_isolated_gmock(test_${PROJECT_NAME}_drivable_area_expansion test/test_drivable_area_expansion.cpp + test/test_footprints.cpp ) - target_link_libraries(test_${PROJECT_NAME}_utilities + target_link_libraries(test_${PROJECT_NAME}_drivable_area_expansion ${PROJECT_NAME} ) diff --git a/planning/behavior_path_planner/autoware_behavior_path_planner_common/include/autoware/behavior_path_planner_common/utils/drivable_area_expansion/footprints.hpp b/planning/behavior_path_planner/autoware_behavior_path_planner_common/include/autoware/behavior_path_planner_common/utils/drivable_area_expansion/footprints.hpp index c0f953578a213..aea3818053171 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_planner_common/include/autoware/behavior_path_planner_common/utils/drivable_area_expansion/footprints.hpp +++ b/planning/behavior_path_planner/autoware_behavior_path_planner_common/include/autoware/behavior_path_planner_common/utils/drivable_area_expansion/footprints.hpp @@ -36,7 +36,7 @@ Polygon2d translate_polygon(const Polygon2d & polygon, const double x, const dou /// @param[in] pose the origin pose of the footprint /// @param[in] base_footprint the base axis-aligned footprint /// @return footprint polygon -Polygon2d create_footprint(const geometry_msgs::msg::Pose & pose, const Polygon2d base_footprint); +Polygon2d create_footprint(const geometry_msgs::msg::Pose & pose, const Polygon2d & base_footprint); /// @brief create footprints of the predicted paths of an object /// @param [in] objects objects from which to create polygons diff --git a/planning/behavior_path_planner/autoware_behavior_path_planner_common/package.xml b/planning/behavior_path_planner/autoware_behavior_path_planner_common/package.xml index b1d3a39379d15..28b79b502599d 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_planner_common/package.xml +++ b/planning/behavior_path_planner/autoware_behavior_path_planner_common/package.xml @@ -63,9 +63,11 @@ tier4_planning_msgs traffic_light_utils visualization_msgs + ament_cmake_ros ament_lint_auto autoware_lint_common + autoware_test_utils ament_cmake diff --git a/planning/behavior_path_planner/autoware_behavior_path_planner_common/src/utils/drivable_area_expansion/footprints.cpp b/planning/behavior_path_planner/autoware_behavior_path_planner_common/src/utils/drivable_area_expansion/footprints.cpp index a25a33c1668fa..10356150fc689 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_planner_common/src/utils/drivable_area_expansion/footprints.cpp +++ b/planning/behavior_path_planner/autoware_behavior_path_planner_common/src/utils/drivable_area_expansion/footprints.cpp @@ -33,7 +33,7 @@ Polygon2d translate_polygon(const Polygon2d & polygon, const double x, const dou return translated_polygon; } -Polygon2d create_footprint(const geometry_msgs::msg::Pose & pose, const Polygon2d base_footprint) +Polygon2d create_footprint(const geometry_msgs::msg::Pose & pose, const Polygon2d & base_footprint) { const auto angle = tf2::getYaw(pose.orientation); return translate_polygon( diff --git a/planning/behavior_path_planner/autoware_behavior_path_planner_common/test/test_footprints.cpp b/planning/behavior_path_planner/autoware_behavior_path_planner_common/test/test_footprints.cpp new file mode 100644 index 0000000000000..debb8e0de6cce --- /dev/null +++ b/planning/behavior_path_planner/autoware_behavior_path_planner_common/test/test_footprints.cpp @@ -0,0 +1,134 @@ +// 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/behavior_path_planner_common/utils/drivable_area_expansion/footprints.hpp" + +#include + +#include + +using autoware::test_utils::createPose; +using autoware::universe_utils::Point2d; +using autoware::universe_utils::Polygon2d; + +constexpr auto epsilon = 1e-6; + +TEST(FootprintTest, translate_polygon) +{ + using autoware::behavior_path_planner::drivable_area_expansion::translate_polygon; + + Polygon2d polygon; + polygon.outer() = { + 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}}; + + Polygon2d translated_polygon = translate_polygon(polygon, 1.0, 2.0); + + ASSERT_EQ(translated_polygon.outer().size(), 5); + EXPECT_NEAR(translated_polygon.outer().at(0).x(), 1.0, epsilon); + EXPECT_NEAR(translated_polygon.outer().at(0).y(), 2.0, epsilon); + EXPECT_NEAR(translated_polygon.outer().at(1).x(), 2.0, epsilon); + EXPECT_NEAR(translated_polygon.outer().at(1).y(), 2.0, epsilon); + EXPECT_NEAR(translated_polygon.outer().at(2).x(), 2.0, epsilon); + EXPECT_NEAR(translated_polygon.outer().at(2).y(), 3.0, epsilon); + EXPECT_NEAR(translated_polygon.outer().at(3).x(), 1.0, epsilon); + EXPECT_NEAR(translated_polygon.outer().at(3).y(), 3.0, epsilon); + EXPECT_NEAR(translated_polygon.outer().at(4).x(), 1.0, epsilon); + EXPECT_NEAR(translated_polygon.outer().at(4).y(), 2.0, epsilon); +} + +TEST(FootprintTest, create_footprint) +{ + using autoware::behavior_path_planner::drivable_area_expansion::create_footprint; + + Polygon2d base_footprint; + base_footprint.outer() = { + 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}}; + + // Condition: without rotation + auto pose = createPose(1.0, 2.0, 0.0, 0.0, 0.0, 0.0); + auto footprint = create_footprint(pose, base_footprint); + ASSERT_EQ(footprint.outer().size(), 5); + EXPECT_NEAR(footprint.outer().at(0).x(), 2.0, epsilon); + EXPECT_NEAR(footprint.outer().at(0).y(), 3.0, epsilon); + EXPECT_NEAR(footprint.outer().at(1).x(), 3.0, epsilon); + EXPECT_NEAR(footprint.outer().at(1).y(), 3.0, epsilon); + EXPECT_NEAR(footprint.outer().at(2).x(), 3.0, epsilon); + EXPECT_NEAR(footprint.outer().at(2).y(), 4.0, epsilon); + EXPECT_NEAR(footprint.outer().at(3).x(), 2.0, epsilon); + EXPECT_NEAR(footprint.outer().at(3).y(), 4.0, epsilon); + EXPECT_NEAR(footprint.outer().at(4).x(), 2.0, epsilon); + EXPECT_NEAR(footprint.outer().at(4).y(), 3.0, epsilon); + + // Condition: with rotation + pose = createPose(1.0, 2.0, 0.0, 0.0, 0.0, M_PI_2); + footprint = create_footprint(pose, base_footprint); + ASSERT_EQ(footprint.outer().size(), 5); + EXPECT_NEAR(footprint.outer().at(0).x(), 0.0, epsilon); + EXPECT_NEAR(footprint.outer().at(0).y(), 3.0, epsilon); + EXPECT_NEAR(footprint.outer().at(1).x(), 0.0, epsilon); + EXPECT_NEAR(footprint.outer().at(1).y(), 4.0, epsilon); + EXPECT_NEAR(footprint.outer().at(2).x(), -1.0, epsilon); + EXPECT_NEAR(footprint.outer().at(2).y(), 4.0, epsilon); + EXPECT_NEAR(footprint.outer().at(3).x(), -1.0, epsilon); + EXPECT_NEAR(footprint.outer().at(3).y(), 3.0, epsilon); + EXPECT_NEAR(footprint.outer().at(4).x(), 0.0, epsilon); + EXPECT_NEAR(footprint.outer().at(4).y(), 3.0, epsilon); +} + +TEST(FootprintTest, create_object_footprints) +{ + using autoware::behavior_path_planner::drivable_area_expansion::create_object_footprints; + + autoware_perception_msgs::msg::PredictedObjects objects; + autoware_perception_msgs::msg::PredictedObject object; + object.shape.dimensions.x = 4.0; + object.shape.dimensions.y = 2.0; + + // Add a predicted path + autoware_perception_msgs::msg::PredictedPath path; + auto pose = createPose(0.0, 0.0, 0.0, 0.0, 0.0, 0.0); + path.path.push_back(pose); + object.kinematics.predicted_paths.push_back(path); + + objects.objects.push_back(object); + + autoware::behavior_path_planner::drivable_area_expansion::DrivableAreaExpansionParameters params; + params.avoid_dynamic_objects = false; + params.dynamic_objects_extra_front_offset = 0.5; + params.dynamic_objects_extra_rear_offset = 0.5; + params.dynamic_objects_extra_left_offset = 0.5; + params.dynamic_objects_extra_right_offset = 0.5; + + // Condition: doesn't avoid dynamic objects + auto footprints = create_object_footprints(objects, params); + EXPECT_TRUE(footprints.empty()); + + // Condition: single object and single point path + params.avoid_dynamic_objects = true; + footprints = create_object_footprints(objects, params); + + ASSERT_EQ(footprints.size(), 1); + ASSERT_EQ(footprints.front().outer().size(), 5); + + EXPECT_NEAR(footprints.front().outer().at(0).x(), 2.5, epsilon); + EXPECT_NEAR(footprints.front().outer().at(0).y(), 1.5, epsilon); + EXPECT_NEAR(footprints.front().outer().at(1).x(), 2.5, epsilon); + EXPECT_NEAR(footprints.front().outer().at(1).y(), -1.5, epsilon); + EXPECT_NEAR(footprints.front().outer().at(2).x(), -2.5, epsilon); + EXPECT_NEAR(footprints.front().outer().at(2).y(), -1.5, epsilon); + EXPECT_NEAR(footprints.front().outer().at(3).x(), -2.5, epsilon); + EXPECT_NEAR(footprints.front().outer().at(3).y(), 1.5, epsilon); + EXPECT_NEAR(footprints.front().outer().at(4).x(), 2.5, epsilon); + EXPECT_NEAR(footprints.front().outer().at(4).y(), 1.5, epsilon); +}