diff --git a/planning/behavior_velocity_run_out_module/config/run_out.param.yaml b/planning/behavior_velocity_run_out_module/config/run_out.param.yaml index 5534228c1b86f..196f7c6296cb4 100644 --- a/planning/behavior_velocity_run_out_module/config/run_out.param.yaml +++ b/planning/behavior_velocity_run_out_module/config/run_out.param.yaml @@ -3,8 +3,8 @@ run_out: detection_method: "Object" # [-] candidate: Object, ObjectWithoutPath, Points use_partition_lanelet: true # [-] whether to use partition lanelet map data - suppress_on_crosswalk: true # [-] whether to suppress on crosswalk lanelet: - specify_decel_jerk: false # [-] whether to specify jerk when ego decelerates + suppress_on_crosswalk: true # [-] whether to suppress on crosswalk lanelet + specify_decel_jerk: true # [-] whether to specify jerk when ego decelerates stop_margin: 2.5 # [m] the vehicle decelerates to be able to stop with this margin passing_margin: 1.1 # [m] the vehicle begins to accelerate if the vehicle's front in predicted position is ahead of the obstacle + this margin deceleration_jerk: -0.3 # [m/s^3] ego decelerates with this jerk when stopping for obstacles @@ -12,15 +12,7 @@ detection_span: 1.0 # [m] calculate collision with this span to reduce calculation time min_vel_ego_kmph: 3.6 # [km/h] min velocity to calculate time to collision - detection_area: - margin_behind: 0.5 # [m] ahead margin for detection area length - margin_ahead: 1.0 # [m] behind margin for detection area length - - # This area uses points not filtered by vector map to ensure safety - mandatory_area: - decel_jerk: -1.2 # [m/s^3] deceleration jerk for obstacles in this area - - # parameter to create abstracted dynamic obstacles + # Parameter to create abstracted dynamic obstacles dynamic_obstacle: use_mandatory_area: false # [-] whether to use mandatory detection area assume_fixed_velocity: @@ -34,26 +26,42 @@ time_step: 0.5 # [sec] time step for each path step. used for creating dynamic obstacles from points or objects without path points_interval: 0.1 # [m] divide obstacle points into groups with this interval, and detect only lateral nearest point. used only for Points method - # approach if ego has stopped in the front of the obstacle for a certain amount of time - approaching: - enable: false - margin: 0.0 # [m] distance on how close ego approaches the obstacle - limit_vel_kmph: 3.0 # [km/h] limit velocity for approaching after stopping - - # parameters for the change of state. used only when approaching is enabled - state: - stop_thresh: 0.01 # [m/s] threshold to decide if ego is stopping - stop_time_thresh: 3.0 # [sec] threshold for stopping time to transit to approaching state - disable_approach_dist: 3.0 # [m] end the approaching state if distance to the obstacle is longer than this value - keep_approach_duration: 1.0 # [sec] keep approach state for this duration to avoid chattering of state transition - - # parameter to avoid sudden stopping + # Parameter to prevent sudden stopping. + # If the deceleration jerk and acceleration exceed this value upon inserting a stop point, + # the deceleration will be moderated to stay under this value. slow_down_limit: - enable: true + enable: false max_jerk: -0.7 # [m/s^3] minimum jerk deceleration for safe brake. max_acc : -2.0 # [m/s^2] minimum accel deceleration for safe brake. - # prevent abrupt stops caused by false positives in perception + # Parameter to prevent abrupt stops caused by false positives in perception ignore_momentary_detection: enable: true time_threshold: 0.15 # [sec] ignores detections that persist for less than this duration + + # Typically used when the "detection_method" is set to ObjectWithoutPath or Points + # Approach if the ego has stopped in front of the obstacle for a certain period + # This avoids the issue of the ego continuously stopping in front of the obstacle + approaching: + enable: false + margin: 0.0 # [m] distance on how close ego approaches the obstacle + limit_vel_kmph: 3.0 # [km/h] limit velocity for approaching after stopping + # Parameters for state change when "approaching" is enabled + state: + stop_thresh: 0.01 # [m/s] threshold to decide if ego is stopping + stop_time_thresh: 3.0 # [sec] threshold for stopping time to transit to approaching state + disable_approach_dist: 3.0 # [m] end the approaching state if distance to the obstacle is longer than this value + keep_approach_duration: 1.0 # [sec] keep approach state for this duration to avoid chattering of state transition + + # Only used when "detection_method" is set to Points + # Filters points by the detection area polygon to reduce computational cost + # The detection area is calculated based on the current velocity and acceleration and deceleration jerk constraints + detection_area: + margin_behind: 0.5 # [m] ahead margin for detection area length + margin_ahead: 1.0 # [m] behind margin for detection area length + + # Only used when "detection_method" is set to Points + # Points in this area are detected even if it is in the no obstacle segmentation area + # The mandatory area is calculated based on the current velocity and acceleration and "decel_jerk" constraints + mandatory_area: + decel_jerk: -1.2 # [m/s^3] deceleration jerk for obstacles in this area diff --git a/planning/behavior_velocity_run_out_module/src/manager.cpp b/planning/behavior_velocity_run_out_module/src/manager.cpp index 3ba9bf8bf52e6..f2ef366cbe121 100644 --- a/planning/behavior_velocity_run_out_module/src/manager.cpp +++ b/planning/behavior_velocity_run_out_module/src/manager.cpp @@ -105,16 +105,14 @@ RunOutModuleManager::RunOutModuleManager(rclcpp::Node & node) p.enable = getOrDeclareParameter(node, ns_a + ".enable"); p.margin = getOrDeclareParameter(node, ns_a + ".margin"); p.limit_vel_kmph = getOrDeclareParameter(node, ns_a + ".limit_vel_kmph"); - } - { - auto & p = planner_param_.state_param; - const std::string ns_s = ns + ".state"; - p.stop_thresh = getOrDeclareParameter(node, ns_s + ".stop_thresh"); - p.stop_time_thresh = getOrDeclareParameter(node, ns_s + ".stop_time_thresh"); - p.disable_approach_dist = getOrDeclareParameter(node, ns_s + ".disable_approach_dist"); - p.keep_approach_duration = - getOrDeclareParameter(node, ns_s + ".keep_approach_duration"); + const std::string ns_as = ns_a + ".state"; + p.state.stop_thresh = getOrDeclareParameter(node, ns_as + ".stop_thresh"); + p.state.stop_time_thresh = getOrDeclareParameter(node, ns_as + ".stop_time_thresh"); + p.state.disable_approach_dist = + getOrDeclareParameter(node, ns_as + ".disable_approach_dist"); + p.state.keep_approach_duration = + getOrDeclareParameter(node, ns_as + ".keep_approach_duration"); } { diff --git a/planning/behavior_velocity_run_out_module/src/scene.cpp b/planning/behavior_velocity_run_out_module/src/scene.cpp index b14152863d8d0..50b69fc6139c9 100644 --- a/planning/behavior_velocity_run_out_module/src/scene.cpp +++ b/planning/behavior_velocity_run_out_module/src/scene.cpp @@ -46,7 +46,7 @@ RunOutModule::RunOutModule( planner_param_(planner_param), dynamic_obstacle_creator_(std::move(dynamic_obstacle_creator)), debug_ptr_(debug_ptr), - state_machine_(std::make_unique(planner_param.state_param)) + state_machine_(std::make_unique(planner_param.approaching.state)) { velocity_factor_.init(PlanningBehavior::UNKNOWN); diff --git a/planning/behavior_velocity_run_out_module/src/utils.hpp b/planning/behavior_velocity_run_out_module/src/utils.hpp index 5524c0f76049d..c4976a119dd00 100644 --- a/planning/behavior_velocity_run_out_module/src/utils.hpp +++ b/planning/behavior_velocity_run_out_module/src/utils.hpp @@ -85,13 +85,6 @@ struct MandatoryArea float decel_jerk; }; -struct ApproachingParam -{ - bool enable; - float margin; - float limit_vel_kmph; -}; - struct StateParam { float stop_thresh; @@ -100,6 +93,14 @@ struct StateParam float keep_approach_duration; }; +struct ApproachingParam +{ + bool enable; + float margin; + float limit_vel_kmph; + StateParam state; +}; + struct SlowDownLimit { bool enable; @@ -143,7 +144,6 @@ struct PlannerParam DetectionArea detection_area; MandatoryArea mandatory_area; ApproachingParam approaching; - StateParam state_param; DynamicObstacleParam dynamic_obstacle; SlowDownLimit slow_down_limit; Smoother smoother;