refactor(behavior_velocity): refactor occlusion spot module

planning/behavior_velocity_planner/CMakeLists.txt

@@ -224,7 +224,7 @@ ament_auto_add_library(scene_module_occlusion_spot SHARED
   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_in_private_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

@@ -1,6 +1,7 @@
 /**:
   ros__parameters:
     occlusion_spot:
+      method: "predicted_object"           # [-] 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

@@ -17,7 +17,7 @@
 
 #include <rclcpp/rclcpp.hpp>
 #include <scene_module/occlusion_spot/occlusion_spot_utils.hpp>
-#include <scene_module/occlusion_spot/scene_occlusion_spot_in_private_road.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>
 
@@ -45,7 +45,7 @@ class OcclusionSpotModuleManager : public SceneModuleManagerInterface
   const char * getModuleName() override { return "occlusion_spot"; }
 
 private:
-  enum class ModuleID { PRIVATE, PUBLIC };
+  enum class ModuleID { OCCUPANCY, OBJECT };
   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_in_private_road.hpp → planning/behavior_velocity_planner/include/scene_module/occlusion_spot/scene_occlusion_spot.hpp

@@ -12,8 +12,8 @@
 // See the License for the specific language governing permissions and
 // limitations under the License.
 
-#ifndef SCENE_MODULE__OCCLUSION_SPOT__SCENE_OCCLUSION_SPOT_IN_PRIVATE_ROAD_HPP_
-#define SCENE_MODULE__OCCLUSION_SPOT__SCENE_OCCLUSION_SPOT_IN_PRIVATE_ROAD_HPP_
+#ifndef SCENE_MODULE__OCCLUSION_SPOT__SCENE_OCCLUSION_SPOT_HPP_
+#define SCENE_MODULE__OCCLUSION_SPOT__SCENE_OCCLUSION_SPOT_HPP_
 
 #include <rclcpp/rclcpp.hpp>
 #include <scene_module/occlusion_spot/occlusion_spot_utils.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 = "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;
   };
 
-  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,
@@ -82,4 +82,4 @@ class OcclusionSpotInPrivateModule : public SceneModuleInterface
 };
 }  // namespace behavior_velocity_planner
 
-#endif  // SCENE_MODULE__OCCLUSION_SPOT__SCENE_OCCLUSION_SPOT_IN_PRIVATE_ROAD_HPP_
+#endif  // SCENE_MODULE__OCCLUSION_SPOT__SCENE_OCCLUSION_SPOT_HPP_

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

@@ -43,7 +43,7 @@ class OcclusionSpotInPublicModule : public SceneModuleInterface
 public:
   struct DebugData
   {
-    std::string road_type = "public";
+    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;

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

@@ -13,7 +13,7 @@
 // limitations under the License.
 
 #include <scene_module/occlusion_spot/occlusion_spot_utils.hpp>
-#include <scene_module/occlusion_spot/scene_occlusion_spot_in_private_road.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>
@@ -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/grid_utils.cpp

@@ -16,6 +16,7 @@
 #include <scene_module/occlusion_spot/grid_utils.hpp>
 
 #include <algorithm>
+#include <stdexcept>
 #include <vector>
 
 namespace behavior_velocity_planner
@@ -181,13 +182,14 @@ void toQuantizedImage(
     for (int y = height - 1; y >= 0; y--) {
       const int idx = (height - 1 - y) + (width - 1 - x) * height;
       int8_t intensity = occupancy_grid.data.at(idx);
-      if (0 <= intensity && intensity < param.free_space_max) {
+      if (0 <= intensity && intensity <= param.free_space_max) {
         intensity = grid_utils::occlusion_cost_value::FREE_SPACE;
-      } else if (  // NOLINT
-        intensity == occlusion_cost_value::NO_INFORMATION || intensity < param.occupied_min) {
+      } else if (param.free_space_max < intensity && intensity < param.occupied_min) {
         intensity = grid_utils::occlusion_cost_value::UNKNOWN;
-      } else {
+      } else if (param.occupied_min <= intensity) {
         intensity = grid_utils::occlusion_cost_value::OCCUPIED;
+      } else {
+        std::logic_error("behavior_velocity[occlusion_spot_grid]: invalid if clause");
       }
       cv_image->at<unsigned char>(y, x) = intensity;
     }

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

@@ -13,7 +13,7 @@
 // limitations under the License.
 
 #include <scene_module/occlusion_spot/manager.hpp>
-#include <scene_module/occlusion_spot/scene_occlusion_spot_in_private_road.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>
 
@@ -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);
-  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_));
+  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_.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,8 @@ 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) {
+    return false;
   };
 }
 }  // namespace behavior_velocity_planner

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

@@ -47,7 +47,7 @@ void applySafeVelocityConsideringPossibleCollision(
 
     // min allowed velocity : min allowed velocity consider maximum allowed braking
     const double v_slow_down =
-      (l_obs < 0)
+      (l_obs < 0 && v0 <= v_safe)
         ? v_safe
         : planning_utils::calcDecelerationVelocityFromDistanceToTarget(j_min, a_min, a0, v0, l_obs);
     // compare safe velocity consider EBS, minimum allowed velocity and original velocity

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

@@ -16,7 +16,7 @@
 #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_in_private_road.hpp>
+#include <scene_module/occlusion_spot/scene_occlusion_spot.hpp>
 #include <utilization/path_utilization.hpp>
 #include <utilization/util.hpp>
 
@@ -27,12 +27,11 @@
 
 namespace behavior_velocity_planner
 {
-using occlusion_spot_utils::ROAD_TYPE::PRIVATE;
 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,
@@ -42,12 +41,11 @@ 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";
   if (path->points.size() < 2) {
     return true;
   }
@@ -83,9 +81,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);
@@ -94,16 +89,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

@@ -30,7 +30,6 @@
 namespace behavior_velocity_planner
 {
 using occlusion_spot_utils::PossibleCollisionInfo;
-using occlusion_spot_utils::ROAD_TYPE::PUBLIC;
 namespace utils = occlusion_spot_utils;
 
 OcclusionSpotInPublicModule::OcclusionSpotInPublicModule(
@@ -47,7 +46,6 @@ bool OcclusionSpotInPublicModule::modifyPathVelocity(
   [[maybe_unused]] tier4_planning_msgs::msg::StopReason * stop_reason)
 {
   debug_data_ = DebugData();
-  debug_data_.road_type = "public";
   if (path->points.size() < 2) {
     return true;
   }
@@ -81,9 +79,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);
@@ -96,7 +91,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);

sensing/laserscan_to_occupancy_grid_map/launch/laserscan_to_occupancy_grid_map.launch.py

@@ -61,7 +61,7 @@ def add_launch_arg(name: str, default_value=None):
                     "max_height": 2.0,
                     "angle_min": -3.141592,  # -M_PI
                     "angle_max": 3.141592,  # M_PI
-                    "angle_increment": 0.00436332222,  # 0.25*M_PI/180.0
+                    "angle_increment": 0.00349065850,  # 0.20*M_PI/180.0
                     "scan_time": 0.0,
                     "range_min": 1.0,
                     "range_max": 200.0,