fix(lane_change): change stopping logic (RT0-33761) (#1581)

* RT0-33761 fix lane change stopping logic

Signed-off-by: Zulfaqar Azmi <zulfaqar.azmi@tier4.jp>

* copied from awf main tested implementation

Signed-off-by: Zulfaqar Azmi <zulfaqar.azmi@tier4.jp>

* doxygen comment

Signed-off-by: Zulfaqar Azmi <zulfaqar.azmi@tier4.jp>

* Update planning/behavior_path_lane_change_module/src/utils/utils.cpp

Co-authored-by: mkquda <168697710+mkquda@users.noreply.github.com>

---------

Signed-off-by: Zulfaqar Azmi <zulfaqar.azmi@tier4.jp>
Co-authored-by: mkquda <168697710+mkquda@users.noreply.github.com>

Author: zulfaqar-azmi-t4

planning/behavior_path_lane_change_module/include/behavior_path_lane_change_module/scene.hpp

@@ -60,6 +60,8 @@ class NormalLaneChange : public LaneChangeBase
 
   void insertStopPoint(const lanelet::ConstLanelets & lanelets, PathWithLaneId & path) override;
 
+  void insert_stop_point_on_current_lanes(PathWithLaneId & path);
+
   PathWithLaneId getReferencePath() const override;
 
   std::optional<PathWithLaneId> extendPath() override;
@@ -178,7 +180,7 @@ class NormalLaneChange : public LaneChangeBase
 
   std::pair<double, double> calcCurrentMinMaxAcceleration() const;
 
-  void setStopPose(const Pose & stop_pose);
+  void set_stop_pose(const double arc_length_to_stop_pose, PathWithLaneId & path);
 
   void updateStopTime();
 

planning/behavior_path_lane_change_module/include/behavior_path_lane_change_module/utils/utils.hpp

@@ -289,5 +289,54 @@ bool isWithinTurnDirectionLanes(const lanelet::ConstLanelet & lanelet, const Pol
 double calcPhaseLength(
   const double velocity, const double maximum_velocity, const double acceleration,
   const double time);
+
+/**
+ * @brief Calculates the minimum distance to a stationary object in the current lanes.
+ *
+ * This function determines the closest distance from the ego vehicle to a stationary object
+ * in the current lanes. It checks if the object is within the stopping criteria based on its
+ * velocity and calculates the distance while accounting for the object's size. Only objects
+ * positioned after the specified distance to the target lane's start are considered.
+ *
+ * @param filtered_objects A collection of lane change target objects, including those in the
+ * current lane.
+ * @param bpp_param Parameters for the behavior path planner, such as vehicle dimensions.
+ * @param lc_param Parameters for the lane change process, including the velocity threshold for
+ * stopping.
+ * @param dist_to_target_lane_start The distance from the ego vehicle to the start of the target
+ * lane.
+ * @param ego_pose The current pose of the ego vehicle.
+ * @param path The current path of the ego vehicle, containing path points and lane information.
+ * @return The minimum distance to a stationary object in the current lanes. If no valid object is
+ * found, returns the maximum possible double value.
+ */
+double get_min_dist_to_current_lanes_obj(
+  const LaneChangeTargetObjects & filtered_objects, const BehaviorPathPlannerParameters & bpp_param,
+  const LaneChangeParameters & lc_param, const double dist_to_target_lane_start, const Pose & pose,
+  const PathWithLaneId & path);
+
+/**
+ * @brief Checks if there is any stationary object in the target lanes that would affect the lane
+ * change stop decision.
+ *
+ * This function determines whether there are any stationary objects in the target lanes that could
+ * impact the decision to insert a stop point for the ego vehicle. It checks each object's velocity,
+ * position relative to the ego vehicle, and overlap with the target lanes to identify if any object
+ * meets the criteria for being a blocking obstacle.
+ *
+ * @param target_lanes A collection of lanelets representing the target lanes for the lane change.
+ * @param filtered_objects A collection of lane change target objects, including those in the target
+ * lanes.
+ * @param lc_param Parameters for the lane change process, such as the stop velocity threshold.
+ * @param stop_arc_length The arc length at which the ego vehicle is expected to stop.
+ * @param ego_pose The current pose of the ego vehicle.
+ * @param path The current path of the ego vehicle, containing path points and lane information.
+ * @return true if there is a stationary object in the target lanes that meets the criteria for
+ * being a blocking object; otherwise, false.
+ */
+bool has_blocking_target_object(
+  const lanelet::ConstLanelets & target_lanes, const LaneChangeTargetObjects & filtered_objects,
+  const LaneChangeParameters & lc_param, const double stop_arc_length, const Pose & ego_pose,
+  const PathWithLaneId & path);
 }  // namespace behavior_path_planner::utils::lane_change
 #endif  // BEHAVIOR_PATH_LANE_CHANGE_MODULE__UTILS__UTILS_HPP_

planning/behavior_path_lane_change_module/src/scene.cpp

@@ -165,8 +165,7 @@ BehaviorModuleOutput NormalLaneChange::generateOutput()
         output.path.points, output.path.points.front().point.pose.position, getEgoPosition());
       const auto stop_dist =
         -(current_velocity * current_velocity / (2.0 * planner_data_->parameters.min_acc));
