Skip to content

Commit 8f41f5e

Browse files
Merge pull request autowarefoundation#150 from tier4/sync-upstream
chore: sync upstream
2 parents afd9e15 + bc15fa2 commit 8f41f5e

File tree

9 files changed

+264
-14
lines changed

9 files changed

+264
-14
lines changed

autoware_launch/rviz/autoware.rviz

+41
Original file line numberDiff line numberDiff line change
@@ -888,6 +888,7 @@ Visualization Manager:
888888
Name: RouteArea
889889
Namespaces:
890890
goal_lanelets: true
891+
lane_start_bound: false
891892
left_lane_bound: false
892893
right_lane_bound: false
893894
route_lanelets: true
@@ -1332,6 +1333,18 @@ Visualization Manager:
13321333
Reliability Policy: Reliable
13331334
Value: /planning/scenario_planning/lane_driving/motion_planning/obstacle_avoidance_planner/debug/wall_marker
13341335
Value: true
1336+
- Class: rviz_default_plugins/MarkerArray
1337+
Enabled: true
1338+
Name: VirtualWall (ObstacleCruise)
1339+
Namespaces:
1340+
{}
1341+
Topic:
1342+
Depth: 5
1343+
Durability Policy: Volatile
1344+
History Policy: Keep Last
1345+
Reliability Policy: Reliable
1346+
Value: /planning/scenario_planning/lane_driving/motion_planning/obstacle_cruise_planner/virtual_wall
1347+
Value: true
13351348
Enabled: true
13361349
Name: VirtualWall
13371350
- Class: rviz_common/Group
@@ -1348,6 +1361,34 @@ Visualization Manager:
13481361
Reliability Policy: Reliable
13491362
Value: /planning/scenario_planning/lane_driving/motion_planning/obstacle_stop_planner/debug/marker
13501363
Value: true
1364+
- Class: rviz_common/Group
1365+
Displays:
1366+
- Class: rviz_default_plugins/MarkerArray
1367+
Enabled: true
1368+
Name: CruiseVirtualWall
1369+
Namespaces:
1370+
{}
1371+
Topic:
1372+
Depth: 5
1373+
Durability Policy: Volatile
1374+
History Policy: Keep Last
1375+
Reliability Policy: Reliable
1376+
Value: /planning/scenario_planning/lane_driving/motion_planning/obstacle_cruise_planner/debug/cruise_wall_marker
1377+
Value: true
1378+
- Class: rviz_default_plugins/MarkerArray
1379+
Enabled: true
1380+
Name: DebugMarker
1381+
Namespaces:
1382+
{}
1383+
Topic:
1384+
Depth: 5
1385+
Durability Policy: Volatile
1386+
History Policy: Keep Last
1387+
Reliability Policy: Reliable
1388+
Value: /planning/scenario_planning/lane_driving/motion_planning/obstacle_cruise_planner/debug/marker
1389+
Value: true
1390+
Enabled: true
1391+
Name: ObstacleCruise
13511392
- Class: rviz_default_plugins/MarkerArray
13521393
Enabled: true
13531394
Name: SurroundObstacleCheck

planning_launch/config/scenario_planning/lane_driving/behavior_planning/behavior_velocity_planner/intersection.param.yaml

+2
Original file line numberDiff line numberDiff line change
@@ -10,6 +10,8 @@
1010
intersection_velocity: 2.778 # 2.778m/s = 10.0km/h
1111
intersection_max_accel: 0.5 # m/ss
1212
detection_area_margin: 0.5 # [m]
13+
detection_area_right_margin: 0.5 # [m]
14+
detection_area_left_margin: 0.5 # [m]
1315
detection_area_length: 200.0 # [m]
1416
min_predicted_path_confidence: 0.05
1517
collision_start_margin_time: 5.0 # [s]

planning_launch/config/scenario_planning/lane_driving/behavior_planning/behavior_velocity_planner/occlusion_spot.param.yaml

