From 64e5a69031f53ec2d0635618f06ab57445d8eb94 Mon Sep 17 00:00:00 2001 From: Go Sakayori Date: Wed, 27 Nov 2024 14:21:41 +0900 Subject: [PATCH] test(bpp_common): add unit test for utils (#9469) * add easy unit test Signed-off-by: Go Sakayori * fix clang tidy warning and add unit test Signed-off-by: Go Sakayori * add more unit test Signed-off-by: Go Sakayori * add docstring Signed-off-by: Go Sakayori --------- Signed-off-by: Go Sakayori Signed-off-by: Go Sakayori --- .../utils/utils.hpp | 116 +++++- .../src/utils/utils.cpp | 17 +- .../test/test_utils.cpp | 347 +++++++++++++++++- 3 files changed, 461 insertions(+), 19 deletions(-) diff --git a/planning/behavior_path_planner/autoware_behavior_path_planner_common/include/autoware/behavior_path_planner_common/utils/utils.hpp b/planning/behavior_path_planner/autoware_behavior_path_planner_common/include/autoware/behavior_path_planner_common/utils/utils.hpp index d6d37c3f581e4..7162f49b450e8 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_planner_common/include/autoware/behavior_path_planner_common/utils/utils.hpp +++ b/planning/behavior_path_planner/autoware_behavior_path_planner_common/include/autoware/behavior_path_planner_common/utils/utils.hpp @@ -137,9 +137,21 @@ double l2Norm(const Vector3 vector); double getDistanceToEndOfLane(const Pose & current_pose, const lanelet::ConstLanelets & lanelets); +/** + * @brief Calculates the distance to the next intersection. + * @param current_pose Ego pose. + * @param lanelets Lanelets to check. + * @return Distance. + */ double getDistanceToNextIntersection( const Pose & current_pose, const lanelet::ConstLanelets & lanelets); +/** + * @brief Calculates the distance to the next crosswalk. + * @param current_pose Ego pose. + * @param lanelets Lanelets to check. + * @return Distance. + */ double getDistanceToCrosswalk( const Pose & current_pose, const lanelet::ConstLanelets & lanelets, const lanelet::routing::RoutingGraphContainer & overall_graphs); @@ -236,6 +248,15 @@ bool set_goal( */ const Pose refineGoal(const Pose & goal, const lanelet::ConstLanelet & goal_lanelet); +/** + * @brief Recreate the path with a given goal pose. + * @param search_radius_range Searching radius. + * @param search_rad_range Searching angle. + * @param input Input path. + * @param goal Goal pose. + * @param goal_lane_id Lane ID of goal lanelet. + * @return Recreated path + */ PathWithLaneId refinePathForGoal( const double search_radius_range, const double search_rad_range, const PathWithLaneId & input, const Pose & goal, const int64_t goal_lane_id); @@ -243,8 +264,22 @@ PathWithLaneId refinePathForGoal( bool isAllowedGoalModification(const std::shared_ptr & route_handler); bool checkOriginalGoalIsInShoulder(const std::shared_ptr & route_handler); +/** + * @brief Checks if the given pose is inside the given lanes. + * @param pose Ego pose. + * @param lanes Lanelets to check. + * @return True if the ego pose is inside the lanes. + */ bool isInLanelets(const Pose & pose, const lanelet::ConstLanelets & lanes); +/** + * @brief Checks if the given pose is inside the given lane within certain yaw difference. + * @param current_pose Ego pose. + * @param lanelet Lanelet to check. + * @param yaw_threshold Yaw angle difference threshold. + * @param radius Search radius + * @return True if the ego pose is inside the lane. + */ bool isInLaneletWithYawThreshold( const Pose & current_pose, const lanelet::ConstLanelet & lanelet, const double yaw_threshold, const double radius = 0.0); @@ -254,6 +289,14 @@ bool isEgoOutOfRoute( const std::optional & modified_goal, const std::shared_ptr & route_handler); +/** + * @brief Checks if the given pose is inside the original lane. + * @param current_lanes Original lanes. + * @param current_pose Ego pose. + * @param common_param Parameters used for behavior path planner. + * @param outer_margin Allowed margin. + * @return True if the ego pose is inside the original lane. + */ bool isEgoWithinOriginalLane( const lanelet::ConstLanelets & current_lanes, const Pose & current_pose, const BehaviorPathPlannerParameters & common_param, const double outer_margin = 0.0); @@ -268,18 +311,48 @@ bool isEgoWithinOriginalLane( std::shared_ptr generateCenterLinePath( const std::shared_ptr & planner_data); +/** + * @brief Inserts a stop point with given length + * @param length Distance to insert stop point. + * @param path Original path. + * @return Inserted stop point. + */ PathPointWithLaneId insertStopPoint(const double length, PathWithLaneId & path); +/** + * @brief Calculates distance to lane boundary. + * @param lanelet Target lanelet. + * @param position Ego position. + * @param left_side Whether to check left side boundary. + * @return Distance to boundary. + */ double getSignedDistanceFromLaneBoundary( const lanelet::ConstLanelet & lanelet, const Point & position, const bool left_side); + double getSignedDistanceFromBoundary( const lanelet::ConstLanelets & lanelets, const Pose & pose, const bool left_side); + +/** + * @brief Calculates distance to lane boundary. + * @param lanelet Target lanelet. + * @param vehicle_width Ego vehicle width. + * @param base_link2front Ego vehicle distance from base link to front. + * @param base_link2rear Ego vehicle distance from base link to rear. + * @param vehicle_pose Ego vehicle pose. + * @param left_side Whether to check left side boundary. + * @return Distance to boundary. + */ std::optional getSignedDistanceFromBoundary( const lanelet::ConstLanelets & lanelets, const double vehicle_width, const double base_link2front, const double base_link2rear, const Pose & vehicle_pose, const bool left_side); // misc +/** + * @brief Convert lanelet to 2D polygon. + * @param lanelet Target lanelet. + * @return Polygon + */ Polygon2d toPolygon2d(const lanelet::ConstLanelet & lanelet); Polygon2d toPolygon2d(const lanelet::BasicPolygon2d & polygon); @@ -316,10 +389,27 @@ lanelet::ConstLanelets extendPrevLane( lanelet::ConstLanelets extendLanes( const std::shared_ptr route_handler, const lanelet::ConstLanelets & lanes); +/** + * @brief Retrieves sequences of preceding lanelets from the target lanes. + * @param route_handler Reference to the route handler. + * @param target_lanes The set of target lanelets. + * @param current_pose The current pose of ego vehicle. + * @param backward_length The backward search length [m]. + * @return A vector of lanelet sequences that precede the target lanes + */ std::vector getPrecedingLanelets( const RouteHandler & route_handler, const lanelet::ConstLanelets & target_lanes, const Pose & current_pose, const double backward_length); +/** + * @brief Retrieves all preceding lanelets as a flat list. + * @param route_handler Reference to the route handler. + * @param target_lanes The set of target lanelets. + * @param current_pose The current pose of ego vehicle. + * @param backward_length The backward search length [m]. + * @return Preceding lanelets within the specified backward length. + */ + lanelet::ConstLanelets getBackwardLanelets( const RouteHandler & route_handler, const lanelet::ConstLanelets & target_lanes, const Pose & current_pose, const double backward_length); @@ -335,17 +425,41 @@ lanelet::ConstLanelets getExtendedCurrentLanesFromPath( const PathWithLaneId & path, const std::shared_ptr & planner_data, const double backward_length, const double forward_length, const bool forward_only_in_route); +/** + * @brief Calculates the sequence of lanelets around a given pose within a specified range. + * @param route_handler A shared pointer to the RouteHandler for accessing route and lanelet + * information. + * @param pose The reference pose to locate the lanelets around. + * @param forward_length The length of the route to extend forward from the reference pose. + * @param backward_length The length of the route to extend backward from the reference pose. + * @param dist_threshold The maximum allowable distance to consider a lanelet as closest. + * @param yaw_threshold The maximum allowable yaw difference (in radians) for lanelet matching. + * @return A sequence of lanelets around the given pose. + */ lanelet::ConstLanelets calcLaneAroundPose( - const std::shared_ptr route_handler, const geometry_msgs::msg::Pose & pose, + const std::shared_ptr & route_handler, const geometry_msgs::msg::Pose & pose, const double forward_length, const double backward_length, const double dist_threshold = std::numeric_limits::max(), const double yaw_threshold = std::numeric_limits::max()); +/** + * @brief Checks whether the relative angles between consecutive triplets of points in a path remain + * within a specified threshold. + * @param path Input path. + * @param angle_threshold The maximum allowable angle in radians. + * @return True if all relative angles are within the threshold or the path has fewer than three + * points. + */ bool checkPathRelativeAngle(const PathWithLaneId & path, const double angle_threshold); lanelet::ConstLanelets getLaneletsFromPath( const PathWithLaneId & path, const std::shared_ptr & route_handler); +/** + * @brief Converts camel case string to snake case string. + * @param input_str Input string. + * @return String + */ std::string convertToSnakeCase(const std::string & input_str); std::optional getPolygonByPoint( diff --git a/planning/behavior_path_planner/autoware_behavior_path_planner_common/src/utils/utils.cpp b/planning/behavior_path_planner/autoware_behavior_path_planner_common/src/utils/utils.cpp index a546c0026d4d6..a224ef16bc0d9 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_planner_common/src/utils/utils.cpp +++ b/planning/behavior_path_planner/autoware_behavior_path_planner_common/src/utils/utils.cpp @@ -380,9 +380,8 @@ PathWithLaneId refinePathForGoal( search_radius_range, search_rad_range, filtered_path, goal, goal_lane_id, &path_with_goal)) { return path_with_goal; - } else { - return filtered_path; } + return filtered_path; } bool isInLanelets(const Pose & pose, const lanelet::ConstLanelets & lanes) @@ -441,9 +440,8 @@ bool isEgoOutOfRoute( RCLCPP_WARN_STREAM( rclcpp::get_logger("behavior_path_planner").get_child("util"), "ego pose is beyond goal"); return true; - } else { - return false; } + return false; } // If ego vehicle is out of the closest lanelet, return true @@ -466,11 +464,7 @@ bool isEgoOutOfRoute( return false; }); - if (!is_in_shoulder_lane && !is_in_road_lane) { - return true; - } - - return false; + return !is_in_shoulder_lane && !is_in_road_lane; } bool isEgoWithinOriginalLane( @@ -1359,8 +1353,9 @@ lanelet::ConstLanelets getBackwardLanelets( } lanelet::ConstLanelets calcLaneAroundPose( - const std::shared_ptr route_handler, const Pose & pose, const double forward_length, - const double backward_length, const double dist_threshold, const double yaw_threshold) + const std::shared_ptr & route_handler, const Pose & pose, + const double forward_length, const double backward_length, const double dist_threshold, + const double yaw_threshold) { lanelet::ConstLanelet current_lane; if (!route_handler->getClosestLaneletWithConstrainsWithinRoute( diff --git a/planning/behavior_path_planner/autoware_behavior_path_planner_common/test/test_utils.cpp b/planning/behavior_path_planner/autoware_behavior_path_planner_common/test/test_utils.cpp index 4326cfdb8e6be..2234574fce83e 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_planner_common/test/test_utils.cpp +++ b/planning/behavior_path_planner/autoware_behavior_path_planner_common/test/test_utils.cpp @@ -15,8 +15,14 @@ #include "autoware/behavior_path_planner_common/utils/utils.hpp" #include +#include #include +#include + +#include +#include +#include using autoware::universe_utils::Point2d; using autoware_perception_msgs::msg::PredictedObject; @@ -25,11 +31,55 @@ using autoware_planning_msgs::msg::Trajectory; using tier4_planning_msgs::msg::PathPointWithLaneId; using tier4_planning_msgs::msg::PathWithLaneId; using ObjectClassification = autoware_perception_msgs::msg::ObjectClassification; +using autoware::behavior_path_planner::PlannerData; +using autoware_planning_msgs::msg::LaneletRoute; using autoware::test_utils::createPose; using autoware::test_utils::generateTrajectory; -TEST(BehaviorPathPlanningUtilTest, l2Norm) +class BehaviorPathPlanningUtilTest : public ::testing::Test +{ +protected: + void SetUp() override + { + planner_data_ = std::make_shared(); + const auto test_data_file = + ament_index_cpp::get_package_share_directory("autoware_behavior_path_planner_common") + + "/test_data/test_traffic_light.yaml"; + YAML::Node config = YAML::LoadFile(test_data_file); + + set_current_pose(config); + set_route_handler(config); + } + + void set_current_pose(YAML::Node config) + { + const auto self_odometry = + autoware::test_utils::parse(config["self_odometry"]); + planner_data_->self_odometry = std::make_shared(self_odometry); + } + + void set_route_handler(YAML::Node config) + { + const auto route = autoware::test_utils::parse(config["route"]); + const auto intersection_map = + autoware::test_utils::make_map_bin_msg(autoware::test_utils::get_absolute_path_to_lanelet_map( + "autoware_test_utils", "intersection/lanelet2_map.osm")); + planner_data_->route_handler->setMap(intersection_map); + planner_data_->route_handler->setRoute(route); + + for (const auto & segment : route.segments) { + current_lanelets.push_back( + planner_data_->route_handler->getLaneletsFromId(segment.preferred_primitive.id)); + } + } + + std::shared_ptr planner_data_; + lanelet::ConstLanelets current_lanelets; + const double epsilon = 1e-06; +}; + +TEST_F(BehaviorPathPlanningUtilTest, l2Norm) { using autoware::behavior_path_planner::utils::l2Norm; @@ -42,7 +92,7 @@ TEST(BehaviorPathPlanningUtilTest, l2Norm) EXPECT_DOUBLE_EQ(norm, 3.0); } -TEST(BehaviorPathPlanningUtilTest, checkCollisionBetweenPathFootprintsAndObjects) +TEST_F(BehaviorPathPlanningUtilTest, checkCollisionBetweenPathFootprintsAndObjects) { using autoware::behavior_path_planner::utils::checkCollisionBetweenPathFootprintsAndObjects; @@ -75,7 +125,7 @@ TEST(BehaviorPathPlanningUtilTest, checkCollisionBetweenPathFootprintsAndObjects checkCollisionBetweenPathFootprintsAndObjects(base_footprint, ego_path, objs, margin)); } -TEST(BehaviorPathPlanningUtilTest, checkCollisionBetweenFootprintAndObjects) +TEST_F(BehaviorPathPlanningUtilTest, checkCollisionBetweenFootprintAndObjects) { using autoware::behavior_path_planner::utils::checkCollisionBetweenFootprintAndObjects; @@ -105,7 +155,7 @@ TEST(BehaviorPathPlanningUtilTest, checkCollisionBetweenFootprintAndObjects) EXPECT_TRUE(checkCollisionBetweenFootprintAndObjects(base_footprint, ego_pose, objs, margin)); } -TEST(BehaviorPathPlanningUtilTest, calcLateralDistanceFromEgoToObject) +TEST_F(BehaviorPathPlanningUtilTest, calcLateralDistanceFromEgoToObject) { using autoware::behavior_path_planner::utils::calcLateralDistanceFromEgoToObject; @@ -130,7 +180,7 @@ TEST(BehaviorPathPlanningUtilTest, calcLateralDistanceFromEgoToObject) EXPECT_DOUBLE_EQ(calcLateralDistanceFromEgoToObject(ego_pose, vehicle_width, obj), 3.0); } -TEST(BehaviorPathPlanningUtilTest, calc_longitudinal_distance_from_ego_to_object) +TEST_F(BehaviorPathPlanningUtilTest, calc_longitudinal_distance_from_ego_to_object) { using autoware::behavior_path_planner::utils::calc_longitudinal_distance_from_ego_to_object; @@ -164,7 +214,7 @@ TEST(BehaviorPathPlanningUtilTest, calc_longitudinal_distance_from_ego_to_object 2.0); } -TEST(BehaviorPathPlanningUtilTest, calcLongitudinalDistanceFromEgoToObjects) +TEST_F(BehaviorPathPlanningUtilTest, calcLongitudinalDistanceFromEgoToObjects) { using autoware::behavior_path_planner::utils::calcLongitudinalDistanceFromEgoToObjects; @@ -198,7 +248,206 @@ TEST(BehaviorPathPlanningUtilTest, calcLongitudinalDistanceFromEgoToObjects) calcLongitudinalDistanceFromEgoToObjects(ego_pose, base_link2front, base_link2rear, objs), 3.0); } -TEST(BehaviorPathPlanningUtilTest, getHighestProbLabel) +TEST_F(BehaviorPathPlanningUtilTest, refineGoal) +{ + using autoware::behavior_path_planner::utils::refineGoal; + + { + const auto goal_lanelet = current_lanelets.front(); + const auto goal_pose = planner_data_->self_odometry->pose.pose; + const auto refined_pose = refineGoal(goal_pose, goal_lanelet); + EXPECT_DOUBLE_EQ(refined_pose.position.x, goal_pose.position.x); + EXPECT_DOUBLE_EQ(refined_pose.position.y, goal_pose.position.y); + } +} + +TEST_F(BehaviorPathPlanningUtilTest, refinePathForGoal) +{ + using autoware::behavior_path_planner::utils::refinePathForGoal; + + auto path = generateTrajectory(10, 1.0, 3.0); + const double search_rad_range = M_PI; + const auto goal_pose = createPose(5.2, 0.0, 0.0, 0.0, 0.0, 0.0); + const int64_t goal_lane_id = 5; + { + const double search_radius_range = 1.0; + const auto refined_path = + refinePathForGoal(search_radius_range, search_rad_range, path, goal_pose, goal_lane_id); + EXPECT_EQ(refined_path.points.size(), 7); + EXPECT_DOUBLE_EQ(refined_path.points.back().point.longitudinal_velocity_mps, 0.0); + EXPECT_DOUBLE_EQ(refined_path.points.back().point.pose.position.x, 5.2); + } +} + +TEST_F(BehaviorPathPlanningUtilTest, isInLanelets) +{ + using autoware::behavior_path_planner::utils::isInLanelets; + using autoware::behavior_path_planner::utils::isInLaneletWithYawThreshold; + + { + const auto pose = createPose(0.0, 0.0, 0.0, 0.0, 0.0, 0.0); + EXPECT_FALSE(isInLanelets(pose, current_lanelets)); + EXPECT_FALSE(isInLaneletWithYawThreshold(pose, current_lanelets.front(), M_PI_2)); + } + { + EXPECT_TRUE(isInLanelets(planner_data_->self_odometry->pose.pose, current_lanelets)); + EXPECT_TRUE(isInLaneletWithYawThreshold( + planner_data_->self_odometry->pose.pose, current_lanelets.front(), M_PI_2)); + } +} + +TEST_F(BehaviorPathPlanningUtilTest, isEgoWithinOriginalLane) +{ + using autoware::behavior_path_planner::utils::isEgoWithinOriginalLane; + BehaviorPathPlannerParameters common_param; + common_param.vehicle_width = 1.0; + common_param.base_link2front = 1.0; + common_param.base_link2rear = 1.0; + + { + const auto pose = createPose(0.0, 0.0, 0.0, 0.0, 0.0, 0.0); + EXPECT_FALSE(isEgoWithinOriginalLane(current_lanelets, pose, common_param)); + } + { + EXPECT_TRUE(isEgoWithinOriginalLane( + current_lanelets, planner_data_->self_odometry->pose.pose, common_param)); + } +} + +TEST_F(BehaviorPathPlanningUtilTest, getDistanceToNextIntersection) +{ + using autoware::behavior_path_planner::utils::getDistanceToNextIntersection; + + const auto current_pose = planner_data_->self_odometry->pose.pose; + { + const lanelet::ConstLanelets empty_lanelets; + EXPECT_DOUBLE_EQ( + getDistanceToNextIntersection(current_pose, empty_lanelets), + std::numeric_limits::infinity()); + } + { + EXPECT_NEAR( + getDistanceToNextIntersection(current_pose, current_lanelets), 117.1599371, epsilon); + } +} + +TEST_F(BehaviorPathPlanningUtilTest, getDistanceToCrosswalk) +{ + using autoware::behavior_path_planner::utils::getDistanceToCrosswalk; + + const auto current_pose = planner_data_->self_odometry->pose.pose; + const auto routing_graph = planner_data_->route_handler->getOverallGraphPtr(); + { + const lanelet::ConstLanelets empty_lanelets; + EXPECT_DOUBLE_EQ( + getDistanceToCrosswalk(current_pose, empty_lanelets, *routing_graph), + std::numeric_limits::infinity()); + } + { + EXPECT_NEAR( + getDistanceToCrosswalk(current_pose, current_lanelets, *routing_graph), 120.4423193, epsilon); + } +} + +TEST_F(BehaviorPathPlanningUtilTest, insertStopPoint) +{ + using autoware::behavior_path_planner::utils::insertStopPoint; + + { + const double length = 100.0; + auto path = generateTrajectory(10, 1.0, 1.0); + EXPECT_DOUBLE_EQ(insertStopPoint(length, path).point.pose.position.x, 0.0); + } + { + const double length = 5.0; + auto path = generateTrajectory(10, 1.0, 1.0); + EXPECT_DOUBLE_EQ(insertStopPoint(length, path).point.pose.position.x, 5.0); + } +} + +TEST_F(BehaviorPathPlanningUtilTest, getSignedDistanceFromBoundary) +{ + using autoware::behavior_path_planner::utils::getSignedDistanceFromBoundary; + + { + lanelet::ConstLanelets empty_lanelets; + EXPECT_DOUBLE_EQ( + getSignedDistanceFromBoundary(empty_lanelets, planner_data_->self_odometry->pose.pose, true), + 0.0); + } + { + EXPECT_NEAR( + getSignedDistanceFromBoundary( + current_lanelets, planner_data_->self_odometry->pose.pose, true), + -1.4952926, epsilon); + EXPECT_NEAR( + getSignedDistanceFromBoundary( + current_lanelets, planner_data_->self_odometry->pose.pose, false), + 1.504715076, epsilon); + } + { + const double vehicle_width = 1.0; + const double base_link2front = 1.0; + const double base_link2rear = 1.0; + + const auto left_distance = getSignedDistanceFromBoundary( + current_lanelets, vehicle_width, base_link2front, base_link2rear, + planner_data_->self_odometry->pose.pose, true); + ASSERT_TRUE(left_distance.has_value()); + EXPECT_NEAR(left_distance.value(), -0.9946984, epsilon); + + const auto right_distance = getSignedDistanceFromBoundary( + current_lanelets, vehicle_width, base_link2front, base_link2rear, + planner_data_->self_odometry->pose.pose, false); + ASSERT_TRUE(right_distance.has_value()); + EXPECT_NEAR(right_distance.value(), 1.0041208, epsilon); + } +} + +TEST_F(BehaviorPathPlanningUtilTest, getArcLengthToTargetLanelet) +{ + using autoware::behavior_path_planner::utils::getArcLengthToTargetLanelet; + { + auto target_lane = current_lanelets.front(); + EXPECT_DOUBLE_EQ( + getArcLengthToTargetLanelet( + current_lanelets, target_lane, planner_data_->self_odometry->pose.pose), + 0.0); + } + { + auto target_lane = current_lanelets.at(1); + EXPECT_NEAR( + getArcLengthToTargetLanelet( + current_lanelets, target_lane, planner_data_->self_odometry->pose.pose), + 86.78265658, epsilon); + } +} + +TEST_F(BehaviorPathPlanningUtilTest, toPolygon2d) +{ + using autoware::behavior_path_planner::utils::toPolygon2d; + + const auto lanelet = current_lanelets.front(); + const auto lanelet_polygon = lanelet.polygon2d().basicPolygon(); + + auto polygon_converted_from_lanelet = toPolygon2d(lanelet); + auto polygon_converted_from_basic_polygon = toPolygon2d(lanelet_polygon); + + EXPECT_EQ( + polygon_converted_from_lanelet.outer().size(), + polygon_converted_from_basic_polygon.outer().size()); + + for (size_t i = 0; i < polygon_converted_from_lanelet.outer().size(); i++) { + EXPECT_DOUBLE_EQ( + polygon_converted_from_lanelet.outer().at(i).x(), + polygon_converted_from_basic_polygon.outer().at(i).x()); + EXPECT_DOUBLE_EQ( + polygon_converted_from_lanelet.outer().at(i).y(), + polygon_converted_from_basic_polygon.outer().at(i).y()); + } +} + +TEST_F(BehaviorPathPlanningUtilTest, getHighestProbLabel) { using autoware::behavior_path_planner::utils::getHighestProbLabel; @@ -217,3 +466,87 @@ TEST(BehaviorPathPlanningUtilTest, getHighestProbLabel) .probability(0.6)); EXPECT_EQ(getHighestProbLabel(obj.classification), ObjectClassification::Type::TRUCK); } + +TEST_F(BehaviorPathPlanningUtilTest, getPrecedingLanelets) +{ + using autoware::behavior_path_planner::utils::getPrecedingLanelets; + const auto & route_handler_ptr = planner_data_->route_handler; + + { + const lanelet::ConstLanelets target_lanes; + EXPECT_TRUE(getPrecedingLanelets( + *route_handler_ptr, target_lanes, planner_data_->self_odometry->pose.pose, 10.0) + .empty()); + } + { + lanelet::ConstLanelets target_lanes; + target_lanes.push_back(route_handler_ptr->getLaneletsFromId(1001)); + EXPECT_TRUE(getPrecedingLanelets( + *route_handler_ptr, target_lanes, planner_data_->self_odometry->pose.pose, 0.0) + .empty()); + } + { + lanelet::ConstLanelets target_lanes; + target_lanes.push_back(route_handler_ptr->getLaneletsFromId(1011)); + target_lanes.push_back(route_handler_ptr->getLaneletsFromId(1101)); + const auto preceding_lanelets = getPrecedingLanelets( + *route_handler_ptr, target_lanes, planner_data_->self_odometry->pose.pose, 10.0); + ASSERT_FALSE(preceding_lanelets.empty()); + EXPECT_EQ(preceding_lanelets.front().data()->id(), 1001); + } +} + +TEST_F(BehaviorPathPlanningUtilTest, getBackwardLanelets) +{ + using autoware::behavior_path_planner::utils::getBackwardLanelets; + + const auto & route_handler_ptr = planner_data_->route_handler; + lanelet::ConstLanelets target_lanes; + target_lanes.push_back(route_handler_ptr->getLaneletsFromId(1011)); + const auto backward_lanelets = getBackwardLanelets( + *route_handler_ptr, target_lanes, planner_data_->self_odometry->pose.pose, 10.0); + ASSERT_FALSE(backward_lanelets.empty()); + EXPECT_EQ(backward_lanelets.front().id(), 1001); +} + +TEST_F(BehaviorPathPlanningUtilTest, calcLaneAroundPose) +{ + using autoware::behavior_path_planner::utils::calcLaneAroundPose; + + { + const auto pose = createPose(0.0, 0.0, 0.0, 0.0, 0.0, 0.0); + std::shared_ptr route_handler_null = + std::make_shared(); + const auto lane = calcLaneAroundPose(route_handler_null, pose, 10.0, 0.0); + EXPECT_TRUE(lane.empty()); + } + { + const auto lane = calcLaneAroundPose( + planner_data_->route_handler, planner_data_->self_odometry->pose.pose, 10.0, 0.0); + EXPECT_EQ(lane.size(), 1); + EXPECT_EQ(lane.front().id(), 1001); + } +} + +TEST_F(BehaviorPathPlanningUtilTest, checkPathRelativeAngle) +{ + using autoware::behavior_path_planner::utils::checkPathRelativeAngle; + + { + auto path = generateTrajectory(2, 1.0); + EXPECT_TRUE(checkPathRelativeAngle(path, 0.0)); + } + { + auto path = generateTrajectory(10, 1.0); + EXPECT_TRUE(checkPathRelativeAngle(path, M_PI_2)); + } +} + +TEST_F(BehaviorPathPlanningUtilTest, convertToSnakeCase) +{ + using autoware::behavior_path_planner::utils::convertToSnakeCase; + + std::string input_string = "testString"; + auto converted_string = convertToSnakeCase(input_string); + EXPECT_EQ(converted_string, "test_string"); +}