feat(behavior_path_planner): enable delay lane change when necessary (#…

…3991)

* feat(behavior_path_planner): enable delay lane change when necessary

Signed-off-by: yutaka <purewater0901@gmail.com>

* update

Signed-off-by: yutaka <purewater0901@gmail.com>

* update

Signed-off-by: yutaka <purewater0901@gmail.com>

* feat(behavior_path_planner): add a function to judge a parked vehicle

Signed-off-by: yutaka <purewater0901@gmail.com>

* update

Signed-off-by: yutaka <purewater0901@gmail.com>

* update

Signed-off-by: yutaka <purewater0901@gmail.com>

* update

Signed-off-by: yutaka <purewater0901@gmail.com>

* update

Signed-off-by: yutaka <purewater0901@gmail.com>

* update

Signed-off-by: yutaka <purewater0901@gmail.com>

* update

Signed-off-by: yutaka <purewater0901@gmail.com>

* update

Signed-off-by: yutaka <purewater0901@gmail.com>

* update

Signed-off-by: yutaka <purewater0901@gmail.com>

* Update planning/behavior_path_planner/src/utils/lane_change/utils.cpp

Co-authored-by: Fumiya Watanabe <rej55.g@gmail.com>

* update name

Signed-off-by: yutaka <purewater0901@gmail.com>

---------

Signed-off-by: yutaka <purewater0901@gmail.com>
Co-authored-by: Fumiya Watanabe <rej55.g@gmail.com>

planning/behavior_path_planner/config/lane_change/lane_change.param.yaml

@@ -15,6 +15,10 @@
       longitudinal_acceleration_sampling_num: 5
       lateral_acceleration_sampling_num: 3
 
+      # side walk parked vehicle
+      object_check_min_road_shoulder_width: 0.5  # [m]
+      object_shiftable_ratio_threshold: 0.6
+
       # turn signal
       min_length_for_turn_signal_activation: 10.0 # [m]
       length_ratio_for_turn_signal_deactivation: 0.8 # ratio (desired end position)

planning/behavior_path_planner/include/behavior_path_planner/utils/lane_change/lane_change_module_data.hpp

@@ -34,6 +34,10 @@ struct LaneChangeParameters
   int longitudinal_acc_sampling_num{10};
   int lateral_acc_sampling_num{10};
 
+  // parked vehicle
+  double object_check_min_road_shoulder_width{0.5};
+  double object_shiftable_ratio_threshold{0.6};
+
   // turn signal
   double min_length_for_turn_signal_activation{10.0};
   double length_ratio_for_turn_signal_deactivation{0.8};

planning/behavior_path_planner/include/behavior_path_planner/utils/lane_change/utils.hpp

@@ -174,5 +174,17 @@ PredictedPath convertToPredictedPath(
   const PathWithLaneId & path, const Twist & vehicle_twist, const Pose & pose,
   const size_t nearest_seg_idx, const double duration, const double resolution,
   const double prepare_time, const double prepare_acc, const double lane_changing_acc);
+
+bool passParkedObject(
+  const RouteHandler & route_handler, const LaneChangePath & lane_change_path,
+  const PathWithLaneId & current_lane_path, const PredictedObjects & objects,
+  const std::vector<size_t> & target_lane_obj_indices, const double minimum_lane_change_length,
+  const bool is_goal_in_route, const double object_check_min_road_shoulder_width,
+  const double object_shiftable_ratio_threshold);
+
+boost::optional<size_t> getLeadingStaticObjectIdx(
+  const RouteHandler & route_handler, const LaneChangePath & lane_change_path,
+  const PredictedObjects & objects, const std::vector<size_t> & obj_indices,
+  const double object_check_min_road_shoulder_width, const double object_shiftable_ratio_threshold);
 }  // namespace behavior_path_planner::utils::lane_change
 #endif  // BEHAVIOR_PATH_PLANNER__UTILS__LANE_CHANGE__UTILS_HPP_

