chore(behavior_velocity): refactor and use planner param

Signed-off-by: tanaka3 <ttatcoder@outlook.jp>
autowarefoundation · Feb 10, 2022 · 053abaf · 053abaf
1 parent 790bb52
commit 053abaf
Show file tree

Hide file tree

Showing 12 changed files with 96 additions and 135 deletions.
diff --git a/planning/behavior_velocity_planner/config/occlusion_spot.param.yaml b/planning/behavior_velocity_planner/config/occlusion_spot.param.yaml
@@ -9,13 +9,13 @@
       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
+        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: 1.5                # [m] size of slices in both length and distance relative to the ego path.
+        max_lateral_distance: 3.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/include/scene_module/occlusion_spot/geometry.hpp b/planning/behavior_velocity_planner/include/scene_module/occlusion_spot/geometry.hpp
@@ -30,33 +30,15 @@ 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 resolution);
-//!< @brief build sidewalk slice from path
+//!< @brief build detection_area slice from path
 void buildDetectionAreaPolygon(
-  std::vector<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> & slice, const lanelet::ConstLanelet & path_lanelet, 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)

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>
@@ -65,10 +64,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
@@ -84,6 +83,12 @@ struct Velocity
   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
 {
   // parameters in yaml
@@ -101,7 +106,7 @@ struct PlannerParam
   double baselink_to_front;   // [m]  wheel_base + front_overhang
 
   Velocity v;
-  Sidewalk sidewalk;
+  DetectionArea detection_area;
   grid_utils::GridParam grid;
 };
 
