feat(autoware_path_generator): function to smooth the route

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

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

@@ -24,19 +24,36 @@
 
 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
 {
   lanelet::LaneletMapPtr lanelet_map_ptr{nullptr};
   lanelet::traffic_rules::TrafficRulesPtr traffic_rules_ptr{nullptr};
   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{};
   lanelet::ConstLanelets preferred_lanelets{};
   lanelet::ConstLanelets start_lanelets{};
   lanelet::ConstLanelets goal_lanelets{};
+
+  PathGeneratorParameters path_generator_parameters{};
 };
 }  // namespace autoware::path_generator
 

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,100 @@ std::vector<geometry_msgs::msg::Point> 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<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 = std::numeric_limits<double>::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<PathWithLaneId> 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.
+ * @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 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);
+
+/**
+ * @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 std::shared_ptr<const PlannerData> & 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<const PlannerData> & 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<const PlannerData> & planner_data);
+
 /**
  * @brief get earliest turn signal based on turn direction specified for lanelets
  * @param path target path

planning/autoware_path_generator/src/node.cpp

@@ -46,6 +46,20 @@ 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<double>("refine_goal_search_radius_range");
+  planner_data_.path_generator_parameters.search_radius_decrement =
+    declare_parameter<double>("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());
 
@@ -155,6 +169,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,
@@ -359,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 =
@@ -385,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