planning/behavior_path_planner/src/behavior_path_planner_node.cpp

@@ -787,6 +787,12 @@ LaneChangeParameters BehaviorPathPlannerNode::getLaneChangeParam()
   p.lateral_acc_sampling_num =
     declare_parameter<int>(parameter("lateral_acceleration_sampling_num"));
 
+  // parked vehicle detection
+  p.object_check_min_road_shoulder_width =
+    declare_parameter<double>(parameter("object_check_min_road_shoulder_width"));
+  p.object_shiftable_ratio_threshold =
+    declare_parameter<double>(parameter("object_shiftable_ratio_threshold"));
+
   // turn signal
   p.min_length_for_turn_signal_activation =
     declare_parameter<double>(parameter("min_length_for_turn_signal_activation"));

planning/behavior_path_planner/src/scene_module/lane_change/normal.cpp

@@ -539,10 +539,10 @@ bool NormalLaneChange::getLaneChangePaths(
 
   const auto is_goal_in_route = route_handler.isInGoalRouteSection(target_lanelets.back());
 
-  const auto shift_intervals =
-    route_handler.getLateralIntervalsToPreferredLane(target_lanelets.back());
-  const double next_lane_change_buffer =
-    utils::calcMinimumLaneChangeLength(common_parameter, shift_intervals);
+  const double lane_change_buffer = utils::calcMinimumLaneChangeLength(
+    common_parameter, route_handler.getLateralIntervalsToPreferredLane(original_lanelets.back()));
+  const double next_lane_change_buffer = utils::calcMinimumLaneChangeLength(
+    common_parameter, route_handler.getLateralIntervalsToPreferredLane(target_lanelets.back()));
 
   const auto dist_to_end_of_current_lanes =
     utils::getDistanceToEndOfLane(getEgoPose(), original_lanelets);
@@ -725,6 +725,20 @@ bool NormalLaneChange::getLaneChangePaths(
           {*candidate_path}, *dynamic_objects, backward_target_lanes_for_object_filtering,
           getEgoPose(), common_parameter.forward_path_length, *lane_change_parameters_,
           lateral_buffer);
+
+        const double object_check_min_road_shoulder_width =
+          lane_change_parameters_->object_check_min_road_shoulder_width;
+        const double object_shiftable_ratio_threshold =
+          lane_change_parameters_->object_shiftable_ratio_threshold;
+        const auto current_lane_path = route_handler.getCenterLinePath(
+          original_lanelets, 0.0, std::numeric_limits<double>::max());
+        const bool pass_parked_object = utils::lane_change::passParkedObject(
+          route_handler, *candidate_path, current_lane_path, *dynamic_objects,
+          dynamic_object_indices.target_lane, lane_change_buffer, is_goal_in_route,
+          object_check_min_road_shoulder_width, object_shiftable_ratio_threshold);
+        if (pass_parked_object) {
+          return false;
+        }
       }
       candidate_paths->push_back(*candidate_path);
 

planning/behavior_path_planner/src/utils/lane_change/utils.cpp

@@ -1058,4 +1058,109 @@ PredictedPath convertToPredictedPath(
 
   return predicted_path;
 }
