chore(behavior_velocity): refactor occlusion spot module

Signed-off-by: tanaka3 <[email protected]>

planning/behavior_velocity_planner/config/occlusion_spot.param.yaml

@@ -1,6 +1,7 @@
 /**:
   ros__parameters:
     occlusion_spot:
+      method: "occupancy_grid"             # [-] candidate is "occupancy_grid" or "predicted_object"
       debug: false                         # [-] whether to publish debug markers. Note Default should be false for performance
       pedestrian_vel: 1.0                  # [m/s] assume pedestrian is dashing from occlusion at this velocity
       threshold:
@@ -18,7 +19,7 @@
         safe_margin: 1.0                    # [m] maximum safety distance for any error
       detection_area:
         min_occlusion_spot_size: 1.0     # [m] occupancy grid must contain an UNKNOWN area of at least size NxN to be considered a hidden obstacle.
-        slice_length: 10.0                # [m] size of slices in both length and distance relative to the ego path.
+        slice_length: 10.0               # [m] size of slices in both length and distance relative to the ego path.
         max_lateral_distance: 4.0        # [m] buffer around the ego path used to build the detection area.
       grid:
         free_space_max: 40  # [-] maximum value of a free space cell in the occupancy grid

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

@@ -45,7 +45,7 @@ class OcclusionSpotModuleManager : public SceneModuleManagerInterface
   const char * getModuleName() override { return "occlusion_spot"; }
 
 private:
-  enum class ModuleID { PRIVATE, PUBLIC };
+  enum class ModuleID { DEFAULT, PUBLIC };
   using PlannerParam = occlusion_spot_utils::PlannerParam;
 
   PlannerParam planner_param_;

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

@@ -65,6 +65,7 @@ using DetectionAreaIdx = boost::optional<std::pair<double, double>>;
 namespace occlusion_spot_utils
 {
 enum ROAD_TYPE { PRIVATE, PUBLIC, HIGHWAY, UNKNOWN };
+enum METHOD { OCCUPANCY_GRID, PREDICTED_OBJECT };
 
 struct DetectionArea
 {
@@ -96,6 +97,7 @@ struct LatLon
 
 struct PlannerParam
 {
+  METHOD method;
   bool debug;  // [-]
   // parameters in yaml
   double detection_area_length;      // [m]

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

@@ -37,23 +37,23 @@
 
 namespace behavior_velocity_planner
 {
-class OcclusionSpotInPrivateModule : public SceneModuleInterface
+class OcclusionSpotModule : public SceneModuleInterface
 {
   using PlannerParam = occlusion_spot_utils::PlannerParam;
 
 public:
   struct DebugData
   {
     double z;
-    std::string road_type = "private";
+    std::string road_type = "general";
     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;
   };
 
-  OcclusionSpotInPrivateModule(
+  OcclusionSpotModule(
     const int64_t module_id, std::shared_ptr<const PlannerData> planner_data,
     const PlannerParam & planner_param, const rclcpp::Logger logger,
     const rclcpp::Clock::SharedPtr clock,

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

@@ -269,7 +269,7 @@ visualization_msgs::msg::MarkerArray OcclusionSpotInPublicModule::createDebugMar
 
   return debug_marker_array;
 }
-visualization_msgs::msg::MarkerArray OcclusionSpotInPrivateModule::createDebugMarkerArray()
+visualization_msgs::msg::MarkerArray OcclusionSpotModule::createDebugMarkerArray()
 {
   const auto current_time = this->clock_->now();
 

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

@@ -25,50 +25,7 @@
 
 namespace behavior_velocity_planner
 {
-namespace
-{
-std::vector<lanelet::ConstLanelet> getLaneletsOnPath(
-  const autoware_auto_planning_msgs::msg::PathWithLaneId & path,
-  const lanelet::LaneletMapPtr lanelet_map)
-{
-  std::vector<lanelet::ConstLanelet> lanelets;
-
-  for (const auto & p : path.points) {
-    const auto lane_id = p.lane_ids.at(0);
-    lanelets.push_back(lanelet_map->laneletLayer.get(lane_id));
-  }
-  return lanelets;
-}
-
-bool hasPublicRoadOnPath(
-  const autoware_auto_planning_msgs::msg::PathWithLaneId & path,
-  const lanelet::LaneletMapPtr lanelet_map)
-{
-  for (const auto & ll : getLaneletsOnPath(path, lanelet_map)) {
-    // Is public load ?
-    const std::string location = ll.attributeOr("location", "else");
-    if (location == "urban" || location == "public") {
-      return true;
-    }
-  }
-  return false;
-}
-
-bool hasPrivateRoadOnPath(
-  const autoware_auto_planning_msgs::msg::PathWithLaneId & path,
-  const lanelet::LaneletMapPtr lanelet_map)
-{
-  for (const auto & ll : getLaneletsOnPath(path, lanelet_map)) {
-    // Is private load ?
-    const std::string location = ll.attributeOr("location", "else");
-    if (location == "private") {
-      return true;
-    }
-  }
-  return false;
-}
-
-}  // namespace
+using occlusion_spot_utils::METHOD;
 
 OcclusionSpotModuleManager::OcclusionSpotModuleManager(rclcpp::Node & node)
 : SceneModuleManagerInterface(node, getModuleName())
@@ -80,6 +37,14 @@ OcclusionSpotModuleManager::OcclusionSpotModuleManager(rclcpp::Node & node)
   // for crosswalk parameters
   auto & pp = planner_param_;
   // assume pedestrian coming out from occlusion spot with this velocity
+  const std::string method = node.declare_parameter(ns + ".method", "occupancy_grid");
+  if (method == "occupancy_grid") {
+    pp.method = METHOD::OCCUPANCY_GRID;
+  } else if (method == "predicted_object") {
+    pp.method = METHOD::PREDICTED_OBJECT;
+  } else {
+    throw std::invalid_argument{"[behavior_velocity]: occlusion spot detection method is invalid"};
+  }
   pp.debug = node.declare_parameter(ns + ".debug", false);
   pp.pedestrian_vel = node.declare_parameter(ns + ".pedestrian_vel", 1.0);
   pp.detection_area_length = node.declare_parameter(ns + ".threshold.detection_area_length", 200.0);
@@ -117,19 +82,20 @@ OcclusionSpotModuleManager::OcclusionSpotModuleManager(rclcpp::Node & node)
 void OcclusionSpotModuleManager::launchNewModules(
   const autoware_auto_planning_msgs::msg::PathWithLaneId & path)
 {
-  const int64_t private_road_module_id = static_cast<int64_t>(ModuleID::PRIVATE);
+  if (path.points.empty()) return;
+  const int64_t module_id = static_cast<int64_t>(ModuleID::DEFAULT);
   const int64_t public_road_module_id = static_cast<int64_t>(ModuleID::PUBLIC);
-  // private
-  if (!isModuleRegistered(private_road_module_id)) {
-    if (hasPrivateRoadOnPath(path, planner_data_->lanelet_map)) {
-      registerModule(std::make_shared<OcclusionSpotInPrivateModule>(
-        private_road_module_id, planner_data_, planner_param_,
-        logger_.get_child("occlusion_spot_in_private_module"), clock_, pub_debug_occupancy_grid_));
+  // general
+  if (!isModuleRegistered(module_id)) {
+    if (planner_param_.method == 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 (hasPublicRoadOnPath(path, planner_data_->lanelet_map)) {
+    if (planner_param_.method == 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_));
@@ -141,17 +107,14 @@ std::function<bool(const std::shared_ptr<SceneModuleInterface> &)>
 OcclusionSpotModuleManager::getModuleExpiredFunction(
   const autoware_auto_planning_msgs::msg::PathWithLaneId & path)
 {
-  const bool has_public_road = hasPublicRoadOnPath(path, planner_data_->lanelet_map);
-  const bool has_private_road = hasPrivateRoadOnPath(path, planner_data_->lanelet_map);
-  return [has_private_road,
-          has_public_road](const std::shared_ptr<SceneModuleInterface> & scene_module) {
-    if (scene_module->getModuleId() == static_cast<int64_t>(ModuleID::PRIVATE)) {
-      return !has_private_road;
-    }
-    if (scene_module->getModuleId() == static_cast<int64_t>(ModuleID::PUBLIC)) {
-      return !has_public_road;
-    }
-    return true;
+  return [path]([[maybe_unused]] const std::shared_ptr<SceneModuleInterface> & scene_module) {
+    // expire if no path points
+    if (path.points.empty()) return true;
+    // expire if there are path points
+    else if (path.points.size() > 0)
+      return false;
+    else
+      return true;
   };
 }
 }  // namespace behavior_velocity_planner

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

@@ -31,7 +31,7 @@ namespace bg = boost::geometry;
 namespace lg = lanelet::geometry;
 namespace utils = occlusion_spot_utils;
 
-OcclusionSpotInPrivateModule::OcclusionSpotInPrivateModule(
+OcclusionSpotModule::OcclusionSpotModule(
   const int64_t module_id, [[maybe_unused]] std::shared_ptr<const PlannerData> planner_data,
   const PlannerParam & planner_param, const rclcpp::Logger logger,
   const rclcpp::Clock::SharedPtr clock,
@@ -41,12 +41,12 @@ OcclusionSpotInPrivateModule::OcclusionSpotInPrivateModule(
   param_ = planner_param;
 }
 
-bool OcclusionSpotInPrivateModule::modifyPathVelocity(
+bool OcclusionSpotModule::modifyPathVelocity(
   autoware_auto_planning_msgs::msg::PathWithLaneId * path,
   [[maybe_unused]] tier4_planning_msgs::msg::StopReason * stop_reason)
 {
   debug_data_ = DebugData();
-  debug_data_.road_type = "private";
+  debug_data_.road_type = "general";
   if (path->points.size() < 2) {
     return true;
   }
@@ -82,9 +82,6 @@ bool OcclusionSpotInPrivateModule::modifyPathVelocity(
   }
   // return if ego is final point of interpolated path
   if (closest_idx == static_cast<int>(interp_path.points.size()) - 1) return true;
-  DetectionAreaIdx focus_area =
-    extractTargetRoadArcLength(lanelet_map_ptr, max_range, *path, PRIVATE);
-  if (!focus_area) return true;
   nav_msgs::msg::OccupancyGrid occ_grid = *occ_grid_ptr;
   grid_map::GridMap grid_map;
   grid_utils::denoiseOccupancyGridCV(occ_grid, grid_map, param_.grid);
@@ -93,16 +90,12 @@ bool OcclusionSpotInPrivateModule::modifyPathVelocity(
   std::vector<Slice> detection_area_polygons;
   utils::buildDetectionAreaPolygon(
     detection_area_polygons, interp_path, offset_from_start_to_ego, param_);
-  RCLCPP_DEBUG_STREAM_THROTTLE(logger_, *clock_, 3000, "closest_idx : " << closest_idx);
-  RCLCPP_DEBUG_STREAM_THROTTLE(
-    logger_, *clock_, 3000, "offset_from_start_to_ego : " << offset_from_start_to_ego);
   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;
-  utils::filterCollisionByRoadType(possible_collisions, focus_area);
   RCLCPP_DEBUG_STREAM_THROTTLE(
     logger_, *clock_, 3000, "num possible collision:" << possible_collisions.size());
   utils::calcSlowDownPointsForPossibleCollision(0, interp_path, 0.0, possible_collisions);

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

@@ -80,9 +80,6 @@ bool OcclusionSpotInPublicModule::modifyPathVelocity(
   }
   // return if ego is final point of interpolated path
   if (closest_idx == static_cast<int>(interp_path.points.size()) - 1) return true;
-  DetectionAreaIdx focus_area =
-    extractTargetRoadArcLength(lanelet_map_ptr, param_.detection_area_length, *path, PUBLIC);
-  if (!focus_area) return true;
   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);
@@ -95,7 +92,6 @@ bool OcclusionSpotInPublicModule::modifyPathVelocity(
   std::vector<PossibleCollisionInfo> possible_collisions =
     utils::generatePossibleCollisionBehindParkedVehicle(
       interp_path, param_, offset_from_start_to_ego, filtered_obj);
-  utils::filterCollisionByRoadType(possible_collisions, focus_area);
   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, 0.0);