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 378fa6e0a..73f06d221 100644 --- a/planning/autoware_path_generator/include/autoware/path_generator/utils.hpp +++ b/planning/autoware_path_generator/include/autoware/path_generator/utils.hpp @@ -160,8 +160,8 @@ std::vector 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 @@ -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. diff --git a/planning/autoware_path_generator/src/node.cpp b/planning/autoware_path_generator/src/node.cpp index a40b3adda..e8810fbcc 100644 --- a/planning/autoware_path_generator/src/node.cpp +++ b/planning/autoware_path_generator/src/node.cpp @@ -53,9 +53,11 @@ PathGenerator::PathGenerator(const rclcpp::NodeOptions & node_options) declare_parameter("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_ = diff --git a/planning/autoware_path_generator/src/utils.cpp b/planning/autoware_path_generator/src/utils.cpp index 3556ca636..002910cbc 100644 --- a/planning/autoware_path_generator/src/utils.cpp +++ b/planning/autoware_path_generator/src/utils.cpp @@ -63,8 +63,8 @@ std::optional 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; @@ -344,7 +344,8 @@ std::vector get_path_bound( } std::optional 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()) { @@ -381,7 +382,9 @@ std::optional 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; @@ -425,8 +428,7 @@ std::optional 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; } } @@ -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);