Skip to content

Commit 0a0a4ab

Browse files
takayuki5168takayuki5168
authored
feat(rviz): update autoware.rviz for motion_velocity_obstacle_<stop/slow_down/cruise>_module (#1314)
* feat: add motion_velocity_obstacle_stop/slow_down/cruise_module Signed-off-by: Takayuki Murooka <takayuki5168@gmail.com> * update autoware.rviz Signed-off-by: Takayuki Murooka <takayuki5168@gmail.com> * update rviz Signed-off-by: Takayuki Murooka <takayuki5168@gmail.com> * disable obstacle_cruise_planner Signed-off-by: Takayuki Murooka <takayuki5168@gmail.com> * update motion velocity planner params Signed-off-by: Takayuki Murooka <takayuki5168@gmail.com> * add module.param.yaml Signed-off-by: Takayuki Murooka <takayuki5168@gmail.com> * enable obstacle_cruise_planner Signed-off-by: Takayuki Murooka <takayuki5168@gmail.com> --------- Signed-off-by: Takayuki Murooka <takayuki5168@gmail.com>
1 parent 0f5aa93 commit 0a0a4ab

File tree

7 files changed

+339
-0
lines changed

7 files changed

+339
-0
lines changed

‎autoware_launch/config/planning/preset/default_preset.yaml

+9
Original file line numberDiff line numberDiff line change
@@ -93,6 +93,15 @@ launch:
9393
# none
9494

9595
# motion velocity planner modules
96+
- arg:
97+
name: launch_obstacle_stop_module
98+
default: "false"
99+
- arg:
100+
name: launch_obstacle_slow_down_module
101+
default: "false"
102+
- arg:
103+
name: launch_obstacle_cruise_module
104+
default: "false"
96105
- arg:
97106
name: launch_dynamic_obstacle_stop_module
98107
default: "true"

‎autoware_launch/config/planning/scenario_planning/lane_driving/motion_planning/motion_velocity_planner/motion_velocity_planner.param.yaml

+11
Original file line numberDiff line numberDiff line change
@@ -1,3 +1,14 @@
11
/**:
22
ros__parameters:
33
smooth_velocity_before_planning: true # [-] if true, smooth the velocity profile of the input trajectory before planning
4+
5+
trajectory_polygon_collision_check:
6+
decimate_trajectory_step_length : 2.0 # longitudinal step length to calculate trajectory polygon for collision checking
7+
goal_extended_trajectory_length: 6.0
8+
9+
# consider the current ego pose (it is not the nearest pose on the reference trajectory)
10+
# Both the lateral error and the yaw error are assumed to decrease to zero by the time duration "time_to_convergence"
11+
# The both errors decrease with constant rates against the time.
12+
consider_current_pose:
13+
enable_to_consider_current_pose: true
14+
time_to_convergence: 1.5 #[s]

‎autoware_launch/config/planning/scenario_planning/lane_driving/motion_planning/motion_velocity_planner/obstacle_cruise.param.yaml

+120
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,120 @@
1+
/**:
2+
ros__parameters:
3+
obstacle_cruise:
4+
option:
5+
planning_algorithm: "pid_base" # currently supported algorithm is "pid_base"
6+
7+
cruise_planning:
8+
idling_time: 2.0 # idling time to detect front vehicle starting deceleration [s]
9+
min_ego_accel_for_rss: -1.0 # ego's acceleration to calculate RSS distance [m/ss]
10+
min_object_accel_for_rss: -1.0 # front obstacle's acceleration to calculate RSS distance [m/ss]
11+
safe_distance_margin : 5.0 # This is also used as a stop margin [m]
12+
13+
pid_based_planner:
14+
use_velocity_limit_based_planner: true
15+
error_function_type: quadratic # choose from linear, quadratic
16+
17+
velocity_limit_based_planner:
18+
# PID gains to keep safe distance with the front vehicle
19+
kp: 10.0
20+
ki: 0.0
21+
kd: 2.0
22+
23+
output_ratio_during_accel: 0.6 # target acceleration is multiplied with this value while ego accelerates to catch up the front vehicle [-]
24+
vel_to_acc_weight: 12.0 # target acceleration is calculated by (target_velocity - current_velocity) * vel_to_acc_weight [-]
25+
26+
enable_jerk_limit_to_output_acc: false
27+
28+
disable_target_acceleration: true
29+
30+
velocity_insertion_based_planner:
31+
kp_acc: 6.0
32+
ki_acc: 0.0
33+
kd_acc: 2.0
34+
35+
kp_jerk: 20.0
36+
ki_jerk: 0.0
37+
kd_jerk: 0.0
38+
39+
output_acc_ratio_during_accel: 0.6 # target acceleration is multiplied with this value while ego accelerates to catch up the front vehicle [-]
40+
output_jerk_ratio_during_accel: 1.0 # target acceleration is multiplied with this value while ego accelerates to catch up the front vehicle [-]
41+
42+
enable_jerk_limit_to_output_acc: true
43+
44+
min_accel_during_cruise: -2.0 # minimum acceleration during cruise to slow down [m/ss]
45+
min_cruise_target_vel: 0.0 # minimum target velocity during slow down [m/s]
46+
time_to_evaluate_rss: 0.0
47+
48+
lpf_normalized_error_cruise_dist_gain: 0.2
49+
50+
optimization_based_planner:
51+
dense_resampling_time_interval: 0.2
52+
sparse_resampling_time_interval: 2.0
53+
dense_time_horizon: 5.0
54+
max_time_horizon: 25.0
55+
velocity_margin: 0.2 #[m/s]
56+
57+
# Parameters for safe distance
58+
t_dangerous: 0.5
59+
60+
# For initial Motion
61+
replan_vel_deviation: 5.0 # velocity deviation to replan initial velocity [m/s]
62+
engage_velocity: 0.25 # engage velocity threshold [m/s] (if the trajectory velocity is higher than this value, use this velocity for engage vehicle speed)
63+
engage_acceleration: 0.1 # engage acceleration [m/ss] (use this acceleration when engagement)
64+
engage_exit_ratio: 0.5 # exit engage sequence to normal velocity planning when the velocity exceeds engage_exit_ratio x engage_velocity.
65+
stop_dist_to_prohibit_engage: 0.5 # if the stop point is in this distance, the speed is set to 0 not to move the vehicle [m]
66+
67+
# Weights for optimization
68+
max_s_weight: 100.0
69+
max_v_weight: 1.0
70+
over_s_safety_weight: 1000000.0
71+
over_s_ideal_weight: 50.0
72+
over_v_weight: 500000.0
73+
over_a_weight: 5000.0
74+
over_j_weight: 10000.0
75+
76+
obstacle_filtering:
77+
object_type:
78+
inside:
79+
unknown: true
80+
car: true
81+
truck: true
82+
bus: true
83+
trailer: true
84+
motorcycle: true
85+
bicycle: true
86+
pedestrian: false
87+
88+
outside:
89+
unknown: false
90+
car: true
91+
truck: true
92+
bus: true
93+
trailer: true
94+
motorcycle: true
95+
bicycle: false
96+
pedestrian: false
97+
98+
max_lat_margin: 1.0 # lateral margin between obstacle and trajectory band with ego's width
99+
100+
# if crossing vehicle is determined as target obstacles or not
101+
crossing_obstacle:
102+
obstacle_velocity_threshold : 1.0 # velocity threshold for crossing obstacle for cruise or stop [m/s]
103+
obstacle_traj_angle_threshold : 0.523599 # [rad] = 70 [deg], yaw threshold of crossing obstacle against the nearest trajectory point for cruise or stop
104+
105+
outside_obstacle:
106+
obstacle_velocity_threshold : 1.5 # minimum velocity threshold of obstacles outside the trajectory to cruise or stop [m/s]
107+
ego_obstacle_overlap_time_threshold : 0.2 # time threshold to decide cut-in obstacle for cruise or stop [s]
108+
max_prediction_time_for_collision_check : 10.0 # prediction time to check collision between obstacle and ego
109+
max_lateral_time_margin: 5.0 # time threshold of lateral margin between obstacle and trajectory band with ego's width [s]
110+
num_of_predicted_paths: 1 # number of the highest confidence predicted path to check collision between obstacle and ego
111+
yield:
112+
enable_yield: true
113+
lat_distance_threshold: 5.0 # lateral margin between obstacle in neighbor lanes and trajectory band with ego's width for yielding
114+
max_lat_dist_between_obstacles: 2.5 # lateral margin between moving obstacle in neighbor lanes and stopped obstacle in front of it
115+
max_obstacles_collision_time: 10.0 # how far the blocking obstacle
116+
stopped_obstacle_velocity_threshold: 0.5
117+
118+
# hysteresis for cruise and stop
119+
obstacle_velocity_threshold_from_cruise : 3.0 # stop planning is executed to the obstacle whose velocity is less than this value [m/s]
120+
obstacle_velocity_threshold_to_cruise : 3.5 # stop planning is executed to the obstacle whose velocity is less than this value [m/s]

‎autoware_launch/config/planning/scenario_planning/lane_driving/motion_planning/motion_velocity_planner/obstacle_slow_down.param.yaml

+50
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,50 @@
1+
/**:
2+
ros__parameters:
3+
obstacle_slow_down:
4+
slow_down_planning:
5+
slow_down_min_acc: -1.0 # slow down min deceleration [m/ss]
6+
slow_down_min_jerk: -1.0 # slow down min jerk [m/sss]
7+
8+
lpf_gain_slow_down_vel: 0.99 # low-pass filter gain for slow down velocity
9+
lpf_gain_lat_dist: 0.999 # low-pass filter gain for lateral distance from obstacle to ego's path
10+
lpf_gain_dist_to_slow_down: 0.7 # low-pass filter gain for distance to slow down start
11+
12+
time_margin_on_target_velocity: 1.5 # [s]
13+
14+
# parameters to calculate slow down velocity by linear interpolation
15+
object_type_specified_params:
16+
types:
17+
- "default"
18+
default:
19+
moving:
20+
min_lat_margin: 0.2
21+
max_lat_margin: 1.0
22+
min_ego_velocity: 2.0
23+
max_ego_velocity: 8.0
24+
static:
25+
min_lat_margin: 0.2
26+
max_lat_margin: 1.0
27+
min_ego_velocity: 4.0
28+
max_ego_velocity: 8.0
29+
30+
moving_object_speed_threshold: 0.5 # [m/s] how fast the object needs to move to be considered as "moving"
31+
moving_object_hysteresis_range: 0.1 # [m/s] hysteresis range used to prevent chattering when obstacle moves close to moving_object_speed_threshold
32+
33+
obstacle_filtering:
34+
object_type:
35+
unknown: false
36+
car: true
37+
truck: true
38+
bus: true
39+
trailer: true
40+
motorcycle: true
41+
bicycle: true
42+
pedestrian: true
43+
pointcloud: false
44+
45+
min_lat_margin: 0.0 # lateral margin between obstacle and trajectory band with ego's width to avoid the conflict with the obstacle_stop
46+
max_lat_margin: 1.1 # lateral margin between obstacle and trajectory band with ego's width
47+
lat_hysteresis_margin: 0.2
48+
49+
successive_num_to_entry_slow_down_condition: 5
50+
successive_num_to_exit_slow_down_condition: 5

‎autoware_launch/config/planning/scenario_planning/lane_driving/motion_planning/motion_velocity_planner/obstacle_stop.param.yaml

+74
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,74 @@
1+
/**:
2+
ros__parameters:
3+
obstacle_stop:
4+
option:
5+
ignore_crossing_obstacle: true
6+
suppress_sudden_stop: true
7+
8+
stop_planning:
9+
stop_margin : 5.0 # longitudinal margin to obstacle [m]
10+
terminal_stop_margin : 3.0 # Stop margin at the goal. This value cannot exceed stop margin. [m]
11+
min_behavior_stop_margin: 3.0 # [m]
12+
13+
hold_stop_velocity_threshold: 0.01 # The maximum ego velocity to hold stopping [m/s]
14+
hold_stop_distance_threshold: 0.3 # The ego keeps stopping if the distance to stop changes within the threshold [m]
15+
16+
stop_on_curve:
17+
enable_approaching: false
18+
additional_stop_margin: 3.0 # [m]
19+
min_stop_margin: 3.0 # [m]
20+
21+
object_type_specified_params:
22+
types: # For the listed types, the node try to read the following type specified values
23+
- "default"
24+
- "unknown"
25+
# default: For the default type, which denotes the other types listed above, the following param is defined implicitly, and the other type-specified parameters are not defined.
26+
# limit_min_acc: common_param.yaml/limit.min_acc
27+
unknown:
28+
limit_min_acc: -1.2 # overwrite the deceleration limit, in usually, common_param.yaml/limit.min_acc is referred.
29+
sudden_object_acc_threshold: -1.1 # If a stop can be achieved by a deceleration smaller than this value, it is not considered as "sudden stop".
30+
sudden_object_dist_threshold: 1000.0 # If a stop distance is longer than this value, it is not considered as "sudden stop".
31+
abandon_to_stop: false # If true, the planner gives up to stop when it cannot avoid to run over while maintaining the deceleration limit.
32+
33+
obstacle_filtering:
34+
object_type:
35+
pointcloud: false
36+
37+
inside:
38+
unknown: true
39+
car: true
40+
truck: true
41+
bus: true
42+
trailer: true
43+
motorcycle: true
44+
bicycle: true
45+
pedestrian: true
46+
47+
outside:
48+
unknown: false
49+
car: false
50+
truck: false
51+
bus: false
52+
trailer: false
53+
motorcycle: false
54+
bicycle: false
55+
pedestrian: false
56+
57+
# hysteresis for velocity
58+
obstacle_velocity_threshold_to_stop : 3.0 # stop planning is executed to the obstacle whose velocity is less than this value [m/s]
59+
obstacle_velocity_threshold_from_stop : 3.5 # stop planning is executed to the obstacle whose velocity is less than this value [m/s]
60+
61+
max_lat_margin: 0.3 # lateral margin between the obstacles except for unknown and ego's footprint
62+
max_lat_margin_against_unknown: 0.3 # lateral margin between the unknown obstacles and ego's footprint
63+
64+
min_velocity_to_reach_collision_point: 2.0 # minimum velocity margin to calculate time to reach collision [m/s]
65+
stop_obstacle_hold_time_threshold : 1.0 # maximum time for holding closest stop obstacle
66+
67+
outside_obstacle:
68+
max_lateral_time_margin: 4.5 # time threshold of lateral margin between the obstacles and ego's footprint [s]
69+
num_of_predicted_paths: 1 # number of the highest confidence predicted path to check collision between obstacle and ego
70+
pedestrian_deceleration_rate: 0.5 # planner perceives pedestrians will stop with this rate to avoid unnecessary stops [m/ss]
71+
bicycle_deceleration_rate: 0.5 # planner perceives bicycles will stop with this rate to avoid unnecessary stops [m/ss]
72+
73+
crossing_obstacle:
74+
collision_time_margin : 1.0 # time threshold of collision between obstacle adn ego for cruise or stop [s]

‎autoware_launch/launch/components/tier4_planning_component.launch.xml

+3
Original file line numberDiff line numberDiff line change
@@ -74,6 +74,9 @@
7474
<arg name="obstacle_stop_planner_acc_param_path" value="$(var motion_config_path)/obstacle_stop_planner/adaptive_cruise_control.param.yaml"/>
7575
<arg name="obstacle_cruise_planner_param_path" value="$(var motion_config_path)/obstacle_cruise_planner/obstacle_cruise_planner.param.yaml"/>
7676
<arg name="motion_velocity_planner_param_path" value="$(var motion_config_path)/motion_velocity_planner/motion_velocity_planner.param.yaml"/>
77+
<arg name="motion_velocity_planner_obstacle_stop_module_param_path" value="$(var motion_config_path)/motion_velocity_planner/obstacle_stop.param.yaml"/>
78+
<arg name="motion_velocity_planner_obstacle_slow_down_module_param_path" value="$(var motion_config_path)/motion_velocity_planner/obstacle_slow_down.param.yaml"/>
79+
<arg name="motion_velocity_planner_obstacle_cruise_module_param_path" value="$(var motion_config_path)/motion_velocity_planner/obstacle_cruise.param.yaml"/>
7780
<arg name="motion_velocity_planner_dynamic_obstacle_stop_module_param_path" value="$(var motion_config_path)/motion_velocity_planner/dynamic_obstacle_stop.param.yaml"/>
7881
<arg name="motion_velocity_planner_out_of_lane_module_param_path" value="$(var motion_config_path)/motion_velocity_planner/out_of_lane.param.yaml"/>
7982
<arg name="motion_velocity_planner_obstacle_velocity_limiter_param_path" value="$(var motion_config_path)/motion_velocity_planner/obstacle_velocity_limiter.param.yaml"/>

‎autoware_launch/rviz/autoware.rviz

+72
Original file line numberDiff line numberDiff line change
@@ -2114,6 +2114,42 @@ Visualization Manager:
21142114
Reliability Policy: Reliable
21152115
Value: /planning/scenario_planning/velocity_smoother/virtual_wall
21162116
Value: true
2117+
- Class: rviz_default_plugins/MarkerArray
2118+
Enabled: true
2119+
Name: VirtualWall (ObstacleStop)
2120+
Namespaces:
2121+
{}
2122+
Topic:
2123+
Depth: 5
2124+
Durability Policy: Volatile
2125+
History Policy: Keep Last
2126+
Reliability Policy: Reliable
2127+
Value: /planning/scenario_planning/lane_driving/motion_planning/motion_velocity_planner/obstacle_stop/virtual_walls
2128+
Value: true
2129+
- Class: rviz_default_plugins/MarkerArray
2130+
Enabled: true
2131+
Name: VirtualWall (ObstacleSlowDown)
2132+
Namespaces:
2133+
{}
2134+
Topic:
2135+
Depth: 5
2136+
Durability Policy: Volatile
2137+
History Policy: Keep Last
2138+
Reliability Policy: Reliable
2139+
Value: /planning/scenario_planning/lane_driving/motion_planning/motion_velocity_planner/obstacle_slow_down/virtual_walls
2140+
Value: true
2141+
- Class: rviz_default_plugins/MarkerArray
2142+
Enabled: true
2143+
Name: VirtualWall (ObstacleCruise)
2144+
Namespaces:
2145+
{}
2146+
Topic:
2147+
Depth: 5
2148+
Durability Policy: Volatile
2149+
History Policy: Keep Last
2150+
Reliability Policy: Reliable
2151+
Value: /planning/scenario_planning/lane_driving/motion_planning/motion_velocity_planner/obstacle_cruise/virtual_walls
2152+
Value: true
21172153
- Class: rviz_default_plugins/MarkerArray
21182154
Enabled: true
21192155
Name: VirtualWall (OutOfLane)
@@ -2251,6 +2287,42 @@ Visualization Manager:
22512287
Value: false
22522288
- Class: rviz_common/Group
22532289
Displays:
2290+
- Class: rviz_default_plugins/MarkerArray
2291+
Enabled: true
2292+
Name: ObstacleStop
2293+
Namespaces:
2294+
{}
2295+
Topic:
2296+
Depth: 5
2297+
Durability Policy: Volatile
2298+
History Policy: Keep Last
2299+
Reliability Policy: Reliable
2300+
Value: /planning/scenario_planning/lane_driving/motion_planning/motion_velocity_planner/obstacle_stop/debug_markers
2301+
Value: true
2302+
- Class: rviz_default_plugins/MarkerArray
2303+
Enabled: true
2304+
Name: ObstacleSlowDown
2305+
Namespaces:
2306+
{}
2307+
Topic:
2308+
Depth: 5
2309+
Durability Policy: Volatile
2310+
History Policy: Keep Last
2311+
Reliability Policy: Reliable
2312+
Value: /planning/scenario_planning/lane_driving/motion_planning/motion_velocity_planner/obstacle_slow_down/debug_markers
2313+
Value: true
2314+
- Class: rviz_default_plugins/MarkerArray
2315+
Enabled: true
2316+
Name: ObstacleCruise
2317+
Namespaces:
2318+
{}
2319+
Topic:
2320+
Depth: 5
2321+
Durability Policy: Volatile
2322+
History Policy: Keep Last
2323+
Reliability Policy: Reliable
2324+
Value: /planning/scenario_planning/lane_driving/motion_planning/motion_velocity_planner/obstacle_cruise/debug_markers
2325+
Value: true
22542326
- Class: rviz_default_plugins/MarkerArray
22552327
Enabled: true
22562328
Name: OutOfLane

0 commit comments

Comments
 (0)