feat(behavior_velocity): add offset from front bumper

Signed-off-by: tanaka3 <ttatcoder@outlook.jp>
autowarefoundation · Mar 24, 2022 · d87e55a · d87e55a
1 parent 148f59b
commit d87e55a
Show file tree

Hide file tree

Showing 4 changed files with 6 additions and 1 deletion.
diff --git a/planning/behavior_velocity_planner/config/occlusion_spot.param.yaml b/planning/behavior_velocity_planner/config/occlusion_spot.param.yaml
@@ -27,6 +27,7 @@
       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.
+        min_longitudinal_offset: 1.0     # [m] detection area safety buffer from front bumper.
         max_lateral_distance: 6.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

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
@@ -82,6 +82,7 @@ enum PASS_JUDGE { SMOOTH_VELOCITY, CURRENT_VELOCITY };
 
 struct DetectionArea
 {
+  double min_longitudinal_offset;  // [m] detection area safety buffer from front bumper
   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

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
@@ -88,6 +88,8 @@ OcclusionSpotModuleManager::OcclusionSpotModuleManager(rclcpp::Node & node)
   // 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.min_longitudinal_offset =
+    node.declare_parameter(ns + ".detection_area.min_longitudinal_offset", 1.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);

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
@@ -66,7 +66,8 @@ bool buildDetectionAreaPolygon(
   const auto & p = param;
   DetectionRange da_range;
   da_range.interval = p.detection_area.slice_length;
-  da_range.min_longitudinal_distance = offset + p.baselink_to_front;
+  da_range.min_longitudinal_distance =
+    offset + std::max(0.0, p.baselink_to_front - p.detection_area.min_longitudinal_offset);
   da_range.max_longitudinal_distance =
     std::min(p.detection_area_max_length, p.detection_area_length) +
     da_range.min_longitudinal_distance;