From f4929164abdd4ecbf383ffe80eee5fffea945578 Mon Sep 17 00:00:00 2001 From: Kyoichi Sugahara Date: Tue, 19 Sep 2023 09:38:46 +0900 Subject: [PATCH] refactor(behavior_path_planner): use util function convertToPredictedPath from lane_change (#5013) --- .../utils/lane_change/utils.hpp | 3 --- .../src/scene_module/lane_change/normal.cpp | 2 +- .../src/utils/lane_change/utils.cpp | 13 ------------- 3 files changed, 1 insertion(+), 17 deletions(-) diff --git a/planning/behavior_path_planner/include/behavior_path_planner/utils/lane_change/utils.hpp b/planning/behavior_path_planner/include/behavior_path_planner/utils/lane_change/utils.hpp index f180afc2d1ffd..0c1fff52c74e2 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/utils/lane_change/utils.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/utils/lane_change/utils.hpp @@ -160,9 +160,6 @@ std::vector convertToPredictedPath( const LaneChangePath & lane_change_path, const Twist & vehicle_twist, const Pose & pose, const BehaviorPathPlannerParameters & common_parameters, const double resolution); -PredictedPath convertToPredictedPath( - const std::vector & path, const double time_resolution); - bool isParkedObject( const PathWithLaneId & path, const RouteHandler & route_handler, const ExtendedPredictedObject & object, const double object_check_min_road_shoulder_width, diff --git a/planning/behavior_path_planner/src/scene_module/lane_change/normal.cpp b/planning/behavior_path_planner/src/scene_module/lane_change/normal.cpp index 2f030689a29f5..333231e0ed0d4 100644 --- a/planning/behavior_path_planner/src/scene_module/lane_change/normal.cpp +++ b/planning/behavior_path_planner/src/scene_module/lane_change/normal.cpp @@ -1346,7 +1346,7 @@ PathSafetyStatus NormalLaneChange::isLaneChangePathSafe( const auto ego_predicted_path = utils::lane_change::convertToPredictedPath( lane_change_path, current_twist, current_pose, common_parameters, time_resolution); const auto debug_predicted_path = - utils::lane_change::convertToPredictedPath(ego_predicted_path, time_resolution); + utils::path_safety_checker::convertToPredictedPath(ego_predicted_path, time_resolution); auto collision_check_objects = target_objects.target_lane; diff --git a/planning/behavior_path_planner/src/utils/lane_change/utils.cpp b/planning/behavior_path_planner/src/utils/lane_change/utils.cpp index 3e098c651feef..29c139ab5a654 100644 --- a/planning/behavior_path_planner/src/utils/lane_change/utils.cpp +++ b/planning/behavior_path_planner/src/utils/lane_change/utils.cpp @@ -829,19 +829,6 @@ std::vector convertToPredictedPath( return predicted_path; } -PredictedPath convertToPredictedPath( - const std::vector & path, const double time_resolution) -{ - PredictedPath predicted_path; - predicted_path.time_step = rclcpp::Duration::from_seconds(time_resolution); - predicted_path.path.resize(path.size()); - - for (size_t i = 0; i < path.size(); ++i) { - predicted_path.path.at(i) = path.at(i).pose; - } - return predicted_path; -} - bool isParkedObject( const PathWithLaneId & path, const RouteHandler & route_handler, const ExtendedPredictedObject & object, const double object_check_min_road_shoulder_width,