+
+bool passParkedObject(
+  const RouteHandler & route_handler, const LaneChangePath & lane_change_path,
+  const PathWithLaneId & current_lane_path, const PredictedObjects & objects,
+  const std::vector<size_t> & target_lane_obj_indices, const double minimum_lane_change_length,
+  const bool is_goal_in_route, const double object_check_min_road_shoulder_width,
+  const double object_shiftable_ratio_threshold)
+{
+  const auto & path = lane_change_path.path;
+
+  if (target_lane_obj_indices.empty() || path.points.empty() || current_lane_path.points.empty()) {
+    return false;
+  }
+
+  const auto leading_obj_idx = getLeadingStaticObjectIdx(
+    route_handler, lane_change_path, objects, target_lane_obj_indices,
+    object_check_min_road_shoulder_width, object_shiftable_ratio_threshold);
+  if (!leading_obj_idx) {
+    return false;
+  }
+
+  const auto & leading_obj = objects.objects.at(*leading_obj_idx);
+  const auto & leading_obj_poly = tier4_autoware_utils::toPolygon2d(leading_obj);
+  if (leading_obj_poly.outer().empty()) {
+    return false;
+  }
+
+  const auto & current_path_end = current_lane_path.points.back().point.pose.position;
+  double min_dist_to_end_of_current_lane = std::numeric_limits<double>::max();
+  for (const auto & point : leading_obj_poly.outer()) {
+    const auto obj_p = tier4_autoware_utils::createPoint(point.x(), point.y(), 0.0);
+    const double dist =
+      motion_utils::calcSignedArcLength(current_lane_path.points, obj_p, current_path_end);
+    min_dist_to_end_of_current_lane = std::min(dist, min_dist_to_end_of_current_lane);
+    if (is_goal_in_route) {
+      const auto goal_pose = route_handler.getGoalPose();
+      const double dist_to_goal =
+        motion_utils::calcSignedArcLength(current_lane_path.points, obj_p, goal_pose.position);
+      min_dist_to_end_of_current_lane = std::min(min_dist_to_end_of_current_lane, dist_to_goal);
+    }
+  }
+
+  // If there are still enough length after the target object, we delay the lane change
+  if (min_dist_to_end_of_current_lane > minimum_lane_change_length) {
+    return true;
+  }
+
+  return false;
+}
+
+boost::optional<size_t> getLeadingStaticObjectIdx(
+  const RouteHandler & route_handler, const LaneChangePath & lane_change_path,
+  const PredictedObjects & objects, const std::vector<size_t> & obj_indices,
+  const double object_check_min_road_shoulder_width, const double object_shiftable_ratio_threshold)
+{
+  const auto & path = lane_change_path.path;
+
+  if (path.points.empty() || obj_indices.empty()) {
+    return {};
+  }
+
+  const auto & lane_change_start = lane_change_path.lane_changing_start;
+  const auto & path_end = path.points.back();
+
+  double dist_lc_start_to_leading_obj = 0.0;
+  boost::optional<size_t> leading_obj_idx = boost::none;
+  for (const auto & obj_idx : obj_indices) {
+    const auto & obj = objects.objects.at(obj_idx);
+    const auto & obj_pose = obj.kinematics.initial_pose_with_covariance.pose;
+
+    // ignore non-static object
+    // TODO(shimizu): parametrize threshold
+    if (obj.kinematics.initial_twist_with_covariance.twist.linear.x > 1.0) {
+      continue;
+    }
+
+    // ignore non-parked object
+    if (!isParkedObject(
+          path, route_handler, obj, object_check_min_road_shoulder_width,
+          object_shiftable_ratio_threshold)) {
+      continue;
+    }
+
+    const double dist_back_to_obj = motion_utils::calcSignedArcLength(
+      path.points, path_end.point.pose.position, obj_pose.position);
+    if (dist_back_to_obj > 0.0) {
+      // object is not on the lane change path
+      continue;
+    }
+
+    const double dist_lc_start_to_obj =
+      motion_utils::calcSignedArcLength(path.points, lane_change_start.position, obj_pose.position);
+    if (dist_lc_start_to_obj < 0.0) {
+      // object is on the lane changing path or behind it. It will be detected in safety check
+      continue;
+    }
+
+    if (dist_lc_start_to_obj > dist_lc_start_to_leading_obj) {
+      dist_lc_start_to_leading_obj = dist_lc_start_to_obj;
+      leading_obj_idx = obj_idx;
+    }
+  }
+
+  return leading_obj_idx;
+}
 }  // namespace behavior_path_planner::utils::lane_change