fix(behavior_path_planner): reduce obj indices call in lane change (#…

…2726)

* fix(behavior_path_planner): reduce obj indices call in lane change

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

* remove unused functions

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

* add continue for the intersect in current

* replace target and current lanes

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

* change isLaneChangePathSafe arguments

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>

planning/behavior_path_planner/include/behavior_path_planner/scene_module/lane_change/util.hpp

@@ -90,7 +90,7 @@ bool selectSafePath(
   std::unordered_map<std::string, CollisionCheckDebug> & debug_data);
 
 bool isLaneChangePathSafe(
-  const PathWithLaneId & path, const lanelet::ConstLanelets & current_lanes,
+  const LaneChangePath & lane_change_path, const lanelet::ConstLanelets & current_lanes,
   const lanelet::ConstLanelets & target_lanes,
   const PredictedObjects::ConstSharedPtr dynamic_objects, const Pose & current_pose,
   const size_t current_seg_idx, const Twist & current_twist,

planning/behavior_path_planner/include/behavior_path_planner/utilities.hpp

@@ -289,14 +289,6 @@ double calcLongitudinalDistanceFromEgoToObjects(
   const Pose & ego_pose, double base_link2front, double base_link2rear,
   const PredictedObjects & dynamic_objects);
 
-/**
- * @brief Get index of the obstacles inside the lanelets with start and end length
- * @return Indices corresponding to the obstacle inside the lanelets
- */
-std::vector<size_t> filterObjectIndicesByLanelets(
-  const PredictedObjects & objects, const lanelet::ConstLanelets & lanelets,
-  const double start_arc_length, const double end_arc_length);
-
 /**
  * @brief Separate index of the obstacles into two part based on whether the object is within
  * lanelet.
@@ -313,10 +305,6 @@ std::pair<std::vector<size_t>, std::vector<size_t>> separateObjectIndicesByLanel
 std::pair<PredictedObjects, PredictedObjects> separateObjectsByLanelets(
   const PredictedObjects & objects, const lanelet::ConstLanelets & target_lanelets);
 
-std::vector<size_t> filterObjectsIndicesByPath(
-  const PredictedObjects & objects, const std::vector<size_t> & object_indices,
-  const PathWithLaneId & ego_path, const double vehicle_width);
-
 PredictedObjects filterObjectsByVelocity(const PredictedObjects & objects, double lim_v);
 
 PredictedObjects filterObjectsByVelocity(

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

@@ -698,9 +698,8 @@ bool ExternalRequestLaneChangeModule::isApprovedPathSafe(Pose & ego_pose_before_
     path.path.points, current_pose, common_parameters.ego_nearest_dist_threshold,
     common_parameters.ego_nearest_yaw_threshold);
   return lane_change_utils::isLaneChangePathSafe(
-    path.path, current_lanes, check_lanes, dynamic_objects, current_pose, current_seg_idx,
-    current_twist, common_parameters, *parameters_,
-    common_parameters.expected_front_deceleration_for_abort,
+    path, current_lanes, check_lanes, dynamic_objects, current_pose, current_seg_idx, current_twist,
+    common_parameters, *parameters_, common_parameters.expected_front_deceleration_for_abort,
     common_parameters.expected_rear_deceleration_for_abort, ego_pose_before_collision, debug_data,
     false, status_.lane_change_path.acceleration);
 }

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

@@ -694,9 +694,8 @@ bool LaneChangeModule::isApprovedPathSafe(Pose & ego_pose_before_collision) cons
     path.path.points, current_pose, common_parameters.ego_nearest_dist_threshold,
     common_parameters.ego_nearest_yaw_threshold);
   return lane_change_utils::isLaneChangePathSafe(
-    path.path, current_lanes, check_lanes, dynamic_objects, current_pose, current_seg_idx,
-    current_twist, common_parameters, *parameters_,
-    common_parameters.expected_front_deceleration_for_abort,
+    path, current_lanes, check_lanes, dynamic_objects, current_pose, current_seg_idx, current_twist,
+    common_parameters, *parameters_, common_parameters.expected_front_deceleration_for_abort,
     common_parameters.expected_rear_deceleration_for_abort, ego_pose_before_collision, debug_data,
     false, status_.lane_change_path.acceleration);
 }

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

@@ -17,6 +17,7 @@
 #include "behavior_path_planner/parameters.hpp"
 #include "behavior_path_planner/path_utilities.hpp"
 #include "behavior_path_planner/scene_module/lane_change/lane_change_module_data.hpp"
+#include "behavior_path_planner/scene_module/lane_change/lane_change_path.hpp"
 #include "behavior_path_planner/scene_module/utils/path_shifter.hpp"
 #include "behavior_path_planner/utilities.hpp"
 
@@ -34,6 +35,85 @@
 #include <string>
 #include <vector>
 
+namespace
+{
+
+using autoware_auto_perception_msgs::msg::ObjectClassification;
+using autoware_auto_perception_msgs::msg::PredictedObjects;
+using autoware_auto_planning_msgs::msg::PathWithLaneId;
+using behavior_path_planner::util::calcObjectPolygon;
+using behavior_path_planner::util::getHighestProbLabel;
+using geometry_msgs::msg::Pose;
+using tier4_autoware_utils::LineString2d;
+using tier4_autoware_utils::Point2d;
+using tier4_autoware_utils::Polygon2d;
+
+void filterObjectIndices(
+  const PredictedObjects & objects, const lanelet::ConstLanelets & current_lanes,
+  const lanelet::ConstLanelets & target_lanes, const PathWithLaneId & ego_path,
+  const Pose & current_pose, const double forward_path_length, const double filter_width,
+  std::vector<size_t> & current_lane_obj_indices, std::vector<size_t> & target_lane_obj_indices,
+  std::vector<size_t> & others_obj_indices, const bool ignore_unknown_obj = false)
+{
+  // Reserve maximum amount possible
+  current_lane_obj_indices.reserve(objects.objects.size());
+  target_lane_obj_indices.reserve(objects.objects.size());
+  others_obj_indices.reserve(objects.objects.size());
+
+  const auto get_basic_polygon =
+    [](const lanelet::ConstLanelets & lanes, const double start_dist, const double end_dist) {
+      const auto polygon_3d = lanelet::utils::getPolygonFromArcLength(lanes, start_dist, end_dist);
+      return lanelet::utils::to2D(polygon_3d).basicPolygon();
+    };
+  const auto arc = lanelet::utils::getArcCoordinates(current_lanes, current_pose);
+  const auto current_polygon =
+    get_basic_polygon(current_lanes, arc.length, arc.length + forward_path_length);
+  const auto target_polygon =
+    get_basic_polygon(target_lanes, 0.0, std::numeric_limits<double>::max());
+  LineString2d ego_path_linestring;
+  ego_path_linestring.reserve(ego_path.points.size());
+  for (const auto & pt : ego_path.points) {
+    const auto & position = pt.point.pose.position;
+    boost::geometry::append(ego_path_linestring, Point2d(position.x, position.y));
+  }
+
+  for (size_t i = 0; i < objects.objects.size(); ++i) {
+    const auto & obj = objects.objects.at(i);
+
+    if (ignore_unknown_obj) {
+      const auto t = getHighestProbLabel(obj.classification);
+      if (t == ObjectClassification::UNKNOWN) {
+        continue;
+      }
+    }
+
+    Polygon2d obj_polygon;
+    if (!calcObjectPolygon(obj, &obj_polygon)) {
+      RCLCPP_ERROR_STREAM(
+        rclcpp::get_logger("behavior_path_planner").get_child("lane_change"),
+        "Failed to calcObjectPolygon...!!!");
+      continue;
+    }
+
+    if (boost::geometry::intersects(current_polygon, obj_polygon)) {
+      const double distance = boost::geometry::distance(obj_polygon, ego_path_linestring);
+
+      if (distance < filter_width) {
+        current_lane_obj_indices.push_back(i);
+        continue;
+      }
+    }
+
+    const bool is_intersect_with_target = boost::geometry::intersects(target_polygon, obj_polygon);
+    if (is_intersect_with_target) {
+      target_lane_obj_indices.push_back(i);
+    } else {
+      others_obj_indices.push_back(i);
+    }
+  }
+}
+}  // namespace
+
 namespace behavior_path_planner::lane_change_utils
 {
 using autoware_auto_planning_msgs::msg::PathPointWithLaneId;
@@ -337,7 +417,7 @@ bool selectSafePath(
       common_parameters.ego_nearest_yaw_threshold);
     Pose ego_pose_before_collision;
     if (isLaneChangePathSafe(
-          path.path, current_lanes, target_lanes, dynamic_objects, current_pose, current_seg_idx,
+          path, current_lanes, target_lanes, dynamic_objects, current_pose, current_seg_idx,
           current_twist, common_parameters, ros_parameters,
           common_parameters.expected_front_deceleration,
           common_parameters.expected_rear_deceleration, ego_pose_before_collision, debug_data, true,
@@ -403,7 +483,7 @@ bool hasEnoughDistance(
 }
 
 bool isLaneChangePathSafe(
-  const PathWithLaneId & path, const lanelet::ConstLanelets & current_lanes,
+  const LaneChangePath & lane_change_path, const lanelet::ConstLanelets & current_lanes,
   const lanelet::ConstLanelets & target_lanes,
   const PredictedObjects::ConstSharedPtr dynamic_objects, const Pose & current_pose,
   const size_t current_seg_idx, const Twist & current_twist,
@@ -417,6 +497,7 @@ bool isLaneChangePathSafe(
     return true;
   }
 
+  const auto & path = lane_change_path.path;
   if (path.points.empty() || target_lanes.empty() || current_lanes.empty()) {
     return false;
   }
@@ -435,24 +516,20 @@ bool isLaneChangePathSafe(
   const auto prepare_phase_ignore_target_speed_thresh =
     lane_change_parameters.prepare_phase_ignore_target_speed_thresh;
 
-  const auto arc = lanelet::utils::getArcCoordinates(current_lanes, current_pose);
-
-  // find obstacle in lane change target lanes
-  // retrieve lanes that are merging target lanes as well
-  const auto [target_lane_object_indices, others] =
-    util::separateObjectIndicesByLanelets(*dynamic_objects, target_lanes);
-
-  // find objects in current lane
-  const double check_distance = common_parameters.forward_path_length;
-  const auto current_lane_object_indices_lanelet = util::filterObjectIndicesByLanelets(
-    *dynamic_objects, current_lanes, arc.length, arc.length + check_distance);
-
-  const double lateral_buffer = (use_buffer) ? 0.5 : 0.0;
   const auto & vehicle_info = common_parameters.vehicle_info;
-  const auto & vehicle_width = common_parameters.vehicle_width;
-  const auto current_lane_object_indices = util::filterObjectsIndicesByPath(
-    *dynamic_objects, current_lane_object_indices_lanelet, path,
-    vehicle_width / 2 + lateral_buffer);
+
+  std::vector<size_t> current_lane_object_indices{};
+  std::vector<size_t> target_lane_object_indices{};
+  std::vector<size_t> other_lane_object_indices{};
+  {
+    const auto lateral_buffer = (use_buffer) ? 0.5 : 0.0;
+    const auto current_obj_filtering_buffer = lateral_buffer + common_parameters.vehicle_width / 2;
+
+    filterObjectIndices(
+      *dynamic_objects, lane_change_path.reference_lanelets, lane_change_path.target_lanelets, path,
+      current_pose, common_parameters.forward_path_length, current_obj_filtering_buffer,
+      current_lane_object_indices, target_lane_object_indices, other_lane_object_indices, true);
+  }
 
   const auto assignDebugData = [](const PredictedObject & obj) {
     CollisionCheckDebug debug;

planning/behavior_path_planner/src/utilities.cpp

@@ -490,48 +490,6 @@ double calcLongitudinalDistanceFromEgoToObjects(
   return min_distance;
 }
 
-// only works with consecutive lanes
-std::vector<size_t> filterObjectIndicesByLanelets(
-  const PredictedObjects & objects, const lanelet::ConstLanelets & target_lanelets,
-  const double start_arc_length, const double end_arc_length)
-{
-  std::vector<size_t> indices;
-  if (target_lanelets.empty()) {
-    return {};
-  }
-  const auto polygon =
-    lanelet::utils::getPolygonFromArcLength(target_lanelets, start_arc_length, end_arc_length);
-  const auto polygon2d = lanelet::utils::to2D(polygon).basicPolygon();
-  if (polygon2d.empty()) {
-    // no lanelet polygon
-    return {};
-  }
-
-  for (size_t i = 0; i < objects.objects.size(); i++) {
-    const auto & obj = objects.objects.at(i);
-    // create object polygon
-    Polygon2d obj_polygon;
-    if (!calcObjectPolygon(obj, &obj_polygon)) {
-      continue;
-    }
-    // create lanelet polygon
-    Polygon2d lanelet_polygon;
-    lanelet_polygon.outer().reserve(polygon2d.size() + 1);
-    for (const auto & lanelet_point : polygon2d) {
-      lanelet_polygon.outer().emplace_back(lanelet_point.x(), lanelet_point.y());
-    }
-
-    lanelet_polygon.outer().push_back(lanelet_polygon.outer().front());
-
-    // check the object does not intersect the lanelet
-    if (!boost::geometry::disjoint(lanelet_polygon, obj_polygon)) {
-      indices.push_back(i);
-      continue;
-    }
-  }
-  return indices;
-}
-
 std::pair<std::vector<size_t>, std::vector<size_t>> separateObjectIndicesByLanelets(
   const PredictedObjects & objects, const lanelet::ConstLanelets & target_lanelets)
 {
@@ -674,30 +632,6 @@ std::vector<double> calcObjectsDistanceToPath(
   return distance_array;
 }
 
-std::vector<size_t> filterObjectsIndicesByPath(
-  const PredictedObjects & objects, const std::vector<size_t> & object_indices,
-  const PathWithLaneId & ego_path, const double vehicle_width)
-{
-  std::vector<size_t> indices;
-  const auto ego_path_point_array = convertToGeometryPointArray(ego_path);
-  for (const auto & i : object_indices) {
-    Polygon2d obj_polygon;
-    if (!calcObjectPolygon(objects.objects.at(i), &obj_polygon)) {
-      continue;
-    }
-    LineString2d ego_path_line;
-    ego_path_line.reserve(ego_path_point_array.size());
-    for (const auto & ego_path_point : ego_path_point_array) {
-      boost::geometry::append(ego_path_line, Point2d(ego_path_point.x, ego_path_point.y));
-    }
-    const double distance = boost::geometry::distance(obj_polygon, ego_path_line);
-    if (distance < vehicle_width) {
-      indices.push_back(i);
-    }
-  }
-  return indices;
-}
-
 PathWithLaneId removeOverlappingPoints(const PathWithLaneId & input_path)
 {
   PathWithLaneId filtered_path;