chore(behavior_velocity): unite variables

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

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

@@ -185,7 +185,7 @@ struct DebugData
   double z;
   std::string road_type = "";
   std::string detection_type = "";
-  std::vector<lanelet::BasicPolygon2d> detection_areas;
+  std::vector<Slice> detection_area_polygons;
   std::vector<lanelet::BasicPolygon2d> partition_lanelets;
   std::vector<geometry_msgs::msg::Point> parked_vehicle_point;
   std::vector<PossibleCollisionInfo> possible_collisions;
@@ -194,7 +194,7 @@ struct DebugData
   PathWithLaneId interp_path;
   void resetData()
   {
-    detection_areas.clear();
+    detection_area_polygons.clear();
     parked_vehicle_point.clear();
     possible_collisions.clear();
     occlusion_points.clear();
@@ -210,7 +210,7 @@ bool isStuckVehicle(PredictedObject obj, const double min_vel);
 double offsetFromStartToEgo(
   const PathWithLaneId & path, const Pose & ego_pose, const int closest_idx);
 std::vector<PredictedObject> filterDynamicObjectByDetectionArea(
-  std::vector<PredictedObject> & objs, const std::vector<Slice> polys);
+  std::vector<PredictedObject> & objs, const std::vector<Slice> & polys);
 std::vector<PredictedObject> getParkedVehicles(
   const PredictedObjects & dyn_objects, const PlannerParam & param,
   std::vector<Point> & debug_point);
@@ -240,10 +240,9 @@ boost::optional<PossibleCollisionInfo> generateOneNotableCollisionFromOcclusionS
   const lanelet::ConstLanelet & path_lanelet, const PlannerParam & param);
 //!< @brief generate possible collisions coming from occlusion spots on the side of the path
 void createPossibleCollisionsInDetectionArea(
-  const std::vector<Slice> & detection_area_polygons,
   std::vector<PossibleCollisionInfo> & possible_collisions, const grid_map::GridMap & grid,
   const PathWithLaneId & path, const double offset_from_start_to_ego, const PlannerParam & param,
-  std::vector<Point> & debug_points);
+  DebugData & debug_data);
 
 }  // namespace occlusion_spot_utils
 }  // namespace behavior_velocity_planner

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

@@ -29,6 +29,7 @@ namespace
 {
 using builtin_interfaces::msg::Time;
 using BasicPolygons = std::vector<lanelet::BasicPolygon2d>;
+using Slices = std::vector<occlusion_spot_utils::Slice>;
 
 visualization_msgs::msg::Marker makeArrowMarker(
   const occlusion_spot_utils::PossibleCollisionInfo & possible_collision, const int id)
@@ -155,7 +156,7 @@ visualization_msgs::msg::MarkerArray makePolygonMarker(
   debug_marker.pose.position = tier4_autoware_utils::createMarkerPosition(0.0, 0.0, z);
   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, 0.0, 1.0, 0.3);
+  debug_marker.color = tier4_autoware_utils::createMarkerColor(1.0, 0.5, 1.0, 0.3);
   debug_marker.lifetime = rclcpp::Duration::from_seconds(0.1);
   debug_marker.ns = ns;
   for (const auto & poly : polygons) {
@@ -171,6 +172,35 @@ visualization_msgs::msg::MarkerArray makePolygonMarker(
   return debug_markers;
 }
 
+visualization_msgs::msg::MarkerArray makeSlicePolygonMarker(
+  const Slices & slices, const std::string ns, const int id, const double z)
+{
+  visualization_msgs::msg::MarkerArray debug_markers;
+  visualization_msgs::msg::Marker debug_marker;
+  debug_marker.header.frame_id = "map";
+  debug_marker.header.stamp = rclcpp::Time(0);
+  debug_marker.id = planning_utils::bitShift(id);
+  debug_marker.type = visualization_msgs::msg::Marker::LINE_STRIP;
+  debug_marker.action = visualization_msgs::msg::Marker::ADD;
+  debug_marker.pose.position = tier4_autoware_utils::createMarkerPosition(0.0, 0.0, z);
+  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, 0.0, 1.0, 0.3);
+  debug_marker.lifetime = rclcpp::Duration::from_seconds(0.1);
+  debug_marker.ns = ns;
+  for (const auto & slice : slices) {
+    for (const auto & p : slice.polygon) {
+      geometry_msgs::msg::Point point =
+        tier4_autoware_utils::createMarkerPosition(p.x(), p.y(), 0.0);
+      debug_marker.points.push_back(point);
+    }
+    debug_markers.markers.push_back(debug_marker);
+    debug_marker.id++;
+    debug_marker.points.clear();
+  }
+  return debug_markers;
+}
+
 visualization_msgs::msg::MarkerArray createPathMarkerArray(
   const PathWithLaneId & path, const std::string & ns, const int64_t lane_id, const double r,
   const double g, const double b)
@@ -262,9 +292,10 @@ visualization_msgs::msg::MarkerArray OcclusionSpotInPublicModule::createDebugMar
     appendMarkerArray(
       createPossibleCollisionMarkers(debug_data_, module_id_), current_time, &debug_marker_array);
   }
