feat(behavior_path_planner): add option for combining arc pull out pa…

…ths (autowarefoundation#2669) * feat(behavior_path_planner): add option for combining arc pull out paths Signed-off-by: kosuke55 <kosuke.tnp@gmail.com> * divide_pull_out_path Signed-off-by: kosuke55 <kosuke.tnp@gmail.com> Signed-off-by: kosuke55 <kosuke.tnp@gmail.com>
tier4 · Jan 17, 2023 · 59adfaf · 59adfaf
1 parent 9f5ac01
commit 59adfaf
Show file tree

Hide file tree

Showing 4 changed files with 14 additions and 1 deletion.
diff --git a/planning/behavior_path_planner/config/pull_out/pull_out.param.yaml b/planning/behavior_path_planner/config/pull_out/pull_out.param.yaml
@@ -16,6 +16,7 @@
       deceleration_interval: 15.0
       # geometric pull out
       enable_geometric_pull_out: true
+      divide_pull_out_path: false
       geometric_pull_out_velocity: 1.0
       arc_path_interval: 1.0
       lane_departure_margin: 0.2

diff --git a/..._path_planner/include/behavior_path_planner/scene_module/pull_out/pull_out_parameters.hpp b/..._path_planner/include/behavior_path_planner/scene_module/pull_out/pull_out_parameters.hpp
@@ -37,6 +37,7 @@ struct PullOutParameters
   double deceleration_interval;
   // geometric pull out
   bool enable_geometric_pull_out;
+  bool divide_pull_out_path;
   double geometric_pull_out_velocity;
   double arc_path_interval;
   double lane_departure_margin;

diff --git a/planning/behavior_path_planner/src/behavior_path_planner_node.cpp b/planning/behavior_path_planner/src/behavior_path_planner_node.cpp
@@ -524,6 +524,7 @@ PullOutParameters BehaviorPathPlannerNode::getPullOutParam()
   p.deceleration_interval = dp("deceleration_interval", 10.0);
   // geometric pull out
   p.enable_geometric_pull_out = dp("enable_geometric_pull_out", true);
+  p.divide_pull_out_path = dp("divide_pull_out_path", false);
   p.geometric_pull_out_velocity = dp("geometric_pull_out_velocity", 1.0);
   p.arc_path_interval = dp("arc_path_interval", 1.0);
   p.lane_departure_margin = dp("lane_departure_margin", 0.2);

diff --git a/planning/behavior_path_planner/src/scene_module/pull_out/geometric_pull_out.cpp b/planning/behavior_path_planner/src/scene_module/pull_out/geometric_pull_out.cpp
@@ -59,7 +59,17 @@ boost::optional<PullOutPath> GeometricPullOut::plan(Pose start_pose, Pose goal_p
     return {};
   }
 
-  output.partial_paths = planner_.getPaths();
+  if (parameters_.divide_pull_out_path) {
+    output.partial_paths = planner_.getPaths();
+  } else {
+    const auto partial_paths = planner_.getPaths();
+    auto combined_path = combineReferencePath(partial_paths.at(0), partial_paths.at(1));
+    // set same velocity to all points not to stop
+    for (auto & point : combined_path.points) {
+      point.point.longitudinal_velocity_mps = parameters_.geometric_pull_out_velocity;
+    }
+    output.partial_paths.push_back(combined_path);
+  }
   output.start_pose = planner_.getArcPaths().at(0).points.back().point.pose;
   output.end_pose = planner_.getArcPaths().at(1).points.back().point.pose;