From d22762f2fefd2812ab7266acbc44c5154ad65d35 Mon Sep 17 00:00:00 2001 From: "pre-commit-ci[bot]" <66853113+pre-commit-ci[bot]@users.noreply.github.com> Date: Sat, 8 Mar 2025 13:37:32 +0000 Subject: [PATCH] style(pre-commit): autofix --- .../include/autoware/path_generator/utils.hpp | 22 +++-- .../autoware_path_generator/src/utils.cpp | 92 ++++++++++--------- 2 files changed, 63 insertions(+), 51 deletions(-) diff --git a/planning/autoware_path_generator/include/autoware/path_generator/utils.hpp b/planning/autoware_path_generator/include/autoware/path_generator/utils.hpp index 4bbec96c1..657555020 100644 --- a/planning/autoware_path_generator/include/autoware/path_generator/utils.hpp +++ b/planning/autoware_path_generator/include/autoware/path_generator/utils.hpp @@ -166,19 +166,22 @@ const geometry_msgs::msg::Pose refine_goal( * @param lanes Lanelets. * @return Pre-goal point. */ -PathPointWithLaneId prepare_pre_goal(const geometry_msgs::msg::Pose & goal, const lanelet::ConstLanelets & lanes); +PathPointWithLaneId prepare_pre_goal( + const geometry_msgs::msg::Pose & goal, const lanelet::ConstLanelets & lanes); /** - * @brief Get the index of the point closest to the circumference of the circle whose center is the goal and outside of it. + * @brief Get the index of the point closest to the circumference of the circle whose center is the + * goal and outside of it. * @param points Points to search. * @param goal Goal pose. * @param goal_lane_id Lane ID of the goal. * @param max_dist Maximum distance to search. - * @return Index of the point closest to the circumference of the circle whose center is the goal and outside of it. + * @return Index of the point closest to the circumference of the circle whose center is the goal + * and outside of it. */ std::optional find_index_out_of_goal_search_range( - const std::vector & points, - const geometry_msgs::msg::Pose & goal, const int64_t goal_lane_id, const double max_dist); + const std::vector & points, const geometry_msgs::msg::Pose & goal, + const int64_t goal_lane_id, const double max_dist); /** * @brief Get the path up to just before the pre_goal. @@ -187,7 +190,8 @@ std::optional find_index_out_of_goal_search_range( * @return Recreated path */ std::optional path_up_to_just_before_pre_goal( - const PathWithLaneId & input, const geometry_msgs::msg::Pose & goal, const lanelet::Id goal_lane_id, const double search_radius_range); + const PathWithLaneId & input, const geometry_msgs::msg::Pose & goal, + const lanelet::Id goal_lane_id, const double search_radius_range); /** * @brief Recreate the path with a given goal pose. @@ -197,7 +201,8 @@ std::optional path_up_to_just_before_pre_goal( * @return Recreated path */ PathWithLaneId refine_path_for_goal( - const PathWithLaneId & input, const geometry_msgs::msg::Pose & goal, const PlannerData & planner_data); + const PathWithLaneId & input, const geometry_msgs::msg::Pose & goal, + const PlannerData & planner_data); /** * @brief Extract lanelets from the path. @@ -229,8 +234,7 @@ bool is_in_lanelets(const geometry_msgs::msg::Pose & pose, const lanelet::ConstL * @param planner_data Planner data. * @return True if the path is valid, false otherwise */ -bool is_path_valid( - const PathWithLaneId & refined_path, const PlannerData & planner_data); +bool is_path_valid(const PathWithLaneId & refined_path, const PlannerData & planner_data); /** * @brief Modify the path to connect smoothly to the goal. diff --git a/planning/autoware_path_generator/src/utils.cpp b/planning/autoware_path_generator/src/utils.cpp index 0ed021e40..8a973daef 100644 --- a/planning/autoware_path_generator/src/utils.cpp +++ b/planning/autoware_path_generator/src/utils.cpp @@ -352,30 +352,32 @@ const geometry_msgs::msg::Pose refine_goal( // To perform smooth goal connection, we need to prepare the point before the goal point. // This function prepares the point before the goal point. // See the link below for more details: -// - https://autowarefoundation.github.io/autoware.universe/main/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/#fixed_goal_planner -PathPointWithLaneId prepare_pre_goal(const geometry_msgs::msg::Pose & goal, const lanelet::ConstLanelets & lanes) +// - +// https://autowarefoundation.github.io/autoware.universe/main/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/#fixed_goal_planner +PathPointWithLaneId prepare_pre_goal( + const geometry_msgs::msg::Pose & goal, const lanelet::ConstLanelets & lanes) { - PathPointWithLaneId pre_refined_goal{}; + PathPointWithLaneId pre_refined_goal{}; - // -1.0 is to prepare the point before the goal point. See the link above for more details. - constexpr double goal_to_pre_goal_distance = -1.0; + // -1.0 is to prepare the point before the goal point. See the link above for more details. + constexpr double goal_to_pre_goal_distance = -1.0; - // First, calculate the pose of the pre_goal point - pre_refined_goal.point.pose = - autoware_utils::calc_offset_pose(goal, goal_to_pre_goal_distance, 0.0, 0.0); + // First, calculate the pose of the pre_goal point + pre_refined_goal.point.pose = + autoware_utils::calc_offset_pose(goal, goal_to_pre_goal_distance, 0.0, 0.0); - // Second, find and set the lane_id that the pre_goal point belongs to - for (const auto & lane : lanes) { - if (lanelet::utils::isInLanelet(pre_refined_goal.point.pose, lane)) { - // Prevent from duplication - if(exists(pre_refined_goal.lane_ids, lane.id())) { - continue; - } - pre_refined_goal.lane_ids.push_back(lane.id()); - } + // Second, find and set the lane_id that the pre_goal point belongs to + for (const auto & lane : lanes) { + if (lanelet::utils::isInLanelet(pre_refined_goal.point.pose, lane)) { + // Prevent from duplication + if (exists(pre_refined_goal.lane_ids, lane.id())) { + continue; + } + pre_refined_goal.lane_ids.push_back(lane.id()); } + } - return pre_refined_goal; + return pre_refined_goal; } // A function that assumes a circle with radius max_dist centered at the goal and returns @@ -423,37 +425,42 @@ std::optional find_index_out_of_goal_search_range( } // Clean up points around the goal for smooth goal connection -// This function returns the cleaned up path. You need to add pre_goal and goal to the returned path. -// This is because we'll do spline interpolation between the tail of the returned path and the pre_goal later at this file. -// - https://github.com/tier4/autoware.universe/blob/908cb7ee5cca01c367f03caf6db4562a620504fb/planning/behavior_path_planner/autoware_behavior_path_planner/src/behavior_path_planner_node.cpp#L724-L725 +// This function returns the cleaned up path. You need to add pre_goal and goal to the returned +// path. This is because we'll do spline interpolation between the tail of the returned path and the +// pre_goal later at this file. +// - +// https://github.com/tier4/autoware.universe/blob/908cb7ee5cca01c367f03caf6db4562a620504fb/planning/behavior_path_planner/autoware_behavior_path_planner/src/behavior_path_planner_node.cpp#L724-L725 std::optional path_up_to_just_before_pre_goal( - const PathWithLaneId & input, const geometry_msgs::msg::Pose & goal, const lanelet::Id goal_lane_id, const double search_radius_range) + const PathWithLaneId & input, const geometry_msgs::msg::Pose & goal, + const lanelet::Id goal_lane_id, const double search_radius_range) { - // Find min_dist_out_of_circle_index whose distance to goal is longer than search_radius_range - const auto min_dist_out_of_circle_index_opt = - find_index_out_of_goal_search_range(input.points, goal, goal_lane_id, search_radius_range); + // Find min_dist_out_of_circle_index whose distance to goal is longer than search_radius_range + const auto min_dist_out_of_circle_index_opt = + find_index_out_of_goal_search_range(input.points, goal, goal_lane_id, search_radius_range); - // It seems we are almost at the goal as no point is found outside of the circle whose center is the goal - if (!min_dist_out_of_circle_index_opt) { - return std::nullopt; - } + // It seems we are almost at the goal as no point is found outside of the circle whose center is + // the goal + if (!min_dist_out_of_circle_index_opt) { + return std::nullopt; + } - // It seems we have a point outside of the circle whose center is the goal - const auto min_dist_out_of_circle_index = min_dist_out_of_circle_index_opt.value(); + // It seems we have a point outside of the circle whose center is the goal + const auto min_dist_out_of_circle_index = min_dist_out_of_circle_index_opt.value(); - // Fill all the points up to just before the point outside of the circle - PathWithLaneId output; - output.points.reserve(output.points.size() + min_dist_out_of_circle_index + 3); - for (size_t i = 0; i <= min_dist_out_of_circle_index; ++i) { - output.points.push_back(input.points.at(i)); - } + // Fill all the points up to just before the point outside of the circle + PathWithLaneId output; + output.points.reserve(output.points.size() + min_dist_out_of_circle_index + 3); + for (size_t i = 0; i <= min_dist_out_of_circle_index; ++i) { + output.points.push_back(input.points.at(i)); + } - return output; + return output; } // Function to refine the path for the goal PathWithLaneId refine_path_for_goal( - const PathWithLaneId & input, const geometry_msgs::msg::Pose & goal, const PlannerData & planner_data) + const PathWithLaneId & input, const geometry_msgs::msg::Pose & goal, + const PlannerData & planner_data) { PathWithLaneId filtered_path = input; @@ -465,7 +472,9 @@ PathWithLaneId refine_path_for_goal( } // Clean up points around the goal for smooth goal connection - auto path_up_to_just_before_pre_goal_opt = path_up_to_just_before_pre_goal(filtered_path, goal, planner_data.goal_lane_id, planner_data.path_generator_parameters.refine_goal_search_radius_range); + auto path_up_to_just_before_pre_goal_opt = path_up_to_just_before_pre_goal( + filtered_path, goal, planner_data.goal_lane_id, + planner_data.path_generator_parameters.refine_goal_search_radius_range); if (!path_up_to_just_before_pre_goal_opt) { // It seems we are almost at the goal and no need to clean up. Lets use the original path. return input; @@ -540,8 +549,7 @@ bool is_in_lanelets(const geometry_msgs::msg::Pose & pose, const lanelet::ConstL return false; } -bool is_path_valid( - const PathWithLaneId & refined_path, const PlannerData & planner_data) +bool is_path_valid(const PathWithLaneId & refined_path, const PlannerData & planner_data) { // Extract lanelets from the refined path const auto lanelets_opt = extract_lanelets_from_path(refined_path, planner_data);