refactor(behavior_velocity): occlusion spot (#601)

* refactor(behavior_velocity): refactor codes Signed-off-by: tanaka3 <[email protected]> * refactor(behavior_velocity): fix typo Signed-off-by: tanaka3 <[email protected]> * refactor(behavior_velocity): refactor debuggings Signed-off-by: tanaka3 <[email protected]> * refactor(behavior_velocity): unite to generate Signed-off-by: tanaka3 <[email protected]> * refactor(behavior_velocity): apply clang tidy Signed-off-by: tanaka3 <[email protected]> * chore(behavior_velocity): retun to default settting Signed-off-by: tanaka3 <[email protected]>
autowarefoundation · Mar 29, 2022 · ca59048 · ca59048
1 parent a00dc18
commit ca59048
Show file tree

Hide file tree

Showing 6 changed files with 61 additions and 69 deletions.
diff --git a/...ng/behavior_velocity_planner/include/scene_module/occlusion_spot/occlusion_spot_utils.hpp b/...ng/behavior_velocity_planner/include/scene_module/occlusion_spot/occlusion_spot_utils.hpp
@@ -189,14 +189,14 @@ struct DebugData
   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;
   std::vector<geometry_msgs::msg::Point> occlusion_points;
   PathWithLaneId path_raw;
   PathWithLaneId interp_path;
   void resetData()
   {
+    close_partition.clear();
     detection_area_polygons.clear();
     parked_vehicle_point.clear();
     possible_collisions.clear();
@@ -220,7 +220,7 @@ std::vector<PredictedObject> filterDynamicObjectByDetectionArea(
 std::vector<PredictedObject> getParkedVehicles(
   const PredictedObjects & dyn_objects, const PlannerParam & param,
   std::vector<Point> & debug_point);
-bool generatePossibleCollisionBehindParkedVehicle(
+bool generatePossibleCollisionsFromObjects(
   std::vector<PossibleCollisionInfo> & possible_collisions, const PathWithLaneId & path,
   const PlannerParam & param, const double offset_from_start_to_ego,
   const std::vector<PredictedObject> & dyn_objects);
@@ -246,7 +246,7 @@ boost::optional<PossibleCollisionInfo> generateOneNotableCollisionFromOcclusionS
   const double offset_from_start_to_ego, const Point2d base_point,
   const lanelet::ConstLanelet & path_lanelet, const PlannerParam & param, DebugData & debug_data);
 //!< @brief generate possible collisions coming from occlusion spots on the side of the path
-bool createPossibleCollisionsInDetectionArea(
+bool generatePossibleCollisionsFromGridMap(
   std::vector<PossibleCollisionInfo> & possible_collisions, const grid_map::GridMap & grid,
   const PathWithLaneId & path, const double offset_from_start_to_ego, const PlannerParam & param,
   DebugData & debug_data);

diff --git a/...ng/behavior_velocity_planner/include/scene_module/occlusion_spot/scene_occlusion_spot.hpp b/...ng/behavior_velocity_planner/include/scene_module/occlusion_spot/scene_occlusion_spot.hpp
@@ -61,6 +61,8 @@ class OcclusionSpotModule : public SceneModuleInterface
   // Parameter
   PlannerParam param_;
   tier4_autoware_utils::StopWatch<std::chrono::milliseconds> stop_watch_;
+  std::vector<lanelet::BasicPolygon2d> partition_lanelets_;
+  std::vector<lanelet::BasicPolygon2d> close_partition_;
 
 protected:
   int64_t module_id_{};

diff --git a/planning/behavior_velocity_planner/src/scene_module/occlusion_spot/debug.cpp b/planning/behavior_velocity_planner/src/scene_module/occlusion_spot/debug.cpp
@@ -295,9 +295,9 @@ visualization_msgs::msg::MarkerArray OcclusionSpotModule::createDebugMarkerArray
         debug_data_.detection_area_polygons, "detection_area", module_id_, debug_data_.z),
       current_time, &debug_marker_array);
   }
-  if (!debug_data_.partition_lanelets.empty()) {
+  if (!debug_data_.close_partition.empty()) {
     appendMarkerArray(
-      makePolygonMarker(debug_data_.close_partition, "partition", module_id_, debug_data_.z),
+      makePolygonMarker(debug_data_.close_partition, "close_partition", module_id_, debug_data_.z),
       current_time, &debug_marker_array);
   }
   if (!debug_data_.interp_path.points.empty()) {

diff --git a/planning/behavior_velocity_planner/src/scene_module/occlusion_spot/occlusion_spot_utils.cpp b/planning/behavior_velocity_planner/src/scene_module/occlusion_spot/occlusion_spot_utils.cpp
@@ -73,6 +73,7 @@ bool buildDetectionAreaPolygon(
     da_range.min_longitudinal_distance;
   da_range.min_lateral_distance = p.half_vehicle_width;
   da_range.max_lateral_distance = p.detection_area.max_lateral_distance;
+  slices.clear();
   return planning_utils::createDetectionAreaPolygons(slices, path, da_range, p.pedestrian_vel);
 }
 
@@ -275,7 +276,7 @@ PossibleCollisionInfo calculateCollisionPathPointFromOcclusionSpot(
   const ArcCoordinates & arc_coord_occlusion_with_offset,
   const lanelet::ConstLanelet & path_lanelet, const PlannerParam & param)
 {
-  auto setPose = [](const lanelet::ConstLanelet & pl, const ArcCoordinates & arc) {
+  auto calcPose = [](const lanelet::ConstLanelet & pl, const ArcCoordinates & arc) {
     const auto & ll = pl.centerline2d();
     BasicPoint2d bp = fromArcCoordinates(ll, arc);
     Pose pose;
@@ -306,15 +307,15 @@ PossibleCollisionInfo calculateCollisionPathPointFromOcclusionSpot(
   pc.arc_lane_dist_at_collision = {distance_to_stop, arc_coord_occlusion_with_offset.distance};
   pc.obstacle_info.safe_motion = sm;
   pc.obstacle_info.ttc = ttc;
-  pc.obstacle_info.position = setPose(path_lanelet, arc_coord_occlusion).position;
+  pc.obstacle_info.position = calcPose(path_lanelet, arc_coord_occlusion).position;
   pc.obstacle_info.max_velocity = param.pedestrian_vel;
-  pc.collision_pose = setPose(path_lanelet, {arc_coord_occlusion_with_offset.length, 0.0});
-  pc.collision_with_margin.pose = setPose(path_lanelet, {distance_to_stop, 0.0});
-  pc.intersection_pose = setPose(path_lanelet, {arc_coord_occlusion.length, 0.0});
+  pc.collision_pose = calcPose(path_lanelet, {arc_coord_occlusion_with_offset.length, 0.0});
+  pc.collision_with_margin.pose = calcPose(path_lanelet, {distance_to_stop, 0.0});
+  pc.intersection_pose = calcPose(path_lanelet, {arc_coord_occlusion.length, 0.0});
   return pc;
 }
 
-bool generatePossibleCollisionBehindParkedVehicle(
+bool generatePossibleCollisionsFromObjects(
   std::vector<PossibleCollisionInfo> & possible_collisions, const PathWithLaneId & path,
   const PlannerParam & param, const double offset_from_start_to_ego,
   const std::vector<PredictedObject> & dyn_objects)
@@ -339,14 +340,7 @@ bool generatePossibleCollisionBehindParkedVehicle(
       arc_coord_occlusion, arc_coord_occlusion_with_offset, path_lanelet, param);
     possible_collisions.emplace_back(pc);
   }
-  // sort by arc length
-  std::sort(
-    possible_collisions.begin(), possible_collisions.end(),
-    [](PossibleCollisionInfo pc1, PossibleCollisionInfo pc2) {
-      return pc1.arc_lane_dist_at_collision.length < pc2.arc_lane_dist_at_collision.length;
-    });
-  if (possible_collisions.empty()) return false;
-  return true;
+  return !possible_collisions.empty();
 }
 
 std::vector<PredictedObject> filterDynamicObjectByDetectionArea(
@@ -366,7 +360,7 @@ std::vector<PredictedObject> filterDynamicObjectByDetectionArea(
   return filtered_obj;
 }
 
-bool createPossibleCollisionsInDetectionArea(
+bool generatePossibleCollisionsFromGridMap(
   std::vector<PossibleCollisionInfo> & possible_collisions, const grid_map::GridMap & grid,
   const PathWithLaneId & path, const double offset_from_start_to_ego, const PlannerParam & param,
   DebugData & debug_data)
@@ -395,28 +389,21 @@ bool createPossibleCollisionsInDetectionArea(
     const auto pc = generateOneNotableCollisionFromOcclusionSpot(
       grid, occlusion_spot_positions, offset_from_start_to_ego, base_point, path_lanelet, param,
       debug_data);
-    if (!pc) continue;
+    if (pc == boost::none) continue;
     const double lateral_distance = std::abs(pc.get().arc_lane_dist_at_collision.distance);
     if (lateral_distance > distance_lower_bound) continue;
     distance_lower_bound = lateral_distance;
     possible_collisions.emplace_back(pc.get());
   }
-  // sort by arc length
-  std::sort(
-    possible_collisions.begin(), possible_collisions.end(),
-    [](PossibleCollisionInfo pc1, PossibleCollisionInfo pc2) {
-      return pc1.arc_lane_dist_at_collision.length < pc2.arc_lane_dist_at_collision.length;
-    });
-  if (possible_collisions.empty()) return false;
-  return true;
+  return !possible_collisions.empty();
 }
 
-bool isNotBlockedByPartition(const LineString2d & direction, const BasicPolygons2d & partitions)
+bool isBlockedByPartition(const LineString2d & direction, const BasicPolygons2d & partitions)
 {
   for (const auto & p : partitions) {
-    if (bg::intersects(direction, p)) return false;
+    if (bg::intersects(direction, p)) return true;
   }
-  return true;
+  return false;
 }
 
 boost::optional<PossibleCollisionInfo> generateOneNotableCollisionFromOcclusionSpot(
@@ -453,23 +440,20 @@ boost::optional<PossibleCollisionInfo> generateOneNotableCollisionFromOcclusionS
     const auto & ip = pc.intersection_pose.position;
     bool collision_free_at_intersection = grid_utils::isCollisionFree(
       grid, occlusion_spot_position, grid_map::Position(ip.x, ip.y), param.pedestrian_radius);
-    bool obstacle_not_blocked_by_partition = true;
+    bool is_obstacle_blocked_by_partition = false;
     if (!collision_free_at_intersection) continue;
     if (param.use_partition_lanelet) {
       const auto & op = obstacle_point;
       const LineString2d obstacle_vec = {{op[0], op[1]}, {ip.x, ip.y}};
-      obstacle_not_blocked_by_partition = isNotBlockedByPartition(obstacle_vec, partition_lanelets);
+      is_obstacle_blocked_by_partition = isBlockedByPartition(obstacle_vec, partition_lanelets);
     }
-    if (!obstacle_not_blocked_by_partition) continue;
+    if (is_obstacle_blocked_by_partition) continue;
     distance_lower_bound = dist;
     candidate = pc;
     has_collision = true;
   }
-  if (has_collision) {
-    return candidate;
-  } else {
-    return {};
-  }
+  if (has_collision) return candidate;
+  return boost::none;
 }
 
 }  // namespace occlusion_spot_utils

diff --git a/...ing/behavior_velocity_planner/src/scene_module/occlusion_spot/risk_predictive_braking.cpp b/...ing/behavior_velocity_planner/src/scene_module/occlusion_spot/risk_predictive_braking.cpp
@@ -27,8 +27,6 @@ void applySafeVelocityConsideringPossibleCollision(
   PathWithLaneId * inout_path, std::vector<PossibleCollisionInfo> & possible_collisions,
   const PlannerParam & param)
 {
-  const auto logger{rclcpp::get_logger("behavior_velocity_planner").get_child("occlusion_spot")};
-  rclcpp::Clock clock{RCL_ROS_TIME};
   // return nullptr or too few points
   if (!inout_path || inout_path->points.size() < 2) {
     return;

diff --git a/planning/behavior_velocity_planner/src/scene_module/occlusion_spot/scene_occlusion_spot.cpp b/planning/behavior_velocity_planner/src/scene_module/occlusion_spot/scene_occlusion_spot.cpp
@@ -29,8 +29,11 @@
 #include <vector>
 
 // turn on only when debugging.
-#define DEBUG_PRINT(enable, x) \
-  if (enable) RCLCPP_INFO_STREAM_THROTTLE(logger_, *clock_, 3000, x);
+#define DEBUG_PRINT(enable, n, x)                                  \
+  if (enable) {                                                    \
+    const std::string time_msg = n + std::to_string(x);            \
+    RCLCPP_INFO_STREAM_THROTTLE(logger_, *clock_, 3000, time_msg); \
+  }
 
 namespace behavior_velocity_planner
 {
@@ -45,20 +48,22 @@ OcclusionSpotModule::OcclusionSpotModule(
   if (param_.detection_method == utils::DETECTION_METHOD::OCCUPANCY_GRID) {
     debug_data_.detection_type = "occupancy";
     //! occupancy grid limitation( 100 * 100 )
-    param_.detection_area_length = std::min(50.0, param_.detection_area_length);
+    const double max_length = 35.0;  // current available length
+    param_.detection_area_length = std::min(max_length, param_.detection_area_length);
   } else if (param_.detection_method == utils::DETECTION_METHOD::PREDICTED_OBJECT) {
     debug_data_.detection_type = "object";
   }
   if (param_.use_partition_lanelet) {
     const lanelet::LaneletMapConstPtr & ll = planner_data->route_handler_->getLaneletMapPtr();
-    planning_utils::getAllPartitionLanelets(ll, debug_data_.partition_lanelets);
+    planning_utils::getAllPartitionLanelets(ll, partition_lanelets_);
   }
 }
 
 bool OcclusionSpotModule::modifyPathVelocity(
   autoware_auto_planning_msgs::msg::PathWithLaneId * path,
   [[maybe_unused]] tier4_planning_msgs::msg::StopReason * stop_reason)
 {
+  if (param_.is_show_processing_time) stop_watch_.tic("total_processing_time");
   debug_data_.resetData();
   if (path->points.size() < 2) {
     return true;
@@ -83,74 +88,77 @@ bool OcclusionSpotModule::modifyPathVelocity(
   PathWithLaneId interp_path;
   //! never change this interpolation interval(will affect module accuracy)
   splineInterpolate(clipped_path, 1.0, &interp_path, logger_);
-  PathWithLaneId predicted_path;
-  if (param_.pass_judge == utils::PASS_JUDGE::CURRENT_VELOCITY) {
-    predicted_path = utils::applyVelocityToPath(interp_path, param_.v.v_ego);
-  } else if (param_.pass_judge == utils::PASS_JUDGE::SMOOTH_VELOCITY) {
-  }
-  debug_data_.interp_path = interp_path;
   const geometry_msgs::msg::Point start_point = interp_path.points.at(0).point.pose.position;
   const auto offset = tier4_autoware_utils::calcSignedArcLength(
     interp_path.points, ego_pose, start_point, param_.dist_thr, param_.angle_thr);
   if (offset == boost::none) return true;
   const double offset_from_start_to_ego = -offset.get();
-  auto & detection_area_polygons = debug_data_.detection_area_polygons;
+  const bool show_time = param_.is_show_processing_time;
+  if (show_time) stop_watch_.tic("processing_time");
+  PathWithLaneId predicted_path;
+  if (param_.pass_judge == utils::PASS_JUDGE::CURRENT_VELOCITY) {
+    predicted_path = utils::applyVelocityToPath(interp_path, param_.v.v_ego);
+  } else if (param_.pass_judge == utils::PASS_JUDGE::SMOOTH_VELOCITY) {
+  }
+  DEBUG_PRINT(show_time, "apply velocity [ms]: ", stop_watch_.toc("processing_time", true));
   if (!utils::buildDetectionAreaPolygon(
-        detection_area_polygons, predicted_path, offset_from_start_to_ego, param_)) {
+        debug_data_.detection_area_polygons, predicted_path, offset_from_start_to_ego, param_)) {
     return true;  // path point is not enough
   }
+  DEBUG_PRINT(show_time, "generate poly[ms]: ", stop_watch_.toc("processing_time", true));
   std::vector<utils::PossibleCollisionInfo> possible_collisions;
   // extract only close lanelet
   if (param_.use_partition_lanelet) {
-    planning_utils::extractClosePartition(
-      ego_pose.position, debug_data_.partition_lanelets, debug_data_.close_partition);
+    planning_utils::extractClosePartition(ego_pose.position, partition_lanelets_, close_partition_);
   }
-  if (param_.is_show_processing_time) stop_watch_.tic("processing_time");
+  DEBUG_PRINT(show_time, "extract[ms]: ", stop_watch_.toc("processing_time", true));
+  std::vector<geometry_msgs::msg::Point> parked_vehicle_point;
   if (param_.detection_method == utils::DETECTION_METHOD::OCCUPANCY_GRID) {
     const auto & occ_grid_ptr = planner_data_->occupancy_grid;
-    if (!occ_grid_ptr) return true;  // mo data
+    if (!occ_grid_ptr) return true;  // no data
     grid_map::GridMap grid_map;
     grid_utils::denoiseOccupancyGridCV(
       occ_grid_ptr, grid_map, param_.grid, param_.is_show_cv_window, param_.filter_occupancy_grid);
     if (param_.use_object_info) {
       grid_utils::addObjectsToGridMap(*planner_data_->predicted_objects, grid_map);
     }
-    DEBUG_PRINT(
-      param_.is_show_processing_time, "grid [ms]: " << stop_watch_.toc("processing_time", true));
+    DEBUG_PRINT(show_time, "grid [ms]: ", stop_watch_.toc("processing_time", true));
     // Note: Don't consider offset from path start to ego here
-    if (!utils::createPossibleCollisionsInDetectionArea(
+    if (!utils::generatePossibleCollisionsFromGridMap(
           possible_collisions, grid_map, interp_path, offset_from_start_to_ego, param_,
           debug_data_)) {
       // no occlusion spot
       return true;
     }
   } else if (param_.detection_method == utils::DETECTION_METHOD::PREDICTED_OBJECT) {
     const auto & dynamic_obj_arr_ptr = planner_data_->predicted_objects;
-    if (!dynamic_obj_arr_ptr) return true;  // mo data
+    if (!dynamic_obj_arr_ptr) return true;  // no data
     std::vector<PredictedObject> obj =
-      utils::getParkedVehicles(*dynamic_obj_arr_ptr, param_, debug_data_.parked_vehicle_point);
+      utils::getParkedVehicles(*dynamic_obj_arr_ptr, param_, parked_vehicle_point);
     const auto filtered_obj =
-      utils::filterDynamicObjectByDetectionArea(obj, detection_area_polygons);
+      utils::filterDynamicObjectByDetectionArea(obj, debug_data_.detection_area_polygons);
     // Note: Don't consider offset from path start to ego here
-    if (!utils::generatePossibleCollisionBehindParkedVehicle(
+    if (!utils::generatePossibleCollisionsFromObjects(
           possible_collisions, interp_path, param_, offset_from_start_to_ego, filtered_obj)) {
       // no occlusion spot
       return true;
     }
   }
-  DEBUG_PRINT(
-    param_.is_show_processing_time, "occlusion [ms]: " << stop_watch_.toc("processing_time", true));
-  DEBUG_PRINT(param_.is_show_occlusion, "num collision:" << possible_collisions.size());
+  DEBUG_PRINT(show_time, "occlusion [ms]: ", stop_watch_.toc("processing_time", true));
+  DEBUG_PRINT(show_time, "num collision:", possible_collisions.size());
   utils::calcSlowDownPointsForPossibleCollision(0, interp_path, 0.0, possible_collisions);
   // Note: Consider offset from path start to ego here
   utils::handleCollisionOffset(possible_collisions, offset_from_start_to_ego);
   // apply safe velocity using ebs and pbs deceleration
   utils::applySafeVelocityConsideringPossibleCollision(path, possible_collisions, param_);
   // these debug topics needs computation resource
-
+  debug_data_.parked_vehicle_point = parked_vehicle_point;
+  debug_data_.close_partition = close_partition_;
   debug_data_.z = path->points.front().point.pose.position.z;
   debug_data_.possible_collisions = possible_collisions;
+  debug_data_.interp_path = interp_path;
   debug_data_.path_raw = clipped_path;
+  DEBUG_PRINT(show_time, "total [ms]: ", stop_watch_.toc("total_processing_time", true));
   return true;
 }