feat(behavior_velocity): add extract close partition

Signed-off-by: tanaka3 <ttatcoder@outlook.jp>

planning/behavior_velocity_planner/include/scene_module/occlusion_spot/occlusion_spot_utils.hpp

@@ -188,6 +188,7 @@ struct DebugData
   std::string road_type = "";
   std::string detection_type = "";
   Polygons2d detection_area_polygons;
+  std::vector<lanelet::BasicPolygon2d> close_partition;
   std::vector<lanelet::BasicPolygon2d> partition_lanelets;
   std::vector<geometry_msgs::msg::Point> parked_vehicle_point;
   std::vector<PossibleCollisionInfo> possible_collisions;

planning/behavior_velocity_planner/include/utilization/util.hpp

@@ -38,6 +38,8 @@
 #include <boost/geometry/geometries/point_xy.hpp>
 
 #include <lanelet2_core/LaneletMap.h>
+#include <lanelet2_core/geometry/Lanelet.h>
+#include <lanelet2_core/geometry/Point.h>
 #include <lanelet2_routing/RoutingGraph.h>
 #include <pcl/point_types.h>
 #include <tf2/utils.h>
@@ -131,7 +133,9 @@ bool createDetectionAreaPolygons(
   const double obstacle_vel_mps);
 PathPoint getLerpPathPointWithLaneId(const PathPoint p0, const PathPoint p1, const double ratio);
 Point2d calculateLateralOffsetPoint2d(const Pose & p, const double offset);
-
+void extractClosePartition(
+  const geometry_msgs::msg::Point position, const BasicPolygons2d & all_partitions,
+  BasicPolygons2d & close_partition, const double distance_thresh = 30.0);
 void getAllPartitionLanelets(const lanelet::LaneletMapConstPtr ll, BasicPolygons2d & polys);
 void setVelocityFrom(const size_t idx, const double vel, PathWithLaneId * input);
 void insertVelocity(

planning/behavior_velocity_planner/src/scene_module/occlusion_spot/debug.cpp

@@ -155,12 +155,12 @@ visualization_msgs::msg::MarkerArray makePolygonMarker(
   debug_marker.pose.orientation = tier4_autoware_utils::createMarkerOrientation(0, 0, 0, 1.0);
   debug_marker.scale = tier4_autoware_utils::createMarkerScale(0.1, 0.1, 0.1);
   debug_marker.color = tier4_autoware_utils::createMarkerColor(1.0, 1.0, 1.0, 0.5);
-  debug_marker.lifetime = rclcpp::Duration::from_seconds(0.1);
+  debug_marker.lifetime = rclcpp::Duration::from_seconds(0.5);
   debug_marker.ns = ns;
   for (const auto & poly : polygons) {
     for (const auto & p : poly) {
       geometry_msgs::msg::Point point =
-        tier4_autoware_utils::createMarkerPosition(p.x(), p.y(), 0.0);
+        tier4_autoware_utils::createMarkerPosition(p.x(), p.y(), z + 1.0);
       debug_marker.points.push_back(point);
     }
     debug_markers.markers.push_back(debug_marker);
@@ -298,7 +298,7 @@ visualization_msgs::msg::MarkerArray OcclusionSpotModule::createDebugMarkerArray
   }
   if (!debug_data_.partition_lanelets.empty()) {
     appendMarkerArray(
-      makePolygonMarker(debug_data_.partition_lanelets, "partition", module_id_, debug_data_.z),
+      makePolygonMarker(debug_data_.close_partition, "partition", module_id_, debug_data_.z),
       current_time, &debug_marker_array);
   }
   if (!debug_data_.interp_path.points.empty()) {

planning/behavior_velocity_planner/src/scene_module/occlusion_spot/occlusion_spot_utils.cpp

@@ -429,7 +429,7 @@ boost::optional<PossibleCollisionInfo> generateOneNotableCollisionFromOcclusionS
   double distance_lower_bound = std::numeric_limits<double>::max();
   PossibleCollisionInfo candidate;
   bool has_collision = false;
-  const auto & partition_lanelets = debug_data.partition_lanelets;
+  const auto & partition_lanelets = debug_data.close_partition;
   for (const grid_map::Position & occlusion_spot_position : occlusion_spot_positions) {
     // arc intersection
     const lanelet::BasicPoint2d obstacle_point = {

planning/behavior_velocity_planner/src/scene_module/occlusion_spot/scene_occlusion_spot.cpp

@@ -99,6 +99,11 @@ bool OcclusionSpotModule::modifyPathVelocity(
     return true;  // path point is not enough
   }
   std::vector<utils::PossibleCollisionInfo> possible_collisions;
+  // extract only close lanelet
+  if (param_.use_partition_lanelet) {
+    extractClosePartition(
+      ego_pose.position, debug_data_.partition_lanelets, debug_data_.close_partition);
+  }
   if (param_.is_show_processing_time) stop_watch_.tic("processing_time");
   if (param_.detection_method == utils::DETECTION_METHOD::OCCUPANCY_GRID) {
     const auto & occ_grid_ptr = planner_data_->occupancy_grid;

planning/behavior_velocity_planner/src/utilization/util.cpp

@@ -142,14 +142,28 @@ bool createDetectionAreaPolygons(
   return true;
 }
 
+void extractClosePartition(
+  const geometry_msgs::msg::Point position, const BasicPolygons2d & all_partitions,
+  BasicPolygons2d & close_partition, const double distance_thresh)
+{
+  for (const auto & p : all_partitions) {
+    if (boost::geometry::distance(Point2d(position.x, position.y), p) < distance_thresh) {
+      close_partition.emplace_back(p);
+    }
+  }
+  return;
+}
+
 void getAllPartitionLanelets(const lanelet::LaneletMapConstPtr ll, BasicPolygons2d & polys)
 {
   const lanelet::ConstLineStrings3d partitions = lanelet::utils::query::getAllPartitions(ll);
   for (const auto & partition : partitions) {
     lanelet::BasicLineString2d line;
-    for (const auto & p : partition) {
+    for (const auto p : partition) {
       line.emplace_back(lanelet::BasicPoint2d{p.x(), p.y()});
     }
+    // corect line to calculate distance accuratry
+    boost::geometry::correct(line);
     polys.emplace_back(lanelet::BasicPolygon2d(line));
   }
 }