feat(goal_planner): align vehicle footprint heading parallel to parki…

…ng side lane boundary (#9159)

Signed-off-by: Mamoru Sobue <[email protected]>

planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/CMakeLists.txt

@@ -27,7 +27,7 @@ if(BUILD_EXAMPLES)
   FetchContent_Declare(
     matplotlibcpp17
     GIT_REPOSITORY https://github.com/soblin/matplotlibcpp17.git
-    GIT_TAG main
+    GIT_TAG master
   )
   FetchContent_MakeAvailable(matplotlibcpp17)
 

planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/examples/plot_map.cpp

@@ -13,6 +13,7 @@
 // limitations under the License.
 
 #include "autoware/behavior_path_goal_planner_module/goal_searcher.hpp"
+#include "autoware/behavior_path_goal_planner_module/util.hpp"
 
 #include <ament_index_cpp/get_package_share_directory.hpp>
 #include <autoware/behavior_path_goal_planner_module/manager.hpp>
@@ -23,6 +24,7 @@
 #include <autoware/behavior_path_planner_common/utils/path_safety_checker/safety_check.hpp>
 #include <autoware/behavior_path_planner_common/utils/path_utils.hpp>
 #include <autoware/route_handler/route_handler.hpp>
+#include <autoware/universe_utils/geometry/boost_geometry.hpp>
 #include <autoware_lanelet2_extension/io/autoware_osm_parser.hpp>
 #include <autoware_lanelet2_extension/projection/mgrs_projector.hpp>
 #include <autoware_lanelet2_extension/utility/message_conversion.hpp>
@@ -40,6 +42,7 @@
 #include <matplotlibcpp17/pyplot.h>
 
 #include <chrono>
+#include <cmath>
 #include <iostream>
 
 using namespace std::chrono_literals;  // NOLINT
@@ -54,16 +57,58 @@ using autoware::behavior_path_planner::utils::parking_departure::calcFeasibleDec
 using autoware_planning_msgs::msg::LaneletRoute;
 using tier4_planning_msgs::msg::PathWithLaneId;
 
+void plot_footprint(
+  matplotlibcpp17::axes::Axes & axes, const autoware::universe_utils::LinearRing2d & footprint,
+  const std::string & color = "blue")
+{
+  std::vector<double> xs, ys;
+  for (const auto & pt : footprint) {
+    xs.push_back(pt.x());
+    ys.push_back(pt.y());
+  }
+  xs.push_back(xs.front());
+  ys.push_back(ys.front());
+  axes.plot(Args(xs, ys), Kwargs("color"_a = color, "linestyle"_a = "dotted"));
+}
+
+void plot_goal_candidates(
+  matplotlibcpp17::axes::Axes & axes, const GoalCandidates & goals,
+  const autoware::universe_utils::LinearRing2d & local_footprint,
+  const std::string & color = "green")
+{
+  std::vector<double> xs, ys;
+  std::vector<double> yaw_cos, yaw_sin;
+  for (const auto & goal : goals) {
+    const auto goal_footprint =
+      transformVector(local_footprint, autoware::universe_utils::pose2transform(goal.goal_pose));
+    plot_footprint(axes, goal_footprint);
+    xs.push_back(goal.goal_pose.position.x);
+    ys.push_back(goal.goal_pose.position.y);
+    axes.text(Args(xs.back(), ys.back(), std::to_string(goal.id)));
+    const double yaw = autoware::universe_utils::getRPY(goal.goal_pose).z;
+    yaw_cos.push_back(std::cos(yaw));
+    yaw_sin.push_back(std::sin(yaw));
+  }
+  axes.scatter(Args(xs, ys), Kwargs("color"_a = color));
+  axes.quiver(
+    Args(xs, ys, yaw_cos, yaw_sin),
+    Kwargs("angles"_a = "xy", "scale_units"_a = "xy", "scale"_a = 1.0));
+}
+
 void plot_path_with_lane_id(
   matplotlibcpp17::axes::Axes & axes, const PathWithLaneId & path,
-  const std::string & color = "red")
+  const std::string & color = "red", const std::string & label = "")
 {
   std::vector<double> xs, ys;
   for (const auto & point : path.points) {
     xs.push_back(point.point.pose.position.x);
     ys.push_back(point.point.pose.position.y);
   }
-  axes.plot(Args(xs, ys), Kwargs("color"_a = color, "linewidth"_a = 1.0));
+  if (label == "") {
+    axes.plot(Args(xs, ys), Kwargs("color"_a = color, "linewidth"_a = 1.0));
+  } else {
+    axes.plot(Args(xs, ys), Kwargs("color"_a = color, "linewidth"_a = 1.0, "label"_a = label));
+  }
 }
 
 void plot_lanelet(
@@ -97,7 +142,7 @@ void plot_lanelet(
     Kwargs("color"_a = "black", "linewidth"_a = linewidth, "linestyle"_a = "dashed"));
 }
 
