Skip to content

Commit f5cef6c

Browse files
refactor(run_out): reorganize the parameter (autowarefoundation#6064)
* chore(run_out): Reorganize the parameter Signed-off-by: Tomohito Ando <tomohito.ando@tier4.jp> * style(pre-commit): autofix --------- Signed-off-by: Tomohito Ando <tomohito.ando@tier4.jp> Co-authored-by: pre-commit-ci[bot] <66853113+pre-commit-ci[bot]@users.noreply.github.com>
1 parent 378bf3a commit f5cef6c

File tree

4 files changed

+51
-45
lines changed

4 files changed

+51
-45
lines changed

planning/behavior_velocity_run_out_module/config/run_out.param.yaml

+35-27
Original file line numberDiff line numberDiff line change
@@ -3,24 +3,16 @@
33
run_out:
44
detection_method: "Object" # [-] candidate: Object, ObjectWithoutPath, Points
55
use_partition_lanelet: true # [-] whether to use partition lanelet map data
6-
suppress_on_crosswalk: true # [-] whether to suppress on crosswalk lanelet:
7-
specify_decel_jerk: false # [-] whether to specify jerk when ego decelerates
6+
suppress_on_crosswalk: true # [-] whether to suppress on crosswalk lanelet
7+
specify_decel_jerk: true # [-] whether to specify jerk when ego decelerates
88
stop_margin: 2.5 # [m] the vehicle decelerates to be able to stop with this margin
99
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
1010
deceleration_jerk: -0.3 # [m/s^3] ego decelerates with this jerk when stopping for obstacles
1111
detection_distance: 45.0 # [m] ahead distance from ego to detect the obstacles
1212
detection_span: 1.0 # [m] calculate collision with this span to reduce calculation time
1313
min_vel_ego_kmph: 3.6 # [km/h] min velocity to calculate time to collision
1414

15-
detection_area:
16-
margin_behind: 0.5 # [m] ahead margin for detection area length
17-
margin_ahead: 1.0 # [m] behind margin for detection area length
18-
19-
# This area uses points not filtered by vector map to ensure safety
20-
mandatory_area:
21-
decel_jerk: -1.2 # [m/s^3] deceleration jerk for obstacles in this area
22-
23-
# parameter to create abstracted dynamic obstacles
15+
# Parameter to create abstracted dynamic obstacles
2416
dynamic_obstacle:
2517
use_mandatory_area: false # [-] whether to use mandatory detection area
2618
assume_fixed_velocity:
@@ -34,26 +26,42 @@
3426
time_step: 0.5 # [sec] time step for each path step. used for creating dynamic obstacles from points or objects without path
3527
points_interval: 0.1 # [m] divide obstacle points into groups with this interval, and detect only lateral nearest point. used only for Points method
3628

37-
# approach if ego has stopped in the front of the obstacle for a certain amount of time
38-
approaching:
39-
enable: false
40-
margin: 0.0 # [m] distance on how close ego approaches the obstacle
41-
limit_vel_kmph: 3.0 # [km/h] limit velocity for approaching after stopping
42-
43-
# parameters for the change of state. used only when approaching is enabled
44-
state:
45-
stop_thresh: 0.01 # [m/s] threshold to decide if ego is stopping
46-
stop_time_thresh: 3.0 # [sec] threshold for stopping time to transit to approaching state
47-
disable_approach_dist: 3.0 # [m] end the approaching state if distance to the obstacle is longer than this value
48-
keep_approach_duration: 1.0 # [sec] keep approach state for this duration to avoid chattering of state transition
49-
50-
# parameter to avoid sudden stopping
29+
# Parameter to prevent sudden stopping.
30+
# If the deceleration jerk and acceleration exceed this value upon inserting a stop point,
31+
# the deceleration will be moderated to stay under this value.
5132
slow_down_limit:
52-
enable: true
33+
enable: false
5334
max_jerk: -0.7 # [m/s^3] minimum jerk deceleration for safe brake.
5435
max_acc : -2.0 # [m/s^2] minimum accel deceleration for safe brake.
5536

56-
# prevent abrupt stops caused by false positives in perception
37+
# Parameter to prevent abrupt stops caused by false positives in perception
5738
ignore_momentary_detection:
5839
enable: true
5940
time_threshold: 0.15 # [sec] ignores detections that persist for less than this duration
41+
42+
# Typically used when the "detection_method" is set to ObjectWithoutPath or Points
43+
# Approach if the ego has stopped in front of the obstacle for a certain period
44+
# This avoids the issue of the ego continuously stopping in front of the obstacle
45+
approaching:
46+
enable: false
47+
margin: 0.0 # [m] distance on how close ego approaches the obstacle
48+
limit_vel_kmph: 3.0 # [km/h] limit velocity for approaching after stopping
49+
# Parameters for state change when "approaching" is enabled
50+
state:
51+
stop_thresh: 0.01 # [m/s] threshold to decide if ego is stopping
52+
stop_time_thresh: 3.0 # [sec] threshold for stopping time to transit to approaching state
53+
disable_approach_dist: 3.0 # [m] end the approaching state if distance to the obstacle is longer than this value
54+
keep_approach_duration: 1.0 # [sec] keep approach state for this duration to avoid chattering of state transition
55+
56+
# Only used when "detection_method" is set to Points
57+
# Filters points by the detection area polygon to reduce computational cost
58+
# The detection area is calculated based on the current velocity and acceleration and deceleration jerk constraints
59+
detection_area:
60+
margin_behind: 0.5 # [m] ahead margin for detection area length
61+
margin_ahead: 1.0 # [m] behind margin for detection area length
62+
63+
# Only used when "detection_method" is set to Points
64+
# Points in this area are detected even if it is in the no obstacle segmentation area
65+
# The mandatory area is calculated based on the current velocity and acceleration and "decel_jerk" constraints
66+
mandatory_area:
67+
decel_jerk: -1.2 # [m/s^3] deceleration jerk for obstacles in this area

planning/behavior_velocity_run_out_module/src/manager.cpp

+7-9
Original file line numberDiff line numberDiff line change
@@ -105,16 +105,14 @@ RunOutModuleManager::RunOutModuleManager(rclcpp::Node & node)
105105
p.enable = getOrDeclareParameter<bool>(node, ns_a + ".enable");
106106
p.margin = getOrDeclareParameter<double>(node, ns_a + ".margin");
107107
p.limit_vel_kmph = getOrDeclareParameter<double>(node, ns_a + ".limit_vel_kmph");
108-
}
109108

