feat(behavior_path_planner): separateObjectsByLanelets

Signed-off-by: kosuke55 <[email protected]>

planning/behavior_path_planner/include/behavior_path_planner/utilities.hpp

@@ -88,7 +88,7 @@ struct ProjectedDistancePoint
   double distance{0.0};
 };
 
-template <typename Pythagoras = bg::strategy::distance::pythagoras<> >
+template <typename Pythagoras = bg::strategy::distance::pythagoras<>>
 ProjectedDistancePoint pointToSegment(
   const Point2d & reference_point, const Point2d & point_from_ego,
   const Point2d & point_from_object);
@@ -266,7 +266,20 @@ std::vector<size_t> filterObjectIndicesByLanelets(
 std::vector<size_t> filterObjectIndicesByLanelets(
   const PredictedObjects & objects, const lanelet::ConstLanelets & target_lanelets);
 
-PredictedObjects filterObjectsByLanelets(
+/**
+ * @brief Separate index of the obstacles into two part based on whether the object is within
+ * lanelet.
+ * @return Indices of objects pair. first objects are in the lanelet, and second others are out of
+ * lanelet.
+ */
+std::pair<std::vector<size_t>, std::vector<size_t>> separateObjectIndicesByLanelets(
+  const PredictedObjects & objects, const lanelet::ConstLanelets & target_lanelets);
+
+/**
+ * @brief Separate the objects into two part based on whether the object is within lanelet.
+ * @return Objects pair. first objects are in the lanelet, and second others are out of lanelet.
+ */
+std::pair<PredictedObjects, PredictedObjects> separateObjectsByLanelets(
   const PredictedObjects & objects, const lanelet::ConstLanelets & target_lanelets);
 
 std::vector<size_t> filterObjectsIndicesByPath(

planning/behavior_path_planner/src/scene_module/pull_out/geometric_pull_out.cpp

@@ -52,8 +52,8 @@ boost::optional<PullOutPath> GeometricPullOut::plan(Pose start_pose, Pose goal_p
 
   // collision check with objects in shoulder lanes
   const auto arc_path = planner_.getArcPath();
-  const auto shoulder_lane_objects =
-    util::filterObjectsByLanelets(*(planner_data_->dynamic_object), shoulder_lanes);
+  const auto [shoulder_lane_objects, others] =
+    util::separateObjectsByLanelets(*(planner_data_->dynamic_object), shoulder_lanes);
   if (util::checkCollisionBetweenPathFootprintsAndObjects(
         vehicle_footprint_, arc_path, shoulder_lane_objects, parameters_.collision_check_margin)) {
     return {};

planning/behavior_path_planner/src/scene_module/pull_out/shift_pull_out.cpp

@@ -56,8 +56,8 @@ boost::optional<PullOutPath> ShiftPullOut::plan(Pose start_pose, Pose goal_pose)
   }
 
   // extract objects in shoulder lane for collision check
-  const auto shoulder_lane_objects =
-    util::filterObjectsByLanelets(*dynamic_objects, shoulder_lanes);
+  const auto [shoulder_lane_objects, others] =
+    util::separateObjectsByLanelets(*dynamic_objects, shoulder_lanes);
 
   // get safe path
   for (auto & pull_out_path : pull_out_paths) {

planning/behavior_path_planner/src/scene_module/pull_over/goal_searcher.cpp

@@ -62,8 +62,8 @@ GoalCandidates GoalSearcher::search(const Pose & original_goal_pose)
     route_handler->getCenterLinePath(pull_over_lanes, s_start, s_end),
     parameters_.goal_search_interval);
 
-  const auto shoulder_lane_objects =
-    util::filterObjectsByLanelets(*(planner_data_->dynamic_object), pull_over_lanes);
+  const auto [shoulder_lane_objects, others] =
+    util::separateObjectsByLanelets(*(planner_data_->dynamic_object), pull_over_lanes);
 
   std::vector<Pose> original_search_poses{};  // for search area visualizing
   size_t goal_id = 0;

planning/behavior_path_planner/src/utilities.cpp

@@ -598,16 +598,79 @@ std::vector<size_t> filterObjectIndicesByLanelets(
   return indices;
 }
 
-PredictedObjects filterObjectsByLanelets(
+std::pair<std::vector<size_t>, std::vector<size_t>> separateObjectIndicesByLanelets(
   const PredictedObjects & objects, const lanelet::ConstLanelets & target_lanelets)
 {
-  PredictedObjects filtered_objects;
-  const auto indices = filterObjectIndicesByLanelets(objects, target_lanelets);
-  filtered_objects.objects.reserve(indices.size());
-  for (const size_t i : indices) {
-    filtered_objects.objects.push_back(objects.objects.at(i));
+  if (target_lanelets.empty()) {
+    return {};
+  }
+
+  std::vector<size_t> target_indices;
+  std::vector<size_t> other_indices;
+
+  for (size_t i = 0; i < objects.objects.size(); i++) {
+    // create object polygon
+    const auto & obj = objects.objects.at(i);
+    // create object polygon
+    Polygon2d obj_polygon;
+    if (!util::calcObjectPolygon(obj, &obj_polygon)) {
+      RCLCPP_ERROR_STREAM(
+        rclcpp::get_logger("behavior_path_planner").get_child("utilities"),
+        "Failed to calcObjectPolygon...!!!");
+      continue;
+    }
+
+    bool is_filtered_object = false;
+
+    for (const auto & llt : target_lanelets) {
+      // create lanelet polygon
+      const auto polygon2d = llt.polygon2d().basicPolygon();
+      if (polygon2d.empty()) {
+        // no lanelet polygon
+        continue;
+      }
+      Polygon2d lanelet_polygon;
+      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)) {
+        target_indices.push_back(i);
+        is_filtered_object = true;
+        break;
+      }
+    }
+
+    if (!is_filtered_object) {
+      other_indices.push_back(i);
+    }
   }
-  return filtered_objects;
+
+  return std::make_pair(target_indices, other_indices);
+}
+
+std::pair<PredictedObjects, PredictedObjects> separateObjectsByLanelets(
+  const PredictedObjects & objects, const lanelet::ConstLanelets & target_lanelets)
+{
+  PredictedObjects target_objects;
+  PredictedObjects other_objects;
+
+  const auto [target_indices, other_indices] =
+    separateObjectIndicesByLanelets(objects, target_lanelets);
+
+  target_objects.objects.reserve(target_indices.size());
+  other_objects.objects.reserve(other_indices.size());
+
+  for (const size_t i : target_indices) {
+    target_objects.objects.push_back(objects.objects.at(i));
+  }
+
+  for (const size_t i : other_indices) {
+    other_objects.objects.push_back(objects.objects.at(i));
+  }
+
+  return std::make_pair(target_objects, other_objects);
 }
 
 bool calcObjectPolygon(const PredictedObject & object, Polygon2d * object_polygon)