From d87e55aba9b8cdd1102384c91881050998c8b418 Mon Sep 17 00:00:00 2001 From: tanaka3 Date: Thu, 24 Mar 2022 14:39:35 +0900 Subject: [PATCH] feat(behavior_velocity): add offset from front bumper Signed-off-by: tanaka3 --- .../behavior_velocity_planner/config/occlusion_spot.param.yaml | 1 + .../scene_module/occlusion_spot/occlusion_spot_utils.hpp | 1 + .../src/scene_module/occlusion_spot/manager.cpp | 2 ++ .../src/scene_module/occlusion_spot/occlusion_spot_utils.cpp | 3 ++- 4 files changed, 6 insertions(+), 1 deletion(-) diff --git a/planning/behavior_velocity_planner/config/occlusion_spot.param.yaml b/planning/behavior_velocity_planner/config/occlusion_spot.param.yaml index 2f566370b0db8..a7537de5fcb6c 100644 --- 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/planning/behavior_velocity_planner/include/scene_module/occlusion_spot/occlusion_spot_utils.hpp b/planning/behavior_velocity_planner/include/scene_module/occlusion_spot/occlusion_spot_utils.hpp index 57fafb97330b2..5568dea6cd3d6 100644 --- a/planning/behavior_velocity_planner/include/scene_module/occlusion_spot/occlusion_spot_utils.hpp +++ b/planning/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 index 179c653799a59..4eb5635461c7e 100644 --- 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 index fc59ba134b7f0..15e57021807b0 100644 --- 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;