-std::shared_ptr<const PlannerData> instantiate_planner_data(
+std::shared_ptr<PlannerData> instantiate_planner_data(
   rclcpp::Node::SharedPtr node, const std::string & map_path, const LaneletRoute & route_msg)
 {
   lanelet::ErrorMessages errors{};
@@ -336,6 +381,51 @@ std::vector<PullOverPath> selectPullOverPaths(
   return selected;
 }
 
+std::optional<PathWithLaneId> calculate_centerline_path(
+  const geometry_msgs::msg::Pose & original_goal_pose,
+  const std::shared_ptr<PlannerData> planner_data, const GoalPlannerParameters & parameters)
+{
+  const auto refined_goal_opt =
+    autoware::behavior_path_planner::goal_planner_utils::calcRefinedGoal(
+      original_goal_pose, planner_data->route_handler, true,
+      planner_data->parameters.vehicle_length, planner_data->parameters.base_link2front,
+      planner_data->parameters.base_link2front, parameters);
+  if (!refined_goal_opt) {
+    return std::nullopt;
+  }
+  const auto & refined_goal = refined_goal_opt.value();
+
+  const auto & route_handler = planner_data->route_handler;
+  const double forward_length = parameters.forward_goal_search_length;
+  const double backward_length = parameters.backward_goal_search_length;
+  const bool use_bus_stop_area = parameters.bus_stop_area.use_bus_stop_area;
+  /*
+  const double margin_from_boundary = parameters.margin_from_boundary;
+  const double lateral_offset_interval = use_bus_stop_area
+                                           ? parameters.bus_stop_area.lateral_offset_interval
+                                           : parameters.lateral_offset_interval;
+  const double max_lateral_offset = parameters.max_lateral_offset;
+  const double ignore_distance_from_lane_start = parameters.ignore_distance_from_lane_start;
+  */
+
+  const auto pull_over_lanes =
+    autoware::behavior_path_planner::goal_planner_utils::getPullOverLanes(
+      *route_handler, true, parameters.backward_goal_search_length,
+      parameters.forward_goal_search_length);
+  const auto departure_check_lane =
+    autoware::behavior_path_planner::goal_planner_utils::createDepartureCheckLanelet(
+      pull_over_lanes, *route_handler, true);
+  const auto goal_arc_coords = lanelet::utils::getArcCoordinates(pull_over_lanes, refined_goal);
+  const double s_start = std::max(0.0, goal_arc_coords.length - backward_length);
+  const double s_end = goal_arc_coords.length + forward_length;
+  const double longitudinal_interval = use_bus_stop_area
+                                         ? parameters.bus_stop_area.goal_search_interval
+                                         : parameters.goal_search_interval;
+  auto center_line_path = autoware::behavior_path_planner::utils::resamplePathWithSpline(
+    route_handler->getCenterLinePath(pull_over_lanes, s_start, s_end), longitudinal_interval);
+  return center_line_path;
+}
+
 int main(int argc, char ** argv)
 {
   using autoware::behavior_path_planner::utils::getReferencePath;
@@ -455,8 +545,8 @@ int main(int argc, char ** argv)
   lane_departure_checker_params.footprint_extra_margin =
     goal_planner_parameter.lane_departure_check_expansion_margin;
   lane_departure_checker.setParam(lane_departure_checker_params);
-  autoware::behavior_path_planner::GoalSearcher goal_searcher(
-    goal_planner_parameter, vehicle_info.createFootprint());
+  const auto footprint = vehicle_info.createFootprint();
+  autoware::behavior_path_planner::GoalSearcher goal_searcher(goal_planner_parameter, footprint);
   const auto goal_candidates = goal_searcher.search(planner_data);
 
   pybind11::scoped_interpreter guard{};
@@ -473,7 +563,9 @@ int main(int argc, char ** argv)
     plot_lanelet(ax2, lanelet);
   }
 
-  plot_path_with_lane_id(ax1, reference_path.path, "green");
+  plot_goal_candidates(ax1, goal_candidates, footprint);
+
+  plot_path_with_lane_id(ax2, reference_path.path, "green", "reference_path");
 
   std::vector<PullOverPath> candidates;
   for (const auto & goal_candidate : goal_candidates) {
@@ -488,14 +580,18 @@ int main(int argc, char ** argv)
       plot_path_with_lane_id(ax1, full_path);
     }
   }
-  const auto filtered_paths = selectPullOverPaths(
+  [[maybe_unused]] const auto filtered_paths = selectPullOverPaths(
     candidates, goal_candidates, planner_data, goal_planner_parameter, reference_path);
-  for (const auto & filtered_path : filtered_paths) {
-    plot_path_with_lane_id(ax2, filtered_path.full_path(), "blue");
-  }
 
+  const auto centerline_path =
+    calculate_centerline_path(route_msg.goal_pose, planner_data, goal_planner_parameter);
+  if (centerline_path) {
+    plot_path_with_lane_id(ax2, centerline_path.value(), "red", "centerline_path");
+  }
   ax1.set_aspect(Args("equal"));
   ax2.set_aspect(Args("equal"));
+  ax1.legend();
+  ax2.legend();
   plt.show();
 
   rclcpp::shutdown();

planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/include/autoware/behavior_path_goal_planner_module/goal_planner_module.hpp

@@ -364,7 +364,6 @@ class GoalPlannerModule : public SceneModuleInterface
     const std::shared_ptr<OccupancyGridBasedCollisionDetector> occupancy_grid_map) const;
 
   // goal seach
-  Pose calcRefinedGoal(const Pose & goal_pose) const;
   GoalCandidates generateGoalCandidates() const;
 
   // stop or decelerate

planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/include/autoware/behavior_path_goal_planner_module/util.hpp

@@ -20,16 +20,17 @@
 
 #include <autoware/lane_departure_checker/lane_departure_checker.hpp>
 
-#include "visualization_msgs/msg/detail/marker_array__struct.hpp"
 #include <autoware_perception_msgs/msg/predicted_objects.hpp>
 #include <autoware_perception_msgs/msg/predicted_path.hpp>
 #include <geometry_msgs/msg/pose_stamped.hpp>
 #include <geometry_msgs/msg/twist_stamped.hpp>
 #include <tier4_planning_msgs/msg/path_with_lane_id.hpp>
+#include <visualization_msgs/msg/marker_array.hpp>
 
 #include <lanelet2_core/Forward.h>
 
 #include <map>
+#include <memory>
 #include <string>
 #include <vector>
 
@@ -175,6 +176,15 @@ lanelet::Points3d combineLanePoints(
 lanelet::Lanelet createDepartureCheckLanelet(
   const lanelet::ConstLanelets & pull_over_lanes, const route_handler::RouteHandler & route_handler,
   const bool left_side_parking);
+
+std::optional<Pose> calcRefinedGoal(
+  const Pose & goal_pose, const std::shared_ptr<RouteHandler> route_handler,
+  const bool left_side_parking, const double vehicle_width, const double base_link2front,
+  const double base_link2rear, const GoalPlannerParameters & parameters);
+
+std::optional<Pose> calcClosestPose(
+  const lanelet::ConstLineString3d line, const Point & query_point);
+
 }  // namespace autoware::behavior_path_planner::goal_planner_utils
 
 #endif  // AUTOWARE__BEHAVIOR_PATH_GOAL_PLANNER_MODULE__UTIL_HPP_

planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/src/goal_planner_module.cpp

@@ -585,8 +585,14 @@ void GoalPlannerModule::updateData()
 
   // update goal searcher and generate goal candidates
   if (thread_safe_data_.get_goal_candidates().empty()) {
-    goal_searcher_->setReferenceGoal(
-      calcRefinedGoal(planner_data_->route_handler->getOriginalGoalPose()));
+    const auto refined_goal = goal_planner_utils::calcRefinedGoal(
+      planner_data_->route_handler->getOriginalGoalPose(), planner_data_->route_handler,
+      left_side_parking_, planner_data_->parameters.vehicle_width,
+      planner_data_->parameters.base_link2front, planner_data_->parameters.base_link2rear,
+      *parameters_);
+    if (refined_goal) {
+      goal_searcher_->setReferenceGoal(refined_goal.value());
+    }
     thread_safe_data_.set_goal_candidates(generateGoalCandidates());
   }
 
@@ -757,61 +763,6 @@ double GoalPlannerModule::calcModuleRequestLength() const
   return std::max(minimum_request_length, parameters_->pull_over_minimum_request_length);
 }
 
