refactor(behavior_velocity): occlusion spot merge scene modules (#528)

* refactor(behavior_velocity): refactor slice to general type

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

* chore(behavior_velocity): Basic Polygon to general polygon2d

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

* chore(behavior_velocity): generalize da polygon usage

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

* chore(behavior_velocity): remove replaced function

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

* refactor(behavior_velocity): unite create detection area polygon function as util

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

* refactor(behavior_velocity): refactor method

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

* refactor(behavior_velocity): add pass judge param

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

* refactor(behavior_velocity): move judgement

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

* refactor(behavior_velocity): replace with tier4 autoware utils

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

* chore(behavior_velocity): fix spell check

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

* refactor(behavior_velocity): merge occlusion spot sepated module

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

* chore(behavior_velocity): handle boundary condition for better

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

planning/behavior_velocity_planner/CMakeLists.txt

@@ -220,10 +220,8 @@ target_link_libraries(scene_module_virtual_traffic_light scene_module_lib)
 # SceneModule OcclusionSpot
 ament_auto_add_library(scene_module_occlusion_spot SHARED
   src/scene_module/occlusion_spot/grid_utils.cpp
-  src/scene_module/occlusion_spot/geometry.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
@@ -313,7 +311,6 @@ if(BUILD_TESTING)
   ament_add_gtest(occlusion_spot-test
     test/src/test_occlusion_spot_utils.cpp
     test/src/test_risk_predictive_braking.cpp
-    test/src/test_geometry.cpp
     test/src/test_grid_utils.cpp
   )
   target_link_libraries(occlusion_spot-test

planning/behavior_velocity_planner/config/occlusion_spot.param.yaml

@@ -1,7 +1,8 @@
 /**:
   ros__parameters:
     occlusion_spot:
-      method: "predicted_object"           # [-] candidate is "occupancy_grid" or "predicted_object"
+      detection_method: "predicted_object" # [-] candidate is "occupancy_grid" or "predicted_object"
+      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.0                  # [m/s] assume pedestrian is dashing from occlusion at this velocity

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

@@ -19,6 +19,7 @@
 #include <grid_map_core/iterators/LineIterator.hpp>
 #include <grid_map_core/iterators/PolygonIterator.hpp>
 #include <opencv2/opencv.hpp>
+#include <utilization/boost_geometry_helper.hpp>
 
 #include <nav_msgs/msg/occupancy_grid.hpp>
 
@@ -67,7 +68,7 @@ bool isOcclusionSpotSquare(
 //!< @brief Find all occlusion spots inside the given lanelet
 void findOcclusionSpots(
   std::vector<grid_map::Position> & occlusion_spot_positions, const grid_map::GridMap & grid,
-  const lanelet::BasicPolygon2d & polygon, const double min_size);
+  const Polygon2d & polygon, const double min_size);
 //!< @brief Return true if the path between the two given points is free of occupied cells
 bool isCollisionFree(
   const grid_map::GridMap & grid, const grid_map::Position & p1, const grid_map::Position & p2);

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

@@ -19,7 +19,7 @@
 #include <lanelet2_extension/utility/utilities.hpp>
 #include <lanelet2_extension/visualization/visualization.hpp>
 #include <scene_module/occlusion_spot/grid_utils.hpp>
-#include <tier4_autoware_utils/geometry/geometry.hpp>
+#include <tier4_autoware_utils/trajectory/trajectory.hpp>
 #include <utilization/util.hpp>
 
 #include <autoware_auto_perception_msgs/msg/object_classification.hpp>
@@ -42,6 +42,16 @@
 #include <utility>
 #include <vector>
 
+namespace tier4_autoware_utils
+{
+template <>
+inline geometry_msgs::msg::Pose getPose(
+  const autoware_auto_planning_msgs::msg::PathPointWithLaneId & p)
+{
+  return p.point.pose;
+}
+}  // namespace tier4_autoware_utils
+
 namespace behavior_velocity_planner
 {
 using autoware_auto_perception_msgs::msg::ObjectClassification;
@@ -67,7 +77,8 @@ using BasicPolygons2d = std::vector<lanelet::BasicPolygon2d>;
 namespace occlusion_spot_utils
 {
 enum ROAD_TYPE { PRIVATE, PUBLIC, HIGHWAY, UNKNOWN };
-enum METHOD { OCCUPANCY_GRID, PREDICTED_OBJECT };
+enum DETECTION_METHOD { OCCUPANCY_GRID, PREDICTED_OBJECT };
+enum PASS_JUDGE { SMOOTH_VELOCITY, CURRENT_VELOCITY };
 
 struct DetectionArea
 {
@@ -99,7 +110,8 @@ struct LatLon
 
 struct PlannerParam
 {
-  METHOD method;
+  DETECTION_METHOD detection_method;
+  PASS_JUDGE pass_judge;
   bool debug;                  // [-]
   bool use_partition_lanelet;  // [-]
   // parameters in yaml
@@ -127,22 +139,6 @@ struct SafeMotion
   double safe_velocity;
 };
 
-// @brief represent the range of a each polygon
-struct SliceRange
-{
-  double min_length{};
-  double max_length{};
-  double min_distance{};
-  double max_distance{};
-};
-
-// @brief representation of a polygon along a path
-struct Slice
-{
-  SliceRange range{};
-  lanelet::BasicPolygon2d polygon{};
-};
-
 struct ObstacleInfo
 {
   SafeMotion safe_motion;  // safe motion of velocity and stop point
@@ -185,7 +181,7 @@ struct DebugData
   double z;
   std::string road_type = "";
   std::string detection_type = "";
-  std::vector<Slice> detection_area_polygons;
+  Polygons2d detection_area_polygons;
   std::vector<lanelet::BasicPolygon2d> partition_lanelets;
   std::vector<geometry_msgs::msg::Point> parked_vehicle_point;
   std::vector<PossibleCollisionInfo> possible_collisions;
@@ -200,22 +196,26 @@ struct DebugData
     occlusion_points.clear();
   }
 };
-
+// apply current velocity to path
+PathWithLaneId applyVelocityToPath(const PathWithLaneId & path, const double v0);
+//!< @brief wrapper for detection area polygon generation
+bool buildDetectionAreaPolygon(
+  Polygons2d & slices, const PathWithLaneId & path, const double offset,
+  const PlannerParam & param);
 lanelet::ConstLanelet toPathLanelet(const PathWithLaneId & path);
 // Note : consider offset_from_start_to_ego and safety margin for collision here
 void handleCollisionOffset(std::vector<PossibleCollisionInfo> & possible_collisions, double offset);
 void clipPathByLength(
   const PathWithLaneId & path, PathWithLaneId & clipped, const double max_length = 100.0);
 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 Polygons2d & polys);
 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);
@@ -236,10 +236,10 @@ void calcSlowDownPointsForPossibleCollision(
 //!< @brief convert a set of occlusion spots found on detection_area slice
 boost::optional<PossibleCollisionInfo> generateOneNotableCollisionFromOcclusionSpot(
   const grid_map::GridMap & grid, const std::vector<grid_map::Position> & occlusion_spot_positions,
-  const double offset_from_start_to_ego, const BasicPoint2d base_point,
+  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/include/scene_module/occlusion_spot/risk_predictive_braking.hpp

@@ -34,15 +34,6 @@ int insertSafeVelocityToPath(
   const geometry_msgs::msg::Pose & in_pose, const double safe_vel, const PlannerParam & param,
   PathWithLaneId * inout_path);
 
-/**
- *
- * @param: longitudinal_distance: longitudinal distance to collision
- * @param: param: planner param
- * @return lateral distance
- **/
-double calculateLateralDistanceFromTTC(
-  const double longitudinal_distance, const PlannerParam & param);
-
 /**
  * @param: v: ego velocity config
  * @param: ttc: time to collision

planning/behavior_velocity_planner/include/utilization/util.hpp

@@ -22,6 +22,7 @@
 #include <autoware_auto_perception_msgs/msg/predicted_object.hpp>
 #include <autoware_auto_perception_msgs/msg/predicted_objects.hpp>
 #include <autoware_auto_planning_msgs/msg/path.hpp>
+#include <autoware_auto_planning_msgs/msg/path_point.hpp>
 #include <autoware_auto_planning_msgs/msg/path_with_lane_id.hpp>
 #include <autoware_auto_planning_msgs/msg/trajectory.hpp>
 #include <autoware_auto_planning_msgs/msg/trajectory_point.hpp>
@@ -56,12 +57,36 @@ inline geometry_msgs::msg::Point getPoint(
 
 namespace behavior_velocity_planner
 {
+struct SearchRangeIndex
+{
+  size_t min_idx;
+  size_t max_idx;
+};
+struct DetectionRange
+{
+  bool use_right = true;
+  bool use_left = true;
+  double interval;
+  double min_longitudinal_distance;
+  double max_longitudinal_distance;
+  double min_lateral_distance;
+  double max_lateral_distance;
+};
+struct PointWithSearchRangeIndex
+{
+  geometry_msgs::msg::Point point;
+  SearchRangeIndex index;
+};
+
 using Point2d = boost::geometry::model::d2::point_xy<double>;
-using Polygons2d = std::vector<lanelet::BasicPolygon2d>;
+using autoware_auto_planning_msgs::msg::PathPoint;
 using autoware_auto_planning_msgs::msg::PathPointWithLaneId;
 using autoware_auto_planning_msgs::msg::PathWithLaneId;
+using BasicPolygons2d = std::vector<lanelet::BasicPolygon2d>;
+using Polygons2d = std::vector<Polygon2d>;
 namespace planning_utils
 {
+using geometry_msgs::msg::Pose;
 inline geometry_msgs::msg::Point getPoint(const geometry_msgs::msg::Point & p) { return p; }
 inline geometry_msgs::msg::Point getPoint(const geometry_msgs::msg::Pose & p) { return p.position; }
 inline geometry_msgs::msg::Point getPoint(const geometry_msgs::msg::PoseStamped & p)
@@ -97,7 +122,15 @@ inline geometry_msgs::msg::Pose getPose(
 {
   return traj.points.at(idx).pose;
 }
-void getAllPartitionLanelets(const lanelet::LaneletMapConstPtr ll, Polygons2d & polys);
+
+// create detection area from given range return false if creation failure
+bool createDetectionAreaPolygons(
+  Polygons2d & slices, const PathWithLaneId & path, const DetectionRange da_range,
+  const double obstacle_vel_mps);
+PathPoint getLerpPathPointWithLaneId(const PathPoint p0, const PathPoint p1, const double ratio);
+Point2d calculateLateralOffsetPoint2d(const Pose & p, const double offset);
+
+void getAllPartitionLanelets(const lanelet::LaneletMapConstPtr ll, BasicPolygons2d & polys);
 void setVelocityFrom(const size_t idx, const double vel, PathWithLaneId * input);
 void insertVelocity(
   PathWithLaneId & path, const PathPointWithLaneId & path_point, const double v,