refactor(behavior_velocity): merge occlusion spot sepated module

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

planning/behavior_velocity_planner/CMakeLists.txt

@@ -222,7 +222,6 @@ ament_auto_add_library(scene_module_occlusion_spot SHARED
   src/scene_module/occlusion_spot/grid_utils.cpp
   src/scene_module/occlusion_spot/manager.cpp
   src/scene_module/occlusion_spot/debug.cpp
-  src/scene_module/occlusion_spot/scene_occlusion_spot_in_public_road.cpp
   src/scene_module/occlusion_spot/scene_occlusion_spot.cpp
   src/scene_module/occlusion_spot/occlusion_spot_utils.cpp
   src/scene_module/occlusion_spot/risk_predictive_braking.cpp

planning/behavior_velocity_planner/config/occlusion_spot.param.yaml

@@ -5,7 +5,7 @@
       pass_judge: "current_velocity"       # [-] candidate is "current_velocity""
       debug: false                         # [-] whether to publish debug markers. Note Default should be false for performance
       use_partition_lanelet: true          # [-] whether to use partition lanelet map data
-      pedestrian_vel: 1.5                  # [m/s] assume pedestrian is dashing from occlusion at this velocity
+      pedestrian_vel: 1.0                  # [m/s] assume pedestrian is dashing from occlusion at this velocity
       threshold:
         detection_area_length: 100.0        # [m] the length of path to consider perception range
         stuck_vehicle_vel: 1.0              # [m/s] velocity below this value is assumed to stop

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

@@ -18,7 +18,6 @@
 #include <rclcpp/rclcpp.hpp>
 #include <scene_module/occlusion_spot/occlusion_spot_utils.hpp>
 #include <scene_module/occlusion_spot/scene_occlusion_spot.hpp>
-#include <scene_module/occlusion_spot/scene_occlusion_spot_in_public_road.hpp>
 #include <scene_module/scene_module_interface.hpp>
 
 #include <autoware_auto_perception_msgs/msg/predicted_object.hpp>
@@ -49,6 +48,7 @@ class OcclusionSpotModuleManager : public SceneModuleManagerInterface
   using PlannerParam = occlusion_spot_utils::PlannerParam;
 
   PlannerParam planner_param_;
+  int64_t module_id_;
 
   void launchNewModules(const autoware_auto_planning_msgs::msg::PathWithLaneId & path) override;
 

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

@@ -213,8 +213,9 @@ std::vector<PredictedObject> filterDynamicObjectByDetectionArea(
 std::vector<PredictedObject> getParkedVehicles(
   const PredictedObjects & dyn_objects, const PlannerParam & param,
   std::vector<Point> & debug_point);
-std::vector<PossibleCollisionInfo> generatePossibleCollisionBehindParkedVehicle(
-  const PathWithLaneId & path, const PlannerParam & param, const double offset_from_start_to_ego,
+bool generatePossibleCollisionBehindParkedVehicle(
+  std::vector<PossibleCollisionInfo> & possible_collisions, const PathWithLaneId & path,
+  const PlannerParam & param, const double offset_from_start_to_ego,
   const std::vector<PredictedObject> & dyn_objects);
 ROAD_TYPE getCurrentRoadType(
   const lanelet::ConstLanelet & current_lanelet, const LaneletMapPtr & lanelet_map_ptr);
@@ -238,7 +239,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
-void createPossibleCollisionsInDetectionArea(
+bool createPossibleCollisionsInDetectionArea(
   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);

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

@@ -14,7 +14,6 @@
 
 #include <scene_module/occlusion_spot/occlusion_spot_utils.hpp>
 #include <scene_module/occlusion_spot/scene_occlusion_spot.hpp>
-#include <scene_module/occlusion_spot/scene_occlusion_spot_in_public_road.hpp>
 #include <tier4_autoware_utils/ros/marker_helper.hpp>
 #include <utilization/marker_helper.hpp>
 #include <utilization/util.hpp>
@@ -282,24 +281,6 @@ visualization_msgs::msg::MarkerArray createOcclusionMarkerArray(
 }
 }  // namespace
 
-visualization_msgs::msg::MarkerArray OcclusionSpotInPublicModule::createDebugMarkerArray()
-{
-  const auto current_time = this->clock_->now();
-
-  visualization_msgs::msg::MarkerArray debug_marker_array;
-  if (!debug_data_.possible_collisions.empty()) {
-    appendMarkerArray(
-      createPossibleCollisionMarkers(debug_data_, module_id_), current_time, &debug_marker_array);
-  }
-  if (!debug_data_.detection_area_polygons.empty()) {
-    appendMarkerArray(
-      makeSlicePolygonMarker(
-        debug_data_.detection_area_polygons, "detection_area", module_id_, debug_data_.z),
-      current_time, &debug_marker_array);
-  }
-
-  return debug_marker_array;
-}
 visualization_msgs::msg::MarkerArray OcclusionSpotModule::createDebugMarkerArray()
 {
   const auto current_time = this->clock_->now();

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

@@ -14,7 +14,6 @@
 
 #include <scene_module/occlusion_spot/manager.hpp>
 #include <scene_module/occlusion_spot/scene_occlusion_spot.hpp>
-#include <scene_module/occlusion_spot/scene_occlusion_spot_in_public_road.hpp>
 #include <utilization/util.hpp>
 
 #include <lanelet2_core/primitives/BasicRegulatoryElements.h>
@@ -40,10 +39,12 @@ OcclusionSpotModuleManager::OcclusionSpotModuleManager(rclcpp::Node & node)
   // for detection type
   {
     const std::string method = node.declare_parameter(ns + ".detection_method", "occupancy_grid");
-    if (method == "occupancy_grid") {
+    if (method == "occupancy_grid") {  // module id 0
       pp.detection_method = DETECTION_METHOD::OCCUPANCY_GRID;
-    } else if (method == "predicted_object") {
+      module_id_ = DETECTION_METHOD::OCCUPANCY_GRID;
+    } else if (method == "predicted_object") {  // module id 1
       pp.detection_method = DETECTION_METHOD::PREDICTED_OBJECT;
+      module_id_ = DETECTION_METHOD::PREDICTED_OBJECT;
     } else {
       throw std::invalid_argument{
         "[behavior_velocity]: occlusion spot detection method has invalid argument"};
@@ -54,6 +55,8 @@ OcclusionSpotModuleManager::OcclusionSpotModuleManager(rclcpp::Node & node)
     const std::string pass_judge = node.declare_parameter(ns + ".pass_judge", "current_velocity");
     if (pass_judge == "current_velocity") {
       pp.pass_judge = PASS_JUDGE::CURRENT_VELOCITY;
+    } else if (pass_judge == "smooth_velocity") {
+      pp.pass_judge = PASS_JUDGE::SMOOTH_VELOCITY;
     } else {
       throw std::invalid_argument{
         "[behavior_velocity]: occlusion spot pass judge method has invalid argument"};
@@ -98,23 +101,11 @@ void OcclusionSpotModuleManager::launchNewModules(
   const autoware_auto_planning_msgs::msg::PathWithLaneId & path)
 {
   if (path.points.empty()) return;
-  const int64_t module_id = static_cast<int64_t>(ModuleID::OCCUPANCY);
-  const int64_t public_road_module_id = static_cast<int64_t>(ModuleID::OBJECT);
   // general
-  if (!isModuleRegistered(module_id)) {
-    if (planner_param_.detection_method == DETECTION_METHOD::OCCUPANCY_GRID) {
-      registerModule(std::make_shared<OcclusionSpotModule>(
-        module_id, planner_data_, planner_param_, logger_.get_child("occlusion_spot_module"),
-        clock_, pub_debug_occupancy_grid_));
-    }
-  }
-  // public
-  if (!isModuleRegistered(public_road_module_id)) {
-    if (planner_param_.detection_method == DETECTION_METHOD::PREDICTED_OBJECT) {
-      registerModule(std::make_shared<OcclusionSpotInPublicModule>(
-        public_road_module_id, planner_data_, planner_param_,
-        logger_.get_child("occlusion_spot_in_public_module"), clock_));
-    }
+  if (!isModuleRegistered(module_id_)) {
+    registerModule(std::make_shared<OcclusionSpotModule>(
+      module_id_, planner_data_, planner_param_, logger_.get_child("occlusion_spot_module"), clock_,
+      pub_debug_occupancy_grid_));
   }
 }
 

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

@@ -314,12 +314,12 @@ PossibleCollisionInfo calculateCollisionPathPointFromOcclusionSpot(
   return pc;
 }
 
-std::vector<PossibleCollisionInfo> generatePossibleCollisionBehindParkedVehicle(
-  const PathWithLaneId & path, const PlannerParam & param, const double offset_from_start_to_ego,
+bool generatePossibleCollisionBehindParkedVehicle(
+  std::vector<PossibleCollisionInfo> & possible_collisions, const PathWithLaneId & path,
+  const PlannerParam & param, const double offset_from_start_to_ego,
   const std::vector<PredictedObject> & dyn_objects)
 {
   lanelet::ConstLanelet path_lanelet = toPathLanelet(path);
-  std::vector<PossibleCollisionInfo> possible_collisions;
   auto ll = path_lanelet.centerline2d();
   for (const auto & dyn : dyn_objects) {
     ArcCoordinates arc_coord_occlusion = getOcclusionPoint(dyn, ll);
@@ -345,7 +345,8 @@ std::vector<PossibleCollisionInfo> generatePossibleCollisionBehindParkedVehicle(
     [](PossibleCollisionInfo pc1, PossibleCollisionInfo pc2) {
       return pc1.arc_lane_dist_at_collision.length < pc2.arc_lane_dist_at_collision.length;
     });
-  return possible_collisions;
+  if (possible_collisions.empty()) return false;
+  return true;
 }
 
 std::vector<PredictedObject> filterDynamicObjectByDetectionArea(
@@ -365,14 +366,14 @@ std::vector<PredictedObject> filterDynamicObjectByDetectionArea(
   return filtered_obj;
 }
 
-void createPossibleCollisionsInDetectionArea(
+bool createPossibleCollisionsInDetectionArea(
   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)
 {
   lanelet::ConstLanelet path_lanelet = toPathLanelet(path);
   if (path_lanelet.centerline2d().empty()) {
-    return;
+    return true;
   }
   double distance_lower_bound = std::numeric_limits<double>::max();
   const Polygons2d & da_polygons = debug_data.detection_area_polygons;
@@ -400,6 +401,14 @@ void createPossibleCollisionsInDetectionArea(
     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;
 }
 
 bool isNotBlockedByPartition(const LineString2d & direction, const BasicPolygons2d & partitions)

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

@@ -37,10 +37,15 @@ OcclusionSpotModule::OcclusionSpotModule(
   const PlannerParam & planner_param, const rclcpp::Logger logger,
   const rclcpp::Clock::SharedPtr clock,
   const rclcpp::Publisher<nav_msgs::msg::OccupancyGrid>::SharedPtr publisher)
-: SceneModuleInterface(module_id, logger, clock), publisher_(publisher)
+: SceneModuleInterface(module_id, logger, clock), publisher_(publisher), param_(planner_param)
 {
-  param_ = planner_param;
-  debug_data_.detection_type = "occupancy";
+  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);
+  } 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);
@@ -66,22 +71,16 @@ bool OcclusionSpotModule::modifyPathVelocity(
       planner_data_->delay_response_time);
   }
   const geometry_msgs::msg::Pose ego_pose = planner_data_->current_pose.pose;
-  const auto & occ_grid_ptr = planner_data_->occupancy_grid;
-  if (!occ_grid_ptr) {
-    return true;
-  }
-
-  const double max_range = 50.0;
   PathWithLaneId clipped_path;
-  utils::clipPathByLength(*path, clipped_path, max_range);
+  utils::clipPathByLength(*path, clipped_path, param_.detection_area_length);
   PathWithLaneId interp_path;
   //! never change this interpolation interval(will affect module accuracy)
   splineInterpolate(clipped_path, 1.0, &interp_path, logger_);
-  debug_data_.interp_path = interp_path;
   if (param_.pass_judge == utils::PASS_JUDGE::CURRENT_VELOCITY) {
     interp_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);
@@ -90,13 +89,35 @@ bool OcclusionSpotModule::modifyPathVelocity(
   auto & detection_area_polygons = debug_data_.detection_area_polygons;
   utils::buildDetectionAreaPolygon(
     detection_area_polygons, interp_path, offset_from_start_to_ego, param_);
-  nav_msgs::msg::OccupancyGrid occ_grid = *occ_grid_ptr;
-  grid_map::GridMap grid_map;
-  grid_utils::denoiseOccupancyGridCV(occ_grid, grid_map, param_.grid);
   std::vector<utils::PossibleCollisionInfo> possible_collisions;
-  // Note: Don't consider offset from path start to ego here
-  utils::createPossibleCollisionsInDetectionArea(
-    possible_collisions, grid_map, interp_path, offset_from_start_to_ego, param_, debug_data_);
+  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
+    nav_msgs::msg::OccupancyGrid occ_grid = *occ_grid_ptr;
+    grid_map::GridMap grid_map;
+    grid_utils::denoiseOccupancyGridCV(occ_grid, grid_map, param_.grid);
+    if (param_.debug) publisher_->publish(occ_grid);  //
+    // Note: Don't consider offset from path start to ego here
+    if (!utils::createPossibleCollisionsInDetectionArea(
+          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
+    std::vector<PredictedObject> obj =
+      utils::getParkedVehicles(*dynamic_obj_arr_ptr, param_, debug_data_.parked_vehicle_point);
+    const auto filtered_obj =
+      utils::filterDynamicObjectByDetectionArea(obj, detection_area_polygons);
+    // Note: Don't consider offset from path start to ego here
+    if (!utils::generatePossibleCollisionBehindParkedVehicle(
+          possible_collisions, interp_path, param_, offset_from_start_to_ego, filtered_obj)) {
+      // no occlusion spot
+      return true;
+    }
+  }
   RCLCPP_DEBUG_STREAM_THROTTLE(
     logger_, *clock_, 3000, "num possible collision:" << possible_collisions.size());
   utils::calcSlowDownPointsForPossibleCollision(0, interp_path, 0.0, possible_collisions);
@@ -105,9 +126,7 @@ bool OcclusionSpotModule::modifyPathVelocity(
   // apply safe velocity using ebs and pbs deceleration
   utils::applySafeVelocityConsideringPossibleCollision(path, possible_collisions, param_);
   // these debug topics needs computation resource
-  if (param_.debug) {
-    publisher_->publish(occ_grid);
-  }
+
   debug_data_.z = path->points.front().point.pose.position.z;
   debug_data_.possible_collisions = possible_collisions;
   debug_data_.path_raw = *path;