From d12367cd0ac884dfaa26dcfa7193b4ada38dabcd Mon Sep 17 00:00:00 2001 From: Junya Sasaki Date: Wed, 26 Feb 2025 22:14:31 +0900 Subject: [PATCH 01/41] feat: function to smooth the route (see below) Description: This commit is kind of feature porting from `autoware.universe` as follows * Import `PathWithLaneId DefaultFixedGoalPlanner::modifyPathForSmoothGoalConnection` from the following `autoware.universe` code https://github.com/autowarefoundation/autoware.universe/blob/a0816b7e3e35fbe822fefbb9c9a8132365608b49/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/src/default_fixed_goal_planner.cpp#L74-L104 * Also import all related functions from the `autoware.universe` side Signed-off-by: Junya Sasaki --- .../path_generator/common_structs.hpp | 10 + .../include/autoware/path_generator/utils.hpp | 85 +++++ planning/autoware_path_generator/src/node.cpp | 12 +- .../autoware_path_generator/src/utils.cpp | 323 ++++++++++++++++++ 4 files changed, 429 insertions(+), 1 deletion(-) diff --git a/planning/autoware_path_generator/include/autoware/path_generator/common_structs.hpp b/planning/autoware_path_generator/include/autoware/path_generator/common_structs.hpp index a1b774e93a..eb0f7ca9d7 100644 --- a/planning/autoware_path_generator/include/autoware/path_generator/common_structs.hpp +++ b/planning/autoware_path_generator/include/autoware/path_generator/common_structs.hpp @@ -24,6 +24,12 @@ namespace autoware::path_generator { + +struct PathGeneratorParameters +{ + double refine_goal_search_radius_range; +}; + struct PlannerData { lanelet::LaneletMapPtr lanelet_map_ptr{nullptr}; @@ -37,6 +43,10 @@ struct PlannerData lanelet::ConstLanelets preferred_lanelets{}; lanelet::ConstLanelets start_lanelets{}; lanelet::ConstLanelets goal_lanelets{}; + + PathGeneratorParameters path_generator_parameters{}; + + lanelet::Id goal_lane_id{}; }; } // namespace autoware::path_generator 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 89eb19594f..df8ce3797f 100644 --- a/planning/autoware_path_generator/include/autoware/path_generator/utils.hpp +++ b/planning/autoware_path_generator/include/autoware/path_generator/utils.hpp @@ -137,6 +137,91 @@ std::vector get_path_bound( const lanelet::CompoundLineString2d & lanelet_bound, const lanelet::CompoundLineString2d & lanelet_centerline, const double s_start, const double s_end); + +/** + * @brief Modify the path points near the goal to smoothly connect the input path and the goal + * point + * @details Remove the path points that are forward from the goal by the distance of + * search_radius_range. Then insert the goal into the path. The previous goal point generated + * from the goal posture information is also inserted for the smooth connection of the goal pose. + * @param [in] search_radius_range distance on path to be modified for goal insertion + * @param [in] search_rad_range [unused] + * @param [in] input original path + * @param [in] goal original goal pose + * @param [in] goal_lane_id [unused] + * @param [in] output_ptr output path with modified points for the goal + */ +bool set_goal( + const double search_radius_range, const double search_rad_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 + * causes the path to twist near the goal. + * @details Return the goal point projected on the straight line of the segment of lanelet + * closest to the original goal. + * @param [in] goal original goal pose + * @param [in] goal_lanelet lanelet containing the goal pose + */ +const geometry_msgs::msg::Pose refine_goal(const geometry_msgs::msg::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 refine_path_for_goal( + const double search_radius_range, const double search_rad_range, const PathWithLaneId & input, + const geometry_msgs::msg::Pose & goal, const int64_t goal_lane_id); + +/** + * @brief Extract lanelets from the path. + * @param path Input path. + * @param planner_data Planner data. + * @return Extracted lanelets + */ +lanelet::ConstLanelets extract_lanelets_from_path( + const PathWithLaneId & refined_path, const std::shared_ptr & planner_data); + +/** + * @brief Get the goal lanelet. + * @param planner_data Planner data. + * @param goal_lanelet Goal lanelet. + * @return True if the goal lanelet is found, false otherwise + */ +bool get_goal_lanelet(const PlannerData & planner_data, lanelet::ConstLanelet * goal_lanelet); + +/** + * @brief Check if the pose is in the lanelets. + * @param pose Pose. + * @param lanes Lanelets. + * @return True if the pose is in the lanelets, false otherwise + */ +bool is_in_lanelets(const geometry_msgs::msg::Pose & pose, const lanelet::ConstLanelets & lanes); + +/** + * @brief Check if the path is valid. + * @param refined_path Input path. + * @param planner_data Planner data. + * @return True if the path is valid, false otherwise + */ +bool is_path_valid( + const PathWithLaneId & refined_path, const std::shared_ptr & planner_data); + +/** + * @brief Modify the path to connect smoothly to the goal. + * @param path Input path. + * @param planner_data Planner data. + * @return Modified path + */ +PathWithLaneId modify_path_for_smooth_goal_connection( + const PathWithLaneId & path, const std::shared_ptr & planner_data +); + } // namespace utils } // namespace autoware::path_generator diff --git a/planning/autoware_path_generator/src/node.cpp b/planning/autoware_path_generator/src/node.cpp index ada61f4f97..62374a70c0 100644 --- a/planning/autoware_path_generator/src/node.cpp +++ b/planning/autoware_path_generator/src/node.cpp @@ -46,6 +46,10 @@ namespace autoware::path_generator PathGenerator::PathGenerator(const rclcpp::NodeOptions & node_options) : Node("path_generator", node_options) { + // Initialize path_generator_parameters + planner_data_.path_generator_parameters.refine_goal_search_radius_range = + declare_parameter("refine_goal_search_radius_range"); + param_listener_ = std::make_shared<::path_generator::ParamListener>(this->get_node_parameters_interface()); @@ -143,6 +147,8 @@ void PathGenerator::set_route(const LaneletRoute::ConstSharedPtr & route_ptr) } } + planner_data_.goal_lane_id = route_ptr->segments.back().preferred_primitive.id; + const auto set_lanelets_from_segment = [&]( const autoware_planning_msgs::msg::LaneletSegment & segment, @@ -195,7 +201,11 @@ std::optional PathGenerator::plan_path(const InputData & input_d return std::nullopt; } - return path; + // Make the path smooth + auto planner_data_ptr = std::make_shared(planner_data_); + const auto smooth_path = utils::modify_path_for_smooth_goal_connection(*path, planner_data_ptr); + + return smooth_path; } std::optional PathGenerator::generate_path( diff --git a/planning/autoware_path_generator/src/utils.cpp b/planning/autoware_path_generator/src/utils.cpp index 3947a7a466..f9eeb2836e 100644 --- a/planning/autoware_path_generator/src/utils.cpp +++ b/planning/autoware_path_generator/src/utils.cpp @@ -14,6 +14,7 @@ #include "autoware/path_generator/utils.hpp" +#include #include #include #include @@ -35,6 +36,97 @@ bool exists(const std::vector & vec, const T & item) { return std::find(vec.begin(), vec.end(), item) != vec.end(); } + +double calc_interpolated_z( + const autoware_internal_planning_msgs::msg::PathWithLaneId & input, + const geometry_msgs::msg::Point target_pos, const size_t seg_idx) +{ + const double closest_to_target_dist = autoware::motion_utils::calcSignedArcLength( + input.points, input.points.at(seg_idx).point.pose.position, + target_pos); // TODO(murooka) implement calcSignedArcLength(points, idx, point) + const double seg_dist = + autoware::motion_utils::calcSignedArcLength(input.points, seg_idx, seg_idx + 1); + + const double closest_z = input.points.at(seg_idx).point.pose.position.z; + const double next_z = input.points.at(seg_idx + 1).point.pose.position.z; + const double interpolated_z = + std::abs(seg_dist) < 1e-6 + ? next_z + : closest_z + (next_z - closest_z) * closest_to_target_dist / seg_dist; + return interpolated_z; +} + +double calc_interpolated_velocity( + const autoware_internal_planning_msgs::msg::PathWithLaneId & input, const size_t seg_idx) +{ + const double seg_dist = + autoware::motion_utils::calcSignedArcLength(input.points, seg_idx, seg_idx + 1); + + const double closest_vel = input.points.at(seg_idx).point.longitudinal_velocity_mps; + const double next_vel = input.points.at(seg_idx + 1).point.longitudinal_velocity_mps; + const double interpolated_vel = std::abs(seg_dist) < 1e-06 ? next_vel : closest_vel; + return interpolated_vel; +} + +template +size_t find_nearest_segment_index( + const std::vector & points, const geometry_msgs::msg::Pose & pose, const double dist_threshold, + const double yaw_threshold) +{ + const auto nearest_idx = + autoware::motion_utils::findNearestSegmentIndex(points, pose, dist_threshold, yaw_threshold); + if (nearest_idx) { + return nearest_idx.value(); + } + + return autoware::motion_utils::findNearestSegmentIndex(points, pose.position); +} + +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 = std::numeric_limits::max()) +{ + if (points.empty()) { + return std::nullopt; + } + + // find goal index + size_t min_dist_index; + { + bool found = false; + double min_dist = std::numeric_limits::max(); + for (size_t i = 0; i < points.size(); ++i) { + const auto & lane_ids = points.at(i).lane_ids; + + const double dist_to_goal = + autoware_utils::calc_distance2d(points.at(i).point.pose, goal); + const bool is_goal_lane_id_in_point = + std::find(lane_ids.begin(), lane_ids.end(), goal_lane_id) != lane_ids.end(); + if (dist_to_goal < max_dist && dist_to_goal < min_dist && is_goal_lane_id_in_point) { + min_dist_index = i; + min_dist = dist_to_goal; + found = true; + } + } + if (!found) { + return std::nullopt; + } + } + + // find index out of goal search range + size_t min_dist_out_of_range_index = min_dist_index; + for (int i = min_dist_index; 0 <= i; --i) { + const double dist = autoware_utils::calc_distance2d(points.at(i).point, goal); + min_dist_out_of_range_index = i; + if (max_dist < dist) { + break; + } + } + + return min_dist_out_of_range_index; +} + } // namespace std::optional get_lanelets_within_route( @@ -294,5 +386,236 @@ std::vector get_path_bound( return path_bound; } + +// goal does not have z +bool set_goal( + const double search_radius_range, [[maybe_unused]] const double search_rad_range, + const PathWithLaneId & input, const geometry_msgs::msg::Pose & goal, const int64_t goal_lane_id, + PathWithLaneId * output_ptr) +{ + try { + if (input.points.empty()) { + return false; + } + + // calculate refined_goal with interpolation + // NOTE: goal does not have valid z, that will be calculated by interpolation here + PathPointWithLaneId refined_goal{}; + const size_t closest_seg_idx_for_goal = + find_nearest_segment_index(input.points, goal, 3.0, M_PI_4); + refined_goal.point.pose = goal; + refined_goal.point.pose.position.z = + calc_interpolated_z(input, goal.position, closest_seg_idx_for_goal); + refined_goal.point.longitudinal_velocity_mps = 0.0; + + // calculate pre_refined_goal with interpolation + // NOTE: z and velocity are filled + PathPointWithLaneId pre_refined_goal{}; + constexpr double goal_to_pre_goal_distance = -1.0; + pre_refined_goal.point.pose = + autoware_utils::calc_offset_pose(goal, goal_to_pre_goal_distance, 0.0, 0.0); + const size_t closest_seg_idx_for_pre_goal = + find_nearest_segment_index(input.points, pre_refined_goal.point.pose, 3.0, M_PI_4); + pre_refined_goal.point.pose.position.z = + calc_interpolated_z(input, pre_refined_goal.point.pose.position, closest_seg_idx_for_pre_goal); + pre_refined_goal.point.longitudinal_velocity_mps = + calc_interpolated_velocity(input, closest_seg_idx_for_pre_goal); + + // 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); + if (!min_dist_out_of_circle_index_opt) { + return false; + } + const size_t min_dist_out_of_circle_index = min_dist_out_of_circle_index_opt.value(); + + // create output points + output_ptr->points.reserve(output_ptr->points.size() + min_dist_out_of_circle_index + 3); + for (size_t i = 0; i <= min_dist_out_of_circle_index; ++i) { + output_ptr->points.push_back(input.points.at(i)); + } + output_ptr->points.push_back(pre_refined_goal); + output_ptr->points.push_back(refined_goal); + + { // fill skipped lane ids + // pre refined goal + auto & pre_goal = output_ptr->points.at(output_ptr->points.size() - 2); + for (size_t i = min_dist_out_of_circle_index + 1; i < input.points.size(); ++i) { + for (const auto target_lane_id : input.points.at(i).lane_ids) { + const bool is_lane_id_found = + std::find(pre_goal.lane_ids.begin(), pre_goal.lane_ids.end(), target_lane_id) != + pre_goal.lane_ids.end(); + if (!is_lane_id_found) { + pre_goal.lane_ids.push_back(target_lane_id); + } + } + } + + // goal + output_ptr->points.back().lane_ids = input.points.back().lane_ids; + } + + output_ptr->left_bound = input.left_bound; + output_ptr->right_bound = input.right_bound; + return true; + } catch (std::out_of_range & ex) { + RCLCPP_ERROR_STREAM( + rclcpp::get_logger("behavior_path_planner").get_child("utils"), + "failed to set goal: " << ex.what()); + return false; + } +} + +const geometry_msgs::msg::Pose refine_goal(const geometry_msgs::msg::Pose & goal, const lanelet::ConstLanelet & goal_lanelet) +{ + const auto lanelet_point = lanelet::utils::conversion::toLaneletPoint(goal.position); + const double distance = boost::geometry::distance( + goal_lanelet.polygon2d().basicPolygon(), lanelet::utils::to2D(lanelet_point).basicPoint()); + if (distance < std::numeric_limits::epsilon()) { + return goal; + } + + const auto segment = lanelet::utils::getClosestSegment( + lanelet::utils::to2D(lanelet_point), goal_lanelet.centerline()); + if (segment.empty()) { + return goal; + } + + geometry_msgs::msg::Pose refined_goal; + { + // find position + const auto p1 = segment.front().basicPoint(); + const auto p2 = segment.back().basicPoint(); + const auto direction_vector = (p2 - p1).normalized(); + const auto p1_to_goal = lanelet_point.basicPoint() - p1; + const double s = direction_vector.dot(p1_to_goal); + const auto refined_point = p1 + direction_vector * s; + + refined_goal.position.x = refined_point.x(); + refined_goal.position.y = refined_point.y(); + refined_goal.position.z = refined_point.z(); + + // find orientation + const double yaw = std::atan2(direction_vector.y(), direction_vector.x()); + tf2::Quaternion tf_quat; + tf_quat.setRPY(0, 0, yaw); + refined_goal.orientation = tf2::toMsg(tf_quat); + } + return refined_goal; +} + +PathWithLaneId refine_path_for_goal( + const double search_radius_range, const double search_rad_range, const PathWithLaneId & input, + const geometry_msgs::msg::Pose & goal, const int64_t goal_lane_id) +{ + PathWithLaneId filtered_path = input; + PathWithLaneId path_with_goal; + filtered_path.points = autoware::motion_utils::removeOverlapPoints(filtered_path.points); + + // always set zero velocity at the end of path for safety + if (!filtered_path.points.empty()) { + filtered_path.points.back().point.longitudinal_velocity_mps = 0.0; + } + + if (set_goal( + search_radius_range, search_rad_range, filtered_path, goal, goal_lane_id, + &path_with_goal)) { + return path_with_goal; + } + return filtered_path; +} + +lanelet::ConstLanelets extract_lanelets_from_path( + const PathWithLaneId & refined_path, + const std::shared_ptr & planner_data) +{ + lanelet::ConstLanelets refined_path_lanelets; + for (size_t i = 0; i < refined_path.points.size(); ++i) { + const auto & path_point = refined_path.points.at(i); + int64_t lane_id = path_point.lane_ids.at(0); + lanelet::ConstLanelet lanelet = planner_data->lanelet_map_ptr->laneletLayer.get(lane_id); + bool is_unique = + std::find(refined_path_lanelets.begin(), refined_path_lanelets.end(), lanelet) == + refined_path_lanelets.end(); + if (is_unique) { + refined_path_lanelets.push_back(lanelet); + } + } + return refined_path_lanelets; +} + +bool get_goal_lanelet(const PlannerData & planner_data, lanelet::ConstLanelet * goal_lanelet) +{ + const lanelet::Id goal_lane_id = planner_data.goal_lane_id; + for (const auto & llt : planner_data.route_lanelets) { + if (llt.id() == goal_lane_id) { + *goal_lanelet = llt; + return true; + } + } + return false; +} + +bool is_in_lanelets(const geometry_msgs::msg::Pose & pose, const lanelet::ConstLanelets & lanes) +{ + for (const auto & lane : lanes) { + if (lanelet::utils::isInLanelet(pose, lane)) { + return true; + } + } + return false; +} + +bool is_path_valid( + const PathWithLaneId & refined_path, + const std::shared_ptr & planner_data) +{ + const auto lanelets = extract_lanelets_from_path(refined_path, planner_data); + // std::any_of detects whether any point lies outside lanelets + bool has_points_outside_lanelet = std::any_of( + refined_path.points.begin(), refined_path.points.end(), + [&lanelets](const auto & refined_path_point) { + return !is_in_lanelets(refined_path_point.point.pose, lanelets); + }); + return !has_points_outside_lanelet; +} + +PathWithLaneId modify_path_for_smooth_goal_connection( + const PathWithLaneId & path, const std::shared_ptr & planner_data +) +{ + const auto goal = planner_data->goal_pose; + const auto goal_lane_id = planner_data->goal_lane_id; + + geometry_msgs::msg::Pose refined_goal{}; + { + lanelet::ConstLanelet goal_lanelet; + + // First, polish up the goal pose if possible + if (get_goal_lanelet(*planner_data, &goal_lanelet)) { + refined_goal = refine_goal(goal, goal_lanelet); + } else { + refined_goal = goal; + } + } + double goal_search_radius{planner_data->path_generator_parameters.refine_goal_search_radius_range}; + + // TODO(shen): define in the parameter + constexpr double range_reduce_by{1.0}; // set a reasonable value, 10% - 20% of the + // refine_goal_search_radius_range is recommended + bool is_valid_path{false}; + PathWithLaneId refined_path; + + // Then, refine the path for the goal + while (goal_search_radius >= 0 && !is_valid_path) { + refined_path = + refine_path_for_goal(goal_search_radius, M_PI * 0.5, path, refined_goal, goal_lane_id); + if (is_path_valid(refined_path, planner_data)) { + is_valid_path = true; + } + goal_search_radius -= range_reduce_by; + } + return refined_path; +} } // namespace utils } // namespace autoware::path_generator From 04596a175b585305ab0e34786e78904a9608c6d0 Mon Sep 17 00:00:00 2001 From: "pre-commit-ci[bot]" <66853113+pre-commit-ci[bot]@users.noreply.github.com> Date: Wed, 26 Feb 2025 13:43:29 +0000 Subject: [PATCH 02/41] style(pre-commit): autofix --- .../include/autoware/path_generator/utils.hpp | 6 ++--- .../autoware_path_generator/src/utils.cpp | 22 +++++++++---------- 2 files changed, 13 insertions(+), 15 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 df8ce3797f..349bf52dd2 100644 --- a/planning/autoware_path_generator/include/autoware/path_generator/utils.hpp +++ b/planning/autoware_path_generator/include/autoware/path_generator/utils.hpp @@ -163,7 +163,8 @@ bool set_goal( * @param [in] goal original goal pose * @param [in] goal_lanelet lanelet containing the goal pose */ -const geometry_msgs::msg::Pose refine_goal(const geometry_msgs::msg::Pose & goal, const lanelet::ConstLanelet & goal_lanelet); +const geometry_msgs::msg::Pose refine_goal( + const geometry_msgs::msg::Pose & goal, const lanelet::ConstLanelet & goal_lanelet); /** * @brief Recreate the path with a given goal pose. @@ -219,8 +220,7 @@ bool is_path_valid( * @return Modified path */ PathWithLaneId modify_path_for_smooth_goal_connection( - const PathWithLaneId & path, const std::shared_ptr & planner_data -); + const PathWithLaneId & path, const std::shared_ptr & planner_data); } // namespace utils } // namespace autoware::path_generator diff --git a/planning/autoware_path_generator/src/utils.cpp b/planning/autoware_path_generator/src/utils.cpp index f9eeb2836e..acc82f4300 100644 --- a/planning/autoware_path_generator/src/utils.cpp +++ b/planning/autoware_path_generator/src/utils.cpp @@ -99,8 +99,7 @@ std::optional find_index_out_of_goal_search_range( for (size_t i = 0; i < points.size(); ++i) { const auto & lane_ids = points.at(i).lane_ids; - const double dist_to_goal = - autoware_utils::calc_distance2d(points.at(i).point.pose, goal); + const double dist_to_goal = autoware_utils::calc_distance2d(points.at(i).point.pose, goal); const bool is_goal_lane_id_in_point = std::find(lane_ids.begin(), lane_ids.end(), goal_lane_id) != lane_ids.end(); if (dist_to_goal < max_dist && dist_to_goal < min_dist && is_goal_lane_id_in_point) { @@ -416,8 +415,8 @@ bool set_goal( autoware_utils::calc_offset_pose(goal, goal_to_pre_goal_distance, 0.0, 0.0); const size_t closest_seg_idx_for_pre_goal = find_nearest_segment_index(input.points, pre_refined_goal.point.pose, 3.0, M_PI_4); - pre_refined_goal.point.pose.position.z = - calc_interpolated_z(input, pre_refined_goal.point.pose.position, closest_seg_idx_for_pre_goal); + pre_refined_goal.point.pose.position.z = calc_interpolated_z( + input, pre_refined_goal.point.pose.position, closest_seg_idx_for_pre_goal); pre_refined_goal.point.longitudinal_velocity_mps = calc_interpolated_velocity(input, closest_seg_idx_for_pre_goal); @@ -466,7 +465,8 @@ bool set_goal( } } -const geometry_msgs::msg::Pose refine_goal(const geometry_msgs::msg::Pose & goal, const lanelet::ConstLanelet & goal_lanelet) +const geometry_msgs::msg::Pose refine_goal( + const geometry_msgs::msg::Pose & goal, const lanelet::ConstLanelet & goal_lanelet) { const auto lanelet_point = lanelet::utils::conversion::toLaneletPoint(goal.position); const double distance = boost::geometry::distance( @@ -526,8 +526,7 @@ PathWithLaneId refine_path_for_goal( } lanelet::ConstLanelets extract_lanelets_from_path( - const PathWithLaneId & refined_path, - const std::shared_ptr & planner_data) + const PathWithLaneId & refined_path, const std::shared_ptr & planner_data) { lanelet::ConstLanelets refined_path_lanelets; for (size_t i = 0; i < refined_path.points.size(); ++i) { @@ -567,8 +566,7 @@ bool is_in_lanelets(const geometry_msgs::msg::Pose & pose, const lanelet::ConstL } bool is_path_valid( - const PathWithLaneId & refined_path, - const std::shared_ptr & planner_data) + const PathWithLaneId & refined_path, const std::shared_ptr & planner_data) { const auto lanelets = extract_lanelets_from_path(refined_path, planner_data); // std::any_of detects whether any point lies outside lanelets @@ -581,8 +579,7 @@ bool is_path_valid( } PathWithLaneId modify_path_for_smooth_goal_connection( - const PathWithLaneId & path, const std::shared_ptr & planner_data -) + const PathWithLaneId & path, const std::shared_ptr & planner_data) { const auto goal = planner_data->goal_pose; const auto goal_lane_id = planner_data->goal_lane_id; @@ -598,7 +595,8 @@ PathWithLaneId modify_path_for_smooth_goal_connection( refined_goal = goal; } } - double goal_search_radius{planner_data->path_generator_parameters.refine_goal_search_radius_range}; + double goal_search_radius{ + planner_data->path_generator_parameters.refine_goal_search_radius_range}; // TODO(shen): define in the parameter constexpr double range_reduce_by{1.0}; // set a reasonable value, 10% - 20% of the From 17054c956505b2f0ab26106c438c23f1c02401bb Mon Sep 17 00:00:00 2001 From: Junya Sasaki Date: Fri, 28 Feb 2025 16:20:24 +0900 Subject: [PATCH 03/41] bugs: fix remaining conflicts Signed-off-by: Junya Sasaki --- planning/autoware_path_generator/src/utils.cpp | 5 ++--- 1 file changed, 2 insertions(+), 3 deletions(-) diff --git a/planning/autoware_path_generator/src/utils.cpp b/planning/autoware_path_generator/src/utils.cpp index b7a80385aa..ee3881d8a5 100644 --- a/planning/autoware_path_generator/src/utils.cpp +++ b/planning/autoware_path_generator/src/utils.cpp @@ -392,7 +392,6 @@ std::vector get_path_bound( return path_bound; } -<<<<<<< HEAD // goal does not have z bool set_goal( const double search_radius_range, [[maybe_unused]] const double search_rad_range, @@ -621,7 +620,8 @@ PathWithLaneId modify_path_for_smooth_goal_connection( goal_search_radius -= range_reduce_by; } return refined_path; -======= +} + TurnIndicatorsCommand get_turn_signal( const PathWithLaneId & path, const PlannerData & planner_data, const geometry_msgs::msg::Pose & current_pose, const double current_vel, @@ -746,7 +746,6 @@ std::optional get_turn_signal_required_end_point( return lanelet::utils::conversion::toLaneletPoint( centerline->compute(intervals.front().start).position); ->>>>>>> main } } // namespace utils } // namespace autoware::path_generator From 5a2766e31d6a9da471fc0ad7efc2ccead919d06f Mon Sep 17 00:00:00 2001 From: Junya Sasaki Date: Fri, 28 Feb 2025 18:11:30 +0900 Subject: [PATCH 04/41] Update planning/autoware_path_generator/src/utils.cpp Signed-off-by: Junya Sasaki Co-authored-by: Kosuke Takeuchi --- planning/autoware_path_generator/src/utils.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/planning/autoware_path_generator/src/utils.cpp b/planning/autoware_path_generator/src/utils.cpp index ee3881d8a5..ec6df4621b 100644 --- a/planning/autoware_path_generator/src/utils.cpp +++ b/planning/autoware_path_generator/src/utils.cpp @@ -539,7 +539,7 @@ lanelet::ConstLanelets extract_lanelets_from_path( const auto & path_point = refined_path.points.at(i); int64_t lane_id = path_point.lane_ids.at(0); lanelet::ConstLanelet lanelet = planner_data->lanelet_map_ptr->laneletLayer.get(lane_id); - bool is_unique = + const bool is_unique = std::find(refined_path_lanelets.begin(), refined_path_lanelets.end(), lanelet) == refined_path_lanelets.end(); if (is_unique) { From 808fb5d12950c5f2aad54bc4d9cb3eae7e93e112 Mon Sep 17 00:00:00 2001 From: Junya Sasaki Date: Fri, 28 Feb 2025 18:13:05 +0900 Subject: [PATCH 05/41] Update planning/autoware_path_generator/src/utils.cpp Signed-off-by: Junya Sasaki Co-authored-by: Kosuke Takeuchi --- planning/autoware_path_generator/src/utils.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/planning/autoware_path_generator/src/utils.cpp b/planning/autoware_path_generator/src/utils.cpp index ec6df4621b..76fdcbd6b1 100644 --- a/planning/autoware_path_generator/src/utils.cpp +++ b/planning/autoware_path_generator/src/utils.cpp @@ -576,7 +576,7 @@ bool is_path_valid( { const auto lanelets = extract_lanelets_from_path(refined_path, planner_data); // std::any_of detects whether any point lies outside lanelets - bool has_points_outside_lanelet = std::any_of( + const bool has_points_outside_lanelet = std::any_of( refined_path.points.begin(), refined_path.points.end(), [&lanelets](const auto & refined_path_point) { return !is_in_lanelets(refined_path_point.point.pose, lanelets); From 19185eb6d1946d891f7b432d0da3def598c8e15b Mon Sep 17 00:00:00 2001 From: Junya Sasaki Date: Fri, 28 Feb 2025 22:18:09 +0900 Subject: [PATCH 06/41] refactor: as follows * Enhance error handlings * Remove unused variables * Simplify the code * Improve readability a little bit Signed-off-by: Junya Sasaki --- .../path_generator/common_structs.hpp | 11 +- .../include/autoware/path_generator/utils.hpp | 11 +- planning/autoware_path_generator/src/node.cpp | 8 + .../autoware_path_generator/src/utils.cpp | 245 ++++++++---------- 4 files changed, 128 insertions(+), 147 deletions(-) diff --git a/planning/autoware_path_generator/include/autoware/path_generator/common_structs.hpp b/planning/autoware_path_generator/include/autoware/path_generator/common_structs.hpp index eb0f7ca9d7..278a8840dd 100644 --- a/planning/autoware_path_generator/include/autoware/path_generator/common_structs.hpp +++ b/planning/autoware_path_generator/include/autoware/path_generator/common_structs.hpp @@ -27,7 +27,15 @@ namespace autoware::path_generator struct PathGeneratorParameters { + /** + * @brief Distance on path to be modified for goal insertion + */ double refine_goal_search_radius_range; + + /** + * @brief Decrement of the search radius range + */ + double search_radius_decrement; }; struct PlannerData @@ -37,6 +45,7 @@ struct PlannerData lanelet::routing::RoutingGraphPtr routing_graph_ptr{nullptr}; std::string route_frame_id{}; + lanelet::Id goal_lane_id{}; geometry_msgs::msg::Pose goal_pose{}; lanelet::ConstLanelets route_lanelets{}; @@ -45,8 +54,6 @@ struct PlannerData lanelet::ConstLanelets goal_lanelets{}; PathGeneratorParameters path_generator_parameters{}; - - lanelet::Id goal_lane_id{}; }; } // namespace autoware::path_generator 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 08a90f3880..378fa6e0a5 100644 --- a/planning/autoware_path_generator/include/autoware/path_generator/utils.hpp +++ b/planning/autoware_path_generator/include/autoware/path_generator/utils.hpp @@ -154,15 +154,14 @@ std::vector get_path_bound( * search_radius_range. Then insert the goal into the path. The previous goal point generated * from the goal posture information is also inserted for the smooth connection of the goal pose. * @param [in] search_radius_range distance on path to be modified for goal insertion - * @param [in] search_rad_range [unused] * @param [in] input original path * @param [in] goal original goal pose - * @param [in] goal_lane_id [unused] + * @param [in] goal_lane_id * @param [in] output_ptr output path with modified points for the goal */ bool set_goal( - const double search_radius_range, const double search_rad_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 @@ -185,8 +184,8 @@ const geometry_msgs::msg::Pose refine_goal( * @return Recreated path */ PathWithLaneId refine_path_for_goal( - const double search_radius_range, const double search_rad_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 862394517d..a40b3adda5 100644 --- a/planning/autoware_path_generator/src/node.cpp +++ b/planning/autoware_path_generator/src/node.cpp @@ -49,6 +49,14 @@ PathGenerator::PathGenerator(const rclcpp::NodeOptions & node_options) // Initialize path_generator_parameters planner_data_.path_generator_parameters.refine_goal_search_radius_range = declare_parameter("refine_goal_search_radius_range"); + planner_data_.path_generator_parameters.search_radius_decrement = + 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"); + } param_listener_ = std::make_shared<::path_generator::ParamListener>(this->get_node_parameters_interface()); diff --git a/planning/autoware_path_generator/src/utils.cpp b/planning/autoware_path_generator/src/utils.cpp index ee3881d8a5..c554b56530 100644 --- a/planning/autoware_path_generator/src/utils.cpp +++ b/planning/autoware_path_generator/src/utils.cpp @@ -43,23 +43,32 @@ bool exists(const std::vector & vec, const T & item) return std::find(vec.begin(), vec.end(), item) != vec.end(); } -double calc_interpolated_z( +std::optional calc_interpolated_z( const autoware_internal_planning_msgs::msg::PathWithLaneId & input, const geometry_msgs::msg::Point target_pos, const size_t seg_idx) { - const double closest_to_target_dist = autoware::motion_utils::calcSignedArcLength( - input.points, input.points.at(seg_idx).point.pose.position, - target_pos); // TODO(murooka) implement calcSignedArcLength(points, idx, point) - const double seg_dist = - autoware::motion_utils::calcSignedArcLength(input.points, seg_idx, seg_idx + 1); + // Check if input is empty or seg_idx is invalid + if (input.points.empty() || seg_idx >= input.points.size() - 1) { + return std::nullopt; + } + + try { + const double closest_to_target_dist = autoware::motion_utils::calcSignedArcLength( + input.points, input.points.at(seg_idx).point.pose.position, target_pos); + + const double seg_dist = + autoware::motion_utils::calcSignedArcLength(input.points, seg_idx, seg_idx + 1); + + const double closest_z = input.points.at(seg_idx).point.pose.position.z; + const double next_z = input.points.at(seg_idx + 1).point.pose.position.z; - const double closest_z = input.points.at(seg_idx).point.pose.position.z; - const double next_z = input.points.at(seg_idx + 1).point.pose.position.z; - const double interpolated_z = - std::abs(seg_dist) < 1e-6 + return std::abs(seg_dist) < 1e-6 ? next_z : closest_z + (next_z - closest_z) * closest_to_target_dist / seg_dist; - return interpolated_z; + + } catch (const std::exception & e) { + return std::nullopt; + } } double calc_interpolated_velocity( @@ -74,64 +83,6 @@ double calc_interpolated_velocity( return interpolated_vel; } -template -size_t find_nearest_segment_index( - const std::vector & points, const geometry_msgs::msg::Pose & pose, const double dist_threshold, - const double yaw_threshold) -{ - const auto nearest_idx = - autoware::motion_utils::findNearestSegmentIndex(points, pose, dist_threshold, yaw_threshold); - if (nearest_idx) { - return nearest_idx.value(); - } - - return autoware::motion_utils::findNearestSegmentIndex(points, pose.position); -} - -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 = std::numeric_limits::max()) -{ - if (points.empty()) { - return std::nullopt; - } - - // find goal index - size_t min_dist_index; - { - bool found = false; - double min_dist = std::numeric_limits::max(); - for (size_t i = 0; i < points.size(); ++i) { - const auto & lane_ids = points.at(i).lane_ids; - - const double dist_to_goal = autoware_utils::calc_distance2d(points.at(i).point.pose, goal); - const bool is_goal_lane_id_in_point = - std::find(lane_ids.begin(), lane_ids.end(), goal_lane_id) != lane_ids.end(); - if (dist_to_goal < max_dist && dist_to_goal < min_dist && is_goal_lane_id_in_point) { - min_dist_index = i; - min_dist = dist_to_goal; - found = true; - } - } - if (!found) { - return std::nullopt; - } - } - - // find index out of goal search range - size_t min_dist_out_of_range_index = min_dist_index; - for (int i = min_dist_index; 0 <= i; --i) { - const double dist = autoware_utils::calc_distance2d(points.at(i).point, goal); - min_dist_out_of_range_index = i; - if (max_dist < dist) { - break; - } - } - - return min_dist_out_of_range_index; -} - } // namespace std::optional get_lanelets_within_route( @@ -392,82 +343,91 @@ std::vector get_path_bound( return path_bound; } -// goal does not have z -bool set_goal( - const double search_radius_range, [[maybe_unused]] const double search_rad_range, - const PathWithLaneId & input, const geometry_msgs::msg::Pose & goal, const int64_t goal_lane_id, - PathWithLaneId * output_ptr) +std::optional set_goal( + const double search_radius_range, const PathWithLaneId & input, const geometry_msgs::msg::Pose & goal) { try { if (input.points.empty()) { - return false; + return std::nullopt; } - // calculate refined_goal with interpolation - // NOTE: goal does not have valid z, that will be calculated by interpolation here + PathWithLaneId output; + + // Calculate refined_goal with interpolation + // Note: `goal` does not have valid z, that will be calculated by interpolation here PathPointWithLaneId refined_goal{}; const size_t closest_seg_idx_for_goal = - find_nearest_segment_index(input.points, goal, 3.0, M_PI_4); + autoware::motion_utils::findFirstNearestIndexWithSoftConstraints( + input.points, goal, 3.0, M_PI_4); + + // Set goal refined_goal.point.pose = goal; - refined_goal.point.pose.position.z = - calc_interpolated_z(input, goal.position, closest_seg_idx_for_goal); + + // In set_goal function: + if (auto z = calc_interpolated_z(input, goal.position, closest_seg_idx_for_goal)) { + refined_goal.point.pose.position.z = *z; + } else { + return std::nullopt; + } refined_goal.point.longitudinal_velocity_mps = 0.0; - // calculate pre_refined_goal with interpolation - // NOTE: z and velocity are filled + // Calculate pre_refined_goal with interpolation + // Note: z and velocity are filled PathPointWithLaneId pre_refined_goal{}; constexpr double goal_to_pre_goal_distance = -1.0; pre_refined_goal.point.pose = autoware_utils::calc_offset_pose(goal, goal_to_pre_goal_distance, 0.0, 0.0); const size_t closest_seg_idx_for_pre_goal = - find_nearest_segment_index(input.points, pre_refined_goal.point.pose, 3.0, M_PI_4); - pre_refined_goal.point.pose.position.z = calc_interpolated_z( - input, pre_refined_goal.point.pose.position, closest_seg_idx_for_pre_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)) { + pre_refined_goal.point.pose.position.z = *z; + } else { + return std::nullopt; + } + pre_refined_goal.point.longitudinal_velocity_mps = calc_interpolated_velocity(input, closest_seg_idx_for_pre_goal); - // 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); - if (!min_dist_out_of_circle_index_opt) { - return false; - } - const size_t min_dist_out_of_circle_index = min_dist_out_of_circle_index_opt.value(); + // 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 = + autoware::motion_utils::findFirstNearestIndexWithSoftConstraints( + input.points, goal, search_radius_range, M_PI_4); - // create output points - output_ptr->points.reserve(output_ptr->points.size() + min_dist_out_of_circle_index + 3); + // Create output points + 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_ptr->points.push_back(input.points.at(i)); + output.points.push_back(input.points.at(i)); } - output_ptr->points.push_back(pre_refined_goal); - output_ptr->points.push_back(refined_goal); - - { // fill skipped lane ids - // pre refined goal - auto & pre_goal = output_ptr->points.at(output_ptr->points.size() - 2); - for (size_t i = min_dist_out_of_circle_index + 1; i < input.points.size(); ++i) { - for (const auto target_lane_id : input.points.at(i).lane_ids) { - const bool is_lane_id_found = - std::find(pre_goal.lane_ids.begin(), pre_goal.lane_ids.end(), target_lane_id) != - pre_goal.lane_ids.end(); - if (!is_lane_id_found) { - pre_goal.lane_ids.push_back(target_lane_id); - } + output.points.push_back(pre_refined_goal); + output.points.push_back(refined_goal); + + // Add lane IDs from skipped points to the pre-goal point + auto & pre_goal = output.points.at(output.points.size() - 2); + for (size_t i = min_dist_out_of_circle_index + 1; i < input.points.size(); ++i) { + for (const auto target_lane_id : input.points.at(i).lane_ids) { + const bool is_lane_id_found = + std::find(pre_goal.lane_ids.begin(), pre_goal.lane_ids.end(), target_lane_id) != + pre_goal.lane_ids.end(); + if (!is_lane_id_found) { + pre_goal.lane_ids.push_back(target_lane_id); } } - - // goal - output_ptr->points.back().lane_ids = input.points.back().lane_ids; } - output_ptr->left_bound = input.left_bound; - output_ptr->right_bound = input.right_bound; - return true; + // Attach goal lane IDs to the last point + output.points.back().lane_ids = input.points.back().lane_ids; + + output.left_bound = input.left_bound; + output.right_bound = input.right_bound; + return output; + } catch (std::out_of_range & ex) { RCLCPP_ERROR_STREAM( - rclcpp::get_logger("behavior_path_planner").get_child("utils"), + rclcpp::get_logger("path_generator").get_child("utils"), "failed to set goal: " << ex.what()); - return false; + return std::nullopt; } } @@ -477,12 +437,17 @@ const geometry_msgs::msg::Pose refine_goal( const auto lanelet_point = lanelet::utils::conversion::toLaneletPoint(goal.position); const double distance = boost::geometry::distance( goal_lanelet.polygon2d().basicPolygon(), lanelet::utils::to2D(lanelet_point).basicPoint()); + + // You are almost at the goal if (distance < std::numeric_limits::epsilon()) { return goal; } + // Get the closest segment to the goal const auto segment = lanelet::utils::getClosestSegment( lanelet::utils::to2D(lanelet_point), goal_lanelet.centerline()); + + // If the segment is empty, return the original goal. if (segment.empty()) { return goal; } @@ -511,23 +476,22 @@ const geometry_msgs::msg::Pose refine_goal( } PathWithLaneId refine_path_for_goal( - const double search_radius_range, const double search_rad_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) { PathWithLaneId filtered_path = input; - PathWithLaneId path_with_goal; filtered_path.points = autoware::motion_utils::removeOverlapPoints(filtered_path.points); - // always set zero velocity at the end of path for safety + // Always set zero velocity at the end of path for safety if (!filtered_path.points.empty()) { filtered_path.points.back().point.longitudinal_velocity_mps = 0.0; } - if (set_goal( - search_radius_range, search_rad_range, filtered_path, goal, goal_lane_id, - &path_with_goal)) { - return path_with_goal; + // If set_goal returns a valid path, return it + if (const auto path_with_goal = set_goal(search_radius_range, filtered_path, goal)) { + return *path_with_goal; } + + // Otherwise, return the original path with zero velocity at the end return filtered_path; } @@ -549,16 +513,15 @@ lanelet::ConstLanelets extract_lanelets_from_path( return refined_path_lanelets; } -bool get_goal_lanelet(const PlannerData & planner_data, lanelet::ConstLanelet * goal_lanelet) +std::optional get_goal_lanelet(const PlannerData & planner_data) { const lanelet::Id goal_lane_id = planner_data.goal_lane_id; for (const auto & llt : planner_data.route_lanelets) { if (llt.id() == goal_lane_id) { - *goal_lanelet = llt; - return true; + return llt; } } - return false; + return std::nullopt; } bool is_in_lanelets(const geometry_msgs::msg::Pose & pose, const lanelet::ConstLanelets & lanes) @@ -574,13 +537,17 @@ bool is_in_lanelets(const geometry_msgs::msg::Pose & pose, const lanelet::ConstL bool is_path_valid( const PathWithLaneId & refined_path, const std::shared_ptr & planner_data) { + // Extract lanelets from the refined path const auto lanelets = extract_lanelets_from_path(refined_path, planner_data); - // std::any_of detects whether any point lies outside lanelets + + // Check if any point lies outside the extracted lanelets bool has_points_outside_lanelet = std::any_of( refined_path.points.begin(), refined_path.points.end(), [&lanelets](const auto & refined_path_point) { return !is_in_lanelets(refined_path_point.point.pose, lanelets); }); + + // Return true if no points lie outside the extracted lanelets return !has_points_outside_lanelet; } @@ -588,15 +555,14 @@ PathWithLaneId modify_path_for_smooth_goal_connection( const PathWithLaneId & path, const std::shared_ptr & planner_data) { const auto goal = planner_data->goal_pose; - const auto goal_lane_id = planner_data->goal_lane_id; geometry_msgs::msg::Pose refined_goal{}; { lanelet::ConstLanelet goal_lanelet; // First, polish up the goal pose if possible - if (get_goal_lanelet(*planner_data, &goal_lanelet)) { - refined_goal = refine_goal(goal, goal_lanelet); + if (const auto goal_lanelet = get_goal_lanelet(*planner_data)) { + refined_goal = refine_goal(goal, *goal_lanelet); } else { refined_goal = goal; } @@ -604,20 +570,21 @@ PathWithLaneId modify_path_for_smooth_goal_connection( double goal_search_radius{ planner_data->path_generator_parameters.refine_goal_search_radius_range}; - // TODO(shen): define in the parameter - constexpr double range_reduce_by{1.0}; // set a reasonable value, 10% - 20% of the - // refine_goal_search_radius_range is recommended bool is_valid_path{false}; PathWithLaneId refined_path; // Then, refine the path for the goal while (goal_search_radius >= 0 && !is_valid_path) { - refined_path = - refine_path_for_goal(goal_search_radius, M_PI * 0.5, path, refined_goal, goal_lane_id); + refined_path = refine_path_for_goal(goal_search_radius, path, refined_goal); if (is_path_valid(refined_path, planner_data)) { is_valid_path = true; } - goal_search_radius -= range_reduce_by; + goal_search_radius -= planner_data->path_generator_parameters.search_radius_decrement; + } + + // It is better to return the original path if the refined path is not valid + if (!is_valid_path) { + return path; } return refined_path; } From c2e91f97cf7c8ea3c1e839fc006ca0a6e7773cb3 Mon Sep 17 00:00:00 2001 From: "pre-commit-ci[bot]" <66853113+pre-commit-ci[bot]@users.noreply.github.com> Date: Fri, 28 Feb 2025 13:21:51 +0000 Subject: [PATCH 07/41] style(pre-commit): autofix --- .../include/autoware/path_generator/utils.hpp | 8 ++++---- planning/autoware_path_generator/src/node.cpp | 8 +++++--- planning/autoware_path_generator/src/utils.cpp | 17 ++++++++++------- 3 files changed, 19 insertions(+), 14 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 378fa6e0a5..73f06d221b 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 a40b3adda5..e8810fbcc5 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 3556ca636a..002910cbcf 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); From 5945ef7cdd66fdc69d9fee5a68880ad91ba805b1 Mon Sep 17 00:00:00 2001 From: Junya Sasaki Date: Sun, 2 Mar 2025 21:44:44 +0900 Subject: [PATCH 08/41] refactor: enhance error handling Signed-off-by: Junya Sasaki --- .../include/autoware/path_generator/utils.hpp | 2 +- .../autoware_path_generator/src/utils.cpp | 102 ++++++++++++++---- 2 files changed, 80 insertions(+), 24 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 73f06d221b..a1e0da4064 100644 --- a/planning/autoware_path_generator/include/autoware/path_generator/utils.hpp +++ b/planning/autoware_path_generator/include/autoware/path_generator/utils.hpp @@ -193,7 +193,7 @@ PathWithLaneId refine_path_for_goal( * @param planner_data Planner data. * @return Extracted lanelets */ -lanelet::ConstLanelets extract_lanelets_from_path( +std::optional extract_lanelets_from_path( const PathWithLaneId & refined_path, const std::shared_ptr & planner_data); /** diff --git a/planning/autoware_path_generator/src/utils.cpp b/planning/autoware_path_generator/src/utils.cpp index 002910cbcf..9111358960 100644 --- a/planning/autoware_path_generator/src/utils.cpp +++ b/planning/autoware_path_generator/src/utils.cpp @@ -47,10 +47,21 @@ std::optional calc_interpolated_z( const autoware_internal_planning_msgs::msg::PathWithLaneId & input, const geometry_msgs::msg::Point target_pos, const size_t seg_idx) { - // Check if input is empty or seg_idx is invalid - if (input.points.empty() || seg_idx >= input.points.size() - 1) { + // Check if input is empty + if (input.points.empty()) { + RCLCPP_WARN(rclcpp::get_logger("path_generator"), "Input is empty"); return std::nullopt; } + // Check if seg_idx is invalid: -2 is to avoid out of bounds error by seg_idx + 1 + if (seg_idx > input.points.size() - 2) { + RCLCPP_WARN(rclcpp::get_logger("path_generator"), + "seg_idx: %zu is invalid for input size: %zu.\n" + "Use the z that of the last point as the interpolated z.", + seg_idx, input.points.size()); + + // Return the z of the last point if interpolation is not possible + return input.points.back().point.pose.position.z; + } try { const double closest_to_target_dist = autoware::motion_utils::calcSignedArcLength( @@ -67,20 +78,42 @@ std::optional calc_interpolated_z( : closest_z + (next_z - closest_z) * closest_to_target_dist / seg_dist; } catch (const std::exception & e) { + RCLCPP_ERROR(rclcpp::get_logger("path_generator"), "Error: %s", e.what()); return std::nullopt; } } -double calc_interpolated_velocity( +std::optional calc_interpolated_velocity( const autoware_internal_planning_msgs::msg::PathWithLaneId & input, const size_t seg_idx) { - const double seg_dist = - autoware::motion_utils::calcSignedArcLength(input.points, seg_idx, seg_idx + 1); + // Check if input is empty + if (input.points.empty()) { + RCLCPP_WARN(rclcpp::get_logger("path_generator"), "Input is empty"); + return std::nullopt; + } + // Check if seg_idx is invalid: -2 is to avoid out of bounds error by seg_idx + 1 + if (seg_idx > input.points.size() - 2) { + RCLCPP_WARN(rclcpp::get_logger("path_generator"), + "seg_idx: %zu is invalid for input size: %zu.\n" + "Use the velocity that of the last point as the interpolated velocity.", + seg_idx, input.points.size()); + + // Return the velocity of the last point if interpolation is not possible + return input.points.back().point.longitudinal_velocity_mps; + } + + try { + const double seg_dist = + autoware::motion_utils::calcSignedArcLength(input.points, seg_idx, seg_idx + 1); - const double closest_vel = input.points.at(seg_idx).point.longitudinal_velocity_mps; - const double next_vel = input.points.at(seg_idx + 1).point.longitudinal_velocity_mps; - const double interpolated_vel = std::abs(seg_dist) < 1e-06 ? next_vel : closest_vel; - return interpolated_vel; + const double closest_vel = input.points.at(seg_idx).point.longitudinal_velocity_mps; + const double next_vel = input.points.at(seg_idx + 1).point.longitudinal_velocity_mps; + const double interpolated_vel = std::abs(seg_dist) < 1e-06 ? next_vel : closest_vel; + return interpolated_vel; + } catch (const std::exception & e) { + RCLCPP_ERROR(rclcpp::get_logger("path_generator"), "Error: %s", e.what()); + return std::nullopt; + } } } // namespace @@ -364,12 +397,13 @@ std::optional set_goal( // Set goal refined_goal.point.pose = goal; - // In set_goal function: if (auto z = calc_interpolated_z(input, goal.position, closest_seg_idx_for_goal)) { refined_goal.point.pose.position.z = *z; } else { + RCLCPP_ERROR(rclcpp::get_logger("path_generator"), "Failed to calculate z for goal"); return std::nullopt; } + refined_goal.point.longitudinal_velocity_mps = 0.0; // Calculate pre_refined_goal with interpolation @@ -387,11 +421,16 @@ std::optional set_goal( input, pre_refined_goal.point.pose.position, closest_seg_idx_for_pre_goal)) { pre_refined_goal.point.pose.position.z = *z; } else { + RCLCPP_ERROR(rclcpp::get_logger("path_generator"), "Failed to calculate z for pre_goal"); return std::nullopt; } - pre_refined_goal.point.longitudinal_velocity_mps = - calc_interpolated_velocity(input, closest_seg_idx_for_pre_goal); + if (auto velocity = calc_interpolated_velocity(input, closest_seg_idx_for_pre_goal)) { + pre_refined_goal.point.longitudinal_velocity_mps = *velocity; + } else { + RCLCPP_ERROR(rclcpp::get_logger("path_generator"), "Failed to calculate velocity for pre_goal"); + return std::nullopt; + } // 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 = @@ -424,8 +463,8 @@ std::optional set_goal( output.left_bound = input.left_bound; output.right_bound = input.right_bound; - return output; + return output; } catch (std::out_of_range & ex) { RCLCPP_ERROR_STREAM( rclcpp::get_logger("path_generator").get_child("utils"), "failed to set goal: " << ex.what()); @@ -482,6 +521,7 @@ PathWithLaneId refine_path_for_goal( const geometry_msgs::msg::Pose & goal) { PathWithLaneId filtered_path = input; + filtered_path.points = autoware::motion_utils::removeOverlapPoints(filtered_path.points); // Always set zero velocity at the end of path for safety @@ -498,20 +538,31 @@ PathWithLaneId refine_path_for_goal( return filtered_path; } -lanelet::ConstLanelets extract_lanelets_from_path( +std::optional extract_lanelets_from_path( const PathWithLaneId & refined_path, const std::shared_ptr & planner_data) { lanelet::ConstLanelets refined_path_lanelets; for (size_t i = 0; i < refined_path.points.size(); ++i) { - const auto & path_point = refined_path.points.at(i); - int64_t lane_id = path_point.lane_ids.at(0); - lanelet::ConstLanelet lanelet = planner_data->lanelet_map_ptr->laneletLayer.get(lane_id); - const bool is_unique = - std::find(refined_path_lanelets.begin(), refined_path_lanelets.end(), lanelet) == - refined_path_lanelets.end(); - if (is_unique) { - refined_path_lanelets.push_back(lanelet); + try { + const auto & path_point = refined_path.points.at(i); + + // TODO(sasakisasaki): It seems sometimes path_point.lane_ids is empty. + // We should check this case. + int64_t lane_id = path_point.lane_ids.at(0); + lanelet::ConstLanelet lanelet = planner_data->lanelet_map_ptr->laneletLayer.get(lane_id); + + const bool is_unique = + std::find(refined_path_lanelets.begin(), refined_path_lanelets.end(), lanelet) == + refined_path_lanelets.end(); + + if (is_unique) { + refined_path_lanelets.push_back(lanelet); + } + } catch (const std::out_of_range & e) { + RCLCPP_ERROR(rclcpp::get_logger("path_generator"), "Out of range error: %s", e.what()); + return std::nullopt; } + } return refined_path_lanelets; } @@ -541,7 +592,12 @@ bool is_path_valid( const PathWithLaneId & refined_path, const std::shared_ptr & planner_data) { // Extract lanelets from the refined path - const auto lanelets = extract_lanelets_from_path(refined_path, planner_data); + const auto lanelets_opt = extract_lanelets_from_path(refined_path, planner_data); + if (!lanelets_opt) { + RCLCPP_ERROR(rclcpp::get_logger("path_generator"), "Failed to extract lanelets from path"); + return false; + } + const auto & lanelets = lanelets_opt.value(); // std::any_of detects whether any point lies outside lanelets const bool has_points_outside_lanelet = std::any_of( From c4b35009abca276a2aa3a01e56b274c4a6caab1c Mon Sep 17 00:00:00 2001 From: "pre-commit-ci[bot]" <66853113+pre-commit-ci[bot]@users.noreply.github.com> Date: Sun, 2 Mar 2025 12:46:17 +0000 Subject: [PATCH 09/41] style(pre-commit): autofix --- .../autoware_path_generator/src/utils.cpp | 22 ++++++++++--------- 1 file changed, 12 insertions(+), 10 deletions(-) diff --git a/planning/autoware_path_generator/src/utils.cpp b/planning/autoware_path_generator/src/utils.cpp index 9111358960..aafda97869 100644 --- a/planning/autoware_path_generator/src/utils.cpp +++ b/planning/autoware_path_generator/src/utils.cpp @@ -54,10 +54,11 @@ std::optional calc_interpolated_z( } // Check if seg_idx is invalid: -2 is to avoid out of bounds error by seg_idx + 1 if (seg_idx > input.points.size() - 2) { - RCLCPP_WARN(rclcpp::get_logger("path_generator"), - "seg_idx: %zu is invalid for input size: %zu.\n" - "Use the z that of the last point as the interpolated z.", - seg_idx, input.points.size()); + RCLCPP_WARN( + rclcpp::get_logger("path_generator"), + "seg_idx: %zu is invalid for input size: %zu.\n" + "Use the z that of the last point as the interpolated z.", + seg_idx, input.points.size()); // Return the z of the last point if interpolation is not possible return input.points.back().point.pose.position.z; @@ -93,10 +94,11 @@ std::optional calc_interpolated_velocity( } // Check if seg_idx is invalid: -2 is to avoid out of bounds error by seg_idx + 1 if (seg_idx > input.points.size() - 2) { - RCLCPP_WARN(rclcpp::get_logger("path_generator"), - "seg_idx: %zu is invalid for input size: %zu.\n" - "Use the velocity that of the last point as the interpolated velocity.", - seg_idx, input.points.size()); + RCLCPP_WARN( + rclcpp::get_logger("path_generator"), + "seg_idx: %zu is invalid for input size: %zu.\n" + "Use the velocity that of the last point as the interpolated velocity.", + seg_idx, input.points.size()); // Return the velocity of the last point if interpolation is not possible return input.points.back().point.longitudinal_velocity_mps; @@ -428,7 +430,8 @@ std::optional set_goal( if (auto velocity = calc_interpolated_velocity(input, closest_seg_idx_for_pre_goal)) { pre_refined_goal.point.longitudinal_velocity_mps = *velocity; } else { - RCLCPP_ERROR(rclcpp::get_logger("path_generator"), "Failed to calculate velocity for pre_goal"); + RCLCPP_ERROR( + rclcpp::get_logger("path_generator"), "Failed to calculate velocity for pre_goal"); return std::nullopt; } @@ -562,7 +565,6 @@ std::optional extract_lanelets_from_path( RCLCPP_ERROR(rclcpp::get_logger("path_generator"), "Out of range error: %s", e.what()); return std::nullopt; } - } return refined_path_lanelets; } From ace5fbe97b1f47d760ade700e80493d1aff2c7ff Mon Sep 17 00:00:00 2001 From: Junya Sasaki Date: Tue, 4 Mar 2025 18:24:08 +0900 Subject: [PATCH 10/41] bug: fix wrong function declaration in header Signed-off-by: Junya Sasaki --- .../include/autoware/path_generator/utils.hpp | 13 +++++-------- 1 file changed, 5 insertions(+), 8 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 a1e0da4064..546aac9eed 100644 --- a/planning/autoware_path_generator/include/autoware/path_generator/utils.hpp +++ b/planning/autoware_path_generator/include/autoware/path_generator/utils.hpp @@ -156,12 +156,11 @@ std::vector get_path_bound( * @param [in] search_radius_range distance on path to be modified for goal insertion * @param [in] input original path * @param [in] goal original goal pose - * @param [in] goal_lane_id - * @param [in] output_ptr output path with modified points for the goal + * @return output path with modified points for the goal (std::nullopt if not found) */ -bool set_goal( +std::optional 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 geometry_msgs::msg::Pose & goal); /** * @brief Recreate the goal pose to prevent the goal point being too far from the lanelet, which @@ -177,15 +176,13 @@ const geometry_msgs::msg::Pose refine_goal( /** * @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. + * @param refined_goal Goal pose. * @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 geometry_msgs::msg::Pose & goal); /** * @brief Extract lanelets from the path. From 0dee050505873f620a291ef16d727131834cdc68 Mon Sep 17 00:00:00 2001 From: Junya Sasaki Date: Tue, 4 Mar 2025 18:32:02 +0900 Subject: [PATCH 11/41] bug: fix wrong point index calculation Signed-off-by: Junya Sasaki --- .../include/autoware/path_generator/utils.hpp | 18 +++++- .../autoware_path_generator/src/utils.cpp | 60 ++++++++++++++++--- 2 files changed, 69 insertions(+), 9 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 546aac9eed..af2697a279 100644 --- a/planning/autoware_path_generator/include/autoware/path_generator/utils.hpp +++ b/planning/autoware_path_generator/include/autoware/path_generator/utils.hpp @@ -147,6 +147,18 @@ std::vector get_path_bound( const lanelet::CompoundLineString2d & lanelet_centerline, const double s_start, const double s_end); +/** + * @brief find index out of goal search range + * @param points points of path + * @param goal goal pose + * @param goal_lane_id lane id of goal lanelet + * @param max_dist maximum distance to search for goal index + * @return index out of goal search range (std::nullopt if not found) + */ +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 = std::numeric_limits::max()); + /** * @brief Modify the path points near the goal to smoothly connect the input path and the goal * point @@ -156,11 +168,12 @@ std::vector get_path_bound( * @param [in] search_radius_range distance on path to be modified for goal insertion * @param [in] input original path * @param [in] goal original goal pose + * @param [in] goal_lane_id Lane ID of goal lanelet. * @return output path with modified points for the goal (std::nullopt if not found) */ std::optional set_goal( const double search_radius_range, const PathWithLaneId & input, - const geometry_msgs::msg::Pose & goal); + const geometry_msgs::msg::Pose & goal, const int64_t goal_lane_id); /** * @brief Recreate the goal pose to prevent the goal point being too far from the lanelet, which @@ -178,11 +191,12 @@ const geometry_msgs::msg::Pose refine_goal( * @param search_radius_range Searching radius. * @param input Input path. * @param refined_goal Goal pose. + * @param goal_lane_id Lane ID of goal lanelet. * @return Recreated path */ PathWithLaneId refine_path_for_goal( const double search_radius_range, const PathWithLaneId & input, - const geometry_msgs::msg::Pose & goal); + 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/utils.cpp b/planning/autoware_path_generator/src/utils.cpp index aafda97869..e0dde946d0 100644 --- a/planning/autoware_path_generator/src/utils.cpp +++ b/planning/autoware_path_generator/src/utils.cpp @@ -378,9 +378,52 @@ std::vector get_path_bound( return path_bound; } +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) +{ + if (points.empty()) { + return std::nullopt; + } + + // find goal index + size_t min_dist_index; + { + bool found = false; + double min_dist = std::numeric_limits::max(); + for (size_t i = 0; i < points.size(); ++i) { + const auto & lane_ids = points.at(i).lane_ids; + + const double dist_to_goal = autoware_utils::calc_distance2d(points.at(i).point.pose, goal); + const bool is_goal_lane_id_in_point = + std::find(lane_ids.begin(), lane_ids.end(), goal_lane_id) != lane_ids.end(); + if (dist_to_goal < max_dist && dist_to_goal < min_dist && is_goal_lane_id_in_point) { + min_dist_index = i; + min_dist = dist_to_goal; + found = true; + } + } + if (!found) { + return std::nullopt; + } + } + + // find index out of goal search range + size_t min_dist_out_of_range_index = min_dist_index; + for (int i = min_dist_index; 0 <= i; --i) { + const double dist = autoware_utils::calc_distance2d(points.at(i).point, goal); + min_dist_out_of_range_index = i; + if (max_dist < dist) { + break; + } + } + + return min_dist_out_of_range_index; +} + std::optional set_goal( const double search_radius_range, const PathWithLaneId & input, - const geometry_msgs::msg::Pose & goal) + const geometry_msgs::msg::Pose & goal, const int64_t goal_lane_id) { try { if (input.points.empty()) { @@ -436,9 +479,12 @@ std::optional set_goal( } // 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 = - autoware::motion_utils::findFirstNearestIndexWithSoftConstraints( - input.points, goal, search_radius_range, M_PI_4); + 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); + if (!min_dist_out_of_circle_index_opt) { + return std::nullopt; + } + const auto min_dist_out_of_circle_index = min_dist_out_of_circle_index_opt.value(); // Create output points output.points.reserve(output.points.size() + min_dist_out_of_circle_index + 3); @@ -521,7 +567,7 @@ 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 geometry_msgs::msg::Pose & goal, const int64_t goal_lane_id) { PathWithLaneId filtered_path = input; @@ -533,7 +579,7 @@ PathWithLaneId refine_path_for_goal( } // If set_goal returns a valid path, return it - if (const auto path_with_goal = set_goal(search_radius_range, filtered_path, goal)) { + if (const auto path_with_goal = set_goal(search_radius_range, filtered_path, goal, goal_lane_id)) { return *path_with_goal; } @@ -636,7 +682,7 @@ PathWithLaneId modify_path_for_smooth_goal_connection( // Then, refine the path for the goal while (goal_search_radius >= 0 && !is_valid_path) { - refined_path = refine_path_for_goal(goal_search_radius, path, refined_goal); + refined_path = refine_path_for_goal(goal_search_radius, path, refined_goal, planner_data->goal_lane_id); if (is_path_valid(refined_path, planner_data)) { is_valid_path = true; } From b6a84927a6fba116891c4de3f66c485cdb187f1e Mon Sep 17 00:00:00 2001 From: Junya Sasaki Date: Tue, 4 Mar 2025 19:40:42 +0900 Subject: [PATCH 12/41] bug: remove meaningless comment * This comment is wrote because of my misunderstanding Signed-off-by: Junya Sasaki --- planning/autoware_path_generator/src/utils.cpp | 3 --- 1 file changed, 3 deletions(-) diff --git a/planning/autoware_path_generator/src/utils.cpp b/planning/autoware_path_generator/src/utils.cpp index e0dde946d0..8027fb6af4 100644 --- a/planning/autoware_path_generator/src/utils.cpp +++ b/planning/autoware_path_generator/src/utils.cpp @@ -594,9 +594,6 @@ std::optional extract_lanelets_from_path( for (size_t i = 0; i < refined_path.points.size(); ++i) { try { const auto & path_point = refined_path.points.at(i); - - // TODO(sasakisasaki): It seems sometimes path_point.lane_ids is empty. - // We should check this case. int64_t lane_id = path_point.lane_ids.at(0); lanelet::ConstLanelet lanelet = planner_data->lanelet_map_ptr->laneletLayer.get(lane_id); From ea3bfeda4d85bc665083dfaac0091cc468e2e22e Mon Sep 17 00:00:00 2001 From: Junya Sasaki Date: Tue, 4 Mar 2025 19:42:44 +0900 Subject: [PATCH 13/41] fix: apply `pre-commit` Signed-off-by: Junya Sasaki --- planning/autoware_path_generator/src/utils.cpp | 6 ++++-- 1 file changed, 4 insertions(+), 2 deletions(-) diff --git a/planning/autoware_path_generator/src/utils.cpp b/planning/autoware_path_generator/src/utils.cpp index 8027fb6af4..6ad4db41da 100644 --- a/planning/autoware_path_generator/src/utils.cpp +++ b/planning/autoware_path_generator/src/utils.cpp @@ -579,7 +579,8 @@ PathWithLaneId refine_path_for_goal( } // If set_goal returns a valid path, return it - if (const auto path_with_goal = set_goal(search_radius_range, filtered_path, goal, goal_lane_id)) { + if ( + const auto path_with_goal = set_goal(search_radius_range, filtered_path, goal, goal_lane_id)) { return *path_with_goal; } @@ -679,7 +680,8 @@ PathWithLaneId modify_path_for_smooth_goal_connection( // Then, refine the path for the goal while (goal_search_radius >= 0 && !is_valid_path) { - refined_path = refine_path_for_goal(goal_search_radius, path, refined_goal, planner_data->goal_lane_id); + refined_path = + refine_path_for_goal(goal_search_radius, path, refined_goal, planner_data->goal_lane_id); if (is_path_valid(refined_path, planner_data)) { is_valid_path = true; } From a322d4dbed70baeca9493ce3ddd7e0fdea9ef9b4 Mon Sep 17 00:00:00 2001 From: Junya Sasaki Date: Tue, 4 Mar 2025 19:53:31 +0900 Subject: [PATCH 14/41] fix: smooth path before cropping trajectory points Signed-off-by: Junya Sasaki --- planning/autoware_path_generator/src/node.cpp | 42 ++++++++++++------- 1 file changed, 27 insertions(+), 15 deletions(-) diff --git a/planning/autoware_path_generator/src/node.cpp b/planning/autoware_path_generator/src/node.cpp index e8810fbcc5..0a6aa57e5b 100644 --- a/planning/autoware_path_generator/src/node.cpp +++ b/planning/autoware_path_generator/src/node.cpp @@ -223,11 +223,7 @@ std::optional PathGenerator::plan_path( return std::nullopt; } - // Make the path smooth - auto planner_data_ptr = std::make_shared(planner_data_); - const auto smooth_path = utils::modify_path_for_smooth_goal_connection(*path, planner_data_ptr); - - return smooth_path; + return path; } std::optional PathGenerator::generate_path( @@ -379,23 +375,39 @@ std::optional PathGenerator::generate_path( } } - auto trajectory = Trajectory::Builder().build(path_points_with_lane_id); + auto planner_data_ptr = std::make_shared(planner_data_); + + // Compose path with lane id + PathWithLaneId path_with_lane_id; + path_with_lane_id.header.frame_id = planner_data_.route_frame_id; + path_with_lane_id.header.stamp = now(); + path_with_lane_id.points = std::move(path_points_with_lane_id); + + // Smooth path for goal connection + const auto smooth_path = + utils::modify_path_for_smooth_goal_connection(path_with_lane_id, planner_data_ptr); + + auto trajectory = Trajectory::Builder().build(smooth_path.points); if (!trajectory) { return std::nullopt; } + // Attach orientation for all the points trajectory->align_orientation_with_trajectory_direction(); + + // Refine the trajectory by cropping trajectory->crop( s_offset + s_start - get_arc_length_along_centerline( - extended_lanelet_sequence, lanelet::utils::conversion::toLaneletPoint( - path_points_with_lane_id.front().point.pose.position)), + extended_lanelet_sequence, + lanelet::utils::conversion::toLaneletPoint(smooth_path.points.front().point.pose.position)), s_end - s_start); - PathWithLaneId path{}; - path.header.frame_id = planner_data_.route_frame_id; - path.header.stamp = now(); - path.points = trajectory->restore(); + // Compose the polished path + PathWithLaneId polished_path{}; + polished_path.header.frame_id = planner_data_.route_frame_id; + polished_path.header.stamp = now(); + polished_path.points = trajectory->restore(); const auto get_path_bound = [&](const lanelet::CompoundLineString2d & lanelet_bound) { const auto s_bound_start = @@ -405,10 +417,10 @@ std::optional PathGenerator::generate_path( return utils::get_path_bound( lanelet_bound, extended_lanelet_sequence.centerline2d(), s_bound_start, s_bound_end); }; - path.left_bound = get_path_bound(extended_lanelet_sequence.leftBound2d()); - path.right_bound = get_path_bound(extended_lanelet_sequence.rightBound2d()); + polished_path.left_bound = get_path_bound(extended_lanelet_sequence.leftBound2d()); + polished_path.right_bound = get_path_bound(extended_lanelet_sequence.rightBound2d()); - return path; + return polished_path; } } // namespace autoware::path_generator From 865b38f7f19488c456a1012df6c541ddf42b2a84 Mon Sep 17 00:00:00 2001 From: Junya Sasaki Date: Wed, 5 Mar 2025 14:36:52 +0900 Subject: [PATCH 15/41] bug: fix shadow variable Signed-off-by: Junya Sasaki --- planning/autoware_path_generator/src/utils.cpp | 5 +++-- 1 file changed, 3 insertions(+), 2 deletions(-) diff --git a/planning/autoware_path_generator/src/utils.cpp b/planning/autoware_path_generator/src/utils.cpp index 6ad4db41da..c062d72541 100644 --- a/planning/autoware_path_generator/src/utils.cpp +++ b/planning/autoware_path_generator/src/utils.cpp @@ -663,10 +663,11 @@ PathWithLaneId modify_path_for_smooth_goal_connection( geometry_msgs::msg::Pose refined_goal{}; { - lanelet::ConstLanelet goal_lanelet; + // Prevent from shadowVariable + const auto goal_lanelet = get_goal_lanelet(*planner_data); // First, polish up the goal pose if possible - if (const auto goal_lanelet = get_goal_lanelet(*planner_data)) { + if (goal_lanelet) { refined_goal = refine_goal(goal, *goal_lanelet); } else { refined_goal = goal; From 6851a32cdbb068b93a311d0bb884e723b19157c9 Mon Sep 17 00:00:00 2001 From: Junya Sasaki Date: Wed, 5 Mar 2025 15:01:49 +0900 Subject: [PATCH 16/41] bug: fix missing parameters for `autoware_path_generator` Signed-off-by: Junya Sasaki --- .../autoware_path_generator/config/path_generator.param.yaml | 2 ++ 1 file changed, 2 insertions(+) diff --git a/planning/autoware_path_generator/config/path_generator.param.yaml b/planning/autoware_path_generator/config/path_generator.param.yaml index 7a4ed323d6..fd69c7d005 100644 --- a/planning/autoware_path_generator/config/path_generator.param.yaml +++ b/planning/autoware_path_generator/config/path_generator.param.yaml @@ -12,3 +12,5 @@ search_distance: 30.0 resampling_interval: 1.0 angle_threshold_deg: 15.0 + refine_goal_search_radius_range: 10.0 # [m] + search_radius_decrement: 1.0 From f743ea15a685ab5f4e711a3398bd695639782f28 Mon Sep 17 00:00:00 2001 From: Junya Sasaki Date: Wed, 5 Mar 2025 15:09:31 +0900 Subject: [PATCH 17/41] bug: fix by cpplint Signed-off-by: Junya Sasaki --- .../include/autoware/path_generator/utils.hpp | 2 ++ planning/autoware_path_generator/src/utils.cpp | 1 - 2 files changed, 2 insertions(+), 1 deletion(-) 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 af2697a279..4abac06697 100644 --- a/planning/autoware_path_generator/include/autoware/path_generator/utils.hpp +++ b/planning/autoware_path_generator/include/autoware/path_generator/utils.hpp @@ -25,6 +25,8 @@ #include #include #include +#include +#include namespace autoware::path_generator { diff --git a/planning/autoware_path_generator/src/utils.cpp b/planning/autoware_path_generator/src/utils.cpp index c062d72541..dc21c2478b 100644 --- a/planning/autoware_path_generator/src/utils.cpp +++ b/planning/autoware_path_generator/src/utils.cpp @@ -77,7 +77,6 @@ std::optional calc_interpolated_z( return std::abs(seg_dist) < 1e-6 ? next_z : closest_z + (next_z - closest_z) * closest_to_target_dist / seg_dist; - } catch (const std::exception & e) { RCLCPP_ERROR(rclcpp::get_logger("path_generator"), "Error: %s", e.what()); return std::nullopt; From cc34c450bffbb3a92a56a976ebef2b8b3c58d192 Mon Sep 17 00:00:00 2001 From: "pre-commit-ci[bot]" <66853113+pre-commit-ci[bot]@users.noreply.github.com> Date: Wed, 5 Mar 2025 06:12:58 +0000 Subject: [PATCH 18/41] style(pre-commit): autofix --- .../include/autoware/path_generator/utils.hpp | 4 ++-- 1 file changed, 2 insertions(+), 2 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 4abac06697..2810e77d5a 100644 --- a/planning/autoware_path_generator/include/autoware/path_generator/utils.hpp +++ b/planning/autoware_path_generator/include/autoware/path_generator/utils.hpp @@ -20,13 +20,13 @@ #include #include +#include +#include #include #include #include #include #include -#include -#include namespace autoware::path_generator { From 82f48df2fc833c2aa11755b5c8dfb014c6ccbdfa Mon Sep 17 00:00:00 2001 From: Junya Sasaki Date: Wed, 5 Mar 2025 15:18:01 +0900 Subject: [PATCH 19/41] bug: apply missing fix proposed by cpplint Signed-off-by: Junya Sasaki --- planning/autoware_path_generator/src/utils.cpp | 2 ++ 1 file changed, 2 insertions(+) diff --git a/planning/autoware_path_generator/src/utils.cpp b/planning/autoware_path_generator/src/utils.cpp index dc21c2478b..8543aa085c 100644 --- a/planning/autoware_path_generator/src/utils.cpp +++ b/planning/autoware_path_generator/src/utils.cpp @@ -30,6 +30,8 @@ #include #include #include +#include +#include namespace autoware::path_generator { From b2bc6179506d140068ff9d84ffe13b4f484574f8 Mon Sep 17 00:00:00 2001 From: "pre-commit-ci[bot]" <66853113+pre-commit-ci[bot]@users.noreply.github.com> Date: Wed, 5 Mar 2025 06:19:58 +0000 Subject: [PATCH 20/41] style(pre-commit): autofix --- planning/autoware_path_generator/src/utils.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/planning/autoware_path_generator/src/utils.cpp b/planning/autoware_path_generator/src/utils.cpp index 8543aa085c..ee542862a9 100644 --- a/planning/autoware_path_generator/src/utils.cpp +++ b/planning/autoware_path_generator/src/utils.cpp @@ -28,10 +28,10 @@ #include #include -#include -#include #include #include +#include +#include namespace autoware::path_generator { From 0e767d4321f372216db30ddad8d6fc5b646197ae Mon Sep 17 00:00:00 2001 From: Junya Sasaki Date: Wed, 5 Mar 2025 17:31:14 +0900 Subject: [PATCH 21/41] bug: `autoware_test_utils` should be in the `test_depend` Signed-off-by: Junya Sasaki --- planning/autoware_path_generator/package.xml | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/planning/autoware_path_generator/package.xml b/planning/autoware_path_generator/package.xml index 7d54f49778..84c153967d 100644 --- a/planning/autoware_path_generator/package.xml +++ b/planning/autoware_path_generator/package.xml @@ -22,7 +22,6 @@ autoware_lanelet2_extension autoware_motion_utils autoware_planning_test_manager - autoware_test_utils autoware_trajectory autoware_vehicle_info_utils generate_parameter_library @@ -31,6 +30,7 @@ ament_cmake_ros ament_lint_auto + autoware_test_utils autoware_lint_common From 023529cd69a00d06bcd2aad5929d5bd8475f26eb Mon Sep 17 00:00:00 2001 From: Junya Sasaki Date: Wed, 5 Mar 2025 17:33:50 +0900 Subject: [PATCH 22/41] fix(autoware_path_generator): add maintainer and author Signed-off-by: Junya Sasaki --- planning/autoware_path_generator/package.xml | 2 ++ 1 file changed, 2 insertions(+) diff --git a/planning/autoware_path_generator/package.xml b/planning/autoware_path_generator/package.xml index 84c153967d..e84f561462 100644 --- a/planning/autoware_path_generator/package.xml +++ b/planning/autoware_path_generator/package.xml @@ -8,12 +8,14 @@ Takayuki Murooka Mitsuhiro Sakamoto Kosuke Takeuchi + Junya Sasaki Apache License 2.0 Satoshi Ota Takayuki Murooka Mitsuhiro Sakamoto Kosuke Takeuchi + Junya Sasaki ament_cmake_auto autoware_cmake From 30b826cb8f45e6bdf9cbc83f706fea560074fe0d Mon Sep 17 00:00:00 2001 From: "pre-commit-ci[bot]" <66853113+pre-commit-ci[bot]@users.noreply.github.com> Date: Wed, 5 Mar 2025 08:36:00 +0000 Subject: [PATCH 23/41] style(pre-commit): autofix --- planning/autoware_path_generator/package.xml | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/planning/autoware_path_generator/package.xml b/planning/autoware_path_generator/package.xml index e84f561462..fa75059724 100644 --- a/planning/autoware_path_generator/package.xml +++ b/planning/autoware_path_generator/package.xml @@ -32,8 +32,8 @@ ament_cmake_ros ament_lint_auto - autoware_test_utils autoware_lint_common + autoware_test_utils ament_cmake From e376cf6ad1e815a4a3fa7929c182dd3c91390350 Mon Sep 17 00:00:00 2001 From: Junya Sasaki Date: Wed, 5 Mar 2025 17:37:14 +0900 Subject: [PATCH 24/41] fix: by pre-commit * Sorry, I was forgetting to do this on my local env. Signed-off-by: Junya Sasaki --- planning/autoware_path_generator/package.xml | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/planning/autoware_path_generator/package.xml b/planning/autoware_path_generator/package.xml index e84f561462..fa75059724 100644 --- a/planning/autoware_path_generator/package.xml +++ b/planning/autoware_path_generator/package.xml @@ -32,8 +32,8 @@ ament_cmake_ros ament_lint_auto - autoware_test_utils autoware_lint_common + autoware_test_utils ament_cmake From 2e286d9c2b35bfad19f6ac0d60864c90fefd8e92 Mon Sep 17 00:00:00 2001 From: Junya Sasaki Date: Thu, 6 Mar 2025 20:51:14 +0900 Subject: [PATCH 25/41] fix: smooth path only when a goal point is included Signed-off-by: Junya Sasaki --- planning/autoware_path_generator/src/node.cpp | 49 +++++++++++-------- 1 file changed, 28 insertions(+), 21 deletions(-) diff --git a/planning/autoware_path_generator/src/node.cpp b/planning/autoware_path_generator/src/node.cpp index 0a6aa57e5b..f46383a4dd 100644 --- a/planning/autoware_path_generator/src/node.cpp +++ b/planning/autoware_path_generator/src/node.cpp @@ -375,19 +375,7 @@ std::optional PathGenerator::generate_path( } } - auto planner_data_ptr = std::make_shared(planner_data_); - - // Compose path with lane id - PathWithLaneId path_with_lane_id; - path_with_lane_id.header.frame_id = planner_data_.route_frame_id; - path_with_lane_id.header.stamp = now(); - path_with_lane_id.points = std::move(path_points_with_lane_id); - - // Smooth path for goal connection - const auto smooth_path = - utils::modify_path_for_smooth_goal_connection(path_with_lane_id, planner_data_ptr); - - auto trajectory = Trajectory::Builder().build(smooth_path.points); + auto trajectory = Trajectory::Builder().build(path_points_with_lane_id); if (!trajectory) { return std::nullopt; } @@ -400,14 +388,33 @@ std::optional PathGenerator::generate_path( s_offset + s_start - get_arc_length_along_centerline( extended_lanelet_sequence, - lanelet::utils::conversion::toLaneletPoint(smooth_path.points.front().point.pose.position)), + lanelet::utils::conversion::toLaneletPoint(path_points_with_lane_id.front().point.pose.position)), s_end - s_start); // Compose the polished path - PathWithLaneId polished_path{}; - polished_path.header.frame_id = planner_data_.route_frame_id; - polished_path.header.stamp = now(); - polished_path.points = trajectory->restore(); + PathWithLaneId preprocessed_path{}; + preprocessed_path.header.frame_id = planner_data_.route_frame_id; + preprocessed_path.header.stamp = now(); + preprocessed_path.points = trajectory->restore(); + + PathWithLaneId finalized_path_with_lane_id{}; + + // Check if the cropped trajectory has a goal point + const auto distance_to_goal = autoware_utils::calc_distance2d(preprocessed_path.points.back().point.pose, planner_data_.goal_pose); + + if (distance_to_goal < 1e-6) { + // Perform smooth goal connection + auto planner_data_ptr = std::make_shared(planner_data_); + finalized_path_with_lane_id = + utils::modify_path_for_smooth_goal_connection(std::move(preprocessed_path), planner_data_ptr); + } else { + finalized_path_with_lane_id = std::move(preprocessed_path); + } + + // check if the path is empty + if (finalized_path_with_lane_id.points.empty()) { + return std::nullopt; + } const auto get_path_bound = [&](const lanelet::CompoundLineString2d & lanelet_bound) { const auto s_bound_start = @@ -417,10 +424,10 @@ std::optional PathGenerator::generate_path( return utils::get_path_bound( lanelet_bound, extended_lanelet_sequence.centerline2d(), s_bound_start, s_bound_end); }; - polished_path.left_bound = get_path_bound(extended_lanelet_sequence.leftBound2d()); - polished_path.right_bound = get_path_bound(extended_lanelet_sequence.rightBound2d()); + finalized_path_with_lane_id.left_bound = get_path_bound(extended_lanelet_sequence.leftBound2d()); + finalized_path_with_lane_id.right_bound = get_path_bound(extended_lanelet_sequence.rightBound2d()); - return polished_path; + return finalized_path_with_lane_id; } } // namespace autoware::path_generator From 8093a9900e5c337d5bba3f9fb7f94751e88cff07 Mon Sep 17 00:00:00 2001 From: Junya Sasaki Date: Thu, 6 Mar 2025 20:57:20 +0900 Subject: [PATCH 26/41] bug: do error handling Signed-off-by: Junya Sasaki --- planning/autoware_path_generator/src/node.cpp | 7 +++++++ 1 file changed, 7 insertions(+) diff --git a/planning/autoware_path_generator/src/node.cpp b/planning/autoware_path_generator/src/node.cpp index f46383a4dd..cf68fd0c7f 100644 --- a/planning/autoware_path_generator/src/node.cpp +++ b/planning/autoware_path_generator/src/node.cpp @@ -289,6 +289,13 @@ std::optional PathGenerator::generate_path( const lanelet::LaneletSequence & lanelet_sequence, const double s_start, const double s_end, const Params & params) const { + + // Sanity check + if (s_start > s_end) { + RCLCPP_ERROR(get_logger(), "s_start must not be greater than s_end: Provided s_start = %f, s_end = %f. Skipping path generation.", s_start, s_end); + return std::nullopt; + } + std::vector path_points_with_lane_id{}; const auto add_path_point = [&](const auto & path_point, const lanelet::ConstLanelet & lanelet) { From 06c0532e18b7ced788f3b98e6f3d78ce4d7d6650 Mon Sep 17 00:00:00 2001 From: "pre-commit-ci[bot]" <66853113+pre-commit-ci[bot]@users.noreply.github.com> Date: Thu, 6 Mar 2025 12:01:30 +0000 Subject: [PATCH 27/41] style(pre-commit): autofix --- planning/autoware_path_generator/src/node.cpp | 17 +++++++++++------ 1 file changed, 11 insertions(+), 6 deletions(-) diff --git a/planning/autoware_path_generator/src/node.cpp b/planning/autoware_path_generator/src/node.cpp index cf68fd0c7f..6139c6728b 100644 --- a/planning/autoware_path_generator/src/node.cpp +++ b/planning/autoware_path_generator/src/node.cpp @@ -289,10 +289,13 @@ std::optional PathGenerator::generate_path( const lanelet::LaneletSequence & lanelet_sequence, const double s_start, const double s_end, const Params & params) const { - // Sanity check if (s_start > s_end) { - RCLCPP_ERROR(get_logger(), "s_start must not be greater than s_end: Provided s_start = %f, s_end = %f. Skipping path generation.", s_start, s_end); + RCLCPP_ERROR( + get_logger(), + "s_start must not be greater than s_end: Provided s_start = %f, s_end = %f. Skipping path " + "generation.", + s_start, s_end); return std::nullopt; } @@ -394,8 +397,8 @@ std::optional PathGenerator::generate_path( trajectory->crop( s_offset + s_start - get_arc_length_along_centerline( - extended_lanelet_sequence, - lanelet::utils::conversion::toLaneletPoint(path_points_with_lane_id.front().point.pose.position)), + extended_lanelet_sequence, lanelet::utils::conversion::toLaneletPoint( + path_points_with_lane_id.front().point.pose.position)), s_end - s_start); // Compose the polished path @@ -407,7 +410,8 @@ std::optional PathGenerator::generate_path( PathWithLaneId finalized_path_with_lane_id{}; // Check if the cropped trajectory has a goal point - const auto distance_to_goal = autoware_utils::calc_distance2d(preprocessed_path.points.back().point.pose, planner_data_.goal_pose); + const auto distance_to_goal = autoware_utils::calc_distance2d( + preprocessed_path.points.back().point.pose, planner_data_.goal_pose); if (distance_to_goal < 1e-6) { // Perform smooth goal connection @@ -432,7 +436,8 @@ std::optional PathGenerator::generate_path( lanelet_bound, extended_lanelet_sequence.centerline2d(), s_bound_start, s_bound_end); }; finalized_path_with_lane_id.left_bound = get_path_bound(extended_lanelet_sequence.leftBound2d()); - finalized_path_with_lane_id.right_bound = get_path_bound(extended_lanelet_sequence.rightBound2d()); + finalized_path_with_lane_id.right_bound = + get_path_bound(extended_lanelet_sequence.rightBound2d()); return finalized_path_with_lane_id; } From 9f217b38a25971a0d26753412ef25c5e260d2f56 Mon Sep 17 00:00:00 2001 From: Junya Sasaki Date: Sat, 8 Mar 2025 15:36:36 +0900 Subject: [PATCH 28/41] bug: fix wrong distance calculation * The goal position is generally separate from the path points Signed-off-by: Junya Sasaki --- planning/autoware_path_generator/src/node.cpp | 7 ++++--- 1 file changed, 4 insertions(+), 3 deletions(-) diff --git a/planning/autoware_path_generator/src/node.cpp b/planning/autoware_path_generator/src/node.cpp index 6139c6728b..ebdab1d5c9 100644 --- a/planning/autoware_path_generator/src/node.cpp +++ b/planning/autoware_path_generator/src/node.cpp @@ -409,11 +409,12 @@ std::optional PathGenerator::generate_path( PathWithLaneId finalized_path_with_lane_id{}; - // Check if the cropped trajectory has a goal point + // Check if the goal point is in the search range + // Note: We assume the current position is the first point of the path const auto distance_to_goal = autoware_utils::calc_distance2d( - preprocessed_path.points.back().point.pose, planner_data_.goal_pose); + preprocessed_path.points.front().point.pose, planner_data_.goal_pose); - if (distance_to_goal < 1e-6) { + if (distance_to_goal < planner_data_.path_generator_parameters.refine_goal_search_radius_range) { // Perform smooth goal connection auto planner_data_ptr = std::make_shared(planner_data_); finalized_path_with_lane_id = From 6509955cbb92c088c1651b1e9723acbc723c6aef Mon Sep 17 00:00:00 2001 From: Junya Sasaki Date: Sat, 8 Mar 2025 15:41:19 +0900 Subject: [PATCH 29/41] fix: remove sanity check temporary as following reasons * CI (especially unit tests) fails due to this sanity check * As this is out of scope for this PR, we will fix the bug where the start and end are reversed in another PR Signed-off-by: Junya Sasaki --- planning/autoware_path_generator/src/node.cpp | 10 ---------- 1 file changed, 10 deletions(-) diff --git a/planning/autoware_path_generator/src/node.cpp b/planning/autoware_path_generator/src/node.cpp index ebdab1d5c9..48246ca1d0 100644 --- a/planning/autoware_path_generator/src/node.cpp +++ b/planning/autoware_path_generator/src/node.cpp @@ -289,16 +289,6 @@ std::optional PathGenerator::generate_path( const lanelet::LaneletSequence & lanelet_sequence, const double s_start, const double s_end, const Params & params) const { - // Sanity check - if (s_start > s_end) { - RCLCPP_ERROR( - get_logger(), - "s_start must not be greater than s_end: Provided s_start = %f, s_end = %f. Skipping path " - "generation.", - s_start, s_end); - return std::nullopt; - } - std::vector path_points_with_lane_id{}; const auto add_path_point = [&](const auto & path_point, const lanelet::ConstLanelet & lanelet) { From daef6de3feb113150dac9630658df086a07eec66 Mon Sep 17 00:00:00 2001 From: Junya Sasaki Date: Sat, 8 Mar 2025 15:55:57 +0900 Subject: [PATCH 30/41] refactor: fix complexity * We should start from the simple one * Then we can add the necessary optimization later Signed-off-by: Junya Sasaki --- .../include/autoware/path_generator/utils.hpp | 20 +- .../autoware_path_generator/src/utils.cpp | 252 +----------------- 2 files changed, 14 insertions(+), 258 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 2810e77d5a..89e924464f 100644 --- a/planning/autoware_path_generator/include/autoware/path_generator/utils.hpp +++ b/planning/autoware_path_generator/include/autoware/path_generator/utils.hpp @@ -161,22 +161,6 @@ 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 = std::numeric_limits::max()); -/** - * @brief Modify the path points near the goal to smoothly connect the input path and the goal - * point - * @details Remove the path points that are forward from the goal by the distance of - * search_radius_range. Then insert the goal into the path. The previous goal point generated - * from the goal posture information is also inserted for the smooth connection of the goal pose. - * @param [in] search_radius_range distance on path to be modified for goal insertion - * @param [in] input original path - * @param [in] goal original goal pose - * @param [in] goal_lane_id Lane ID of goal lanelet. - * @return output path with modified points for the goal (std::nullopt if not found) - */ -std::optional set_goal( - const double search_radius_range, const PathWithLaneId & input, - const geometry_msgs::msg::Pose & goal, const int64_t goal_lane_id); - /** * @brief Recreate the goal pose to prevent the goal point being too far from the lanelet, which * causes the path to twist near the goal. @@ -193,12 +177,10 @@ const geometry_msgs::msg::Pose refine_goal( * @param search_radius_range Searching radius. * @param input Input path. * @param refined_goal Goal pose. - * @param goal_lane_id Lane ID of goal lanelet. * @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 PathWithLaneId & input, const geometry_msgs::msg::Pose & goal); /** * @brief Extract lanelets from the path. diff --git a/planning/autoware_path_generator/src/utils.cpp b/planning/autoware_path_generator/src/utils.cpp index ee542862a9..d6594870de 100644 --- a/planning/autoware_path_generator/src/utils.cpp +++ b/planning/autoware_path_generator/src/utils.cpp @@ -44,81 +44,6 @@ bool exists(const std::vector & vec, const T & item) { return std::find(vec.begin(), vec.end(), item) != vec.end(); } - -std::optional calc_interpolated_z( - const autoware_internal_planning_msgs::msg::PathWithLaneId & input, - const geometry_msgs::msg::Point target_pos, const size_t seg_idx) -{ - // Check if input is empty - if (input.points.empty()) { - RCLCPP_WARN(rclcpp::get_logger("path_generator"), "Input is empty"); - return std::nullopt; - } - // Check if seg_idx is invalid: -2 is to avoid out of bounds error by seg_idx + 1 - if (seg_idx > input.points.size() - 2) { - RCLCPP_WARN( - rclcpp::get_logger("path_generator"), - "seg_idx: %zu is invalid for input size: %zu.\n" - "Use the z that of the last point as the interpolated z.", - seg_idx, input.points.size()); - - // Return the z of the last point if interpolation is not possible - return input.points.back().point.pose.position.z; - } - - try { - const double closest_to_target_dist = autoware::motion_utils::calcSignedArcLength( - input.points, input.points.at(seg_idx).point.pose.position, target_pos); - - const double seg_dist = - autoware::motion_utils::calcSignedArcLength(input.points, seg_idx, seg_idx + 1); - - const double closest_z = input.points.at(seg_idx).point.pose.position.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; - } catch (const std::exception & e) { - RCLCPP_ERROR(rclcpp::get_logger("path_generator"), "Error: %s", e.what()); - return std::nullopt; - } -} - -std::optional calc_interpolated_velocity( - const autoware_internal_planning_msgs::msg::PathWithLaneId & input, const size_t seg_idx) -{ - // Check if input is empty - if (input.points.empty()) { - RCLCPP_WARN(rclcpp::get_logger("path_generator"), "Input is empty"); - return std::nullopt; - } - // Check if seg_idx is invalid: -2 is to avoid out of bounds error by seg_idx + 1 - if (seg_idx > input.points.size() - 2) { - RCLCPP_WARN( - rclcpp::get_logger("path_generator"), - "seg_idx: %zu is invalid for input size: %zu.\n" - "Use the velocity that of the last point as the interpolated velocity.", - seg_idx, input.points.size()); - - // Return the velocity of the last point if interpolation is not possible - return input.points.back().point.longitudinal_velocity_mps; - } - - try { - const double seg_dist = - autoware::motion_utils::calcSignedArcLength(input.points, seg_idx, seg_idx + 1); - - const double closest_vel = input.points.at(seg_idx).point.longitudinal_velocity_mps; - const double next_vel = input.points.at(seg_idx + 1).point.longitudinal_velocity_mps; - const double interpolated_vel = std::abs(seg_dist) < 1e-06 ? next_vel : closest_vel; - return interpolated_vel; - } catch (const std::exception & e) { - RCLCPP_ERROR(rclcpp::get_logger("path_generator"), "Error: %s", e.what()); - return std::nullopt; - } -} - } // namespace std::optional get_lanelets_within_route( @@ -379,155 +304,13 @@ std::vector get_path_bound( return path_bound; } -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) -{ - if (points.empty()) { - return std::nullopt; - } - - // find goal index - size_t min_dist_index; - { - bool found = false; - double min_dist = std::numeric_limits::max(); - for (size_t i = 0; i < points.size(); ++i) { - const auto & lane_ids = points.at(i).lane_ids; - - const double dist_to_goal = autoware_utils::calc_distance2d(points.at(i).point.pose, goal); - const bool is_goal_lane_id_in_point = - std::find(lane_ids.begin(), lane_ids.end(), goal_lane_id) != lane_ids.end(); - if (dist_to_goal < max_dist && dist_to_goal < min_dist && is_goal_lane_id_in_point) { - min_dist_index = i; - min_dist = dist_to_goal; - found = true; - } - } - if (!found) { - return std::nullopt; - } - } - - // find index out of goal search range - size_t min_dist_out_of_range_index = min_dist_index; - for (int i = min_dist_index; 0 <= i; --i) { - const double dist = autoware_utils::calc_distance2d(points.at(i).point, goal); - min_dist_out_of_range_index = i; - if (max_dist < dist) { - break; - } - } - - return min_dist_out_of_range_index; -} - -std::optional set_goal( - const double search_radius_range, const PathWithLaneId & input, - const geometry_msgs::msg::Pose & goal, const int64_t goal_lane_id) -{ - try { - if (input.points.empty()) { - return std::nullopt; - } - - PathWithLaneId output; - - // Calculate refined_goal with interpolation - // Note: `goal` does not have valid z, that will be calculated by interpolation here - PathPointWithLaneId refined_goal{}; - const size_t closest_seg_idx_for_goal = - autoware::motion_utils::findFirstNearestIndexWithSoftConstraints( - input.points, goal, 3.0, M_PI_4); - - // Set goal - refined_goal.point.pose = goal; - - if (auto z = calc_interpolated_z(input, goal.position, closest_seg_idx_for_goal)) { - refined_goal.point.pose.position.z = *z; - } else { - RCLCPP_ERROR(rclcpp::get_logger("path_generator"), "Failed to calculate z for goal"); - return std::nullopt; - } - - refined_goal.point.longitudinal_velocity_mps = 0.0; - - // Calculate pre_refined_goal with interpolation - // Note: z and velocity are filled - PathPointWithLaneId pre_refined_goal{}; - constexpr double goal_to_pre_goal_distance = -1.0; - pre_refined_goal.point.pose = - autoware_utils::calc_offset_pose(goal, goal_to_pre_goal_distance, 0.0, 0.0); - const size_t closest_seg_idx_for_pre_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)) { - pre_refined_goal.point.pose.position.z = *z; - } else { - RCLCPP_ERROR(rclcpp::get_logger("path_generator"), "Failed to calculate z for pre_goal"); - return std::nullopt; - } - - if (auto velocity = calc_interpolated_velocity(input, closest_seg_idx_for_pre_goal)) { - pre_refined_goal.point.longitudinal_velocity_mps = *velocity; - } else { - RCLCPP_ERROR( - rclcpp::get_logger("path_generator"), "Failed to calculate velocity for pre_goal"); - return std::nullopt; - } - - // 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); - if (!min_dist_out_of_circle_index_opt) { - return std::nullopt; - } - const auto min_dist_out_of_circle_index = min_dist_out_of_circle_index_opt.value(); - - // Create output points - 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)); - } - output.points.push_back(pre_refined_goal); - output.points.push_back(refined_goal); - - // Add lane IDs from skipped points to the pre-goal point - auto & pre_goal = output.points.at(output.points.size() - 2); - for (size_t i = min_dist_out_of_circle_index + 1; i < input.points.size(); ++i) { - for (const auto target_lane_id : input.points.at(i).lane_ids) { - const bool is_lane_id_found = - std::find(pre_goal.lane_ids.begin(), pre_goal.lane_ids.end(), target_lane_id) != - pre_goal.lane_ids.end(); - if (!is_lane_id_found) { - pre_goal.lane_ids.push_back(target_lane_id); - } - } - } - - // Attach goal lane IDs to the last point - output.points.back().lane_ids = input.points.back().lane_ids; - - output.left_bound = input.left_bound; - output.right_bound = input.right_bound; - - return output; - } catch (std::out_of_range & ex) { - RCLCPP_ERROR_STREAM( - rclcpp::get_logger("path_generator").get_child("utils"), "failed to set goal: " << ex.what()); - return std::nullopt; - } -} - const geometry_msgs::msg::Pose refine_goal( const geometry_msgs::msg::Pose & goal, const lanelet::ConstLanelet & goal_lanelet) { - const auto lanelet_point = lanelet::utils::conversion::toLaneletPoint(goal.position); + const auto goal_point_on_lanelet = lanelet::utils::conversion::toLaneletPoint(goal.position); const double distance = boost::geometry::distance( - goal_lanelet.polygon2d().basicPolygon(), lanelet::utils::to2D(lanelet_point).basicPoint()); + goal_lanelet.polygon2d().basicPolygon(), + lanelet::utils::to2D(goal_point_on_lanelet).basicPoint()); // You are almost at the goal if (distance < std::numeric_limits::epsilon()) { @@ -536,7 +319,7 @@ const geometry_msgs::msg::Pose refine_goal( // Get the closest segment to the goal const auto segment = lanelet::utils::getClosestSegment( - lanelet::utils::to2D(lanelet_point), goal_lanelet.centerline()); + lanelet::utils::to2D(goal_point_on_lanelet), goal_lanelet.centerline()); // If the segment is empty, return the original goal. if (segment.empty()) { @@ -549,7 +332,7 @@ const geometry_msgs::msg::Pose refine_goal( const auto p1 = segment.front().basicPoint(); const auto p2 = segment.back().basicPoint(); const auto direction_vector = (p2 - p1).normalized(); - const auto p1_to_goal = lanelet_point.basicPoint() - p1; + const auto p1_to_goal = goal_point_on_lanelet.basicPoint() - p1; const double s = direction_vector.dot(p1_to_goal); const auto refined_point = p1 + direction_vector * s; @@ -567,8 +350,7 @@ 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 int64_t goal_lane_id) + const PathWithLaneId & input, const geometry_msgs::msg::Pose & goal) { PathWithLaneId filtered_path = input; @@ -579,13 +361,11 @@ PathWithLaneId refine_path_for_goal( filtered_path.points.back().point.longitudinal_velocity_mps = 0.0; } - // If set_goal returns a valid path, return it - if ( - const auto path_with_goal = set_goal(search_radius_range, filtered_path, goal, goal_lane_id)) { - return *path_with_goal; - } + // Replace the goal point with the refined goal + filtered_path.points.back().point.pose = goal; + + // If necessary, do more fine tuning for goal connection here - // Otherwise, return the original path with zero velocity at the end return filtered_path; } @@ -674,20 +454,14 @@ PathWithLaneId modify_path_for_smooth_goal_connection( refined_goal = goal; } } - double goal_search_radius{ - planner_data->path_generator_parameters.refine_goal_search_radius_range}; bool is_valid_path{false}; PathWithLaneId refined_path; // Then, refine the path for the goal - while (goal_search_radius >= 0 && !is_valid_path) { - refined_path = - refine_path_for_goal(goal_search_radius, path, refined_goal, planner_data->goal_lane_id); - if (is_path_valid(refined_path, planner_data)) { - is_valid_path = true; - } - goal_search_radius -= planner_data->path_generator_parameters.search_radius_decrement; + refined_path = refine_path_for_goal(path, refined_goal); + if (is_path_valid(refined_path, planner_data)) { + is_valid_path = true; } // It is better to return the original path if the refined path is not valid From 764092b46fa5ae5e734c41184f00887d2afcfa33 Mon Sep 17 00:00:00 2001 From: Junya Sasaki Date: Sat, 8 Mar 2025 20:28:59 +0900 Subject: [PATCH 31/41] bug: missing fixes in the include header Signed-off-by: Junya Sasaki --- .../include/autoware/path_generator/utils.hpp | 13 ------------- 1 file changed, 13 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 89e924464f..0ea9a74baf 100644 --- a/planning/autoware_path_generator/include/autoware/path_generator/utils.hpp +++ b/planning/autoware_path_generator/include/autoware/path_generator/utils.hpp @@ -149,18 +149,6 @@ std::vector get_path_bound( const lanelet::CompoundLineString2d & lanelet_centerline, const double s_start, const double s_end); -/** - * @brief find index out of goal search range - * @param points points of path - * @param goal goal pose - * @param goal_lane_id lane id of goal lanelet - * @param max_dist maximum distance to search for goal index - * @return index out of goal search range (std::nullopt if not found) - */ -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 = std::numeric_limits::max()); - /** * @brief Recreate the goal pose to prevent the goal point being too far from the lanelet, which * causes the path to twist near the goal. @@ -174,7 +162,6 @@ const geometry_msgs::msg::Pose refine_goal( /** * @brief Recreate the path with a given goal pose. - * @param search_radius_range Searching radius. * @param input Input path. * @param refined_goal Goal pose. * @return Recreated path From 4688b8b90d92931ab33d6e013344f2f4e3c55f66 Mon Sep 17 00:00:00 2001 From: Junya Sasaki Date: Sat, 8 Mar 2025 20:29:43 +0900 Subject: [PATCH 32/41] bug: inconsistent function declaration * The type of returned value and arguments were wrong Signed-off-by: Junya Sasaki --- .../include/autoware/path_generator/utils.hpp | 5 ++--- 1 file changed, 2 insertions(+), 3 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 0ea9a74baf..2c70526780 100644 --- a/planning/autoware_path_generator/include/autoware/path_generator/utils.hpp +++ b/planning/autoware_path_generator/include/autoware/path_generator/utils.hpp @@ -181,10 +181,9 @@ std::optional extract_lanelets_from_path( /** * @brief Get the goal lanelet. * @param planner_data Planner data. - * @param goal_lanelet Goal lanelet. - * @return True if the goal lanelet is found, false otherwise + * @return Goal lanelet (std::nullopt if not found) */ -bool get_goal_lanelet(const PlannerData & planner_data, lanelet::ConstLanelet * goal_lanelet); +std::optional get_goal_lanelet(const PlannerData & planner_data); /** * @brief Check if the pose is in the lanelets. From 576a0025d9d88cfcf1d4f39d1a41f7ecaec686f5 Mon Sep 17 00:00:00 2001 From: Junya Sasaki Date: Sat, 8 Mar 2025 20:38:01 +0900 Subject: [PATCH 33/41] Update planning/autoware_path_generator/include/autoware/path_generator/common_structs.hpp Signed-off-by: Junya Sasaki Co-authored-by: Kosuke Takeuchi --- .../include/autoware/path_generator/common_structs.hpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/planning/autoware_path_generator/include/autoware/path_generator/common_structs.hpp b/planning/autoware_path_generator/include/autoware/path_generator/common_structs.hpp index 278a8840dd..8285e8417c 100644 --- a/planning/autoware_path_generator/include/autoware/path_generator/common_structs.hpp +++ b/planning/autoware_path_generator/include/autoware/path_generator/common_structs.hpp @@ -30,12 +30,12 @@ struct PathGeneratorParameters /** * @brief Distance on path to be modified for goal insertion */ - double refine_goal_search_radius_range; + double refine_goal_search_radius_range{0.0}; /** * @brief Decrement of the search radius range */ - double search_radius_decrement; + double search_radius_decrement{0.0}; }; struct PlannerData From 56f36c72aa5ead24cfa581676a828d628a2804ef Mon Sep 17 00:00:00 2001 From: Junya Sasaki Date: Sat, 8 Mar 2025 20:38:15 +0900 Subject: [PATCH 34/41] Update planning/autoware_path_generator/src/node.cpp Signed-off-by: Junya Sasaki Co-authored-by: Kosuke Takeuchi --- planning/autoware_path_generator/src/node.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/planning/autoware_path_generator/src/node.cpp b/planning/autoware_path_generator/src/node.cpp index 48246ca1d0..becf067de9 100644 --- a/planning/autoware_path_generator/src/node.cpp +++ b/planning/autoware_path_generator/src/node.cpp @@ -406,7 +406,7 @@ std::optional PathGenerator::generate_path( if (distance_to_goal < planner_data_.path_generator_parameters.refine_goal_search_radius_range) { // Perform smooth goal connection - auto planner_data_ptr = std::make_shared(planner_data_); + const auto planner_data_ptr = std::make_shared(planner_data_); finalized_path_with_lane_id = utils::modify_path_for_smooth_goal_connection(std::move(preprocessed_path), planner_data_ptr); } else { From 5872f9c5da841de209ff1f0d89ba7095e076c339 Mon Sep 17 00:00:00 2001 From: Junya Sasaki Date: Sat, 8 Mar 2025 20:39:28 +0900 Subject: [PATCH 35/41] Update planning/autoware_path_generator/src/utils.cpp Signed-off-by: Junya Sasaki Co-authored-by: Kosuke Takeuchi --- planning/autoware_path_generator/src/utils.cpp | 12 +----------- 1 file changed, 1 insertion(+), 11 deletions(-) diff --git a/planning/autoware_path_generator/src/utils.cpp b/planning/autoware_path_generator/src/utils.cpp index d6594870de..ad85febd76 100644 --- a/planning/autoware_path_generator/src/utils.cpp +++ b/planning/autoware_path_generator/src/utils.cpp @@ -442,18 +442,8 @@ PathWithLaneId modify_path_for_smooth_goal_connection( { const auto goal = planner_data->goal_pose; - geometry_msgs::msg::Pose refined_goal{}; - { - // Prevent from shadowVariable const auto goal_lanelet = get_goal_lanelet(*planner_data); - - // First, polish up the goal pose if possible - if (goal_lanelet) { - refined_goal = refine_goal(goal, *goal_lanelet); - } else { - refined_goal = goal; - } - } + const auto refined_goal = goal_lanelet.has_value() ? refine_goal(goal, *goal_lanelet) : goal; bool is_valid_path{false}; PathWithLaneId refined_path; From 468539ce08edb6abd1720ba75830d09c43413d3d Mon Sep 17 00:00:00 2001 From: Junya Sasaki Date: Sat, 8 Mar 2025 20:39:49 +0900 Subject: [PATCH 36/41] Update planning/autoware_path_generator/src/utils.cpp Signed-off-by: Junya Sasaki Co-authored-by: Kosuke Takeuchi --- planning/autoware_path_generator/src/utils.cpp | 16 ++-------------- 1 file changed, 2 insertions(+), 14 deletions(-) diff --git a/planning/autoware_path_generator/src/utils.cpp b/planning/autoware_path_generator/src/utils.cpp index ad85febd76..eb99801383 100644 --- a/planning/autoware_path_generator/src/utils.cpp +++ b/planning/autoware_path_generator/src/utils.cpp @@ -445,20 +445,8 @@ PathWithLaneId modify_path_for_smooth_goal_connection( const auto goal_lanelet = get_goal_lanelet(*planner_data); const auto refined_goal = goal_lanelet.has_value() ? refine_goal(goal, *goal_lanelet) : goal; - bool is_valid_path{false}; - PathWithLaneId refined_path; - - // Then, refine the path for the goal - refined_path = refine_path_for_goal(path, refined_goal); - if (is_path_valid(refined_path, planner_data)) { - is_valid_path = true; - } - - // It is better to return the original path if the refined path is not valid - if (!is_valid_path) { - return path; - } - return refined_path; + const PathWithLaneId refined_path = refine_path_for_goal(path, refined_goal); + return is_path_valid(refined_path, planner_data) ? refined_path : path; } TurnIndicatorsCommand get_turn_signal( From fb39145a7cee053dc4c80626486c8f6577a19d68 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 11:40:02 +0000 Subject: [PATCH 37/41] style(pre-commit): autofix --- planning/autoware_path_generator/src/utils.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/planning/autoware_path_generator/src/utils.cpp b/planning/autoware_path_generator/src/utils.cpp index eb99801383..191da0528b 100644 --- a/planning/autoware_path_generator/src/utils.cpp +++ b/planning/autoware_path_generator/src/utils.cpp @@ -442,8 +442,8 @@ PathWithLaneId modify_path_for_smooth_goal_connection( { const auto goal = planner_data->goal_pose; - const auto goal_lanelet = get_goal_lanelet(*planner_data); - const auto refined_goal = goal_lanelet.has_value() ? refine_goal(goal, *goal_lanelet) : goal; + const auto goal_lanelet = get_goal_lanelet(*planner_data); + const auto refined_goal = goal_lanelet.has_value() ? refine_goal(goal, *goal_lanelet) : goal; const PathWithLaneId refined_path = refine_path_for_goal(path, refined_goal); return is_path_valid(refined_path, planner_data) ? refined_path : path; From 7d64da2e933c062d3c6e4b3cc8d95641721ab035 Mon Sep 17 00:00:00 2001 From: Junya Sasaki Date: Sat, 8 Mar 2025 22:35:52 +0900 Subject: [PATCH 38/41] fix: apply comment in the following PR * https://github.com/autowarefoundation/autoware.core/pull/227#discussion_r1986045016 Signed-off-by: Junya Sasaki --- .../include/autoware/path_generator/utils.hpp | 36 +++- .../autoware_path_generator/src/utils.cpp | 167 ++++++++++++++++-- 2 files changed, 190 insertions(+), 13 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 2c70526780..4bbec96c18 100644 --- a/planning/autoware_path_generator/include/autoware/path_generator/utils.hpp +++ b/planning/autoware_path_generator/include/autoware/path_generator/utils.hpp @@ -160,14 +160,44 @@ std::vector get_path_bound( const geometry_msgs::msg::Pose refine_goal( const geometry_msgs::msg::Pose & goal, const lanelet::ConstLanelet & goal_lanelet); +/** + * @brief Prepare the point before the goal point. + * @param goal Goal pose. + * @param lanes Lanelets. + * @return Pre-goal point. + */ +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. + * @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. + */ +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); + +/** + * @brief Get the path up to just before the pre_goal. + * @param input Input path. + * @param refined_goal Goal pose. + * @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); + /** * @brief Recreate the path with a given goal pose. * @param input Input path. * @param refined_goal Goal pose. + * @param planner_data Planner data. * @return Recreated path */ PathWithLaneId refine_path_for_goal( - const PathWithLaneId & input, const geometry_msgs::msg::Pose & goal); + const PathWithLaneId & input, const geometry_msgs::msg::Pose & goal, const PlannerData & planner_data); /** * @brief Extract lanelets from the path. @@ -176,7 +206,7 @@ PathWithLaneId refine_path_for_goal( * @return Extracted lanelets */ std::optional extract_lanelets_from_path( - const PathWithLaneId & refined_path, const std::shared_ptr & planner_data); + const PathWithLaneId & refined_path, const PlannerData & planner_data); /** * @brief Get the goal lanelet. @@ -200,7 +230,7 @@ bool is_in_lanelets(const geometry_msgs::msg::Pose & pose, const lanelet::ConstL * @return True if the path is valid, false otherwise */ bool is_path_valid( - const PathWithLaneId & refined_path, const std::shared_ptr & planner_data); + 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 191da0528b..0ed021e408 100644 --- a/planning/autoware_path_generator/src/utils.cpp +++ b/planning/autoware_path_generator/src/utils.cpp @@ -349,8 +349,111 @@ const geometry_msgs::msg::Pose refine_goal( return refined_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) +{ + 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; + + // 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()); + } + } + + return pre_refined_goal; +} + +// A function that assumes a circle with radius max_dist centered at the goal and returns +// the index of the point closest to the circumference of the circle 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) +{ + if (points.empty()) { + return std::nullopt; + } + + // find goal index + size_t min_dist_index; + { + bool found = false; + double min_dist = std::numeric_limits::max(); + for (size_t i = 0; i < points.size(); ++i) { + const auto & lane_ids = points.at(i).lane_ids; + + const double dist_to_goal = autoware_utils::calc_distance2d(points.at(i).point.pose, goal); + const bool is_goal_lane_id_in_point = exists(lane_ids, goal_lane_id); + if (dist_to_goal < max_dist && dist_to_goal < min_dist && is_goal_lane_id_in_point) { + min_dist_index = i; + min_dist = dist_to_goal; + found = true; + } + } + if (!found) { + return std::nullopt; + } + } + + // Find index out of goal search range + size_t min_dist_out_of_range_index = min_dist_index; + for (int i = min_dist_index; 0 <= i; --i) { + const double dist = autoware_utils::calc_distance2d(points.at(i).point, goal); + min_dist_out_of_range_index = i; + if (max_dist < dist) { + break; + } + } + + return min_dist_out_of_range_index; +} + +// 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 +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) +{ + // 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 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)); + } + + 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 PathWithLaneId & input, const geometry_msgs::msg::Pose & goal, const PlannerData & planner_data) { PathWithLaneId filtered_path = input; @@ -361,23 +464,45 @@ PathWithLaneId refine_path_for_goal( filtered_path.points.back().point.longitudinal_velocity_mps = 0.0; } + // 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); + 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; + } + + // Get the value from the optional + auto path_up_to_just_before_pre_goal = path_up_to_just_before_pre_goal_opt.value(); + + // Add pre_goal to the path + const auto lanes_opt = extract_lanelets_from_path(filtered_path, planner_data); + if (!lanes_opt) { + // It might be better to use the original path when the lanelets are not found + return input; + } + const auto lanes = lanes_opt.value(); + + // Reserve the size of the path + pre_goal + goal + path_up_to_just_before_pre_goal.points.reserve(path_up_to_just_before_pre_goal.points.size() + 2); + path_up_to_just_before_pre_goal.points.push_back(prepare_pre_goal(goal, lanes)); + // Replace the goal point with the refined goal - filtered_path.points.back().point.pose = goal; + path_up_to_just_before_pre_goal.points.back().point.pose = goal; // If necessary, do more fine tuning for goal connection here - return filtered_path; + return path_up_to_just_before_pre_goal; } std::optional extract_lanelets_from_path( - const PathWithLaneId & refined_path, const std::shared_ptr & planner_data) + const PathWithLaneId & refined_path, const PlannerData & planner_data) { lanelet::ConstLanelets refined_path_lanelets; for (size_t i = 0; i < refined_path.points.size(); ++i) { try { const auto & path_point = refined_path.points.at(i); int64_t lane_id = path_point.lane_ids.at(0); - lanelet::ConstLanelet lanelet = planner_data->lanelet_map_ptr->laneletLayer.get(lane_id); + lanelet::ConstLanelet lanelet = planner_data.lanelet_map_ptr->laneletLayer.get(lane_id); const bool is_unique = std::find(refined_path_lanelets.begin(), refined_path_lanelets.end(), lanelet) == @@ -416,7 +541,7 @@ bool is_in_lanelets(const geometry_msgs::msg::Pose & pose, const lanelet::ConstL } bool is_path_valid( - const PathWithLaneId & refined_path, const std::shared_ptr & planner_data) + 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); @@ -442,11 +567,33 @@ PathWithLaneId modify_path_for_smooth_goal_connection( { const auto goal = planner_data->goal_pose; - const auto goal_lanelet = get_goal_lanelet(*planner_data); - const auto refined_goal = goal_lanelet.has_value() ? refine_goal(goal, *goal_lanelet) : goal; + geometry_msgs::msg::Pose refined_goal{}; + { + // Prevent from shadowVariable + const auto goal_lanelet = get_goal_lanelet(*planner_data); - const PathWithLaneId refined_path = refine_path_for_goal(path, refined_goal); - return is_path_valid(refined_path, planner_data) ? refined_path : path; + // First, polish up the goal pose if possible + if (goal_lanelet) { + refined_goal = refine_goal(goal, *goal_lanelet); + } else { + refined_goal = goal; + } + } + + bool is_valid_path{false}; + PathWithLaneId refined_path; + + // Then, refine the path for the goal + refined_path = refine_path_for_goal(path, refined_goal, *planner_data); + if (is_path_valid(refined_path, *planner_data)) { + is_valid_path = true; + } + + // It is better to return the original path if the refined path is not valid + if (!is_valid_path) { + return path; + } + return refined_path; } TurnIndicatorsCommand get_turn_signal( From ed64aaf699e4ff21fbe291583859b08848195a21 Mon Sep 17 00:00:00 2001 From: Junya Sasaki Date: Sat, 8 Mar 2025 22:37:29 +0900 Subject: [PATCH 39/41] fix: sorry, I was missing one comment to be applied Signed-off-by: Junya Sasaki --- .../autoware_path_generator/config/path_generator.param.yaml | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/planning/autoware_path_generator/config/path_generator.param.yaml b/planning/autoware_path_generator/config/path_generator.param.yaml index fd69c7d005..67981033be 100644 --- a/planning/autoware_path_generator/config/path_generator.param.yaml +++ b/planning/autoware_path_generator/config/path_generator.param.yaml @@ -12,5 +12,5 @@ search_distance: 30.0 resampling_interval: 1.0 angle_threshold_deg: 15.0 - refine_goal_search_radius_range: 10.0 # [m] + refine_goal_search_radius_range: 7.5 # [m] search_radius_decrement: 1.0 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 40/41] 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 4bbec96c18..6575550202 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 0ed021e408..8a973daef4 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); From d9b3a8de65d3dfbb9422e982cbf5b3f6dc6dfff6 Mon Sep 17 00:00:00 2001 From: Junya Sasaki Date: Sat, 8 Mar 2025 22:45:28 +0900 Subject: [PATCH 41/41] bug: fix wrong goal point interpolation Signed-off-by: Junya Sasaki --- planning/autoware_path_generator/src/utils.cpp | 7 ++++++- 1 file changed, 6 insertions(+), 1 deletion(-) diff --git a/planning/autoware_path_generator/src/utils.cpp b/planning/autoware_path_generator/src/utils.cpp index 8a973daef4..e8247f41b5 100644 --- a/planning/autoware_path_generator/src/utils.cpp +++ b/planning/autoware_path_generator/src/utils.cpp @@ -493,9 +493,14 @@ PathWithLaneId refine_path_for_goal( // Reserve the size of the path + pre_goal + goal path_up_to_just_before_pre_goal.points.reserve(path_up_to_just_before_pre_goal.points.size() + 2); + + // Insert pre_goal to the path path_up_to_just_before_pre_goal.points.push_back(prepare_pre_goal(goal, lanes)); - // Replace the goal point with the refined goal + // Insert goal (obtained from the tail of input path) to the cleaned up path + path_up_to_just_before_pre_goal.points.push_back(input.points.back()); + + // Finally, replace the goal point with the refined one path_up_to_just_before_pre_goal.points.back().point.pose = goal; // If necessary, do more fine tuning for goal connection here