@@ -111,6 +116,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
@@ -228,14 +249,14 @@ 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(
+//!< @brief convert a set of occlusion spots found on detection_area slice
+void generateDetectionAreaPossibleCollisionFromOcclusionSpot(
   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 generate possible collisions coming from occlusion spots on the side of the path
-void generateSidewalkPossibleCollisions(
+void generateDetectionAreaPossibleCollisions(
   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);

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,7 +46,7 @@ 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;
     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
@@ -45,7 +45,7 @@ class OcclusionSpotInPublicModule : public SceneModuleInterface
   {
     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;
   };

diff --git a/planning/behavior_velocity_planner/occlusion-spot-design.md b/planning/behavior_velocity_planner/occlusion-spot-design.md
@@ -93,16 +93,16 @@ while ego is cruising from safe margin to collision path point ego keeps same ve
 | `delay_time`          | double | [m/s] time buffer for the system delay                       |
 | `safe_margin`         | double | [m] maximum error to stop with emergency braking system.     |
 
-| Parameter /sidewalk       | Type   | Description                                                     |
-| ------------------------- | ------ | --------------------------------------------------------------- |
-| `min_occlusion_spot_size` | double | [m] the length of path to consider occlusion spot               |
-| `slice_size`              | double | [m] the distance of divided detection area                      |
-| `focus_range`             | double | [m] buffer around the ego path used to build the sidewalk area. |
-
-| Parameter /grid  | Type   | Description                                                     |
-| ---------------- | ------ | --------------------------------------------------------------- |
-| `free_space_max` | double | [-] maximum value of a free space cell in the occupancy grid    |
-| `occupied_min`   | double | [-] buffer around the ego path used to build the sidewalk area. |
+| Parameter /detection_area | Type   | Description                                                           |
+| ------------------------- | ------ | --------------------------------------------------------------------- |
+| `min_occlusion_spot_size` | double | [m] the length of path to consider occlusion spot                     |
+| `slice_length`            | double | [m] the distance of divided detection area                            |
+| `max_lateral_distance`    | double | [m] buffer around the ego path used to build the detection_area area. |
+
+| Parameter /grid  | Type   | Description                                                           |
+| ---------------- | ------ | --------------------------------------------------------------------- |
+| `free_space_max` | double | [-] maximum value of a free space cell in the occupancy grid          |
+| `occupied_min`   | double | [-] buffer around the ego path used to build the detection_area area. |
 
 #### Flowchart
 

diff --git a/planning/behavior_velocity_planner/src/scene_module/occlusion_spot/debug.cpp b/planning/behavior_velocity_planner/src/scene_module/occlusion_spot/debug.cpp
@@ -152,7 +152,7 @@ visualization_msgs::msg::MarkerArray makePolygonMarker(
   debug_marker.scale = tier4_autoware_utils::createMarkerScale(0.1, 0.1, 0.1);
   debug_marker.color = tier4_autoware_utils::createMarkerColor(1.0, 0.0, 1.0, 0.3);
   debug_marker.lifetime = rclcpp::Duration::from_seconds(0.1);
-  debug_marker.ns = "sidewalk";
+  debug_marker.ns = "detection_area";
   for (const auto & poly : polygons) {
     for (const auto & p : poly) {
       geometry_msgs::msg::Point point =
@@ -252,7 +252,8 @@ visualization_msgs::msg::MarkerArray OcclusionSpotInPrivateModule::createDebugMa
   visualization_msgs::msg::MarkerArray debug_marker_array;
   appendMarkerArray(createMarkers(debug_data_, module_id_), current_time, &debug_marker_array);
   appendMarkerArray(
-    makePolygonMarker(debug_data_.sidewalks, debug_data_.z), current_time, &debug_marker_array);
+    makePolygonMarker(debug_data_.detection_areas, debug_data_.z), current_time,
+    &debug_marker_array);
   appendMarkerArray(
     createPathMarkerArray(debug_data_.path_raw, "path_raw", 0, 0.0, 1.0, 1.0), current_time,
     &debug_marker_array);

diff --git a/planning/behavior_velocity_planner/src/scene_module/occlusion_spot/geometry.cpp b/planning/behavior_velocity_planner/src/scene_module/occlusion_spot/geometry.cpp
@@ -16,6 +16,7 @@
 #include <lanelet2_extension/utility/utilities.hpp>
 #include <rclcpp/rclcpp.hpp>
 #include <scene_module/occlusion_spot/geometry.hpp>
+#include <scene_module/occlusion_spot/occlusion_spot_utils.hpp>
 
 #include <algorithm>
 #include <vector>
@@ -57,7 +58,7 @@ void createOffsetLineString(
 
 void buildSlices(
   std::vector<Slice> & slices, const lanelet::ConstLanelet & path_lanelet, const SliceRange & range,
-  const double slice_length, [[maybe_unused]] const double slice_width, const double resolution)
+  const double slice_length, const double resolution)
 {
   /**
    * @brief bounds
@@ -105,24 +106,22 @@ void buildSlices(
 }
 
 void buildDetectionAreaPolygon(
-  std::vector<Slice> & slices, const lanelet::ConstLanelet & path_lanelet,
-  const double longitudinal_offset, const double lateral_offset, const double slice_size,
-  const double lateral_max_dist)
+  std::vector<Slice> & slices, const lanelet::ConstLanelet & path_lanelet, const double offset,
+  const PlannerParam & param)
 {
   std::vector<Slice> left_slices;
   std::vector<Slice> right_slices;
-  const double longitudinal_max_dist = lg::length2d(path_lanelet);
+  const double d_min = param.half_vehicle_width;
+  const double d_max = param.detection_area.max_lateral_distance;
   SliceRange left_slice_range = {
-    longitudinal_offset, longitudinal_max_dist, lateral_offset, lateral_offset + lateral_max_dist};
+    offset + param.baselink_to_front, param.detection_area_length, d_min, d_max};
   // in most case lateral distance is much more effective for velocity planning
-  const double slice_length = 4.0 * slice_size;
-  const double slice_width = slice_size;
-  const double resolution = 1.0;
-  buildSlices(left_slices, path_lanelet, left_slice_range, slice_length, slice_width, resolution);
+  const double slice_length = param.detection_area.slice_length;
+  const double resolution = 1.0;  // interpolation interval TODO parametrize this
+  buildSlices(left_slices, path_lanelet, left_slice_range, slice_length, resolution);
   SliceRange right_slice_range = {
-    longitudinal_offset, longitudinal_max_dist, -lateral_offset,
-    -lateral_offset - lateral_max_dist};
-  buildSlices(right_slices, path_lanelet, right_slice_range, slice_length, slice_width, resolution);
+    offset + param.baselink_to_front, param.detection_area_length, -d_min, -d_max};
+  buildSlices(right_slices, path_lanelet, right_slice_range, slice_length, resolution);
   // Properly order lanelets from closest to furthest
   slices = left_slices;
   slices.insert(slices.end(), right_slices.begin(), right_slices.end());

diff --git a/planning/behavior_velocity_planner/src/scene_module/occlusion_spot/manager.cpp b/planning/behavior_velocity_planner/src/scene_module/occlusion_spot/manager.cpp
@@ -95,11 +95,12 @@ OcclusionSpotModuleManager::OcclusionSpotModuleManager(rclcpp::Node & node)
   pp.v.safe_margin = node.declare_parameter(ns + ".motion.safe_margin", 1.0);
   pp.v.max_slow_down_accel = node.declare_parameter(ns + ".motion.max_slow_down_accel", -1.5);
   pp.v.min_allowed_velocity = node.declare_parameter(ns + ".motion.min_allowed_velocity", 1.0);
-  // sidewalk param
-  pp.sidewalk.min_occlusion_spot_size =
-    node.declare_parameter(ns + ".sidewalk.min_occlusion_spot_size", 2.0);
-  pp.sidewalk.focus_range = node.declare_parameter(ns + ".sidewalk.focus_range", 4.0);
-  pp.sidewalk.slice_size = node.declare_parameter(ns + ".sidewalk.slice_size", 1.5);
+  // detection_area param
+  pp.detection_area.min_occlusion_spot_size =
+    node.declare_parameter(ns + ".detection_area.min_occlusion_spot_size", 2.0);
+  pp.detection_area.max_lateral_distance =
+    node.declare_parameter(ns + ".detection_area.max_lateral_distance", 4.0);
+  pp.detection_area.slice_length = node.declare_parameter(ns + ".detection_area.slice_length", 1.5);
   // occupancy grid param
   pp.grid.free_space_max = node.declare_parameter(ns + ".grid.free_space_max", 10);
   pp.grid.occupied_min = node.declare_parameter(ns + ".grid.occupied_min", 51);

diff --git a/planning/behavior_velocity_planner/src/scene_module/occlusion_spot/occlusion_spot_utils.cpp b/planning/behavior_velocity_planner/src/scene_module/occlusion_spot/occlusion_spot_utils.cpp
@@ -14,6 +14,7 @@
 
 #include <interpolation/spline_interpolation.hpp>
 #include <rclcpp/rclcpp.hpp>
+#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 <tier4_autoware_utils/geometry/geometry.hpp>
@@ -415,7 +416,7 @@ void filterCollisionByRoadType(
   }
 }
 
-void generateSidewalkPossibleCollisions(
+void generateDetectionAreaPossibleCollisions(
   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<lanelet::BasicPolygon2d> & debug)
@@ -425,44 +426,45 @@ void generateSidewalkPossibleCollisions(
     return;
   }
   using Slice = occlusion_spot_utils::Slice;
-  std::vector<Slice> sidewalk_slices;
+  std::vector<Slice> detection_area_polygons;
   occlusion_spot_utils::buildDetectionAreaPolygon(
-    sidewalk_slices, path_lanelet, 0.0, param.half_vehicle_width, param.sidewalk.slice_size,
-    param.sidewalk.focus_range);
+    detection_area_polygons, path_lanelet, offset_from_start_to_ego, param);
   double length_lower_bound = std::numeric_limits<double>::max();
   double distance_lower_bound = std::numeric_limits<double>::max();
   // sort distance closest first to skip inferior collision
-  std::sort(sidewalk_slices.begin(), sidewalk_slices.end(), [](const Slice & s1, const Slice s2) {
-    return std::abs(s1.range.min_distance) < std::abs(s2.range.min_distance);
-  });
+  std::sort(
+    detection_area_polygons.begin(), detection_area_polygons.end(),
+    [](const Slice & s1, const Slice s2) {
+      return std::abs(s1.range.min_distance) < std::abs(s2.range.min_distance);
+    });
 
-  std::sort(sidewalk_slices.begin(), sidewalk_slices.end(), [](const Slice s1, const Slice s2) {
-    return s1.range.min_length < s2.range.min_length;
-  });
+  std::sort(
+    detection_area_polygons.begin(), detection_area_polygons.end(),
+    [](const Slice s1, const Slice s2) { return s1.range.min_length < s2.range.min_length; });
 
-  for (const Slice sidewalk_slice : sidewalk_slices) {
-    debug.push_back(sidewalk_slice.polygon);
-    if ((sidewalk_slice.range.min_length < length_lower_bound ||
-         std::abs(sidewalk_slice.range.min_distance) < distance_lower_bound)) {
+  for (const Slice detection_area_slice : detection_area_polygons) {
+    debug.push_back(detection_area_slice.polygon);
+    if ((detection_area_slice.range.min_length < length_lower_bound ||
+         std::abs(detection_area_slice.range.min_distance) < distance_lower_bound)) {
       std::vector<grid_map::Position> occlusion_spot_positions;
       grid_utils::findOcclusionSpots(
-        occlusion_spot_positions, grid, sidewalk_slice.polygon,
-        param.sidewalk.min_occlusion_spot_size);
-      generateSidewalkPossibleCollisionFromOcclusionSpot(
+        occlusion_spot_positions, grid, detection_area_slice.polygon,
+        param.detection_area.min_occlusion_spot_size);
+      generateDetectionAreaPossibleCollisionFromOcclusionSpot(
         possible_collisions, grid, occlusion_spot_positions, offset_from_start_to_ego, path_lanelet,
         param);
       if (!possible_collisions.empty()) {
-        length_lower_bound = sidewalk_slice.range.min_length;
-        distance_lower_bound = std::abs(sidewalk_slice.range.min_distance);
+        length_lower_bound = detection_area_slice.range.min_length;
+        distance_lower_bound = std::abs(detection_area_slice.range.min_distance);
         possible_collisions.insert(
           possible_collisions.end(), possible_collisions.begin(), possible_collisions.end());
-        debug.push_back(sidewalk_slice.polygon);
+        debug.push_back(detection_area_slice.polygon);
       }
     }
   }
 }
 
-void generateSidewalkPossibleCollisionFromOcclusionSpot(
+void generateDetectionAreaPossibleCollisionFromOcclusionSpot(
   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,

diff --git a/...velocity_planner/src/scene_module/occlusion_spot/scene_occlusion_spot_in_private_road.cpp b/...velocity_planner/src/scene_module/occlusion_spot/scene_occlusion_spot_in_private_road.cpp
@@ -91,9 +91,9 @@ bool OcclusionSpotInPrivateModule::modifyPathVelocity(
     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::generateSidewalkPossibleCollisions(
+  utils::generateDetectionAreaPossibleCollisions(
     possible_collisions, grid_map, interp_path, offset_from_start_to_ego, param_,
-    debug_data_.sidewalks);
+    debug_data_.detection_areas);
   utils::filterCollisionByRoadType(possible_collisions, focus_area);
   RCLCPP_DEBUG_STREAM_THROTTLE(
     logger_, *clock_, 3000, "num possible collision:" << possible_collisions.size());