Skip to content

Commit

Permalink
style(pre-commit): autofix
Browse files Browse the repository at this point in the history
  • Loading branch information
pre-commit-ci[bot] committed Feb 28, 2025
1 parent fc90ae4 commit c2e91f9
Show file tree
Hide file tree
Showing 3 changed files with 19 additions and 14 deletions.
Original file line number Diff line number Diff line change
Expand Up @@ -160,8 +160,8 @@ std::vector<geometry_msgs::msg::Point> get_path_bound(
* @param [in] output_ptr output path with modified points for the goal
*/
bool set_goal(
const double search_radius_range, const PathWithLaneId & input, const geometry_msgs::msg::Pose & goal,
const int64_t goal_lane_id, PathWithLaneId * output_ptr);
const double search_radius_range, const PathWithLaneId & input,
const geometry_msgs::msg::Pose & goal, const int64_t goal_lane_id, PathWithLaneId * output_ptr);

/**
* @brief Recreate the goal pose to prevent the goal point being too far from the lanelet, which
Expand All @@ -184,8 +184,8 @@ const geometry_msgs::msg::Pose refine_goal(
* @return Recreated path
*/
PathWithLaneId refine_path_for_goal(
const double search_radius_range, const PathWithLaneId & input, const geometry_msgs::msg::Pose & goal,
const int64_t goal_lane_id);
const double search_radius_range, const PathWithLaneId & input,
const geometry_msgs::msg::Pose & goal, const int64_t goal_lane_id);

/**
* @brief Extract lanelets from the path.
Expand Down
8 changes: 5 additions & 3 deletions planning/autoware_path_generator/src/node.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -53,9 +53,11 @@ PathGenerator::PathGenerator(const rclcpp::NodeOptions & node_options)
declare_parameter<double>("search_radius_decrement");

// Ensure that the refine_goal_search_radius_range and search_radius_decrement must be positive
if (planner_data_.path_generator_parameters.refine_goal_search_radius_range <= 0 ||
planner_data_.path_generator_parameters.search_radius_decrement <= 0) {
throw std::runtime_error("refine_goal_search_radius_range and search_radius_decrement must be positive");
if (
planner_data_.path_generator_parameters.refine_goal_search_radius_range <= 0 ||
planner_data_.path_generator_parameters.search_radius_decrement <= 0) {
throw std::runtime_error(
"refine_goal_search_radius_range and search_radius_decrement must be positive");
}

param_listener_ =
Expand Down
17 changes: 10 additions & 7 deletions planning/autoware_path_generator/src/utils.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -63,8 +63,8 @@ std::optional<double> calc_interpolated_z(
const double next_z = input.points.at(seg_idx + 1).point.pose.position.z;

return std::abs(seg_dist) < 1e-6
? next_z
: closest_z + (next_z - closest_z) * closest_to_target_dist / seg_dist;
? next_z
: closest_z + (next_z - closest_z) * closest_to_target_dist / seg_dist;

} catch (const std::exception & e) {
return std::nullopt;
Expand Down Expand Up @@ -344,7 +344,8 @@ std::vector<geometry_msgs::msg::Point> get_path_bound(
}

std::optional<PathWithLaneId> set_goal(
const double search_radius_range, const PathWithLaneId & input, const geometry_msgs::msg::Pose & goal)
const double search_radius_range, const PathWithLaneId & input,
const geometry_msgs::msg::Pose & goal)
{
try {
if (input.points.empty()) {
Expand Down Expand Up @@ -381,7 +382,9 @@ std::optional<PathWithLaneId> set_goal(
autoware::motion_utils::findFirstNearestIndexWithSoftConstraints(
input.points, pre_refined_goal.point.pose, 3.0, M_PI_4);

if (auto z = calc_interpolated_z(input, pre_refined_goal.point.pose.position, closest_seg_idx_for_pre_goal)) {
if (
auto z = calc_interpolated_z(
input, pre_refined_goal.point.pose.position, closest_seg_idx_for_pre_goal)) {
pre_refined_goal.point.pose.position.z = *z;
} else {
return std::nullopt;
Expand Down Expand Up @@ -425,8 +428,7 @@ std::optional<PathWithLaneId> set_goal(

} catch (std::out_of_range & ex) {
RCLCPP_ERROR_STREAM(
rclcpp::get_logger("path_generator").get_child("utils"),
"failed to set goal: " << ex.what());
rclcpp::get_logger("path_generator").get_child("utils"), "failed to set goal: " << ex.what());
return std::nullopt;
}
}
Expand Down Expand Up @@ -476,7 +478,8 @@ const geometry_msgs::msg::Pose refine_goal(
}

PathWithLaneId refine_path_for_goal(
const double search_radius_range, const PathWithLaneId & input, const geometry_msgs::msg::Pose & goal)
const double search_radius_range, const PathWithLaneId & input,
const geometry_msgs::msg::Pose & goal)
{
PathWithLaneId filtered_path = input;
filtered_path.points = autoware::motion_utils::removeOverlapPoints(filtered_path.points);
Expand Down

0 comments on commit c2e91f9

Please sign in to comment.