feat(autoware_path_generator): function to smooth the path

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: 7.5  # [m]
+    search_radius_decrement: 1.0

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

@@ -20,6 +20,8 @@
 #include <autoware_internal_planning_msgs/msg/path_with_lane_id.hpp>
 #include <autoware_vehicle_msgs/msg/turn_indicators_command.hpp>
 
+#include <limits>
+#include <memory>
 #include <optional>
 #include <string>
 #include <unordered_map>
@@ -147,6 +149,97 @@ std::vector<geometry_msgs::msg::Point> get_path_bound(
   const lanelet::CompoundLineString2d & lanelet_centerline, const double s_start,
   const double s_end);
 
+/**
+ * @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 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<size_t> find_index_out_of_goal_search_range(
+  const std::vector<PathPointWithLaneId> & 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<PathWithLaneId> get_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 PlannerData & planner_data);
+
+/**
+ * @brief Extract lanelets from the path.
+ * @param path Input path.
+ * @param planner_data Planner data.
+ * @return Extracted lanelets
+ */
+std::optional<lanelet::ConstLanelets> extract_lanelets_from_path(
+  const PathWithLaneId & refined_path, const PlannerData & planner_data);
+
+/**
+ * @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 PlannerData & planner_data);
+
+/**
+ * @brief Modify the path to connect smoothly to the goal.
+ * @param path Input path.
+ * @param planner_data Planner data.
+ * @param refine_goal_search_radius_range Refine goal search radius range.
+ * @return Modified path
+ */
+PathWithLaneId modify_path_for_smooth_goal_connection(
+  const PathWithLaneId & path, const PlannerData & planner_data,
+  const double refine_goal_search_radius_range);
+
 /**
  * @brief get earliest turn signal based on turn direction specified for lanelets
  * @param path target path

planning/autoware_path_generator/package.xml

@@ -8,12 +8,14 @@
   <maintainer email="takayuki.murooka@tier4.jp">Takayuki Murooka</maintainer>
   <maintainer email="mitsuhiro.sakamoto@tier4.jp">Mitsuhiro Sakamoto</maintainer>
   <maintainer email="kosuke.takeuchi@tier4.jp">Kosuke Takeuchi</maintainer>
+  <maintainer email="junya.sasaki@tier4.jp">Junya Sasaki</maintainer>
   <license>Apache License 2.0</license>
 
   <author email="satoshi.ota@tier4.jp">Satoshi Ota</author>
   <author email="takayuki.murooka@tier4.jp">Takayuki Murooka</author>
   <author email="mitsuhiro.sakamoto@tier4.jp">Mitsuhiro Sakamoto</author>
   <author email="kosuke.takeuchi@tier4.jp">Kosuke Takeuchi</author>
+  <author email="junya.sasaki@tier4.jp">Junya Sasaki</author>
 
   <buildtool_depend>ament_cmake_auto</buildtool_depend>
   <buildtool_depend>autoware_cmake</buildtool_depend>
@@ -22,7 +24,6 @@
   <depend>autoware_lanelet2_extension</depend>
   <depend>autoware_motion_utils</depend>
   <depend>autoware_planning_test_manager</depend>
-  <depend>autoware_test_utils</depend>
   <depend>autoware_trajectory</depend>
   <depend>autoware_vehicle_info_utils</depend>
   <depend>generate_parameter_library</depend>
@@ -32,6 +33,7 @@
   <test_depend>ament_cmake_ros</test_depend>
   <test_depend>ament_lint_auto</test_depend>
   <test_depend>autoware_lint_common</test_depend>
+  <test_depend>autoware_test_utils</test_depend>
 
   <export>
     <build_type>ament_cmake</build_type>

planning/autoware_path_generator/param/path_generator_parameters.yaml

@@ -28,3 +28,9 @@ path_generator:
 
     angle_threshold_deg:
       type: double
+
+  refine_goal_search_radius_range:
+    type: double
+
+  search_radius_decrement:
+    type: double

planning/autoware_path_generator/src/node.cpp

@@ -57,6 +57,13 @@
   vehicle_info_ = autoware::vehicle_info_utils::VehicleInfoUtils(*this).getVehicleInfo();
 
   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));
@@ -364,18 +371,46 @@
     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)),
     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 preprocessed_path{};
+  preprocessed_path.points = trajectory->restore();
+
+  PathWithLaneId finalized_path_with_lane_id{};
+
+  // Check if the goal point is in the search range
+  // Note: We only see if the goal is approaching the tail of the path.
+  const auto distance_to_goal = autoware_utils::calc_distance2d(
+    preprocessed_path.points.back().point.pose, planner_data_.goal_pose);
+
+  if (distance_to_goal < params.refine_goal_search_radius_range) {
+    // Perform smooth goal connection
+    const auto params = param_listener_->get_params();
+
+    finalized_path_with_lane_id = utils::modify_path_for_smooth_goal_connection(
+      std::move(preprocessed_path), planner_data_, params.refine_goal_search_radius_range);
+  } 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;
+  }
+
+  // Set header which is needed to engage
+  finalized_path_with_lane_id.header.frame_id = planner_data_.route_frame_id;
+  finalized_path_with_lane_id.header.stamp = now();
 
   const auto get_path_bound = [&](const lanelet::CompoundLineString2d & lanelet_bound) {
     const auto s_bound_start =
@@ -385,10 +420,11 @@
     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());
+  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 path;
+  return finalized_path_with_lane_id;
 }
 }  // namespace autoware::path_generator