From d89046625db559886fac0a5a3ea28a4afaff2736 Mon Sep 17 00:00:00 2001
From: beyza <bnk@leodrive.ai>
Date: Thu, 29 Aug 2024 01:39:26 +0300
Subject: [PATCH] temp

Signed-off-by: beyza <bnk@leodrive.ai>
---
 .../config/behavior_path_planner.param.yaml     |  1 +
 .../behavior_path_planner_node.hpp              |  1 +
 .../src/behavior_path_planner_node.cpp          | 17 +++++++++++++++++
 .../behavior_path_planner_common/parameters.hpp |  4 ++++
 4 files changed, 23 insertions(+)

diff --git a/planning/behavior_path_planner/autoware_behavior_path_planner/config/behavior_path_planner.param.yaml b/planning/behavior_path_planner/autoware_behavior_path_planner/config/behavior_path_planner.param.yaml
index 7d5249dea689f..ea0efd04225d1 100644
--- a/planning/behavior_path_planner/autoware_behavior_path_planner/config/behavior_path_planner.param.yaml
+++ b/planning/behavior_path_planner/autoware_behavior_path_planner/config/behavior_path_planner.param.yaml
@@ -18,6 +18,7 @@
     turn_signal_shift_length_threshold: 0.3
     turn_signal_remaining_shift_length_threshold: 0.1
     turn_signal_on_swerving: true
+    traffic_flow: "left_side" # right or left
 
     enable_akima_spline_first: false
     enable_cog_on_centerline: false
diff --git a/planning/behavior_path_planner/autoware_behavior_path_planner/include/autoware/behavior_path_planner/behavior_path_planner_node.hpp b/planning/behavior_path_planner/autoware_behavior_path_planner/include/autoware/behavior_path_planner/behavior_path_planner_node.hpp
index 11f9d9f140214..05b8f99246b03 100644
--- a/planning/behavior_path_planner/autoware_behavior_path_planner/include/autoware/behavior_path_planner/behavior_path_planner_node.hpp
+++ b/planning/behavior_path_planner/autoware_behavior_path_planner/include/autoware/behavior_path_planner/behavior_path_planner_node.hpp
@@ -238,6 +238,7 @@ 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);
   std::unique_ptr<autoware::universe_utils::LoggerLevelConfigure> logger_configure_;
 
   std::unique_ptr<autoware::universe_utils::PublishedTimePublisher> published_time_publisher_;
diff --git a/planning/behavior_path_planner/autoware_behavior_path_planner/src/behavior_path_planner_node.cpp b/planning/behavior_path_planner/autoware_behavior_path_planner/src/behavior_path_planner_node.cpp
index 135e420d113f3..e5ae134aa5a01 100644
--- a/planning/behavior_path_planner/autoware_behavior_path_planner/src/behavior_path_planner_node.cpp
+++ b/planning/behavior_path_planner/autoware_behavior_path_planner/src/behavior_path_planner_node.cpp
@@ -212,6 +212,7 @@ BehaviorPathPlannerParameters BehaviorPathPlannerNode::getCommonParam()
     declare_parameter<double>("turn_signal_remaining_shift_length_threshold");
   p.turn_signal_on_swerving = declare_parameter<bool>("turn_signal_on_swerving");
 
+  p.traffic_flow = declare_parameter<std::string>("traffic_flow");
   p.enable_akima_spline_first = declare_parameter<bool>("enable_akima_spline_first");
   p.enable_cog_on_centerline = declare_parameter<bool>("enable_cog_on_centerline");
   p.input_path_interval = declare_parameter<double>("input_path_interval");
@@ -503,6 +504,22 @@ void BehaviorPathPlannerNode::run()
   RCLCPP_DEBUG(get_logger(), "----- behavior path planner end -----\n\n");
 }
 
+[[maybe_unused]] 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;
+    shifted_pose.point.pose.position.x += shift_distance;
+    shifted_path.points.push_back(shifted_pose);
+  }
+
+  return shifted_path;
+}
+
 void BehaviorPathPlannerNode::computeTurnSignal(
   const std::shared_ptr<PlannerData> planner_data, const PathWithLaneId & path,
   const BehaviorModuleOutput & output)
diff --git a/planning/behavior_path_planner/autoware_behavior_path_planner_common/include/autoware/behavior_path_planner_common/parameters.hpp b/planning/behavior_path_planner/autoware_behavior_path_planner_common/include/autoware/behavior_path_planner_common/parameters.hpp
index ecbe9b5208347..ebd2f8b157783 100644
--- a/planning/behavior_path_planner/autoware_behavior_path_planner_common/include/autoware/behavior_path_planner_common/parameters.hpp
+++ b/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};
@@ -53,6 +55,8 @@ struct BehaviorPathPlannerParameters
   double turn_signal_remaining_shift_length_threshold;
   bool turn_signal_on_swerving;
 
+  std::string traffic_flow{"left_side"};
+
   bool enable_akima_spline_first;
   bool enable_cog_on_centerline;
   double input_path_interval;