-Pose GoalPlannerModule::calcRefinedGoal(const Pose & goal_pose) const
-{
-  const double vehicle_width = planner_data_->parameters.vehicle_width;
-  const double base_link2front = planner_data_->parameters.base_link2front;
-  const double base_link2rear = planner_data_->parameters.base_link2rear;
-
-  const lanelet::ConstLanelets pull_over_lanes = goal_planner_utils::getPullOverLanes(
-    *(planner_data_->route_handler), left_side_parking_, parameters_->backward_goal_search_length,
-    parameters_->forward_goal_search_length);
-
-  lanelet::Lanelet closest_pull_over_lanelet{};
-  lanelet::utils::query::getClosestLanelet(pull_over_lanes, goal_pose, &closest_pull_over_lanelet);
-
-  // calc closest center line pose
-  Pose center_pose{};
-  {
-    // find position
-    const auto lanelet_point = lanelet::utils::conversion::toLaneletPoint(goal_pose.position);
-    const auto segment = lanelet::utils::getClosestSegment(
-      lanelet::utils::to2D(lanelet_point), closest_pull_over_lanelet.centerline());
-    const auto p1 = segment.front().basicPoint();
-    const auto p2 = segment.back().basicPoint();
-    const auto direction_vector = (p2 - p1).normalized();
-    const auto p1_to_goal = lanelet_point.basicPoint() - p1;
-    const double s = direction_vector.dot(p1_to_goal);
-    const auto refined_point = p1 + direction_vector * s;
-
-    center_pose.position.x = refined_point.x();
-    center_pose.position.y = refined_point.y();
-    center_pose.position.z = refined_point.z();
-
-    // find orientation
-    const double yaw = std::atan2(direction_vector.y(), direction_vector.x());
-    tf2::Quaternion tf_quat;
-    tf_quat.setRPY(0, 0, yaw);
-    center_pose.orientation = tf2::toMsg(tf_quat);
-  }
-
-  const auto distance_from_bound = utils::getSignedDistanceFromBoundary(
-    pull_over_lanes, vehicle_width, base_link2front, base_link2rear, center_pose,
-    left_side_parking_);
-  if (!distance_from_bound) {
-    RCLCPP_ERROR(getLogger(), "fail to calculate refined goal");
-    return goal_pose;
-  }
-
-  const double sign = left_side_parking_ ? -1.0 : 1.0;
-  const double offset_from_center_line =
-    sign * (distance_from_bound.value() + parameters_->margin_from_boundary);
-
-  const auto refined_goal_pose = calcOffsetPose(center_pose, 0, -offset_from_center_line, 0);
-
-  return refined_goal_pose;
-}
-
 bool GoalPlannerModule::planFreespacePath(
   std::shared_ptr<const PlannerData> planner_data,
   const std::shared_ptr<GoalSearcherBase> goal_searcher,

planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/src/goal_searcher.cpp

@@ -20,8 +20,11 @@
 #include "autoware/universe_utils/geometry/boost_polygon_utils.hpp"
 #include "autoware_lanelet2_extension/regulatory_elements/no_parking_area.hpp"
 #include "autoware_lanelet2_extension/regulatory_elements/no_stopping_area.hpp"
+#include "autoware_lanelet2_extension/utility/query.hpp"
 #include "autoware_lanelet2_extension/utility/utilities.hpp"
 
+#include <autoware_vehicle_info_utils/vehicle_info.hpp>
+
 #include <boost/geometry/algorithms/union.hpp>
 
 #include <lanelet2_core/geometry/Polygon.h>
@@ -193,8 +196,27 @@ GoalCandidates GoalSearcher::search(const std::shared_ptr<const PlannerData> & p
         continue;
       }
 
+      // modify the goal_pose orientation so that vehicle footprint front heading is parallel to the
+      // lane boundary
+      const auto vehicle_front_midpoint =
+        (transformed_vehicle_footprint.at(vehicle_info_utils::VehicleInfo::FrontLeftIndex) +
+         transformed_vehicle_footprint.at(vehicle_info_utils::VehicleInfo::FrontRightIndex)) /
+        2.0;
+      lanelet::ConstLanelet vehicle_front_closest_lanelet;
+      lanelet::utils::query::getClosestLanelet(
+        pull_over_lanes, search_pose, &vehicle_front_closest_lanelet);
+      const auto vehicle_front_pose_for_bound_opt = goal_planner_utils::calcClosestPose(
+        left_side_parking_ ? vehicle_front_closest_lanelet.leftBound()
+                           : vehicle_front_closest_lanelet.rightBound(),
+        autoware::universe_utils::createPoint(
+          vehicle_front_midpoint.x(), vehicle_front_midpoint.y(), search_pose.position.z));
+      if (!vehicle_front_pose_for_bound_opt) {
+        continue;
+      }
+      const auto & vehicle_front_pose_for_bound = vehicle_front_pose_for_bound_opt.value();
       GoalCandidate goal_candidate{};
       goal_candidate.goal_pose = search_pose;
+      goal_candidate.goal_pose.orientation = vehicle_front_pose_for_bound.orientation;
       goal_candidate.lateral_offset = dy;
       goal_candidate.id = goal_id;
       goal_id++;