feat(behavior_velocity): add partition lanelet

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

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

@@ -61,6 +61,7 @@ using lanelet::LaneletMapPtr;
 using lanelet::geometry::fromArcCoordinates;
 using lanelet::geometry::toArcCoordinates;
 using DetectionAreaIdx = boost::optional<std::pair<double, double>>;
+using BasicPolygons2d = std::vector<lanelet::BasicPolygon2d>;
 
 namespace occlusion_spot_utils
 {
@@ -177,6 +178,27 @@ struct PossibleCollisionInfo
   }
 };
 
+struct DebugData
+{
+  double z;
+  std::string road_type = "";
+  std::string detection_type = "";
+  std::vector<lanelet::BasicPolygon2d> detection_areas;
+  std::vector<lanelet::BasicPolygon2d> partition_lanelets;
+  std::vector<geometry_msgs::msg::Point> parked_vehicle_point;
+  std::vector<PossibleCollisionInfo> possible_collisions;
+  std::vector<geometry_msgs::msg::Point> occlusion_points;
+  PathWithLaneId path_raw;
+  PathWithLaneId interp_path;
+  void resetData()
+  {
+    detection_areas.clear();
+    parked_vehicle_point.clear();
+    possible_collisions.clear();
+    occlusion_points.clear();
+  }
+};
+
 lanelet::ConstLanelet toPathLanelet(const PathWithLaneId & path);
 // Note : consider offset_from_start_to_ego and safety margin for collision here
 void handleCollisionOffset(std::vector<PossibleCollisionInfo> & possible_collisions, double offset);

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

