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 c407d2ad8168f..bd2e601a4f91a 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 @@ -115,6 +115,7 @@ struct PlannerParam bool use_moving_object_ray_cast; // [-] bool use_partition_lanelet; // [-] // parameters in yaml + double detection_area_offset; // [m] double detection_area_length; // [m] double detection_area_max_length; // [m] double stuck_vehicle_vel; // [m/s] 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 5666439bb4215..2ee67408825e8 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 @@ -70,6 +70,7 @@ OcclusionSpotModuleManager::OcclusionSpotModuleManager(rclcpp::Node & node) pp.is_show_processing_time = node.declare_parameter(ns + ".debug.is_show_processing_time", false); // threshold + pp.detection_area_offset = node.declare_parameter(ns + ".threshold.detection_area_offset", 30.0); pp.detection_area_length = node.declare_parameter(ns + ".threshold.detection_area_length", 200.0); pp.stuck_vehicle_vel = node.declare_parameter(ns + ".threshold.stuck_vehicle_vel", 1.0); pp.lateral_distance_thr = node.declare_parameter(ns + ".threshold.lateral_distance", 10.0); diff --git a/planning/behavior_velocity_planner/src/scene_module/occlusion_spot/scene_occlusion_spot.cpp b/planning/behavior_velocity_planner/src/scene_module/occlusion_spot/scene_occlusion_spot.cpp index 5fbd213c34d0f..8ae23587e0f07 100644 --- a/planning/behavior_velocity_planner/src/scene_module/occlusion_spot/scene_occlusion_spot.cpp +++ b/planning/behavior_velocity_planner/src/scene_module/occlusion_spot/scene_occlusion_spot.cpp @@ -93,12 +93,11 @@ bool OcclusionSpotModule::modifyPathVelocity( param_.v.v_ego = planner_data_->current_velocity->twist.linear.x; param_.v.a_ego = planner_data_->current_accel.get(); param_.v.delay_time = planner_data_->system_delay; - const double detection_area_offset = 5.0; // for visualization and stability param_.detection_area_max_length = planning_utils::calcJudgeLineDistWithJerkLimit( param_.v.v_ego, param_.v.a_ego, param_.v.non_effective_accel, param_.v.non_effective_jerk, planner_data_->delay_response_time) + - detection_area_offset; + param_.detection_area_offset; // To fill difference between planned and measured acc } const geometry_msgs::msg::Pose ego_pose = planner_data_->current_pose.pose; PathWithLaneId clipped_path;