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/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/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,
@@ -273,6 +289,16 @@ std::optional<PathWithLaneId> 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<PathPointWithLaneId> path_points_with_lane_id{};
 
   const auto add_path_point = [&](const auto & path_point, const lanelet::ConstLanelet & lanelet) {
@@ -364,18 +390,42 @@ std::optional<PathWithLaneId> PathGenerator::generate_path(
     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.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<const PlannerData>(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 =
@@ -385,10 +435,11 @@ 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());
+  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