@@ -40,19 +40,9 @@ namespace behavior_velocity_planner
 class OcclusionSpotModule : public SceneModuleInterface
 {
   using PlannerParam = occlusion_spot_utils::PlannerParam;
+  using DebugData = occlusion_spot_utils::DebugData;
 
 public:
-  struct DebugData
-  {
-    double z;
-    std::string road_type = "occupancy";
-    std::vector<lanelet::BasicPolygon2d> detection_areas;
-    std::vector<occlusion_spot_utils::PossibleCollisionInfo> possible_collisions;
-    std::vector<geometry_msgs::msg::Point> occlusion_points;
-    PathWithLaneId path_raw;
-    PathWithLaneId interp_path;
-  };
-
   OcclusionSpotModule(
     const int64_t module_id, std::shared_ptr<const PlannerData> planner_data,
     const PlannerParam & planner_param, const rclcpp::Logger logger,

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

@@ -39,20 +39,9 @@ namespace behavior_velocity_planner
 class OcclusionSpotInPublicModule : public SceneModuleInterface
 {
   using PlannerParam = occlusion_spot_utils::PlannerParam;
+  using DebugData = occlusion_spot_utils::DebugData;
 
 public:
-  struct DebugData
-  {
-    std::string road_type = "object";
-    std::vector<lanelet::BasicPolygon2d> detection_areas;
-    std::vector<geometry_msgs::msg::Point> parked_vehicle_point;
-    std::vector<occlusion_spot_utils::PossibleCollisionInfo> possible_collisions;
-    std::vector<geometry_msgs::msg::Point> occlusion_points;
-    PathWithLaneId path_raw;
-    PathWithLaneId interp_path;
-    double z;
-  };
-
   OcclusionSpotInPublicModule(
     const int64_t module_id, std::shared_ptr<const PlannerData> planner_data,
     const PlannerParam & planner_param, const rclcpp::Logger logger,

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

@@ -28,6 +28,7 @@ namespace behavior_velocity_planner
 namespace
 {
 using builtin_interfaces::msg::Time;
+using BasicPolygons = std::vector<lanelet::BasicPolygon2d>;
 
 visualization_msgs::msg::Marker makeArrowMarker(
   const occlusion_spot_utils::PossibleCollisionInfo & possible_collision, const int id)
@@ -51,7 +52,7 @@ visualization_msgs::msg::Marker makeArrowMarker(
 
 std::vector<visualization_msgs::msg::Marker> makeSlowDownMarkers(
   const occlusion_spot_utils::PossibleCollisionInfo & possible_collision,
-  const std::string road_type, const int id)
+  const std::string detection_type, const int id)
 {
   // virtual wall
   std::vector<visualization_msgs::msg::Marker> debug_markers;
@@ -86,7 +87,7 @@ std::vector<visualization_msgs::msg::Marker> makeSlowDownMarkers(
   debug_markers.emplace_back(slowdown_reason_marker);
   slowdown_reason_marker.scale = tier4_autoware_utils::createMarkerScale(0.0, 0.0, 0.5);
   slowdown_reason_marker.id = id + 100;
-  slowdown_reason_marker.text = "\n \n" + road_type;
+  slowdown_reason_marker.text = "\n \n" + detection_type;
   debug_markers.emplace_back(slowdown_reason_marker);
   debug_markers.push_back(makeArrowMarker(possible_collision, id));
   return debug_markers;
@@ -142,7 +143,7 @@ std::vector<visualization_msgs::msg::Marker> makeCollisionMarkers(
 }
 
 visualization_msgs::msg::MarkerArray makePolygonMarker(
-  const std::vector<lanelet::BasicPolygon2d> & polygons, const int id, const double z)
+  const BasicPolygons & polygons, const std::string ns, const int id, const double z)
 {
   visualization_msgs::msg::MarkerArray debug_markers;
   visualization_msgs::msg::Marker debug_marker;
@@ -156,7 +157,7 @@ visualization_msgs::msg::MarkerArray makePolygonMarker(
   debug_marker.scale = tier4_autoware_utils::createMarkerScale(0.1, 0.1, 0.1);
   debug_marker.color = tier4_autoware_utils::createMarkerColor(1.0, 0.0, 1.0, 0.3);
   debug_marker.lifetime = rclcpp::Duration::from_seconds(0.1);
-  debug_marker.ns = "detection_area";
+  debug_marker.ns = ns;
   for (const auto & poly : polygons) {
     for (const auto & p : poly) {
       geometry_msgs::msg::Point point =
@@ -211,7 +212,7 @@ visualization_msgs::msg::MarkerArray createPossibleCollisionMarkers(
   int id = planning_utils::bitShift(module_id_);
   for (const auto & possible_collision : possible_collisions) {
     std::vector<visualization_msgs::msg::Marker> collision_markers =
-      makeSlowDownMarkers(possible_collision, debug_data.road_type, id++);
+      makeSlowDownMarkers(possible_collision, debug_data.detection_type, id++);
     occlusion_spot_slowdown_markers.markers.insert(
       occlusion_spot_slowdown_markers.markers.end(), collision_markers.begin(),
       collision_markers.end());
@@ -263,8 +264,8 @@ visualization_msgs::msg::MarkerArray OcclusionSpotInPublicModule::createDebugMar
   }
   if (!debug_data_.detection_areas.empty()) {
     appendMarkerArray(
-      makePolygonMarker(debug_data_.detection_areas, module_id_, debug_data_.z), current_time,
-      &debug_marker_array);
+      makePolygonMarker(debug_data_.detection_areas, "detection_area", module_id_, debug_data_.z),
+      current_time, &debug_marker_array);
   }
 
   return debug_marker_array;
@@ -280,8 +281,13 @@ visualization_msgs::msg::MarkerArray OcclusionSpotModule::createDebugMarkerArray
   }
   if (!debug_data_.detection_areas.empty()) {
     appendMarkerArray(
-      makePolygonMarker(debug_data_.detection_areas, module_id_, debug_data_.z), current_time,
-      &debug_marker_array);
+      makePolygonMarker(debug_data_.detection_areas, "detection_area", module_id_, debug_data_.z),
+      current_time, &debug_marker_array);
+  }
+  if (!debug_data_.partition_lanelets.empty()) {
+    appendMarkerArray(
+      makePolygonMarker(debug_data_.partition_lanelets, "partition", module_id_, debug_data_.z),
+      current_time, &debug_marker_array);
   }
   if (!debug_data_.interp_path.points.empty()) {
     appendMarkerArray(

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

@@ -419,6 +419,16 @@ void createPossibleCollisionsInDetectionArea(
   }
 }
 
+bool isBlockedByPartition(const LineString2d & direction, const BasicPolygons2d & partitions)
+{
+  for (const auto & p : partitions) {
+    if (bg::intersects(direction, p)) {
+      return true;
+    }
+  }
+  return false;
+}
+
 boost::optional<PossibleCollisionInfo> generateOneNotableCollisionFromOcclusionSpot(
   const grid_map::GridMap & grid, const std::vector<grid_map::Position> & occlusion_spot_positions,
   const double offset_from_start_to_ego, const BasicPoint2d base_point,

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

@@ -12,11 +12,13 @@
 // See the License for the specific language governing permissions and
 // limitations under the License.
 
+#include <lanelet2_extension/utility/query.hpp>
 #include <lanelet2_extension/utility/utilities.hpp>
 #include <scene_module/occlusion_spot/geometry.hpp>
 #include <scene_module/occlusion_spot/occlusion_spot_utils.hpp>
 #include <scene_module/occlusion_spot/risk_predictive_braking.hpp>
 #include <scene_module/occlusion_spot/scene_occlusion_spot.hpp>
+#include <utilization/boost_geometry_helper.hpp>
 #include <utilization/path_utilization.hpp>
 #include <utilization/util.hpp>
 
@@ -39,13 +41,23 @@ OcclusionSpotModule::OcclusionSpotModule(
 : SceneModuleInterface(module_id, logger, clock), publisher_(publisher)
 {
   param_ = planner_param;
+  debug_data_.detection_type = "occupancy";
+  const lanelet::LaneletMapConstPtr & ll = planner_data->route_handler_->getLaneletMapPtr();
+  const lanelet::ConstLineStrings3d partitions = lanelet::utils::query::getAllPartitions(ll);
+  for (const auto & partition : partitions) {
+    lanelet::BasicLineString2d line;
+    for (const auto & p : partition) {
+      line.emplace_back(lanelet::BasicPoint2d{p.x(), p.y()});
+    }
+    debug_data_.partition_lanelets.emplace_back(lanelet::BasicPolygon2d(line));
+  }
 }
 
 bool OcclusionSpotModule::modifyPathVelocity(
   autoware_auto_planning_msgs::msg::PathWithLaneId * path,
   [[maybe_unused]] tier4_planning_msgs::msg::StopReason * stop_reason)
 {
-  debug_data_ = DebugData();
+  debug_data_.resetData();
   if (path->points.size() < 2) {
     return true;
   }

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

@@ -39,13 +39,14 @@ OcclusionSpotInPublicModule::OcclusionSpotInPublicModule(
 : SceneModuleInterface(module_id, logger, clock)
 {
   param_ = planner_param;
+  debug_data_.detection_type = "object";
 }
 
 bool OcclusionSpotInPublicModule::modifyPathVelocity(
   autoware_auto_planning_msgs::msg::PathWithLaneId * path,
   [[maybe_unused]] tier4_planning_msgs::msg::StopReason * stop_reason)
 {
-  debug_data_ = DebugData();
+  debug_data_.resetData();
   if (path->points.size() < 2) {
     return true;
   }