fix: smooth path before cropping trajectory points

Signed-off-by: Junya Sasaki <junya.sasaki@tier4.jp>

planning/autoware_path_generator/src/node.cpp

@@ -223,11 +223,7 @@ std::optional<PathWithLaneId> PathGenerator::plan_path(
     return std::nullopt;
   }
 
-  // Make the path smooth
-  auto planner_data_ptr = std::make_shared<const PlannerData>(planner_data_);
-  const auto smooth_path = utils::modify_path_for_smooth_goal_connection(*path, planner_data_ptr);
-
-  return smooth_path;
+  return path;
 }
 
 std::optional<PathWithLaneId> PathGenerator::generate_path(
@@ -379,23 +375,39 @@ std::optional<PathWithLaneId> PathGenerator::generate_path(
     }
   }
 
-  auto trajectory = Trajectory::Builder().build(path_points_with_lane_id);
+  auto planner_data_ptr = std::make_shared<const PlannerData>(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<PathWithLaneId> 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