fix(behavior_path_planner): lane change candidate resolution (autowar…

…efoundation#2426) * fix(behavior_path_planner): lane change candidate resolution Signed-off-by: Muhammad Zulfaqar Azmi <zulfaqar.azmi@tier4.jp> * rework sampling based on current speed Signed-off-by: Muhammad Zulfaqar Azmi <zulfaqar.azmi@tier4.jp> * refactor code Signed-off-by: Muhammad Zulfaqar Azmi <zulfaqar.azmi@tier4.jp> * use util's resampler Signed-off-by: Muhammad Zulfaqar Azmi <zulfaqar.azmi@tier4.jp> * consider min_resampling_points and resampling dt Signed-off-by: Muhammad Zulfaqar <zulfaqar.azmi@tier4.jp> * simplify code Signed-off-by: Muhammad Zulfaqar <zulfaqar.azmi@tier4.jp> Signed-off-by: Muhammad Zulfaqar Azmi <zulfaqar.azmi@tier4.jp> Signed-off-by: Muhammad Zulfaqar <zulfaqar.azmi@tier4.jp>
tier4 · Dec 20, 2022 · b9b8084 · b9b8084
1 parent 4f28314
commit b9b8084
Show file tree

Hide file tree

Showing 3 changed files with 12 additions and 10 deletions.
diff --git a/...ing/behavior_path_planner/include/behavior_path_planner/scene_module/lane_change/util.hpp b/...ing/behavior_path_planner/include/behavior_path_planner/scene_module/lane_change/util.hpp
@@ -110,7 +110,7 @@ PathWithLaneId getReferencePathFromTargetLane(
   const RouteHandler & route_handler, const lanelet::ConstLanelets & target_lanes,
   const Pose & lane_changing_start_pose, const double & prepare_distance,
   const double & lane_changing_distance, const double & forward_path_length,
-  const int & num_to_preferred_lane, const double & minimum_lane_change_length);
+  const int & num_to_preferred_lane, const double & minimum_lane_change_length, const double lane_changing_speed);
 
 PathWithLaneId getReferencePathFromTargetLane(
   const RouteHandler & route_handler, const lanelet::ConstLanelets & target_lanes,

diff --git a/planning/behavior_path_planner/include/behavior_path_planner/utilities.hpp b/planning/behavior_path_planner/include/behavior_path_planner/utilities.hpp
@@ -165,12 +165,6 @@ bool calcObjectPolygon(const PredictedObject & object, Polygon2d * object_polygo
 bool calcObjectPolygon(
   const Shape & object_shape, const Pose & object_pose, Polygon2d * object_polygon);
 
-<<<<<<< HEAD
-=======
-bool calcObjectPolygon(
-  const Shape & object_shape, const Pose & object_pose, Polygon2d * object_polygon);
-
->>>>>>> fd5ac602e (feat(behavior_path_planner): revise lane change module (#1139))
 PredictedPath resamplePredictedPath(
   const PredictedPath & input_path, const double resolution, const double duration);
 

diff --git a/planning/behavior_path_planner/src/scene_module/lane_change/util.cpp b/planning/behavior_path_planner/src/scene_module/lane_change/util.cpp
@@ -265,7 +265,7 @@ LaneChangePaths getLaneChangePaths(
     const PathWithLaneId target_lane_reference_path = getReferencePathFromTargetLane(
       route_handler, target_lanelets, lane_changing_start_pose, prepare_distance,
       lane_changing_distance, forward_path_length, num_to_preferred_lane,
-      minimum_lane_change_length + backward_length_buffer);
+      minimum_lane_change_length + backward_length_buffer, lane_changing_speed);
 
     const ShiftPoint shift_point = getLaneChangeShiftPoint(
       prepare_segment_reference, lane_changing_segment_reference, target_lanelets,
@@ -527,7 +527,7 @@ PathWithLaneId getReferencePathFromTargetLane(
   const RouteHandler & route_handler, const lanelet::ConstLanelets & target_lanes,
   const Pose & lane_changing_start_pose, const double & prepare_distance,
   const double & lane_changing_distance, const double & forward_path_length,
-  const int & num_to_preferred_lane, const double & minimum_lane_change_length)
+  const int & num_to_preferred_lane, const double & minimum_lane_change_length, const double lane_changing_speed)
 {
   const ArcCoordinates lane_change_start_arc_position =
     lanelet::utils::getArcCoordinates(target_lanes, lane_changing_start_pose);
@@ -549,7 +549,15 @@ PathWithLaneId getReferencePathFromTargetLane(
       lanelet::utils::getArcCoordinates(target_lanes, route_handler.getGoalPose());
     s_end = std::min(s_end, goal_arc_coordinates.length - total_minimum_lane_change_length);
   }
-  return route_handler.getCenterLinePath(target_lanes, s_start, s_end);
+
+  const auto lane_changing_reference_path =
+    route_handler.getCenterLinePath(target_lanes, s_start, s_end);
+
+  constexpr auto min_resampling_points{30.0};
+  constexpr auto resampling_dt{0.2};
+  const auto resample_interval =
+    std::max((s_start - s_end) / min_resampling_points, lane_changing_speed * resampling_dt);
+  return util::resamplePathWithSpline(lane_changing_reference_path, resample_interval);
 }
 
 PathWithLaneId getReferencePathFromTargetLane(