+1-2
Original file line numberDiff line numberDiff line change
@@ -3,9 +3,8 @@
33
occlusion_spot:
44
detection_method: "occupancy_grid" # [-] candidate is "occupancy_grid" or "predicted_object"
55
pass_judge: "smooth_velocity" # [-] candidate is "smooth_velocity" or "current_velocity"
6-
filter_occupancy_grid: true # [-] whether to filter occupancy grid by morphologyEx or not
7-
use_moving_object_ray_cast: true # [-] whether to reflect moving object ray shadow grid map
86
use_object_info: true # [-] whether to reflect object info to occupancy grid map or not
7+
use_moving_object_ray_cast: true # [-] whether to reflect moving object ray shadow grid map
98
use_partition_lanelet: true # [-] whether to use partition lanelet map data
109
pedestrian_vel: 1.5 # [m/s] assume pedestrian is dashing from occlusion at this velocity
1110
pedestrian_radius: 0.3 # [m] assume pedestrian width(0.2m) + margin(0.1m)

planning_launch/config/scenario_planning/lane_driving/behavior_planning/behavior_velocity_planner/stop_line.param.yaml

+1
Original file line numberDiff line numberDiff line change
@@ -4,5 +4,6 @@
44
stop_margin: 0.0
55
stop_check_dist: 2.0
66
stop_duration_sec: 1.0
7+
use_initialization_stop_line_state: true
78
debug:
89
show_stopline_collision_check: false # [-] whether to show stopline collision
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,29 @@
1+
/**:
2+
ros__parameters:
3+
module_list:
4+
- "behavior_velocity_planner/blind_spot"
5+
- "behavior_velocity_planner/crosswalk"
6+
- "behavior_velocity_planner/detection_area"
7+
- "behavior_velocity_planner/intersection"
8+
- "behavior_velocity_planner/no_stopping_area"
9+
- "behavior_velocity_planner/traffic_light"
10+
- "behavior_path_planner/lane_change_left"
11+
- "behavior_path_planner/lane_change_right"
12+
- "behavior_path_planner/avoidance_left"
13+
- "behavior_path_planner/avoidance_right"
14+
- "behavior_path_planner/pull_over"
15+
- "behavior_path_planner/pull_out"
16+
17+
default_enable_list:
18+
- "behavior_velocity_planner/blind_spot"
19+
- "behavior_velocity_planner/crosswalk"
20+
- "behavior_velocity_planner/detection_area"
21+
- "behavior_velocity_planner/intersection"
22+
- "behavior_velocity_planner/no_stopping_area"
23+
- "behavior_velocity_planner/traffic_light"
24+
- "behavior_path_planner/lane_change_left"
25+
- "behavior_path_planner/lane_change_right"
26+
- "behavior_path_planner/avoidance_left"
27+
- "behavior_path_planner/avoidance_right"
28+
- "behavior_path_planner/pull_over"
29+
- "behavior_path_planner/pull_out"
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,105 @@
1+
/**:
2+
ros__parameters:
3+
common:
4+
planning_algorithm: "pid_base" # currently supported algorithm is "pid_base"
5+
6+
is_showing_debug_info: true
7+
8+
# longitudinal info
9+
idling_time: 4.0 # idling time to detect front vehicle starting deceleration [s]
10+
min_ego_accel_for_rss: -1.0 # ego's acceleration to calculate RSS distance [m/ss]
11+
min_object_accel_for_rss: -1.0 # front obstacle's acceleration to calculate RSS distance [m/ss]
12+
safe_distance_margin : 6.0 # This is also used as a stop margin [m]
13+
14+
lpf_gain_for_accel: 0.2 # gain of low pass filter for ego acceleration [-]
15+
16+
nearest_dist_deviation_threshold: 3.0 # [m] for finding nearest index
17+
nearest_yaw_deviation_threshold: 1.57 # [rad] for finding nearest index
18+
min_behavior_stop_margin: 3.0 # [m]
19+
obstacle_velocity_threshold_from_cruise_to_stop : 3.0 # stop planning is executed to the obstacle whose velocity is less than this value [m/s]
20+
21+
cruise_obstacle_type:
22+
unknown: true
23+
car: true
24+
truck: true
25+
bus: true
26+
trailer: true
27+
motorcycle: true
28+
bicycle: false
29+
pedestrian: false
30+
31+
stop_obstacle_type:
32+
unknown: true
33+
car: true
34+
truck: true
35+
bus: true
36+
trailer: true
37+
motorcycle: true
38+
bicycle: true
39+
pedestrian: true
40+
41+
obstacle_filtering:
42+
rough_detection_area_expand_width : 5.0 # rough lateral margin for rough detection area expansion [m]
43+
detection_area_expand_width : 0.0 # lateral margin for precise detection area expansion
44+
decimate_trajectory_step_length : 2.0 # longitudinal step length to calculate trajectory polygon for collision checking
45+
46+
# if crossing vehicle is decided as target obstacles or not
47+
crossing_obstacle_velocity_threshold : 3.0 # velocity threshold for crossing obstacle for cruise or stop [m/s]
48+
crossing_obstacle_traj_angle_threshold : 1.22 # [rad] = 70 [deg], yaw threshold of crossing obstacle against the nearest trajectory point for cruise or stop
49+
collision_time_margin : 8.0 # time threshold of collision between obstacle adn ego for cruise or stop [s]
50+
51+
ego_obstacle_overlap_time_threshold : 1.0 # time threshold to decide cut-in obstacle for cruise or stop [s]
52+
max_prediction_time_for_collision_check : 20.0 # prediction time to check collision between obstacle and ego
53+
54+
ignored_outside_obstacle_type:
55+
unknown: false
56+
car: false
57+
truck: false
58+
bus: false
59+
trailer: false
60+
motorcycle: false
61+
bicycle: true
62+
pedestrian: true
63+
64+
pid_based_planner:
65+
# use_predicted_obstacle_pose: false
66+
67+
# PID gains to keep safe distance with the front vehicle
68+
kp: 0.6
69+
ki: 0.0
70+
kd: 0.5
71+
72+
min_accel_during_cruise: -2.0 # minimum acceleration during cruise to slow down [m/ss]
73+
74+
output_ratio_during_accel: 2.5 # target acceleration is multiplied with this value while ego accelerates to catch up the front vehicle [-]
75+
vel_to_acc_weight: 8.0 # target acceleration is calculated by (target_velocity - current_velocity) * vel_to_acc_weight [-]
76+
77+
min_cruise_target_vel: 0.0 # minimum target velocity during slow down [m/s]
78+
79+
optimization_based_planner:
80+
resampling_s_interval: 2.0
81+
dense_resampling_time_interval: 0.1
82+
sparse_resampling_time_interval: 1.0
83+
dense_time_horizon: 5.0
84+
max_time_horizon: 25.0
85+
max_trajectory_length: 200.0
86+
velocity_margin: 0.1 #[m/s]
87+
88+
# Parameters for safe distance
89+
t_dangerous: 0.5
90+
91+
# For initial Motion
92+
replan_vel_deviation: 5.0 # velocity deviation to replan initial velocity [m/s]
93+
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)
94+
engage_acceleration: 0.1 # engage acceleration [m/ss] (use this acceleration when engagement)
95+
engage_exit_ratio: 0.5 # exit engage sequence to normal velocity planning when the velocity exceeds engage_exit_ratio x engage_velocity.
96+
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]
97+
98+
# Weights for optimization
99+
max_s_weight: 100.0
100+
max_v_weight: 1.0
101+
over_s_safety_weight: 1000000.0
102+
over_s_ideal_weight: 50.0
103+
over_v_weight: 500000.0
104+
over_a_weight: 5000.0
105+
over_j_weight: 10000.0

planning_launch/launch/scenario_planning/lane_driving.launch.xml

+4
Original file line numberDiff line numberDiff line change
@@ -17,6 +17,10 @@
1717
<arg name="use_pointcloud_container" value="$(var use_pointcloud_container)"/>
1818
<arg name="container_name" value="$(var pointcloud_container_name)"/>
1919
</include>
20+
<arg name="rtc_auto_approver_param_path" default="$(find-pkg-share planning_launch)/config/scenario_planning/lane_driving/behavior_planning/rtc_auto_approver/rtc_auto_approver.param.yaml"/>
21+
<include file="$(find-pkg-share rtc_auto_approver)/launch/rtc_auto_approver.launch.xml">
22+
<arg name="param_path" value="$(var rtc_auto_approver_param_path)"/>
23+
</include>
2024
</group>
2125

2226
<!-- motion planning module -->

planning_launch/launch/scenario_planning/lane_driving/motion_planning/motion_planning.launch.py

+71-1
Original file line numberDiff line numberDiff line change
@@ -19,6 +19,7 @@
1919
from launch.actions import DeclareLaunchArgument
2020
from launch.actions import SetLaunchConfiguration
2121
from launch.conditions import IfCondition
22+
from launch.conditions import LaunchConfigurationEquals
2223
from launch.conditions import UnlessCondition
2324
from launch.substitutions import LaunchConfiguration
2425
from launch_ros.actions import ComposableNodeContainer
@@ -107,6 +108,39 @@ def generate_launch_description():
107108
extra_arguments=[{"use_intra_process_comms": LaunchConfiguration("use_intra_process")}],
108109
)
109110

111+
# obstacle cruise planner
112+
obstacle_cruise_planner_param_path = os.path.join(
113+
get_package_share_directory("planning_launch"),
114+
"config",
115+
"scenario_planning",
116+
"lane_driving",
117+
"motion_planning",
118+
"obstacle_cruise_planner",
119+
"obstacle_cruise_planner.param.yaml",
120+
)
121+
with open(obstacle_cruise_planner_param_path, "r") as f:
122+
obstacle_cruise_planner_param = yaml.safe_load(f)["/**"]["ros__parameters"]
123+
obstacle_cruise_planner_component = ComposableNode(
124+
package="obstacle_cruise_planner",
125+
plugin="motion_planning::ObstacleCruisePlannerNode",
126+
name="obstacle_cruise_planner",
127+
namespace="",
128+
remappings=[
129+
("~/input/trajectory", "obstacle_avoidance_planner/trajectory"),
130+
("~/input/odometry", "/localization/kinematic_state"),
131+
("~/input/objects", "/perception/object_recognition/objects"),
132+
("~/output/trajectory", "/planning/scenario_planning/lane_driving/trajectory"),
133+
("~/output/velocity_limit", "/planning/scenario_planning/max_velocity_candidates"),
134+
("~/output/clear_velocity_limit", "/planning/scenario_planning/clear_velocity_limit"),
135+
("~/output/stop_reasons", "/planning/scenario_planning/status/stop_reasons"),
136+
],
137+
parameters=[
138+
common_param,
139+
obstacle_cruise_planner_param,
140+
],
141+
extra_arguments=[{"use_intra_process_comms": LaunchConfiguration("use_intra_process")}],
142+
)
143+
110144
# obstacle stop planner
111145
obstacle_stop_planner_param_path = os.path.join(
112146
get_package_share_directory("planning_launch"),
@@ -161,17 +195,47 @@ def generate_launch_description():
161195
extra_arguments=[{"use_intra_process_comms": LaunchConfiguration("use_intra_process")}],
162196
)
163197

198+
obstacle_cruise_planner_relay_component = ComposableNode(
199+
package="topic_tools",
200+
plugin="topic_tools::RelayNode",
201+
name="obstacle_cruise_planner_relay",
202+
namespace="",
203+
parameters=[
204+
{"input_topic": "obstacle_avoidance_planner/trajectory"},
205+
{"output_topic": "/planning/scenario_planning/lane_driving/trajectory"},
206+
{"type": "autoware_auto_planning_msgs/msg/Trajectory"},
207+
],
208+
extra_arguments=[{"use_intra_process_comms": LaunchConfiguration("use_intra_process")}],
209+
)
210+
164211
container = ComposableNodeContainer(
165212
name="motion_planning_container",
166213
namespace="",
167214
package="rclcpp_components",
168215
executable=LaunchConfiguration("container_executable"),
169216
composable_node_descriptions=[
170217
obstacle_avoidance_planner_component,
171-
obstacle_stop_planner_component,
172218
],
173219
)
174220

221+
obstacle_stop_planner_loader = LoadComposableNodes(
222+
composable_node_descriptions=[obstacle_stop_planner_component],
223+
target_container=container,
224+
condition=LaunchConfigurationEquals("cruise_planner", "obstacle_stop_planner"),
225+
)
226+
227+
obstacle_cruise_planner_loader = LoadComposableNodes(
228+
composable_node_descriptions=[obstacle_cruise_planner_component],
229+
target_container=container,
230+
condition=LaunchConfigurationEquals("cruise_planner", "obstacle_cruise_planner"),
231+
)
232+
233+
obstacle_cruise_planner_relay_loader = LoadComposableNodes(
234+
composable_node_descriptions=[obstacle_cruise_planner_relay_component],
235+
target_container=container,
236+
condition=LaunchConfigurationEquals("cruise_planner", "none"),
237+
)
238+
175239
surround_obstacle_checker_loader = LoadComposableNodes(
176240
composable_node_descriptions=[surround_obstacle_checker_component],
177241
target_container=container,
@@ -196,11 +260,17 @@ def generate_launch_description():
196260
default_value="/planning/scenario_planning/lane_driving/behavior_planning/path",
197261
),
198262
DeclareLaunchArgument("use_surround_obstacle_check", default_value="true"),
263+
DeclareLaunchArgument(
264+
"cruise_planner", default_value="obstacle_stop_planner"
265+
), # select from "obstacle_stop_planner", "obstacle_cruise_planner", "none"
199266
DeclareLaunchArgument("use_intra_process", default_value="false"),
200267
DeclareLaunchArgument("use_multithread", default_value="false"),
201268
set_container_executable,
202269
set_container_mt_executable,
203270
container,
204271
surround_obstacle_checker_loader,
272+
obstacle_stop_planner_loader,
273+
obstacle_cruise_planner_loader,
274+
obstacle_cruise_planner_relay_loader,
205275
]
206276
)

simulator_launch/launch/simulator.launch.xml

+10-11
Original file line numberDiff line numberDiff line change
@@ -17,6 +17,16 @@
1717
<arg name="config_file" value="$(find-pkg-share simulator_launch)/config/fault_injection.param.yaml"/>
1818
</include>
1919
</group>
20+
<group unless="$(var scenario_simulation)">
21+
<!-- Occupancy Grid -->
22+
<push-ros-namespace namespace="occupancy_grid_map"/>
23+
<include file="$(find-pkg-share perception_launch)/launch/occupancy_grid_map/laserscan_based_occupancy_grid_map.launch.py">
24+
<arg name="input_obstacle_pointcloud" value="true" />
25+
<arg name="input_obstacle_and_raw_pointcloud" value="false" />
26+
<arg name="input/obstacle_pointcloud" value="/perception/obstacle_segmentation/pointcloud" />
27+
<arg name="output" value="/perception/occupancy_grid_map/map" />
28+
</include>
29+
</group>
2030

2131
<!-- Dummy Perception -->
2232
<group if="$(var launch_dummy_perception)">
@@ -56,17 +66,6 @@
5666
</node>
5767
</group>
5868

59-
<!-- object segmentation module -->
60-
<group>
61-
<push-ros-namespace namespace="occupancy_grid_map"/>
62-
<!-- Occupancy Grid -->
63-
<include file="$(find-pkg-share perception_launch)/launch/occupancy_grid_map/laserscan_based_occupancy_grid_map.launch.py">
64-
<arg name="input_obstacle_pointcloud" value="true" />
65-
<arg name="input_obstacle_and_raw_pointcloud" value="false" />
66-
<arg name="input/obstacle_pointcloud" value="/perception/obstacle_segmentation/pointcloud" />
67-
<arg name="output" value="/perception/occupancy_grid_map/map" />
68-
</include>
69-
</group>
7069

7170
<!-- traffic light module -->
7271
<group if="$(var perception/enable_traffic_light)">

0 commit comments

Comments
 (0)