RT1-8532 separate target lane leading based on behavior

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

Author: zulfaqar-azmi-t4

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

@@ -267,14 +267,16 @@ struct TargetLaneLeadingObjects
   ExtendedPredictedObjects moving;
   ExtendedPredictedObjects stopped;
   ExtendedPredictedObjects expanded;
+
+  [[nodiscard]] size_t size() const { return moving.size() + stopped.size() + expanded.size(); }
 };
 
 struct LanesObjects
 {
-  ExtendedPredictedObjects others{};
-  ExtendedPredictedObjects current_lane{};
-  ExtendedPredictedObjects target_lane_trailing{};
-  TargetLaneLeadingObjects target_lane_leading{};
+  ExtendedPredictedObjects others;
+  ExtendedPredictedObjects current_lane;
+  ExtendedPredictedObjects target_lane_trailing;
+  TargetLaneLeadingObjects target_lane_leading;
 };
 
 struct TargetObjects
@@ -414,8 +416,7 @@ using LaneChangeStates = lane_change::States;
 using LaneChangePhaseInfo = lane_change::PhaseInfo;
 using LaneChangePhaseMetrics = lane_change::PhaseMetrics;
 using LaneChangeInfo = lane_change::Info;
-using FilteredByLanesObjects = lane_change::LanesObjects<std::vector<PredictedObject>>;
-using FilteredByLanesExtendedObjects = lane_change::LanesObjects<ExtendedPredictedObjects>;
+using FilteredByLanesExtendedObjects = lane_change::LanesObjects;
 using LateralAccelerationMap = lane_change::LateralAccelerationMap;
 }  // namespace autoware::behavior_path_planner
 

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

@@ -55,9 +55,11 @@ struct Debug
     collision_check_objects.clear();
     collision_check_objects_after_approval.clear();
     filtered_objects.current_lane.clear();
-    filtered_objects.target_lane_leading.clear();
     filtered_objects.target_lane_trailing.clear();
-    filtered_objects.other_lane.clear();
+    filtered_objects.target_lane_leading.moving.clear();
+    filtered_objects.target_lane_leading.stopped.clear();
+    filtered_objects.target_lane_leading.expanded.clear();
+    filtered_objects.others.clear();
     execution_area.points.clear();
     current_lanes.clear();
     target_lanes.clear();

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

@@ -54,6 +54,7 @@ using behavior_path_planner::lane_change::CommonDataPtr;
 using behavior_path_planner::lane_change::LanesPolygon;
 using behavior_path_planner::lane_change::ModuleType;
 using behavior_path_planner::lane_change::PathSafetyStatus;
+using behavior_path_planner::lane_change::TargetLaneLeadingObjects;
 using geometry_msgs::msg::Point;
 using geometry_msgs::msg::Pose;
 using geometry_msgs::msg::Twist;
@@ -290,8 +291,8 @@ double get_min_dist_to_current_lanes_obj(
  * otherwise, false.
  */
 bool has_blocking_target_object(
-  const CommonDataPtr & common_data_ptr, const FilteredByLanesExtendedObjects & filtered_objects,
-  const double stop_arc_length, const PathWithLaneId & path);
+  const TargetLaneLeadingObjects & target_leading_objects, const double stop_arc_length,
+  const PathWithLaneId & path);
 
 /**
  * @brief Checks if the ego vehicle has passed any turn direction within an intersection.
@@ -338,5 +339,11 @@ std::vector<LineString2d> get_line_string_paths(const ExtendedPredictedObject &
  */
 bool has_overtaking_turn_lane_object(
   const CommonDataPtr & common_data_ptr, const ExtendedPredictedObjects & trailing_objects);
+
+void filter_target_lane_objects(
+  const CommonDataPtr & common_data_ptr, const ExtendedPredictedObject & object,
+  const double dist_ego_to_current_lanes_center, const bool ahead_of_ego,
+  const bool before_terminal, TargetLaneLeadingObjects & leading_objects,
+  ExtendedPredictedObjects & trailing_objects);
 }  // namespace autoware::behavior_path_planner::utils::lane_change
 #endif  // AUTOWARE__BEHAVIOR_PATH_LANE_CHANGE_MODULE__UTILS__UTILS_HPP_

planning/behavior_path_planner/autoware_behavior_path_lane_change_module/src/scene.cpp

