fix(path_generator): ensure refined path connects start and goal

planning/autoware_path_generator/config/path_generator.param.yaml

@@ -12,5 +12,6 @@
       search_distance: 30.0
       resampling_interval: 1.0
       angle_threshold_deg: 15.0
-    refine_goal_search_radius_range: 7.5  # [m]
-    search_radius_decrement: 1.0
+    smooth_goal_connection:
+      search_radius_range: 7.5
+      pre_goal_offset: 1.0

planning/autoware_path_generator/include/autoware/path_generator/utils.hpp

@@ -199,7 +199,7 @@ PathRange<std::optional<double>> get_arc_length_on_centerline(
 experimental::trajectory::Trajectory<PathPointWithLaneId> refine_path_for_goal(
   const experimental::trajectory::Trajectory<PathPointWithLaneId> & input,
   const geometry_msgs::msg::Pose & goal_pose, const lanelet::Id goal_lane_id,
-  const double refine_goal_search_radius_range);
+  const double search_radius_range, const double pre_goal_offset);
 
 /**
  * @brief Extract lanelets from the trajectory.
@@ -232,7 +232,7 @@ bool is_trajectory_inside_lanelets(
 std::optional<experimental::trajectory::Trajectory<PathPointWithLaneId>>
 modify_path_for_smooth_goal_connection(
   const experimental::trajectory::Trajectory<PathPointWithLaneId> & trajectory,
-  const PlannerData & planner_data, const double refine_goal_search_radius_range);
+  const PlannerData & planner_data, const double search_radius_range, const double pre_goal_offset);
 
 /**
  * @brief get earliest turn signal based on turn direction specified for lanelets

planning/autoware_path_generator/param/path_generator_parameters.yaml

@@ -1,42 +1,69 @@
 path_generator:
   planning_hz:
     type: double
+    validation:
+      gt<>: [0]
 
   path_length:
     backward:
       type: double
+      validation:
+        gt<>: [0]
 
     forward:
       type: double
+      validation:
+        gt<>: [0]
 
   waypoint_group:
     separation_threshold:
       type: double
+      validation:
+        gt<>: [0]
 
     interval_margin_ratio:
       type: double
+      validation:
+        gt<>: [0]
 
   turn_signal:
     search_time:
       type: double
+      validation:
+        gt<>: [0]
 
     search_distance:
       type: double
+      validation:
+        gt<>: [0]
 
     resampling_interval:
       type: double
+      validation:
+        gt<>: [0]
 
     angle_threshold_deg:
       type: double
+      validation:
+        gt<>: [0]
 
-  refine_goal_search_radius_range:
-    type: double
+  smooth_goal_connection:
+    search_radius_range:
+      type: double
+      validation:
+        gt<>: [0]
 
-  search_radius_decrement:
-    type: double
+    pre_goal_offset:
+      type: double
+      validation:
+        gt<>: [0]
 
   ego_nearest_dist_threshold:
     type: double
+    validation:
+      gt<>: [0]
 
   ego_nearest_yaw_threshold:
     type: double
+    validation:
+      gt<>: [0]

planning/autoware_path_generator/schema/path_generator.schema.json

@@ -60,15 +60,15 @@
           "default": "15.0",
           "minimum": 0.0
         },
-        "refine_goal_search_radius_range": {
+        "smooth_goal_connection.search_radius_range": {
           "type": "number",
-          "description": "Search radius for goal point refinement [m]",
+          "description": "Search radius for smooth goal connection [m]",
           "default": "7.5",
           "minimum": 0.0
         },
-        "search_radius_decrement": {
+        "smooth_goal_connection.pre_goal_offset": {
           "type": "number",
-          "description": "Decrement value for search radius [m]",
+          "description": "Offset for pre-goal [m]",
           "default": "1.0",
           "minimum": 0.0
         }
@@ -83,8 +83,8 @@
         "turn_signal.search_distance",
         "turn_signal.resampling_interval",
         "turn_signal.angle_threshold_deg",
-        "refine_goal_search_radius_range",
-        "search_radius_decrement"
+        "smooth_goal_connection.search_radius_range",
+        "smooth_goal_connection.pre_goal_offset"
       ],
       "additionalProperties": false
     }

planning/autoware_path_generator/src/node.cpp

@@ -60,12 +60,6 @@
 
   const auto params = param_listener_->get_params();
 
-  // Ensure that the refine_goal_search_radius_range and search_radius_decrement must be positive
-  if (params.refine_goal_search_radius_range <= 0 || params.search_radius_decrement <= 0) {
-    throw std::runtime_error(
-      "refine_goal_search_radius_range and search_radius_decrement must be positive");
-  }
-
   timer_ = rclcpp::create_timer(
     this, get_clock(), rclcpp::Rate(params.planning_hz).period(),
     std::bind(&PathGenerator::run, this));
@@ -439,9 +433,10 @@
   const auto distance_to_goal = autoware_utils::calc_distance2d(
     trajectory->compute(trajectory->length()), planner_data_.goal_pose);
 
-  if (distance_to_goal < params.refine_goal_search_radius_range) {
+  if (distance_to_goal < params.smooth_goal_connection.search_radius_range) {
     auto refined_path = utils::modify_path_for_smooth_goal_connection(
-      *trajectory, planner_data_, params.refine_goal_search_radius_range);
+      *trajectory, planner_data_, params.smooth_goal_connection.search_radius_range,
+      params.smooth_goal_connection.pre_goal_offset);
 
     if (refined_path) {
       refined_path->align_orientation_with_trajectory_direction();

planning/autoware_path_generator/src/utils.cpp

@@ -638,10 +638,9 @@
 
 // Function to refine the path for the goal
 experimental::trajectory::Trajectory<PathPointWithLaneId> refine_path_for_goal(
-  const experimental::trajectory::Trajectory<PathPointWithLaneId> & input,  //
-  const geometry_msgs::msg::Pose & goal_pose,                               //
-  const lanelet::Id goal_lane_id,                                           //
-  const double refine_goal_search_radius_range)
+  const experimental::trajectory::Trajectory<PathPointWithLaneId> & input,
+  const geometry_msgs::msg::Pose & goal_pose, const lanelet::Id goal_lane_id,
+  const double search_radius_range, const double pre_goal_offset)
 {
   auto contain_goal_lane_id = [&](const PathPointWithLaneId & point) {
     const auto & lane_ids = point.lane_ids;
@@ -652,7 +651,7 @@
 
   auto outside_circle = [&](const PathPointWithLaneId & point) {
     const double dist = autoware_utils::calc_distance2d(point.point.pose, goal_pose);
-    return dist > refine_goal_search_radius_range;
+    return dist > search_radius_range;
   };
 
   auto closest_to_goal = autoware::experimental::trajectory::closest_with_constraint(
@@ -672,6 +671,10 @@
   if (!intervals.empty()) {
     auto cropped = autoware::experimental::trajectory::crop(cropped_path, 0, intervals.back().end);
     goal_connected_trajectory_points = cropped.restore(2);
+  } else if (cropped_path.length() > pre_goal_offset) {
+    // If distance from start to goal is smaller than refine_goal_search_radius_range and start is
+    // farther from goal than pre_goal, we just connect start, pre_goal, and goal.
+    goal_connected_trajectory_points = {cropped_path.compute(0)};
   }
 
   auto goal = input.compute(autoware::experimental::trajectory::closest(input, goal_pose));
@@ -680,7 +683,8 @@
 
   goal.point.longitudinal_velocity_mps = 0.0;
 
-  auto pre_goal_pose = autoware_utils_geometry::calc_offset_pose(goal_pose, -1.0, 0.0, 0.0);
+  auto pre_goal_pose =
+    autoware_utils_geometry::calc_offset_pose(goal_pose, -pre_goal_offset, 0.0, 0.0);
 
   auto pre_goal = input.compute(autoware::experimental::trajectory::closest(input, pre_goal_pose));
 
@@ -747,7 +751,7 @@
 std::optional<experimental::trajectory::Trajectory<PathPointWithLaneId>>
 modify_path_for_smooth_goal_connection(
   const experimental::trajectory::Trajectory<PathPointWithLaneId> & trajectory,
-  const PlannerData & planner_data, const double refine_goal_search_radius_range)
+  const PlannerData & planner_data, const double search_radius_range, const double pre_goal_offset)
 {
   if (planner_data.preferred_lanelets.empty()) {
     return std::nullopt;
@@ -757,10 +761,10 @@
   // This process is to fit the trajectory inside the lanelets. By reducing
   // refine_goal_search_radius_range, we can fit the trajectory inside lanelets even if the
   // trajectory has a high curvature.
-  for (double s = refine_goal_search_radius_range; s > 0; s -= 0.1) {
+  for (double s = search_radius_range; s > 0; s -= 0.1) {
     const auto refined_trajectory = refine_path_for_goal(
       trajectory, planner_data.goal_pose, planner_data.preferred_lanelets.back().id(),
-      refine_goal_search_radius_range);
+      search_radius_range, pre_goal_offset);
     const bool is_inside = is_trajectory_inside_lanelets(refined_trajectory, lanelets);
     if (is_inside) {
       return refined_trajectory;