temp

Signed-off-by: beyza <[email protected]>
autowarefoundation · Sep 2, 2024 · d890466 · d890466
1 parent 980890f
commit d890466
Show file tree

Hide file tree

Showing 4 changed files with 23 additions and 0 deletions.
diff --git a/...avior_path_planner/autoware_behavior_path_planner/config/behavior_path_planner.param.yaml b/...avior_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/...havior_path_planner/include/autoware/behavior_path_planner/behavior_path_planner_node.hpp b/...havior_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/...g/behavior_path_planner/autoware_behavior_path_planner/src/behavior_path_planner_node.cpp b/...g/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/...behavior_path_planner_common/include/autoware/behavior_path_planner_common/parameters.hpp b/...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;