@@ -49,6 +49,7 @@ namespace autoware::behavior_path_planner
 using autoware::motion_utils::calcSignedArcLength;
 using utils::lane_change::create_lanes_polygon;
 namespace calculation = utils::lane_change::calculation;
+using utils::path_safety_checker::filter::velocity_filter;
 
 NormalLaneChange::NormalLaneChange(
   const std::shared_ptr<LaneChangeParameters> & parameters, LaneChangeModuleType type,
@@ -568,7 +569,7 @@ void NormalLaneChange::insert_stop_point_on_current_lanes(PathWithLaneId & path)
   //    [ego]>          | <--- stop margin --->  [obj]>
   //  ----------------------------------------------------------
   const auto has_blocking_target_lane_obj = utils::lane_change::has_blocking_target_object(
-    common_data_ptr_, filtered_objects_, stop_arc_length_behind_obj, path);
+    filtered_objects_.target_lane_leading, stop_arc_length_behind_obj, path);
 
   if (has_blocking_target_lane_obj || stop_arc_length_behind_obj <= 0.0) {
     set_stop_pose(dist_to_terminal_stop, path);
@@ -931,20 +932,21 @@ lane_change::TargetObjects NormalLaneChange::get_target_objects(
   const FilteredByLanesExtendedObjects & filtered_objects,
   [[maybe_unused]] const lanelet::ConstLanelets & current_lanes) const
 {
-  ExtendedPredictedObjects leading_objects = filtered_objects.target_lane_leading;
+  ExtendedPredictedObjects leading_objects = filtered_objects.target_lane_leading.moving;
   auto insert_leading_objects = [&](const auto & objects) {
     ranges::actions::insert(leading_objects, leading_objects.end(), objects);
   };
 
-  insert_leading_objects(filtered_objects.target_lane_expanded);
+  insert_leading_objects(filtered_objects.target_lane_leading.stopped);
+  insert_leading_objects(filtered_objects.target_lane_leading.expanded);
   const auto chk_obj_in_curr_lanes = lane_change_parameters_->check_objects_on_current_lanes;
   if (chk_obj_in_curr_lanes || common_data_ptr_->transient_data.is_ego_stuck) {
     insert_leading_objects(filtered_objects.current_lane);
   }
 
   const auto chk_obj_in_other_lanes = lane_change_parameters_->check_objects_on_other_lanes;
   if (chk_obj_in_other_lanes) {
-    insert_leading_objects(filtered_objects.other_lane);
+    insert_leading_objects(filtered_objects.others);
   }
 
   return {leading_objects, filtered_objects.target_lane_trailing};
@@ -996,7 +998,8 @@ void NormalLaneChange::filterOncomingObjects(PredictedObjects & objects) const
   const auto is_stopped_object = [](const auto & object) -> bool {
     constexpr double min_vel_th = -0.5;
     constexpr double max_vel_th = 0.5;
-    return utils::path_safety_checker::filter::velocity_filter(object, min_vel_th, max_vel_th);
+    return velocity_filter(
+      object.kinematics.initial_twist_with_covariance.twist, min_vel_th, max_vel_th);
   };
 
   utils::path_safety_checker::filterObjects(objects, [&](const PredictedObject & object) {
@@ -1012,35 +1015,23 @@ void NormalLaneChange::filterOncomingObjects(PredictedObjects & objects) const
 FilteredByLanesExtendedObjects NormalLaneChange::filterObjectsByLanelets(
   const PredictedObjects & objects) const
 {
-  FilteredByLanesExtendedObjects ext_objects;
-
   const auto & current_pose = common_data_ptr_->get_ego_pose();
   const auto & current_lanes = common_data_ptr_->lanes_ptr->current;
   const auto & route_handler = getRouteHandler();
-  const auto & common_parameters = planner_data_->parameters;
 
   const auto & current_lanes_ref_path = common_data_ptr_->current_lanes_path;
-  const auto is_object_in_lanes = [](const auto & object_poly, const auto & polygon) {
-    return !polygon.empty() && !boost::geometry::disjoint(polygon, object_poly);
-  };
-
-  const auto is_within_vel_th = [](
-                                  const ExtendedPredictedObject & object, const double min_th = 1.0,
-                                  const double max_th =
-                                    std::numeric_limits<double>::max()) -> bool {
-    const auto v_norm = std::hypot(object.initial_twist.linear.x, object.initial_twist.linear.y);
-    return (min_th < v_norm && v_norm < max_th);
-  };
 
   const auto & lanes_polygon = *common_data_ptr_->lanes_polygon_ptr;
   const auto dist_ego_to_current_lanes_center =
     lanelet::utils::getLateralDistanceToClosestLanelet(current_lanes, current_pose);
 
+  FilteredByLanesExtendedObjects ext_objects;
   const auto reserve_size = objects.objects.size();
   ext_objects.current_lane.reserve(reserve_size);
-  ext_objects.target_lane_leading.stopped.reserve(reserve_size);
-  ext_objects.target_lane_leading.moving.reserve(reserve_size);
-  ext_objects.target_lane_leading.expanded.reserve(reserve_size);
+  auto & target_lane_leading = ext_objects.target_lane_leading;
+  target_lane_leading.stopped.reserve(reserve_size);
+  target_lane_leading.moving.reserve(reserve_size);
+  target_lane_leading.expanded.reserve(reserve_size);
   ext_objects.target_lane_trailing.reserve(reserve_size);
   ext_objects.others.reserve(reserve_size);
 
@@ -1055,59 +1046,20 @@ FilteredByLanesExtendedObjects NormalLaneChange::filterObjectsByLanelets(
     const auto is_before_terminal =
       utils::lane_change::is_before_terminal(common_data_ptr_, current_lanes_ref_path, ext_object);
 
-    if (!is_before_terminal) {
-      ext_objects.others.push_back(ext_object);
-      continue;
-    }
-
-    const auto is_lateral_far = std::invoke([&]() -> bool {
-      const auto dist_object_to_current_lanes_center =
-        lanelet::utils::getLateralDistanceToClosestLanelet(current_lanes, ext_object.initial_pose);
-      const auto lateral = dist_object_to_current_lanes_center - dist_ego_to_current_lanes_center;
-      return std::abs(lateral) > (common_parameters.vehicle_width / 2);
-    });
-
     const auto ahead_of_ego =
       utils::lane_change::is_ahead_of_ego(common_data_ptr_, current_lanes_ref_path, ext_object);
 
-    if (is_lateral_far) {
-      const auto is_stopped = is_within_vel_th(ext_object, -floating_err_th, stopped_obj_vel_th);
-      if (is_object_in_lanes(ext_object.initial_polygon, lanes_polygon.target)) {
-        if (ahead_of_ego) {
-          if (is_stopped) {
-            ext_objects.target_lane_leading.stopped.push_back(ext_object);
-          } else {
-            ext_objects.target_lane_leading.moving.push_back(ext_object);
-          }
-        } else {
-          ext_objects.target_lane_trailing.push_back(ext_object);
-        }
-        continue;
-      }
-
-      if (
-        is_object_in_lanes(ext_object.initial_polygon, lanes_polygon.expanded_target) &&
-        is_stopped && ahead_of_ego) {
-        ext_objects.target_lane_leading.expanded.push_back(ext_object);
-        continue;
-      }
-    }
-
-    const auto is_overlap_target_backward =
-      ranges::any_of(lanes_polygon.preceding_target, [&](const auto & target_backward_polygon) {
-        return is_object_in_lanes(ext_object.initial_polygon, target_backward_polygon);
-      });
-
-    // check if the object intersects with target backward lanes
-    if (is_overlap_target_backward) {
-      ext_objects.target_lane_trailing.push_back(ext_object);
-      continue;
-    }
+    utils::lane_change::filter_target_lane_objects(
+      common_data_ptr_, ext_object, dist_ego_to_current_lanes_center, ahead_of_ego,
+      is_before_terminal, target_lane_leading, ext_objects.target_lane_trailing);
 
     // TODO(Azu): We have to think about how to remove is_within_vel_th without breaking AW behavior
+    const auto is_moving = velocity_filter(
+      ext_object.initial_twist, stopped_obj_vel_th, std::numeric_limits<double>::max());
+
     if (
-      ahead_of_ego && is_within_vel_th(ext_object) &&
-      is_object_in_lanes(ext_object.initial_polygon, lanes_polygon.current)) {
+      ahead_of_ego && is_moving && is_before_terminal &&
+      !boost::geometry::disjoint(ext_object.initial_polygon, lanes_polygon.current)) {
       // check only the objects that are in front of the ego vehicle
       ext_objects.current_lane.push_back(ext_object);
       continue;
@@ -1123,9 +1075,9 @@ FilteredByLanesExtendedObjects NormalLaneChange::filterObjectsByLanelets(
   // There are no use cases for other lane objects yet, so to save some computation time, we dont
   // have to sort them.
   ranges::sort(ext_objects.current_lane, dist_comparator);
-  ranges::sort(ext_objects.target_lane_leading.expanded, dist_comparator);
-  ranges::sort(ext_objects.target_lane_leading.stopped, dist_comparator);
-  ranges::sort(ext_objects.target_lane_leading.moving, dist_comparator);
+  ranges::sort(target_lane_leading.expanded, dist_comparator);
+  ranges::sort(target_lane_leading.stopped, dist_comparator);
+  ranges::sort(target_lane_leading.moving, dist_comparator);
   ranges::sort(ext_objects.target_lane_trailing, [&](const auto & obj1, const auto & obj2) {
     return !dist_comparator(obj1, obj2);
   });
@@ -1388,7 +1340,7 @@ bool NormalLaneChange::check_candidate_path_safety(
 
   if (
     !is_stuck && !utils::lane_change::passed_parked_objects(
-                   common_data_ptr_, candidate_path, filtered_objects_.target_lane_leading,
+                   common_data_ptr_, candidate_path, filtered_objects_.target_lane_leading.stopped,
                    lane_change_debug_.collision_check_objects)) {
     throw std::logic_error(
       "Ego is not stuck and parked vehicle exists in the target lane. Skip lane change.");
@@ -1569,7 +1521,7 @@ PathSafetyStatus NormalLaneChange::isApprovedPathSafe() const
   }
 
   const auto has_passed_parked_objects = utils::lane_change::passed_parked_objects(
-    common_data_ptr_, path, filtered_objects_.target_lane_leading, debug_data);
+    common_data_ptr_, path, filtered_objects_.target_lane_leading.stopped, debug_data);
 
   if (!has_passed_parked_objects) {
     RCLCPP_DEBUG(logger_, "Lane change has been delayed.");

planning/behavior_path_planner/autoware_behavior_path_lane_change_module/src/utils/markers.cpp

@@ -105,8 +105,7 @@ MarkerArray showFilteredObjects(
 {
   int32_t update_id = 0;
   MarkerArray marker_array;
-  auto reserve_size = filtered_objects.current_lane.size() + filtered_objects.other_lane.size() +
-                      filtered_objects.target_lane_expanded.size() +
+  auto reserve_size = filtered_objects.current_lane.size() + filtered_objects.others.size() +
                       filtered_objects.target_lane_leading.size() +
                       filtered_objects.target_lane_trailing.size();
   marker_array.markers.reserve(2 * reserve_size);
@@ -119,10 +118,11 @@ MarkerArray showFilteredObjects(
     };
 
   add_objects_to_marker(filtered_objects.current_lane, colors::yellow());
-  add_objects_to_marker(filtered_objects.target_lane_leading, colors::aqua());
+  add_objects_to_marker(filtered_objects.target_lane_leading.moving, colors::aqua());
+  add_objects_to_marker(filtered_objects.target_lane_leading.stopped, colors::light_steel_blue());
   add_objects_to_marker(filtered_objects.target_lane_trailing, colors::blue());
-  add_objects_to_marker(filtered_objects.target_lane_expanded, colors::light_pink());
-  add_objects_to_marker(filtered_objects.other_lane, colors::medium_orchid());
+  add_objects_to_marker(filtered_objects.target_lane_leading.expanded, colors::light_pink());
+  add_objects_to_marker(filtered_objects.others, colors::medium_orchid());
 
   auto add_text = [&](const ExtendedPredictedObjects & objects) {
     for (const auto & target_lead_obj : objects) {
@@ -142,7 +142,9 @@ MarkerArray showFilteredObjects(
   };
 
   add_text(filtered_objects.current_lane);
-  add_text(filtered_objects.target_lane_leading);
+  add_text(filtered_objects.target_lane_leading.moving);
+  add_text(filtered_objects.target_lane_leading.expanded);
+  add_text(filtered_objects.target_lane_leading.stopped);
   add_text(filtered_objects.target_lane_trailing);
 
   return marker_array;