fix(lane_change): filtering object ahead of terminal (#8093)

* employ lanelet based filtering before distance based filtering

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

* use distance based to terminal check instead

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

* remove RCLCPP INFO

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

* update flow chart

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

---------

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

Author: zulfaqar-azmi-t4

planning/behavior_path_planner/autoware_behavior_path_lane_change_module/README.md

@@ -331,8 +331,7 @@ title NormalLaneChange::filterObjects Method Execution Flow
 start
 
 group "Filter Objects by Class" {
-:Iterate through each object in objects list;
-while (has not finished iterating through object list) is (TRUE)
+while (has not finished iterating through predicted object list) is (TRUE)
   if (current object type != param.object_types_to_check?) then (TRUE)
   #LightPink:Remove current object;
 else (FALSE)
@@ -341,17 +340,15 @@ endif
 end while
 end group
 
-if (object list is empty?) then (TRUE)
+if (predicted object list is empty?) then (TRUE)
   :Return empty result;
   stop
 else (FALSE)
 endif
 
 group "Filter Oncoming Objects" #PowderBlue {
-:Iterate through each object in target lane objects list;
-while (has not finished iterating through object list?) is (TRUE)
-:check object's yaw with reference to ego's yaw.;
-if (yaw difference < 90 degree?) then (TRUE)
+while (has not finished iterating through predicted object list?) is (TRUE)
+if (object's yaw with reference to ego's yaw difference < 90 degree?) then (TRUE)
   :Keep current object;
 else (FALSE)
 if (object is stopping?) then (TRUE)
@@ -363,53 +360,35 @@ endif
 endwhile
 end group
 
-if (object list is empty?) then (TRUE)
-  :Return empty result;
-  stop
-else (FALSE)
-endif
-
-group "Filter Objects Ahead Terminal" #Beige {
-:Calculate lateral distance from ego to current lanes center;
-
-:Iterate through each object in objects list;
-while (has not finished iterating through object list) is (TRUE)
-  :Get current object's polygon;
-  :Initialize distance to terminal from object to max;
-  while (has not finished iterating through object polygon's vertices) is (TRUE)
-    :Calculate object's lateral distance to end of lane;
-    :Update minimum distance to terminal from object;
-  end while
-  if (Is object's distance to terminal exceeds minimum lane change length?) then (TRUE)
-      #LightPink:Remove current object;
-  else (FALSE)
-  endif
-end while
-end group
-
-if (object list is empty?) then (TRUE)
+if (predicted object list is empty?) then (TRUE)
   :Return empty result;
   stop
 else (FALSE)
 endif
 
 group "Filter Objects By Lanelets" #LightGreen {
 
-:Iterate through each object in objects list;
-while (has not finished iterating through object list) is (TRUE)
-  :lateral distance diff = difference between object's lateral distance and ego's lateral distance to the current lanes' centerline.;
-  if (Object in target lane polygon, and lateral distance diff is more than half of ego's width?) then (TRUE)
-    :Add to target_lane_objects;
-    else (FALSE)
-      if (Object overlaps with backward target lanes?) then (TRUE)
+while (has not finished iterating through predicted object list) is (TRUE)
+  :Calculate lateral distance diff;
+  if (Object in target lane polygon?) then (TRUE)
+    if (lateral distance diff > half of ego's width?) then (TRUE)
+      if (Object's physical position is before terminal point?) then (TRUE)
         :Add to target_lane_objects;
       else (FALSE)
-        if (Object in current lane polygon?) then (TRUE)
-          :Add to current_lane_objects;
-        else (FALSE)
-          :Add to other_lane_objects;
-        endif
       endif
+    else (FALSE)
+    endif
+  else (FALSE)
+  endif
+
+  if (Object overlaps with backward target lanes?) then (TRUE)
+    :Add to target_lane_objects;
+  else (FALSE)
+    if (Object in current lane polygon?) then (TRUE)
+      :Add to current_lane_objects;
+    else (FALSE)
+      :Add to other_lane_objects;
+    endif
   endif
 end while
 
@@ -426,13 +405,10 @@ endif
 
 group "Filter Target Lanes' objects" #LightCyan {
 
-:Iterate through each object in target lane objects list;
-while (has not finished iterating through object list) is (TRUE)
-  :check object's velocity;
+while (has not finished iterating through target lanes' object list) is (TRUE)
   if(velocity is within threshold?) then (TRUE)
   :Keep current object;
   else (FALSE)
-    :check whether object is ahead of ego;
     if(object is ahead of ego?) then (TRUE)
       :keep current object;
     else (FALSE)
@@ -444,11 +420,8 @@ end group
 
 group "Filter Current Lanes' objects"  #LightYellow {
 
-:Iterate through each object in current lane objects list;
-while (has not finished iterating through object list) is (TRUE)
-  :check object's velocity;
+while (has not finished iterating through current lanes' object list) is (TRUE)
   if(velocity is within threshold?) then (TRUE)
-  :check whether object is ahead of ego;
     if(object is ahead of ego?) then (TRUE)
       :keep current object;
     else (FALSE)
@@ -462,11 +435,8 @@ end group
 
 group "Filter Other Lanes' objects"  #Lavender {
 
-:Iterate through each object in other lane objects list;
-while (has not finished iterating through object list) is (TRUE)
-  :check object's velocity;
+while (has not finished iterating through other lanes' object list) is (TRUE)
   if(velocity is within threshold?) then (TRUE)
-  :check whether object is ahead of ego;
     if(object is ahead of ego?) then (TRUE)
       :keep current object;
     else (FALSE)
@@ -478,7 +448,7 @@ while (has not finished iterating through object list) is (TRUE)
 endwhile
 end group
 
-:Trasform the objects into extended predicted object and return them as lane_change_target_objects;
+:Transform the objects into extended predicted object and return them as lane_change_target_objects;
 stop
 
 @enduml

planning/behavior_path_planner/autoware_behavior_path_lane_change_module/include/autoware/behavior_path_lane_change_module/scene.hpp

@@ -128,12 +128,8 @@ class NormalLaneChange : public LaneChangeBase
 
   void filterOncomingObjects(PredictedObjects & objects) const;
 
-  void filterAheadTerminalObjects(
-    PredictedObjects & objects, const lanelet::ConstLanelets & current_lanes) const;
-
   void filterObjectsByLanelets(
-    const PredictedObjects & objects, const lanelet::ConstLanelets & current_lanes,
-    const lanelet::ConstLanelets & target_lanes,
+    const PredictedObjects & objects, const PathWithLaneId & current_lanes_ref_path,
     std::vector<PredictedObject> & current_lane_objects,
     std::vector<PredictedObject> & target_lane_objects,
     std::vector<PredictedObject> & other_lane_objects) const;

planning/behavior_path_planner/autoware_behavior_path_lane_change_module/include/autoware/behavior_path_lane_change_module/utils/data_structs.hpp

@@ -185,6 +185,7 @@ struct PhaseInfo
 
 struct Lanes
 {
+  bool current_lane_in_goal_section{false};
   lanelet::ConstLanelets current;
   lanelet::ConstLanelets target;
   std::vector<lanelet::ConstLanelets> preceding_target;

planning/behavior_path_planner/autoware_behavior_path_lane_change_module/include/autoware/behavior_path_lane_change_module/utils/utils.hpp

@@ -302,6 +302,18 @@ LanesPolygon create_lanes_polygon(const CommonDataPtr & common_data_ptr);
 bool is_same_lane_with_prev_iteration(
   const CommonDataPtr & common_data_ptr, const lanelet::ConstLanelets & current_lanes,
   const lanelet::ConstLanelets & target_lanes);
+
+Pose to_pose(
+  const autoware::universe_utils::Point2d & point,
+  const geometry_msgs::msg::Quaternion & orientation);
+
+bool is_ahead_of_ego(
+  const CommonDataPtr & common_data_ptr, const PathWithLaneId & path,
+  const PredictedObject & object);
+
+bool is_before_terminal(
+  const CommonDataPtr & common_data_ptr, const PathWithLaneId & path,
+  const PredictedObject & object);
 }  // namespace autoware::behavior_path_planner::utils::lane_change
 
 namespace autoware::behavior_path_planner::utils::lane_change::debug

planning/behavior_path_planner/autoware_behavior_path_lane_change_module/src/scene.cpp

@@ -83,8 +83,11 @@ void NormalLaneChange::update_lanes(const bool is_approved)
   common_data_ptr_->lanes_ptr->current = current_lanes;
   common_data_ptr_->lanes_ptr->target = target_lanes;
 
+  const auto & route_handler_ptr = common_data_ptr_->route_handler_ptr;
+  common_data_ptr_->lanes_ptr->current_lane_in_goal_section =
+    route_handler_ptr->isInGoalRouteSection(current_lanes.back());
   common_data_ptr_->lanes_ptr->preceding_target = utils::getPrecedingLanelets(
-    *common_data_ptr_->route_handler_ptr, get_target_lanes(), common_data_ptr_->get_ego_pose(),
+    *route_handler_ptr, get_target_lanes(), common_data_ptr_->get_ego_pose(),
     common_data_ptr_->lc_param_ptr->backward_lane_length);
 
   *common_data_ptr_->lanes_polygon_ptr = create_lanes_polygon(common_data_ptr_);
@@ -983,7 +986,6 @@ ExtendedPredictedObjects NormalLaneChange::getTargetObjects(
 
 LaneChangeLanesFilteredObjects NormalLaneChange::filterObjects() const
 {
-  const auto & current_pose = getEgoPose();
   const auto & route_handler = getRouteHandler();
   const auto & common_parameters = planner_data_->parameters;
   auto objects = *planner_data_->dynamic_object;
@@ -1006,12 +1008,6 @@ LaneChangeLanesFilteredObjects NormalLaneChange::filterObjects() const
     return {};
   }
 
-  filterAheadTerminalObjects(objects, current_lanes);
-
-  if (objects.objects.empty()) {
-    return {};
-  }
-
   std::vector<PredictedObject> target_lane_objects;
   std::vector<PredictedObject> current_lane_objects;
   std::vector<PredictedObject> other_lane_objects;
@@ -1022,48 +1018,37 @@ LaneChangeLanesFilteredObjects NormalLaneChange::filterObjects() const
     return {};
   }
 
+  const auto path =
+    route_handler->getCenterLinePath(current_lanes, 0.0, std::numeric_limits<double>::max());
+
   filterObjectsByLanelets(
-    objects, current_lanes, target_lanes, current_lane_objects, target_lane_objects,
-    other_lane_objects);
+    objects, path, current_lane_objects, target_lane_objects, other_lane_objects);
 
   const auto is_within_vel_th = [](const auto & object) -> bool {
     constexpr double min_vel_th = 1.0;
     constexpr double max_vel_th = std::numeric_limits<double>::max();
     return utils::path_safety_checker::filter::velocity_filter(object, min_vel_th, max_vel_th);
   };
 
-  const auto path =
-    route_handler->getCenterLinePath(current_lanes, 0.0, std::numeric_limits<double>::max());
-
-  if (path.points.empty()) {
-    return {};
-  }
-
-  const auto is_ahead_of_ego = [&path, &current_pose](const auto & object) {
-    const auto obj_polygon = autoware::universe_utils::toPolygon2d(object).outer();
-
-    double max_dist_ego_to_obj = std::numeric_limits<double>::lowest();
-    for (const auto & polygon_p : obj_polygon) {
-      const auto obj_p = autoware::universe_utils::createPoint(polygon_p.x(), polygon_p.y(), 0.0);
-      const auto dist_ego_to_obj = calcSignedArcLength(path.points, current_pose.position, obj_p);
-      max_dist_ego_to_obj = std::max(dist_ego_to_obj, max_dist_ego_to_obj);
-    }
-    return max_dist_ego_to_obj >= 0.0;
-  };
-
   utils::path_safety_checker::filterObjects(
     target_lane_objects, [&](const PredictedObject & object) {
-      return (is_within_vel_th(object) || is_ahead_of_ego(object));
+      const auto ahead_of_ego = utils::lane_change::is_ahead_of_ego(common_data_ptr_, path, object);
+      return is_within_vel_th(object) || ahead_of_ego;
     });
 
-  utils::path_safety_checker::filterObjects(
-    other_lane_objects, [&](const PredictedObject & object) {
-      return is_within_vel_th(object) && is_ahead_of_ego(object);
-    });
+  if (lane_change_parameters_->check_objects_on_other_lanes) {
+    utils::path_safety_checker::filterObjects(
+      other_lane_objects, [&](const PredictedObject & object) {
+        const auto ahead_of_ego =
+          utils::lane_change::is_ahead_of_ego(common_data_ptr_, path, object);
+        return is_within_vel_th(object) && ahead_of_ego;
+      });
+  }
 
   utils::path_safety_checker::filterObjects(
     current_lane_objects, [&](const PredictedObject & object) {
-      return is_within_vel_th(object) && is_ahead_of_ego(object);
+      const auto ahead_of_ego = utils::lane_change::is_ahead_of_ego(common_data_ptr_, path, object);
+      return is_within_vel_th(object) && ahead_of_ego;
     });
 
   LaneChangeLanesFilteredObjects lane_change_target_objects;
@@ -1119,42 +1104,15 @@ void NormalLaneChange::filterOncomingObjects(PredictedObjects & objects) const
   });
 }
 
-void NormalLaneChange::filterAheadTerminalObjects(
-  PredictedObjects & objects, const lanelet::ConstLanelets & current_lanes) const
-{
-  const auto & current_pose = getEgoPose();
-  const auto & route_handler = getRouteHandler();
-  const auto minimum_lane_change_length = utils::lane_change::calcMinimumLaneChangeLength(
-    route_handler, current_lanes.back(), *lane_change_parameters_, direction_);
-
-  const auto dist_ego_to_current_lanes_center =
-    lanelet::utils::getLateralDistanceToClosestLanelet(current_lanes, current_pose);
-
-  // ignore object that are ahead of terminal lane change start
-  utils::path_safety_checker::filterObjects(objects, [&](const PredictedObject & object) {
-    const auto obj_polygon = autoware::universe_utils::toPolygon2d(object).outer();
-    // ignore object that are ahead of terminal lane change start
-    auto distance_to_terminal_from_object = std::numeric_limits<double>::max();
-    for (const auto & polygon_p : obj_polygon) {
-      const auto obj_p = autoware::universe_utils::createPoint(polygon_p.x(), polygon_p.y(), 0.0);
-      Pose polygon_pose;
-      polygon_pose.position = obj_p;
-      polygon_pose.orientation = object.kinematics.initial_pose_with_covariance.pose.orientation;
-      const auto dist = utils::getDistanceToEndOfLane(polygon_pose, current_lanes);
-      distance_to_terminal_from_object = std::min(dist_ego_to_current_lanes_center, dist);
-    }
-
-    return (minimum_lane_change_length > distance_to_terminal_from_object);
-  });
-}
-
 void NormalLaneChange::filterObjectsByLanelets(
-  const PredictedObjects & objects, const lanelet::ConstLanelets & current_lanes,
-  const lanelet::ConstLanelets & target_lanes, std::vector<PredictedObject> & current_lane_objects,
+  const PredictedObjects & objects, const PathWithLaneId & current_lanes_ref_path,
+  std::vector<PredictedObject> & current_lane_objects,
   std::vector<PredictedObject> & target_lane_objects,
   std::vector<PredictedObject> & other_lane_objects) const
 {
   const auto & current_pose = getEgoPose();
+  const auto & current_lanes = common_data_ptr_->lanes_ptr->current;
+  const auto & target_lanes = common_data_ptr_->lanes_ptr->target;
   const auto & route_handler = getRouteHandler();
   const auto & common_parameters = planner_data_->parameters;
   const auto check_optional_polygon = [](const auto & object, const auto & polygon) {
@@ -1199,7 +1157,14 @@ void NormalLaneChange::filterObjectsByLanelets(
       return std::abs(lateral) > (common_parameters.vehicle_width / 2);
     });
 
-    if (check_optional_polygon(object, lanes_polygon.expanded_target) && is_lateral_far) {
+    const auto is_before_terminal = [&]() {
+      return utils::lane_change::is_before_terminal(
+        common_data_ptr_, current_lanes_ref_path, object);
+    };
+
+    if (
+      check_optional_polygon(object, lanes_polygon.expanded_target) && is_lateral_far &&
+      is_before_terminal()) {
       target_lane_objects.push_back(object);
       continue;
     }