feat(behavior_velocity): improve occlusion spot detection area polygon (

autowarefoundation#354) * feat(behavior_velocity): create triangle detection area polygon Signed-off-by: tanaka3 <ttatcoder@outlook.jp> * chore(behavior_velocity): refactor Signed-off-by: tanaka3 <ttatcoder@outlook.jp> * chore(behavior_velocity): refactor and use planner param Signed-off-by: tanaka3 <ttatcoder@outlook.jp> * chore(behavior_velocity): refactor detection area creation Signed-off-by: tanaka3 <ttatcoder@outlook.jp> * chore(behavior_velocity): refactor possible collision search Signed-off-by: tanaka3 <ttatcoder@outlook.jp> * feat(behavior_velocity): create dynamic poly Signed-off-by: tanaka3 <ttatcoder@outlook.jp> * feat(behavior_velocity): move dynamic object poly to upper function layer Signed-off-by: taikitanaka <ttatcoder@outlook.jp> Signed-off-by: tanaka3 <ttatcoder@outlook.jp> * chore(behavior_velocity): refactor utils Signed-off-by: taikitanaka <ttatcoder@outlook.jp> Signed-off-by: tanaka3 <ttatcoder@outlook.jp> * feat(behavior_velocity): add da polygon to public module too Signed-off-by: taikitanaka <ttatcoder@outlook.jp> Signed-off-by: tanaka3 <ttatcoder@outlook.jp> * feat(behavior_velocity): add da polygon gtest Signed-off-by: taikitanaka <ttatcoder@outlook.jp> Signed-off-by: tanaka3 <ttatcoder@outlook.jp> * chore(behavior_velocity): improve visualization Signed-off-by: tanaka3 <ttatcoder@outlook.jp> * chore(behavior_velocity): visualize occlusion spot Signed-off-by: tanaka3 <ttatcoder@outlook.jp> * chore(behavior_velocity): add debug topics Signed-off-by: tanaka3 <ttatcoder@outlook.jp> * feat(behavior_velocity): better occlusion spot detection Signed-off-by: tanaka3 <ttatcoder@outlook.jp> * chore(behavior_velocity): better expression and early return Signed-off-by: tanaka3 <ttatcoder@outlook.jp> * feat(behavior_velocity): change cmp method of notable collision Signed-off-by: tanaka3 <ttatcoder@outlook.jp> * !chore(dummy_perception): disable raytracing need to revert this before merge Signed-off-by: tanaka3 <ttatcoder@outlook.jp> * chore(behavior_velocity): fix typo Signed-off-by: tanaka3 <ttatcoder@outlook.jp> * chore(behavior_velocity): add description of detection area polygon Signed-off-by: tanaka3 <ttatcoder@outlook.jp> * feat(behavior_velocity): filter occlusion by detection area Signed-off-by: tanaka3 <ttatcoder@outlook.jp> * chore(behavior_velocity): early skip at continue case Signed-off-by: tanaka3 <ttatcoder@outlook.jp> * chore(behavior_velocity): refactor names Signed-off-by: tanaka3 <ttatcoder@outlook.jp> * feat(behavior_velocity): care corner position at grid Signed-off-by: tanaka3 <ttatcoder@outlook.jp> * feat(behavior_velocity): introduce detection area max length Signed-off-by: tanaka3 <ttatcoder@outlook.jp> * chore(behavior_velocity): minor update Signed-off-by: tanaka3 <ttatcoder@outlook.jp> * chore(behavior_velocity): better visualization Signed-off-by: tanaka3 <ttatcoder@outlook.jp> * ci(pre-commit): autofix * chore(behavior_velocity): better comment Signed-off-by: tanaka3 <ttatcoder@outlook.jp> * doc(behavior_velocity): update doc Signed-off-by: tanaka3 <ttatcoder@outlook.jp> * chore(behavior_velocity): minor fix Signed-off-by: tanaka3 <ttatcoder@outlook.jp> * !chore(behavior_velocity): revert debug values Signed-off-by: tanaka3 <ttatcoder@outlook.jp> Co-authored-by: pre-commit-ci[bot] <66853113+pre-commit-ci[bot]@users.noreply.github.com>
isamu-takagi · Feb 16, 2022 · dbb7cbb · dbb7cbb
1 parent 942f81c
commit dbb7cbb
Show file tree

Hide file tree

Showing 19 changed files with 522 additions and 548 deletions.
diff --git a/planning/behavior_velocity_planner/config/occlusion_spot.param.yaml b/planning/behavior_velocity_planner/config/occlusion_spot.param.yaml
@@ -1,6 +1,7 @@
 /**:
   ros__parameters:
     occlusion_spot:
+      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:
         detection_area_length: 100.0        # [m] the length of path to consider perception range
@@ -9,13 +10,15 @@
       motion:
         safety_ratio: 0.8                   # [-] jerk/acceleration ratio for safety
         max_slow_down_acceleration: -1.5    # [m/s^2] minimum deceleration for safe deceleration.
-        min_allowed_velocity: 1.0       # [m/s] minimum velocity allowed
+        non_effective_jerk: -0.3            # [m/s^3] weak jerk for velocity planning.
+        non_effective_acceleration: -1.0    # [m/s^2] weak deceleration for velocity planning.
+        min_allowed_velocity: 1.0           # [m/s] minimum velocity allowed
         delay_time: 0.1                     # [s] safety time buffer for delay system response
         safe_margin: 1.0                    # [m] maximum safety distance for any error
-      sidewalk:
+      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_size: 1.5     # [m] size of sidewalk slices in both length and distance relative to the ego trajectory.
-        focus_range: 3.0            # [m] buffer around the ego path used to build the sidewalk area.
+        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
-        occupied_min: 60   # minimum value of an occupied cell in the occupancy grid
+        free_space_max: 40  # [-] maximum value of a free space cell in the occupancy grid
+        occupied_min: 60    # [-] minimum value of an occupied cell in the occupancy grid
diff --git a/planning/behavior_velocity_planner/docs/occlusion_spot/detection_area_poly.svg b/planning/behavior_velocity_planner/docs/occlusion_spot/detection_area_poly.svg
diff --git a/planning/behavior_velocity_planner/include/scene_module/occlusion_spot/geometry.hpp b/planning/behavior_velocity_planner/include/scene_module/occlusion_spot/geometry.hpp
@@ -15,6 +15,8 @@
 #ifndef SCENE_MODULE__OCCLUSION_SPOT__GEOMETRY_HPP_
 #define SCENE_MODULE__OCCLUSION_SPOT__GEOMETRY_HPP_
 
+#include <scene_module/occlusion_spot/occlusion_spot_utils.hpp>
+
 #include <boost/geometry.hpp>
 
 #include <lanelet2_core/geometry/Lanelet.h>
@@ -24,45 +26,27 @@
 
 namespace behavior_velocity_planner
 {
-namespace geometry
+namespace occlusion_spot_utils
 {
 namespace bg = boost::geometry;
 
-// @brief represent the range of a slice
-// (either by index on the lanelet centerline or by arc coordinates)
-struct SliceRange
-{
-  double min_length{};
-  double max_length{};
-  double min_distance{};
-  double max_distance{};
-};
-
-// @brief representation of a slice along a path
-struct Slice
-{
-  SliceRange range{};
-  lanelet::BasicPolygon2d polygon{};
-};
-
 //!< @brief build slices all along the trajectory
 // using the given range and desired slice length and width
 void buildSlices(
-  std::vector<Slice> & slices, const lanelet::ConstLanelet & path_lanelet, const SliceRange & range,
-  const double slice_length, const double slice_width, const double resolution);
-//!< @brief build sidewalk slice from path
-void buildSidewalkSlices(
-  std::vector<geometry::Slice> & slice, const lanelet::ConstLanelet & path_lanelet,
-  const double longitudinal_offset, const double lateral_offset, const double min_size,
-  const double lateral_max_dist);
+  std::vector<Slice> & slices, const lanelet::ConstLanelet & path_lanelet, const double offset,
+  const bool is_on_right, const PlannerParam & param);
+//!< @brief build detection_area slice from path
+void buildDetectionAreaPolygon(
+  std::vector<Slice> & slices, const PathWithLaneId & path, const double offset,
+  const PlannerParam & param);
 //!< @brief calculate interpolation between a and b at distance ratio t
 template <typename T>
 T lerp(T a, T b, double t)
 {
   return a + t * (b - a);
 }
 
-}  // namespace geometry
+}  // namespace occlusion_spot_utils
 }  // namespace behavior_velocity_planner
 
 #endif  // SCENE_MODULE__OCCLUSION_SPOT__GEOMETRY_HPP_
diff --git a/planning/behavior_velocity_planner/include/scene_module/occlusion_spot/grid_utils.hpp b/planning/behavior_velocity_planner/include/scene_module/occlusion_spot/grid_utils.hpp
@@ -19,7 +19,6 @@
 #include <grid_map_core/iterators/LineIterator.hpp>
 #include <grid_map_core/iterators/PolygonIterator.hpp>
 #include <opencv2/opencv.hpp>
-#include <scene_module/occlusion_spot/geometry.hpp>
 
 #include <nav_msgs/msg/occupancy_grid.hpp>
 
@@ -51,7 +50,7 @@ struct OcclusionSpotSquare
 {
   grid_map::Index index;        // index of the anchor
   grid_map::Position position;  // position of the anchor
-  int side_size;                // number of cells for each side of the square
+  int min_occlusion_size;       // number of cells for each side of the square
 };
 // @brief structure representing a OcclusionSpot on the OccupancyGrid
 struct OcclusionSpot
@@ -64,7 +63,7 @@ struct OcclusionSpot
 // if the given cell is a occlusion_spot square of size min_size*min_size in the given grid
 bool isOcclusionSpotSquare(
   OcclusionSpotSquare & occlusion_spot, const grid_map::Matrix & grid_data,
-  const grid_map::Index & cell, const int side_size, const grid_map::Size & grid_size);
+  const grid_map::Index & cell, const int min_occlusion_size, const grid_map::Size & grid_size);
 //!< @brief Find all occlusion spots inside the given lanelet
 void findOcclusionSpots(
   std::vector<grid_map::Position> & occlusion_spot_positions, const grid_map::GridMap & grid,

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
@@ -18,7 +18,6 @@
 #include <lanelet2_extension/utility/query.hpp>
 #include <lanelet2_extension/utility/utilities.hpp>
 #include <lanelet2_extension/visualization/visualization.hpp>
-#include <scene_module/occlusion_spot/geometry.hpp>
 #include <scene_module/occlusion_spot/grid_utils.hpp>
 #include <tier4_autoware_utils/geometry/geometry.hpp>
 #include <utilization/util.hpp>
@@ -30,6 +29,8 @@
 #include <autoware_auto_planning_msgs/msg/path_with_lane_id.hpp>
 #include <visualization_msgs/msg/marker.hpp>
 
+#include <boost/optional.hpp>
+
 #include <lanelet2_core/LaneletMap.h>
 #include <lanelet2_core/geometry/LaneletMap.h>
 #include <tf2/utils.h>
@@ -65,10 +66,10 @@ namespace occlusion_spot_utils
 {
 enum ROAD_TYPE { PRIVATE, PUBLIC, HIGHWAY, UNKNOWN };
 
-struct Sidewalk
+struct DetectionArea
 {
-  double focus_range;              // [m] distance to care about occlusion spot
-  double slice_size;               // [m] size of each slice
+  double max_lateral_distance;     // [m] distance to care about occlusion spot
+  double slice_length;             // [m] size of each slice
   double min_occlusion_spot_size;  // [m] minumum size to care about the occlusion spot
 };
 struct Velocity
@@ -77,31 +78,40 @@ struct Velocity
   double max_stop_jerk;         // [m/s^3] emergency braking system jerk
   double max_stop_accel;        // [m/s^2] emergency braking system deceleration
   double max_slow_down_accel;   // [m/s^2] maximum allowed deceleration
+  double non_effective_jerk;    // [m/s^3] too weak jerk for velocity planning.
+  double non_effective_accel;   // [m/s^2] too weak deceleration for velocity planning.
   double min_allowed_velocity;  // [m/s]   minimum allowed velocity not to stop
   double a_ego;                 // [m/s^2] current ego acceleration
   double v_ego;                 // [m/s]   current ego velocity
   double delay_time;            // [s] safety time buffer for delay response
   double safe_margin;           // [m] maximum safety distance for any error
 };
 
+struct LatLon
+{
+  double lateral_distance;       // [m] lateral distance
+  double longitudinal_distance;  // [m] longitudinal distance
+};
+
 struct PlannerParam
 {
+  bool debug;  // [-]
   // parameters in yaml
-  double detection_area_length;  // [m]
-  double stuck_vehicle_vel;      // [m/s]
-  double lateral_distance_thr;   // [m] lateral distance threshold to consider
-  double pedestrian_vel;         // [m/s]
+  double detection_area_length;      // [m]
+  double detection_area_max_length;  // [m]
+  double stuck_vehicle_vel;          // [m/s]
+  double lateral_distance_thr;       // [m] lateral distance threshold to consider
+  double pedestrian_vel;             // [m/s]
 
-  double dist_thr;       // [m]
-  double angle_thr;      // [rad]
-  bool show_debug_grid;  // [-]
+  double dist_thr;   // [m]
+  double angle_thr;  // [rad]
 
   // vehicle info
   double half_vehicle_width;  // [m]  half vehicle_width from vehicle info
   double baselink_to_front;   // [m]  wheel_base + front_overhang
 
   Velocity v;
-  Sidewalk sidewalk;
+  DetectionArea detection_area;
   grid_utils::GridParam grid;
 };
 
@@ -111,6 +121,22 @@ 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
@@ -199,6 +225,8 @@ inline bool isStuckVehicle(PredictedObject obj, const double min_vel)
 }
 void filterCollisionByRoadType(
   std::vector<PossibleCollisionInfo> & possible_collisions, const DetectionAreaIdx road_type);
+std::vector<PredictedObject> filterDynamicObjectByDetectionArea(
+  std::vector<PredictedObject> & objs, const std::vector<Slice> polys);
 bool splineInterpolate(
   const PathWithLaneId & input, const double interval, PathWithLaneId * output,
   const rclcpp::Logger logger);
@@ -228,17 +256,17 @@ void calcSlowDownPointsForPossibleCollision(
 DetectionAreaIdx extractTargetRoadArcLength(
   const LaneletMapPtr lanelet_map_ptr, const double max_range, const PathWithLaneId & path,
   const ROAD_TYPE & target_road_type);
-//!< @brief convert a set of occlusion spots found on sidewalk slice
-void generateSidewalkPossibleCollisionFromOcclusionSpot(
-  std::vector<PossibleCollisionInfo> & possible_collisions, const grid_map::GridMap & grid,
-  const std::vector<grid_map::Position> & occlusion_spot_positions,
-  const double offset_from_start_to_ego, const lanelet::ConstLanelet & path_lanelet,
-  const PlannerParam & param);
+//!< @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 basic_point,
+  const lanelet::ConstLanelet & path_lanelet, const PlannerParam & param);
 //!< @brief generate possible collisions coming from occlusion spots on the side of the path
-void generateSidewalkPossibleCollisions(
+void createPossibleCollisionsInDetectionArea(
+  const std::vector<Slice> & detection_area_polygons,
   std::vector<PossibleCollisionInfo> & possible_collisions, const grid_map::GridMap & grid,
   const PathWithLaneId & path, const double offset_from_start_to_ego, const PlannerParam & param,
-  std::vector<BasicPolygon2d> & debug);
+  std::vector<Point> & debug_points);
 
 }  // namespace occlusion_spot_utils
 }  // namespace behavior_velocity_planner

diff --git a/...behavior_velocity_planner/include/scene_module/occlusion_spot/risk_predictive_braking.hpp b/...behavior_velocity_planner/include/scene_module/occlusion_spot/risk_predictive_braking.hpp
@@ -44,7 +44,32 @@ inline double calculateMinSlowDownVelocity(
 }
 
 /**
- * @param: sv: ego velocity config
+ *
+ * @param: longitudinal_distance: longitudinal distance to collision
+ * @param: param: planner param
+ * @return lateral distance
+ **/
+inline double calculateLateralDistanceFromTTC(
+  const double longitudinal_distance, const PlannerParam & param)
+{
+  const auto & v = param.v;
+  const auto & p = param;
+  double v_min = 1.0;
+  const double lateral_buffer = 0.5;
+  const double min_distance = p.half_vehicle_width + lateral_buffer;
+  const double max_distance = p.detection_area.max_lateral_distance;
+  if (longitudinal_distance <= 0) return min_distance;
+  if (v_min < param.v.min_allowed_velocity) v_min = param.v.min_allowed_velocity;
+  // use min velocity if ego velocity is below min allowed
+  const double v0 = (v.v_ego > v_min) ? v.v_ego : v_min;
+  // here is a part where ego t(ttc) can be replaced by calculation of velocity smoother or ?
+  double t = longitudinal_distance / v0;
+  double lateral_distance = t * param.pedestrian_vel + p.half_vehicle_width;
+  return std::min(max_distance, std::max(min_distance, lateral_distance));
+}
+
+/**
+ * @param: v: ego velocity config
  * @param: ttc: time to collision
  * @return safe motion
  **/

diff --git a/...city_planner/include/scene_module/occlusion_spot/scene_occlusion_spot_in_private_road.hpp b/...city_planner/include/scene_module/occlusion_spot/scene_occlusion_spot_in_private_road.hpp
@@ -46,8 +46,9 @@ class OcclusionSpotInPrivateModule : public SceneModuleInterface
   {
     double z;
     std::string road_type = "private";
-    std::vector<lanelet::BasicPolygon2d> sidewalks;
+    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;
   };

diff --git a/...ocity_planner/include/scene_module/occlusion_spot/scene_occlusion_spot_in_public_road.hpp b/...ocity_planner/include/scene_module/occlusion_spot/scene_occlusion_spot_in_public_road.hpp
@@ -44,10 +44,13 @@ class OcclusionSpotInPublicModule : public SceneModuleInterface
   struct DebugData
   {
     std::string road_type = "public";
-    double z;
-    std::vector<lanelet::BasicPolygon2d> sidewalks;
+    std::vector<lanelet::BasicPolygon2d> detection_areas;
     std::vector<geometry_msgs::msg::Point> parked_vehicle_point;
     std::vector<occlusion_spot_utils::PossibleCollisionInfo> possible_collisions;
+    std::vector<geometry_msgs::msg::Point> occlusion_points;
+    PathWithLaneId path_raw;
+    PathWithLaneId interp_path;
+    double z;
   };
 
   OcclusionSpotInPublicModule(