-      const auto stop_point = utils::insertStopPoint(stop_dist + current_dist, output.path);
-      setStopPose(stop_point.point.pose);
+      set_stop_pose(stop_dist + current_dist, output.path);
     } else {
       insertStopPoint(status_.target_lanes, output.path);
     }
@@ -204,7 +203,7 @@ void NormalLaneChange::extendOutputDrivableArea(BehaviorModuleOutput & output)
 void NormalLaneChange::insertStopPoint(
   const lanelet::ConstLanelets & lanelets, PathWithLaneId & path)
 {
-  if (lanelets.empty()) {
+  if (lanelets.empty() || status_.current_lanes.empty()) {
     return;
   }
 
@@ -214,133 +213,119 @@ void NormalLaneChange::insertStopPoint(
     return;
   }
 
-  const auto shift_intervals = route_handler->getLateralIntervalsToPreferredLane(lanelets.back());
-  const double lane_change_buffer =
-    utils::lane_change::calcMinimumLaneChangeLength(*lane_change_parameters_, shift_intervals, 0.0);
+  const auto & current_lanes = status_.current_lanes;
+  const auto is_current_lane = lanelets.front().id() == current_lanes.front().id() &&
+                               lanelets.back().id() == current_lanes.back().id();
+
+  // if input is not current lane, we can just insert the points at terminal.
+  if (!is_current_lane) {
+    const auto shift_intervals = route_handler->getLateralIntervalsToPreferredLane(lanelets.back());
+    const auto min_dist_buffer = utils::lane_change::calcMinimumLaneChangeLength(
+      *lane_change_parameters_, shift_intervals, 0.0);
+    const auto backward_length_buffer =
+      lane_change_parameters_->backward_length_buffer_for_end_of_lane;
+    const auto arc_length_to_stop_pose =
+      motion_utils::calcArcLength(path.points) - backward_length_buffer + min_dist_buffer;
+    set_stop_pose(arc_length_to_stop_pose, path);
+    return;
+  }
 
-  const auto getDistanceAlongLanelet = [&](const geometry_msgs::msg::Pose & target) {
-    return utils::getSignedDistance(path.points.front().point.pose, target, lanelets);
-  };
+  insert_stop_point_on_current_lanes(path);
+}
 
-  // If lanelets.back() is in goal route section, get distance to goal.
-  // Otherwise, get distance to end of lane.
-  double distance_to_terminal = 0.0;
-  if (route_handler->isInGoalRouteSection(lanelets.back())) {
-    const auto goal = route_handler->getGoalPose();
-    distance_to_terminal = getDistanceAlongLanelet(goal);
-  } else {
-    distance_to_terminal = utils::getDistanceToEndOfLane(path.points.front().point.pose, lanelets);
-  }
+void NormalLaneChange::insert_stop_point_on_current_lanes(PathWithLaneId & path)
+{
+  const auto & path_front_pose = path.points.front().point.pose;
+  const auto & route_handler_ptr = getRouteHandler();
+  const auto & current_lanes = status_.current_lanes;
+  const auto current_path =
+    route_handler_ptr->getCenterLinePath(current_lanes, 0.0, std::numeric_limits<double>::max());
+  const auto & center_line = current_path.points;
+  const auto get_arc_length_along_lanelet = [&](const geometry_msgs::msg::Pose & target) {
+    return motion_utils::calcSignedArcLength(
+      center_line, path_front_pose.position, target.position);
+  };
 
-  const double stop_point_buffer = lane_change_parameters_->backward_length_buffer_for_end_of_lane;
-  const auto target_objects = getTargetObjects(status_.current_lanes, status_.target_lanes);
-  double stopping_distance = distance_to_terminal - lane_change_buffer - stop_point_buffer;
-
-  const auto is_valid_start_point = std::invoke([&]() -> bool {
-    auto lc_start_point = lanelet::utils::conversion::toLaneletPoint(
-      status_.lane_change_path.info.lane_changing_start.position);
-    const auto target_neighbor_preferred_lane_poly_2d =
-      utils::lane_change::getTargetNeighborLanesPolygon(
-        *route_handler, status_.current_lanes, type_);
-    return boost::geometry::covered_by(
-      lanelet::traits::to2D(lc_start_point), target_neighbor_preferred_lane_poly_2d);
+  const auto dist_to_terminal = std::invoke([&]() -> double {
+    const auto target_pose = route_handler_ptr->isInGoalRouteSection(current_lanes.back())
+                               ? route_handler_ptr->getGoalPose()
+                               : center_line.back().point.pose;
+    return get_arc_length_along_lanelet(target_pose);
   });
 
-  if (!is_valid_start_point) {
-    const auto stop_point = utils::insertStopPoint(stopping_distance, path);
-    setStopPose(stop_point.point.pose);
+  const auto shift_intervals =
+    route_handler_ptr->getLateralIntervalsToPreferredLane(current_lanes.back());
+  const auto min_dist_buffer =
+    utils::lane_change::calcMinimumLaneChangeLength(*lane_change_parameters_, shift_intervals, 0.0);
+  const auto backward_length_buffer =
+    lane_change_parameters_->backward_length_buffer_for_end_of_lane;
+  const auto dist_to_terminal_start = dist_to_terminal - min_dist_buffer - backward_length_buffer;
 
+  const auto filtered_objects = getTargetObjects(status_.current_lanes, status_.target_lanes);
+  if (filtered_objects.current_lane.empty()) {
+    set_stop_pose(dist_to_terminal_start, path);
     return;
   }
 
-  // calculate minimum distance from path front to the stationary object on the ego lane.
-  const auto distance_to_ego_lane_obj = [&]() -> double {
-    double distance_to_obj = distance_to_terminal;
-    const double distance_to_ego = getDistanceAlongLanelet(getEgoPose());
-
-    for (const auto & object : target_objects.current_lane) {
-      // check if stationary
-      const auto obj_v = std::abs(object.initial_twist.twist.linear.x);
-      if (obj_v > lane_change_parameters_->stop_velocity_threshold) {
-        continue;
-      }
-
-      // calculate distance from path front to the stationary object polygon on the ego lane.
-      const auto polygon =
-        tier4_autoware_utils::toPolygon2d(object.initial_pose.pose, object.shape).outer();
-      for (const auto & polygon_p : polygon) {
-        const auto p_fp = tier4_autoware_utils::toMsg(polygon_p.to_3d());
-        const auto lateral_fp = motion_utils::calcLateralOffset(path.points, p_fp);
+  const auto dist_to_target_lane_start = std::invoke([&]() -> double {
+    const auto & front_lane = status_.target_lanes.front();
+    const auto & ll_front_pt = front_lane.centerline2d().front();
+    const auto front_pt = lanelet::utils::conversion::toGeomMsgPt(ll_front_pt);
+    const auto yaw = lanelet::utils::getLaneletAngle(front_lane, front_pt);
+    Pose target_front;
+    target_front.position = front_pt;
+    tf2::Quaternion tf_quat;
+    tf_quat.setRPY(0, 0, yaw);
+    target_front.orientation = tf2::toMsg(tf_quat);
 
-        // ignore if the point is around the ego path
-        if (std::abs(lateral_fp) > planner_data_->parameters.vehicle_width) {
-          continue;
-        }
+    return get_arc_length_along_lanelet(target_front);
+  });
 
-        const double current_distance_to_obj = calcSignedArcLength(path.points, 0, p_fp);
+  const auto arc_length_to_current_obj = utils::lane_change::get_min_dist_to_current_lanes_obj(
+    filtered_objects, getCommonParam(), *lane_change_parameters_, dist_to_target_lane_start,
+    getEgoPose(), path);
 
-        // ignore backward object
-        if (current_distance_to_obj < distance_to_ego) {
-          continue;
-        }
-        distance_to_obj = std::min(distance_to_obj, current_distance_to_obj);
-      }
-    }
-    return distance_to_obj;
-  }();
-
-  // Need to stop before blocking obstacle
-  if (distance_to_ego_lane_obj < distance_to_terminal) {
-    // consider rss distance when the LC need to avoid obstacles
-    const auto rss_dist = calcRssDistance(
-      0.0, lane_change_parameters_->minimum_lane_changing_velocity,
-      lane_change_parameters_->rss_params);
-    const double lane_change_buffer_for_blocking_object =
-      utils::lane_change::calcMinimumLaneChangeLength(*lane_change_parameters_, shift_intervals);
-
-    const auto stopping_distance_for_obj =
-      distance_to_ego_lane_obj - lane_change_buffer_for_blocking_object -
-      lane_change_parameters_->backward_length_buffer_for_blocking_object - rss_dist -
-      getCommonParam().base_link2front;
-
-    //  If the target lane in the lane change section is blocked by a stationary obstacle, there
-    //  is no reason for stopping with a lane change margin. Instead, stop right behind the
-    //  obstacle.
-    //  ----------------------------------------------------------
-    //                            [obj]>
-    //  ----------------------------------------------------------
-    //    [ego]>          | <--- lane change margin --->  [obj]>
-    //  ----------------------------------------------------------
-    const bool has_blocking_target_lane_obj = std::any_of(
-      target_objects.target_lane.begin(), target_objects.target_lane.end(), [&](const auto & o) {
-        const auto v = std::abs(o.initial_twist.twist.linear.x);
-        if (v > lane_change_parameters_->stop_velocity_threshold) {
-          return false;
-        }
+  // margin with leading vehicle
+  // consider rss distance when the LC need to avoid obstacles
+  const auto rss_dist = calcRssDistance(
+    0.0, lane_change_parameters_->minimum_lane_changing_velocity,
+    lane_change_parameters_->rss_params);
 
-        // target_objects includes objects out of target lanes, so filter them out
-        if (!boost::geometry::intersects(
-              tier4_autoware_utils::toPolygon2d(o.initial_pose.pose, o.shape).outer(),
-              lanelet::utils::combineLaneletsShape(status_.target_lanes)
-                .polygon2d()
-                .basicPolygon())) {
-          return false;
-        }
+  const auto min_single_lc_length = utils::lane_change::calcMinimumLaneChangeLength(
+    *lane_change_parameters_, {shift_intervals.front()}, 0.0);
+  const auto stop_margin = min_single_lc_length +
+                           lane_change_parameters_->backward_length_buffer_for_blocking_object +
+                           rss_dist + getCommonParam().base_link2front;
+  const auto stop_arc_length_behind_obj = arc_length_to_current_obj - stop_margin;
 
-        const double distance_to_target_lane_obj = getDistanceAlongLanelet(o.initial_pose.pose);
-        return stopping_distance_for_obj < distance_to_target_lane_obj &&
-               distance_to_target_lane_obj < distance_to_ego_lane_obj;
-      });
+  if (stop_arc_length_behind_obj < dist_to_target_lane_start) {
+    set_stop_pose(dist_to_target_lane_start, path);
+    return;
+  }
 
-    if (!has_blocking_target_lane_obj) {
-      stopping_distance = stopping_distance_for_obj;
-    }
+  if (stop_arc_length_behind_obj > dist_to_terminal_start) {
+    set_stop_pose(dist_to_terminal_start, path);
+    return;
   }
 
-  if (stopping_distance > 0.0) {
-    const auto stop_point = utils::insertStopPoint(stopping_distance, path);
-    setStopPose(stop_point.point.pose);
+  //  If the target lane in the lane change section is blocked by a stationary obstacle, there
+  //  is no reason for stopping with a lane change margin. Instead, stop right behind the
+  //  obstacle.
+  //  ----------------------------------------------------------
+  //                            [obj]>
+  //  ----------------------------------------------------------
+  //    [ego]>          | <--- stop margin --->  [obj]>
+  //  ----------------------------------------------------------
+  const auto has_blocking_target_lane_obj = utils::lane_change::has_blocking_target_object(
+    status_.target_lanes, filtered_objects, *lane_change_parameters_, stop_arc_length_behind_obj,
+    getEgoPose(), path);
+
+  if (has_blocking_target_lane_obj || stop_arc_length_behind_obj <= 0.0) {
+    set_stop_pose(dist_to_terminal_start, path);
+    return;
   }
+
+  set_stop_pose(stop_arc_length_behind_obj, path);
 }
 
 PathWithLaneId NormalLaneChange::getReferencePath() const
@@ -1842,9 +1827,10 @@ bool NormalLaneChange::isVehicleStuck(const lanelet::ConstLanelets & current_lan
   return isVehicleStuck(current_lanes, detection_distance);
 }
 
-void NormalLaneChange::setStopPose(const Pose & stop_pose)
+void NormalLaneChange::set_stop_pose(const double arc_length_to_stop_pose, PathWithLaneId & path)
 {
-  lane_change_stop_pose_ = stop_pose;
+  const auto stop_point = utils::insertStopPoint(arc_length_to_stop_pose, path);
+  lane_change_stop_pose_ = stop_point.point.pose;
 }
 
 void NormalLaneChange::updateStopTime()