-  if (!debug_data_.detection_areas.empty()) {
+  if (!debug_data_.detection_area_polygons.empty()) {
     appendMarkerArray(
-      makePolygonMarker(debug_data_.detection_areas, "detection_area", module_id_, debug_data_.z),
+      makeSlicePolygonMarker(
+        debug_data_.detection_area_polygons, "detection_area", module_id_, debug_data_.z),
       current_time, &debug_marker_array);
   }
 
@@ -279,9 +310,10 @@ visualization_msgs::msg::MarkerArray OcclusionSpotModule::createDebugMarkerArray
     appendMarkerArray(
       createPossibleCollisionMarkers(debug_data_, module_id_), current_time, &debug_marker_array);
   }
-  if (!debug_data_.detection_areas.empty()) {
+  if (!debug_data_.detection_area_polygons.empty()) {
     appendMarkerArray(
-      makePolygonMarker(debug_data_.detection_areas, "detection_area", module_id_, debug_data_.z),
+      makeSlicePolygonMarker(
+        debug_data_.detection_area_polygons, "detection_area", module_id_, debug_data_.z),
       current_time, &debug_marker_array);
   }
   if (!debug_data_.partition_lanelets.empty()) {

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

@@ -338,7 +338,7 @@ std::vector<PossibleCollisionInfo> generatePossibleCollisionBehindParkedVehicle(
 }
 
 std::vector<PredictedObject> filterDynamicObjectByDetectionArea(
-  std::vector<PredictedObject> & objs, const std::vector<Slice> polys)
+  std::vector<PredictedObject> & objs, const std::vector<Slice> & polys)
 {
   std::vector<PredictedObject> filtered_obj;
   // stuck points by predicted objects
@@ -355,26 +355,26 @@ std::vector<PredictedObject> filterDynamicObjectByDetectionArea(
 }
 
 void createPossibleCollisionsInDetectionArea(
-  const std::vector<Slice> & detection_area_polygons,
   std::vector<PossibleCollisionInfo> & possible_collisions, const grid_map::GridMap & grid,
   const PathWithLaneId & path, const double offset_from_start_to_ego, const PlannerParam & param,
-  std::vector<Point> & debug_points)
+  DebugData & debug_data)
 {
   lanelet::ConstLanelet path_lanelet = toPathLanelet(path);
   if (path_lanelet.centerline2d().empty()) {
     return;
   }
   double distance_lower_bound = std::numeric_limits<double>::max();
-  for (const Slice & detection_area_slice : detection_area_polygons) {
+  for (const Slice & detection_area_slice : debug_data.detection_area_polygons) {
     std::vector<grid_map::Position> occlusion_spot_positions;
     grid_utils::findOcclusionSpots(
       occlusion_spot_positions, grid, detection_area_slice.polygon,
       param.detection_area.min_occlusion_spot_size);
-    Point p;
-    for (const auto & op : occlusion_spot_positions) {
-      p.x = op[0];
-      p.y = op[1];
-      debug_points.emplace_back(p);
+    if (param.debug) {
+      for (const auto & op : occlusion_spot_positions) {
+        Point p =
+          tier4_autoware_utils::createPoint(op[0], op[1], path.points.at(0).point.pose.position.z);
+        debug_data.occlusion_points.emplace_back(p);
+      }
     }
     if (occlusion_spot_positions.empty()) continue;
     // for each partition find nearest occlusion spot from polygon's origin
@@ -409,7 +409,7 @@ boost::optional<PossibleCollisionInfo> generateOneNotableCollisionFromOcclusionS
   double distance_lower_bound = std::numeric_limits<double>::max();
   PossibleCollisionInfo candidate;
   bool has_collision = false;
-  for (grid_map::Position occlusion_spot_position : occlusion_spot_positions) {
+  for (const grid_map::Position & occlusion_spot_position : occlusion_spot_positions) {
     // arc intersection
     lanelet::BasicPoint2d obstacle_point = {occlusion_spot_position[0], occlusion_spot_position[1]};
     lanelet::ArcCoordinates arc_coord_occlusion_point =

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

@@ -90,16 +90,13 @@ bool OcclusionSpotModule::modifyPathVelocity(
   grid_map::GridMap grid_map;
   grid_utils::denoiseOccupancyGridCV(occ_grid, grid_map, param_.grid);
   double offset_from_start_to_ego = utils::offsetFromStartToEgo(interp_path, ego_pose, closest_idx);
-  using Slice = occlusion_spot_utils::Slice;
-  std::vector<Slice> detection_area_polygons;
+  auto & detection_area_polygons = debug_data_.detection_area_polygons;
   utils::buildDetectionAreaPolygon(
     detection_area_polygons, interp_path, offset_from_start_to_ego, param_);
   std::vector<utils::PossibleCollisionInfo> possible_collisions;
   // Note: Don't consider offset from path start to ego here
   utils::createPossibleCollisionsInDetectionArea(
-    detection_area_polygons, possible_collisions, grid_map, interp_path, offset_from_start_to_ego,
-    param_, debug_data_.occlusion_points);
-  if (detection_area_polygons.empty()) return true;
+    possible_collisions, grid_map, interp_path, offset_from_start_to_ego, param_, debug_data_);
   RCLCPP_DEBUG_STREAM_THROTTLE(
     logger_, *clock_, 3000, "num possible collision:" << possible_collisions.size());
   utils::calcSlowDownPointsForPossibleCollision(0, interp_path, 0.0, possible_collisions);
@@ -110,11 +107,6 @@ bool OcclusionSpotModule::modifyPathVelocity(
   // these debug topics needs computation resource
   if (param_.debug) {
     publisher_->publish(occ_grid);
-    for (const auto & p : detection_area_polygons) {
-      debug_data_.detection_areas.emplace_back(p.polygon);
-    }
-  } else {
-    debug_data_.occlusion_points.clear();
   }
   debug_data_.z = path->points.front().point.pose.position.z;
   debug_data_.possible_collisions = possible_collisions;

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

@@ -84,8 +84,7 @@ bool OcclusionSpotInPublicModule::modifyPathVelocity(
   std::vector<PredictedObject> obj =
     utils::getParkedVehicles(*dynamic_obj_arr_ptr, param_, debug_data_.parked_vehicle_point);
   double offset_from_start_to_ego = utils::offsetFromStartToEgo(interp_path, ego_pose, closest_idx);
-  using Slice = occlusion_spot_utils::Slice;
-  std::vector<Slice> detection_area_polygons;
+  auto & detection_area_polygons = debug_data_.detection_area_polygons;
   utils::buildDetectionAreaPolygon(
     detection_area_polygons, interp_path, offset_from_start_to_ego, param_);
   const auto filtered_obj = utils::filterDynamicObjectByDetectionArea(obj, detection_area_polygons);
@@ -98,11 +97,6 @@ bool OcclusionSpotInPublicModule::modifyPathVelocity(
   utils::handleCollisionOffset(possible_collisions, offset_from_start_to_ego);
   // apply safe velocity using ebs and pbs deceleration
   utils::applySafeVelocityConsideringPossibleCollision(path, possible_collisions, param_);
-  if (param_.debug) {
-    for (const auto & p : detection_area_polygons) {
-      debug_data_.detection_areas.emplace_back(p.polygon);
-    }
-  }
   debug_data_.possible_collisions = possible_collisions;
   return true;
 }