shift path according to traffic flow if lanes is bidirectional

#8672

planning/behavior_path_planner/autoware_behavior_path_planner/config/behavior_path_planner.param.yaml

@@ -18,6 +18,8 @@
     turn_signal_shift_length_threshold: 0.3
     turn_signal_remaining_shift_length_threshold: 0.1
     turn_signal_on_swerving: true
+    check_bidirectional_lane: true
+    traffic_flow: "left_side" # right or left
 
     enable_akima_spline_first: false
     enable_cog_on_centerline: false

planning/behavior_path_planner/autoware_behavior_path_planner/include/autoware/behavior_path_planner/behavior_path_planner_node.hpp

@@ -235,6 +235,8 @@ class BehaviorPathPlannerNode : public rclcpp::Node
     const std::shared_ptr<PathWithLaneId> & path_candidate_ptr, const bool is_ready,
     const std::shared_ptr<PlannerData> & planner_data);
 
+  PathWithLaneId shiftPath(const PathWithLaneId & ref_paths, const double shift_distance);
+  bool isLaneBidirectional(const lanelet::ConstLanelets & current_lanelets);
   std::unique_ptr<autoware::universe_utils::LoggerLevelConfigure> logger_configure_;
 
   std::unique_ptr<autoware::universe_utils::PublishedTimePublisher> published_time_publisher_;

planning/behavior_path_planner/autoware_behavior_path_planner/src/behavior_path_planner_node.cpp

@@ -382,14 +382,29 @@ void BehaviorPathPlannerNode::run()
   // NOTE: In order to keep backward_path_length at least, resampling interval is added to the
   // backward.
   const auto current_pose = planner_data_->self_odometry->pose.pose;
+
+  const auto current_lanelets = planner_data_->route_handler->getRoadLaneletsAtPose(current_pose);
+  const auto is_bidirectional = isLaneBidirectional(current_lanelets);
   if (!path->points.empty()) {
     const size_t current_seg_idx = planner_data_->findEgoSegmentIndex(path->points);
     path->points = autoware::motion_utils::cropPoints(
       path->points, current_pose.position, current_seg_idx,
       planner_data_->parameters.forward_path_length,
       planner_data_->parameters.backward_path_length +
         planner_data_->parameters.input_path_interval);
-
+    if (planner_data_->parameters.check_bidirectional_lane && is_bidirectional) {
+      double bound_to_centerline = 0.0;
+      for (const auto & current_lanelet : current_lanelets) {
+        bound_to_centerline = lanelet::geometry::distance2d(
+          lanelet::utils::to2D(current_lanelet.leftBound().basicLineString()),
+          lanelet::utils::to2D(current_lanelet.centerline().basicLineString()));
+      }
+      if (planner_data_->parameters.traffic_flow == "right_side") {
+        path->points = shiftPath(*path, bound_to_centerline * 0.5).points;
+      } else if (planner_data_->parameters.traffic_flow == "left_side") {
+        path->points = shiftPath(*path, -bound_to_centerline * 0.5).points;
+      }
+    }
     if (!path->points.empty()) {
       path_publisher_->publish(*path);
       published_time_publisher_->publish_if_subscribed(path_publisher_, path->header.stamp);
@@ -433,6 +448,55 @@ void BehaviorPathPlannerNode::run()
   RCLCPP_DEBUG(get_logger(), "----- behavior path planner end -----\n\n");
 }
 
+PathWithLaneId BehaviorPathPlannerNode::shiftPath(
+  const PathWithLaneId & path, const double shift_distance)
+{
+  PathWithLaneId shifted_path;
+  shifted_path.header.stamp = path.header.stamp;
+  shifted_path.points.reserve(path.points.size());
+  for (const auto & pose : path.points) {
+    tier4_planning_msgs::msg::PathPointWithLaneId shifted_pose{};
+    shifted_pose = pose;
+    // Get yaw from quaternion
+    tf2::Quaternion quat(
+      pose.point.pose.orientation.x, pose.point.pose.orientation.y, pose.point.pose.orientation.z,
+      pose.point.pose.orientation.w);
+    tf2::Matrix3x3 mat(quat);
+    double roll, pitch, yaw;
+    mat.getRPY(roll, pitch, yaw);
+
+    // Calculate the lateral shift
+    double delta_x = shift_distance * cos(yaw + M_PI_2);
+    double delta_y = shift_distance * sin(yaw + M_PI_2);
+
+    // Apply the shift
+    shifted_pose.point.pose.position.x += delta_x;
+    shifted_pose.point.pose.position.y += delta_y;
+
+    shifted_path.points.push_back(shifted_pose);
+  }
+
+  return shifted_path;
+}
+
+bool BehaviorPathPlannerNode::isLaneBidirectional(const lanelet::ConstLanelets & current_lanelets)
+{
+  for (const auto & current_lanelet : current_lanelets) {
+    const auto left_bound_id = current_lanelet.leftBound().id();
+    const auto right_bound_id = current_lanelet.rightBound().id();
+    for (const auto & other_lanelet : current_lanelets) {
+      if (current_lanelet != other_lanelet) {
+        if (
+          left_bound_id == other_lanelet.rightBound().id() ||
+          right_bound_id == other_lanelet.leftBound().id()) {
+          return true;
+        }
+      }
+    }
+  }
+  return false;
+}
+
 void BehaviorPathPlannerNode::computeTurnSignal(
   const std::shared_ptr<PlannerData> planner_data, const PathWithLaneId & path,
   const BehaviorModuleOutput & output)

planning/behavior_path_planner/autoware_behavior_path_planner_common/include/autoware/behavior_path_planner_common/data_manager.hpp

@@ -88,6 +88,12 @@ struct ModuleNameStamped
   rclcpp::Time stamp{0, 0, RCL_ROS_TIME};
 };
 
+struct LaneletData
+{
+  lanelet::Lanelet lanelet;
+  bool is_bidirectional{false};
+};
+
 struct DrivableLanes
 {
   lanelet::ConstLanelet right_lane;
@@ -240,7 +246,9 @@ struct PlannerData
       node.declare_parameter<double>("ego_nearest_dist_threshold");
     parameters.ego_nearest_yaw_threshold =
       node.declare_parameter<double>("ego_nearest_yaw_threshold");
+    parameters.check_bidirectional_lane = node.declare_parameter<bool>("check_bidirectional_lane");
 
+    parameters.traffic_flow = node.declare_parameter<std::string>("traffic_flow");
     drivable_area_expansion_parameters.init(node);
   }
 

planning/behavior_path_planner/autoware_behavior_path_planner_common/include/autoware/behavior_path_planner_common/parameters.hpp

@@ -17,6 +17,8 @@
 
 #include <autoware_vehicle_info_utils/vehicle_info_utils.hpp>
 
+#include <string>
+
 struct ModuleConfigParameters
 {
   bool enable_module{false};
@@ -52,6 +54,8 @@ struct BehaviorPathPlannerParameters
   double turn_signal_shift_length_threshold;
   double turn_signal_remaining_shift_length_threshold;
   bool turn_signal_on_swerving;
+  bool check_bidirectional_lane;
+  std::string traffic_flow{"left_side"};
 
   bool enable_akima_spline_first;
   bool enable_cog_on_centerline;