110-
{
111-
auto & p = planner_param_.state_param;
112-
const std::string ns_s = ns + ".state";
113-
p.stop_thresh = getOrDeclareParameter<double>(node, ns_s + ".stop_thresh");
114-
p.stop_time_thresh = getOrDeclareParameter<double>(node, ns_s + ".stop_time_thresh");
115-
p.disable_approach_dist = getOrDeclareParameter<double>(node, ns_s + ".disable_approach_dist");
116-
p.keep_approach_duration =
117-
getOrDeclareParameter<double>(node, ns_s + ".keep_approach_duration");
109+
const std::string ns_as = ns_a + ".state";
110+
p.state.stop_thresh = getOrDeclareParameter<double>(node, ns_as + ".stop_thresh");
111+
p.state.stop_time_thresh = getOrDeclareParameter<double>(node, ns_as + ".stop_time_thresh");
112+
p.state.disable_approach_dist =
113+
getOrDeclareParameter<double>(node, ns_as + ".disable_approach_dist");
114+
p.state.keep_approach_duration =
115+
getOrDeclareParameter<double>(node, ns_as + ".keep_approach_duration");
118116
}
119117

120118
{

planning/behavior_velocity_run_out_module/src/scene.cpp

+1-1
Original file line numberDiff line numberDiff line change
@@ -46,7 +46,7 @@ RunOutModule::RunOutModule(
4646
planner_param_(planner_param),
4747
dynamic_obstacle_creator_(std::move(dynamic_obstacle_creator)),
4848
debug_ptr_(debug_ptr),
49-
state_machine_(std::make_unique<run_out_utils::StateMachine>(planner_param.state_param))
49+
state_machine_(std::make_unique<run_out_utils::StateMachine>(planner_param.approaching.state))
5050
{
5151
velocity_factor_.init(PlanningBehavior::UNKNOWN);
5252

planning/behavior_velocity_run_out_module/src/utils.hpp

+8-8
Original file line numberDiff line numberDiff line change
@@ -85,13 +85,6 @@ struct MandatoryArea
8585
float decel_jerk;
8686
};
8787

88-
struct ApproachingParam
89-
{
90-
bool enable;
91-
float margin;
92-
float limit_vel_kmph;
93-
};
94-
9588
struct StateParam
9689
{
9790
float stop_thresh;
@@ -100,6 +93,14 @@ struct StateParam
10093
float keep_approach_duration;
10194
};
10295

96+
struct ApproachingParam
97+
{
98+
bool enable;
99+
float margin;
100+
float limit_vel_kmph;
101+
StateParam state;
102+
};
103+
103104
struct SlowDownLimit
104105
{
105106
bool enable;
@@ -143,7 +144,6 @@ struct PlannerParam
143144
DetectionArea detection_area;
144145
MandatoryArea mandatory_area;
145146
ApproachingParam approaching;
146-
StateParam state_param;
147147
DynamicObstacleParam dynamic_obstacle;
148148
SlowDownLimit slow_down_limit;
149149
Smoother smoother;

0 commit comments

Comments
 (0)