Skip to content

Commit 0256188

Browse files
authored
feat(static_centerline_generator): organize AUTO/GUI/VMB modes (autowarefoundation#7432)
Signed-off-by: Takagi, Isamu <isamu.takagi@tier4.jp> Signed-off-by: Takayuki Murooka <takayuki5168@gmail.com>
1 parent 84bede5 commit 0256188

22 files changed

+847
-268
lines changed

planning/autoware_static_centerline_generator/CMakeLists.txt

+2
Original file line numberDiff line numberDiff line change
@@ -60,5 +60,7 @@ endif()
6060

6161
install(PROGRAMS
6262
scripts/app.py
63+
scripts/centerline_updater_helper.py
64+
scripts/show_lanelet2_map_diff.py
6365
DESTINATION lib/${PROJECT_NAME}
6466
)
Original file line numberDiff line numberDiff line change
@@ -1,5 +1,9 @@
11
/**:
22
ros__parameters:
3-
marker_color: ["FF0000", "00FF00", "0000FF"]
3+
marker_color: ["FF0000", "FF5A00", "FFFF00"]
44
marker_color_dist_thresh : [0.1, 0.2, 0.3]
55
output_trajectory_interval: 1.0
6+
7+
validation:
8+
dist_threshold_to_road_border: 0.0
9+
max_steer_angle_margin: 0.0 # [rad] NOTE: Positive value makes max steer angle threshold to decrease.

planning/autoware_static_centerline_generator/launch/static_centerline_generator.launch.xml

+16-11
Original file line numberDiff line numberDiff line change
@@ -3,17 +3,19 @@
33
<arg name="vehicle_model"/>
44

55
<!-- flag -->
6-
<arg name="run_background" default="false"/>
6+
<arg name="mode" default="AUTO" description="select from AUTO, GUI, and VMB"/>
77
<arg name="rviz" default="true"/>
88
<arg name="centerline_source" default="optimization_trajectory_base" description="select from optimization_trajectory_base and bag_ego_trajectory_base"/>
9-
<arg name="bag_filename" default="bag.db3"/>
109

11-
<!-- mandatory arguments when run_background is true -->
10+
<!-- mandatory arguments when mode is AUTO -->
1211
<arg name="lanelet2_input_file_path" default=""/>
13-
<arg name="lanelet2_output_file_path" default="/tmp/static_centerline_generator/lanelet2_map.osm"/>
12+
<arg name="lanelet2_output_file_path" default="/tmp/autoware_static_centerline_generator/lanelet2_map.osm"/>
1413
<arg name="start_lanelet_id" default=""/>
1514
<arg name="end_lanelet_id" default=""/>
1615

16+
<!-- mandatory arguments when mode is GUI -->
17+
<arg name="bag_filename" default="bag.db3"/>
18+
1719
<!-- topic -->
1820
<arg name="lanelet2_map_topic" default="/map/vector_map"/>
1921
<arg name="lanelet2_map_marker_topic" default="/map/vector_map_marker"/>
@@ -28,7 +30,7 @@
2830
default="$(find-pkg-share autoware_launch)/config/planning/scenario_planning/lane_driving/behavior_planning/behavior_velocity_planner/behavior_velocity_planner.param.yaml"
2931
/>
3032
<arg name="path_smoother_param" default="$(find-pkg-share autoware_launch)/config/planning/scenario_planning/lane_driving/motion_planning/path_smoother/elastic_band_smoother.param.yaml"/>
31-
<arg name="path_optimizer_param" default="$(find-pkg-share autoware_launch)/config/planning/scenario_planning/lane_driving/motion_planning/path_optimizer/path_optimizer.param.yaml"/>
33+
<arg name="path_optimizer_param" default="$(find-pkg-share autoware_launch)/config/planning/scenario_planning/lane_driving/motion_planning/autoware_path_optimizer/path_optimizer.param.yaml"/>
3234
<arg name="mission_planner_param" default="$(find-pkg-share autoware_launch)/config/planning/mission_planning/mission_planner/mission_planner.param.yaml"/>
3335

3436
<!-- Global parameters (for PathFootprint in tier4_planning_rviz_plugin) -->
@@ -55,12 +57,8 @@
5557
<node pkg="autoware_static_centerline_generator" exec="main" name="static_centerline_generator">
5658
<remap from="lanelet2_map_topic" to="$(var lanelet2_map_topic)"/>
5759
<remap from="input_centerline" to="~/input_centerline"/>
58-
<remap from="output_whole_centerline" to="~/output_whole_centerline"/>
59-
<remap from="output_centerline" to="~/output_centerline"/>
60-
<remap from="debug/raw_centerline" to="~/debug/raw_centerline"/>
61-
<remap from="debug/unsafe_footprints" to="~/debug/unsafe_footprints"/>
6260

63-
<param name="run_background" value="$(var run_background)"/>
61+
<param name="mode" value="$(var mode)"/>
6462
<param name="lanelet2_input_file_path" value="$(var lanelet2_input_file_path)"/>
6563
<param name="lanelet2_output_file_path" value="$(var lanelet2_output_file_path)"/>
6664
<param name="start_lanelet_id" value="$(var start_lanelet_id)"/>
@@ -75,12 +73,19 @@
7573
<param from="$(var path_smoother_param)"/>
7674
<param from="$(var path_optimizer_param)"/>
7775
<param from="$(var mission_planner_param)"/>
76+
<param name="check_footprint_inside_lanes" value="false"/>
77+
<!-- override the mission_planner's parameter -->
7878
<!-- node param -->
7979
<param from="$(find-pkg-share autoware_static_centerline_generator)/config/static_centerline_generator.param.yaml"/>
8080
<param name="centerline_source" value="$(var centerline_source)"/>
8181
<param name="bag_filename" value="$(var bag_filename)"/>
8282
</node>
8383

84+
<!-- GUI to select the range of centerline -->
85+
<group if="$(eval &quot;'$(var mode)'=='GUI'&quot;)">
86+
<node pkg="autoware_static_centerline_generator" exec="centerline_updater_helper.py" name="centerline_updater_helper"/>
87+
</group>
88+
8489
<!-- rviz -->
85-
<node pkg="rviz2" exec="rviz2" name="rviz2" output="screen" args="-d $(find-pkg-share autoware_static_centerline_generator)/rviz/static_centerline_generator.param" if="$(var rviz)"/>
90+
<node pkg="rviz2" exec="rviz2" name="rviz2" output="screen" args="-d $(find-pkg-share autoware_static_centerline_generator)/rviz/static_centerline_generator.rviz" if="$(var rviz)"/>
8691
</launch>

planning/autoware_static_centerline_generator/rviz/static_centerline_generator.rviz

+29-5
Original file line numberDiff line numberDiff line change
@@ -179,11 +179,11 @@ Visualization Manager:
179179
Filter size: 10
180180
History Policy: Keep Last
181181
Reliability Policy: Reliable
182-
Value: /static_centerline_generator/output_centerline
182+
Value: /static_centerline_generator/output/centerline
183183
Value: true
184184
View Footprint:
185-
Alpha: 1
186-
Color: 255; 0; 0
185+
Alpha: 0.5
186+
Color: 0; 255; 0
187187
Offset from BaseLink: 0
188188
Rear Overhang: 1.0299999713897705
189189
Value: true
@@ -268,9 +268,33 @@ Visualization Manager:
268268
Durability Policy: Transient Local
269269
History Policy: Keep Last
270270
Reliability Policy: Reliable
271-
Value: /static_centerline_generator/debug/unsafe_footprints
271+
Value: /static_centerline_generator/output/validation_results
272272
Value: true
273-
Enabled: false
273+
- Class: rviz_default_plugins/MarkerArray
274+
Enabled: true
275+
Name: Debug Markers
276+
Namespaces:
277+
curvature: false
278+
Topic:
279+
Depth: 5
280+
Durability Policy: Transient Local
281+
History Policy: Keep Last
282+
Reliability Policy: Reliable
283+
Value: /static_centerline_generator/debug/markers
284+
Value: true
285+
- Class: rviz_default_plugins/MarkerArray
286+
Enabled: false
287+
Name: MarkerArray
288+
Namespaces:
289+
{}
290+
Topic:
291+
Depth: 5
292+
Durability Policy: Transient Local
293+
History Policy: Keep Last
294+
Reliability Policy: Reliable
295+
Value: /static_centerline_generator/debug/ego_footprint_bounds
296+
Value: true
297+
Enabled: true
274298
Name: debug
275299
Enabled: true
276300
Global Options:

planning/autoware_static_centerline_generator/scripts/centerline_updater_helper.py

+81-82
Original file line numberDiff line numberDiff line change
@@ -20,18 +20,20 @@
2020

2121
from PyQt5 import QtCore
2222
from PyQt5.QtWidgets import QApplication
23-
from PyQt5.QtWidgets import QGridLayout
23+
from PyQt5.QtWidgets import QGroupBox
2424
from PyQt5.QtWidgets import QMainWindow
2525
from PyQt5.QtWidgets import QPushButton
2626
from PyQt5.QtWidgets import QSizePolicy
2727
from PyQt5.QtWidgets import QSlider
28+
from PyQt5.QtWidgets import QVBoxLayout
2829
from PyQt5.QtWidgets import QWidget
2930
from autoware_planning_msgs.msg import Trajectory
3031
import rclpy
3132
from rclpy.node import Node
3233
from rclpy.qos import QoSDurabilityPolicy
3334
from rclpy.qos import QoSProfile
34-
from std_msgs.msg import Bool
35+
from std_msgs.msg import Empty
36+
from std_msgs.msg import Float32
3537
from std_msgs.msg import Int32
3638

3739

@@ -46,95 +48,69 @@ def setupUI(self):
4648
self.setWindowFlags(QtCore.Qt.WindowStaysOnTopHint)
4749

4850
central_widget = QWidget(self)
49-
central_widget.setObjectName("central_widget")
50-
51-
self.grid_layout = QGridLayout(central_widget)
51+
self.grid_layout = QVBoxLayout(central_widget)
5252
self.grid_layout.setContentsMargins(10, 10, 10, 10)
53-
self.grid_layout.setObjectName("grid_layout")
54-
55-
# button to update the trajectory's start and end index
56-
self.update_button = QPushButton("update slider")
57-
self.update_button.setSizePolicy(QSizePolicy.Expanding, QSizePolicy.Expanding)
58-
self.update_button.clicked.connect(self.onUpdateButton)
59-
60-
# button to reset the trajectory's start and end index
61-
self.reset_button = QPushButton("reset")
62-
self.reset_button.setSizePolicy(QSizePolicy.Expanding, QSizePolicy.Expanding)
63-
self.reset_button.clicked.connect(self.onResetButton)
64-
65-
# button to save map
66-
self.save_map_button = QPushButton("save map")
67-
self.save_map_button.setSizePolicy(QSizePolicy.Expanding, QSizePolicy.Expanding)
6853

6954
# slide of the trajectory's start and end index
7055
self.traj_start_index_slider = QSlider(QtCore.Qt.Horizontal)
7156
self.traj_end_index_slider = QSlider(QtCore.Qt.Horizontal)
7257
self.min_traj_start_index = 0
7358
self.max_traj_end_index = None
7459

75-
# set layout
76-
self.grid_layout.addWidget(self.update_button, 1, 0, 1, -1)
77-
self.grid_layout.addWidget(self.reset_button, 2, 0, 1, -1)
78-
self.grid_layout.addWidget(self.save_map_button, 3, 0, 1, -1)
79-
self.grid_layout.addWidget(self.traj_start_index_slider, 4, 0, 1, -1)
80-
self.grid_layout.addWidget(self.traj_end_index_slider, 5, 0, 1, -1)
81-
self.setCentralWidget(central_widget)
82-
83-
def initWithEndIndex(self, max_traj_end_index):
84-
self.max_traj_end_index = max_traj_end_index
85-
86-
# initialize slider
87-
self.displayed_min_traj_start_index = self.min_traj_start_index
88-
self.displayed_max_traj_end_index = self.max_traj_end_index
89-
self.initializeSlider()
90-
91-
def initializeSlider(self, move_value_to_end=True):
92-
self.traj_start_index_slider.setMinimum(0)
93-
self.traj_end_index_slider.setMinimum(0)
94-
self.traj_start_index_slider.setMaximum(
95-
self.displayed_max_traj_end_index - self.displayed_min_traj_start_index
96-
)
97-
self.traj_end_index_slider.setMaximum(
98-
self.displayed_max_traj_end_index - self.displayed_min_traj_start_index
60+
# Layout: Range of Centerline
61+
centerline_vertical_box = QVBoxLayout(self)
62+
centerline_vertical_box.addWidget(self.traj_start_index_slider)
63+
centerline_vertical_box.addWidget(self.traj_end_index_slider)
64+
centerline_group = QGroupBox("Centerline")
65+
centerline_group.setLayout(centerline_vertical_box)
66+
self.grid_layout.addWidget(centerline_group)
67+
68+
# 2. Road Boundary
69+
road_boundary_group = QGroupBox("Road Boundary")
70+
road_boundary_vertical_box = QVBoxLayout(self)
71+
road_boundary_group.setLayout(road_boundary_vertical_box)
72+
self.grid_layout.addWidget(road_boundary_group)
73+
74+
# 2.1. Slider
75+
self.road_boundary_lateral_margin_slider = QSlider(QtCore.Qt.Horizontal)
76+
road_boundary_vertical_box.addWidget(self.road_boundary_lateral_margin_slider)
77+
self.road_boundary_lateral_margin_ratio = 10
78+
self.road_boundary_lateral_margin_slider.setMinimum(0)
79+
self.road_boundary_lateral_margin_slider.setMaximum(
80+
5 * self.road_boundary_lateral_margin_ratio
9981
)
10082

101-
if move_value_to_end:
102-
self.traj_start_index_slider.setValue(0)
103-
self.traj_end_index_slider.setValue(self.traj_end_index_slider.maximum())
83+
road_boundary_vertical_box.addWidget(QPushButton("reset"))
10484

105-
def onResetButton(self, event):
106-
current_traj_start_index = self.displayed_min_traj_start_index
107-
current_traj_end_index = self.displayed_max_traj_end_index
85+
# 3. General
86+
general_group = QGroupBox("General")
87+
general_vertical_box = QVBoxLayout(self)
88+
general_group.setLayout(general_vertical_box)
89+
self.grid_layout.addWidget(general_group)
10890

109-
self.displayed_min_traj_start_index = self.min_traj_start_index
110-
self.displayed_max_traj_end_index = self.max_traj_end_index
91+
# 3.1. Validate Centerline
92+
self.validate_button = QPushButton("validate")
93+
self.validate_button.setSizePolicy(QSizePolicy.Expanding, QSizePolicy.Expanding)
94+
general_vertical_box.addWidget(self.validate_button)
11195

112-
self.initializeSlider(False)
113-
self.traj_start_index_slider.setValue(current_traj_start_index)
114-
if (
115-
current_traj_start_index == self.min_traj_start_index
116-
and current_traj_end_index == self.max_traj_end_index
117-
):
118-
self.traj_end_index_slider.setValue(self.displayed_max_traj_end_index)
119-
else:
120-
self.traj_end_index_slider.setValue(
121-
current_traj_start_index + self.traj_end_index_slider.value()
122-
)
123-
124-
def onUpdateButton(self, event):
125-
current_traj_start_index = self.getCurrentStartIndex()
126-
current_traj_end_index = self.getCurrentEndIndex()
96+
# 3.2. Save Map
97+
self.save_map_button = QPushButton("save map")
98+
self.save_map_button.setSizePolicy(QSizePolicy.Expanding, QSizePolicy.Expanding)
99+
general_vertical_box.addWidget(self.save_map_button)
127100

128-
self.displayed_min_traj_start_index = current_traj_start_index
129-
self.displayed_max_traj_end_index = current_traj_end_index
101+
self.setCentralWidget(central_widget)
130102

131-
self.initializeSlider()
103+
def initWithEndIndex(self, max_traj_end_index):
104+
self.max_traj_end_index = max_traj_end_index
132105

133-
def getCurrentStartIndex(self):
134-
return self.displayed_min_traj_start_index + self.traj_start_index_slider.value()
106+
# initialize sliders
107+
self.traj_start_index_slider.setMinimum(self.min_traj_start_index)
108+
self.traj_start_index_slider.setMaximum(self.max_traj_end_index)
109+
self.traj_start_index_slider.setValue(self.min_traj_start_index)
135110

136-
def getCurrentEndIndex(self):
137-
return self.displayed_min_traj_start_index + self.traj_end_index_slider.value()
111+
self.traj_end_index_slider.setMinimum(self.min_traj_start_index)
112+
self.traj_end_index_slider.setMaximum(self.max_traj_end_index)
113+
self.traj_end_index_slider.setValue(self.max_traj_end_index)
138114

139115

140116
class CenterlineUpdaterHelper(Node):
@@ -144,18 +120,30 @@ def __init__(self):
144120
self.widget = CenterlineUpdaterWidget()
145121
self.widget.show()
146122
self.widget.save_map_button.clicked.connect(self.onSaveMapButtonPushed)
123+
self.widget.validate_button.clicked.connect(self.onValidateButtonPushed)
147124
self.widget.traj_start_index_slider.valueChanged.connect(self.onStartIndexChanged)
148125
self.widget.traj_end_index_slider.valueChanged.connect(self.onEndIndexChanged)
126+
self.widget.road_boundary_lateral_margin_slider.valueChanged.connect(
127+
self.onRoadBoundaryLateralMargin
128+
)
149129

150130
# ROS
151-
self.pub_save_map = self.create_publisher(Bool, "~/save_map", 1)
152-
self.pub_traj_start_index = self.create_publisher(Int32, "~/traj_start_index", 1)
153-
self.pub_traj_end_index = self.create_publisher(Int32, "~/traj_end_index", 1)
131+
self.pub_save_map = self.create_publisher(Empty, "/static_centerline_generator/save_map", 1)
132+
self.pub_validate = self.create_publisher(Empty, "/static_centerline_generator/validate", 1)
133+
self.pub_traj_start_index = self.create_publisher(
134+
Int32, "/static_centerline_generator/traj_start_index", 1
135+
)
136+
self.pub_traj_end_index = self.create_publisher(
137+
Int32, "/static_centerline_generator/traj_end_index", 1
138+
)
139+
self.pub_road_boundary_lateral_margin = self.create_publisher(
140+
Float32, "/static_centerline_generator/road_boundary_lateral_margin", 1
141+
)
154142

155143
transient_local_qos = QoSProfile(depth=1, durability=QoSDurabilityPolicy.TRANSIENT_LOCAL)
156144
self.sub_whole_centerline = self.create_subscription(
157145
Trajectory,
158-
"/static_centerline_generator/output_whole_centerline",
146+
"/static_centerline_generator/output/whole_centerline",
159147
self.onWholeCenterline,
160148
transient_local_qos,
161149
)
@@ -168,20 +156,31 @@ def onWholeCenterline(self, whole_centerline):
168156
self.widget.initWithEndIndex(len(whole_centerline.points) - 1)
169157

170158
def onSaveMapButtonPushed(self, event):
171-
msg = Bool()
172-
msg.data = True
159+
msg = Empty()
173160
self.pub_save_map.publish(msg)
174161

162+
def onValidateButtonPushed(self, event):
163+
msg = Empty()
164+
self.pub_validate.publish(msg)
165+
175166
def onStartIndexChanged(self, event):
176167
msg = Int32()
177-
msg.data = self.widget.getCurrentStartIndex()
168+
msg.data = self.widget.traj_start_index_slider.value()
178169
self.pub_traj_start_index.publish(msg)
179170

180171
def onEndIndexChanged(self, event):
181172
msg = Int32()
182-
msg.data = self.widget.getCurrentEndIndex()
173+
msg.data = self.widget.traj_end_index_slider.value()
183174
self.pub_traj_end_index.publish(msg)
184175

176+
def onRoadBoundaryLateralMargin(self, event):
177+
msg = Float32()
178+
msg.data = (
179+
self.widget.road_boundary_lateral_margin_slider.value()
180+
/ self.widget.road_boundary_lateral_margin_ratio
181+
)
182+
self.pub_road_boundary_lateral_margin.publish(msg)
183+
185184

186185
def main(args=None):
187186
app = QApplication(sys.argv)
Original file line numberDiff line numberDiff line change
@@ -1,3 +1,5 @@
11
#!/bin/bash
22

3-
ros2 launch autoware_static_centerline_generator static_centerline_generator.launch.xml centerline_source:="bag_ego_trajectory_base" run_background:=false lanelet2_input_file_path:="$HOME/autoware_map/sample_map/lanelet2_map.osm" bag_filename:="$(ros2 pkg prefix autoware_static_centerline_generator --share)/test/data/bag_ego_trajectory.db3" vehicle_model:=lexus
3+
ros2 launch autoware_static_centerline_generator static_centerline_generator.launch.xml centerline_source:="bag_ego_trajectory_base" mode:="GUI" lanelet2_input_file_path:="$HOME/autoware_map/sample_map/lanelet2_map.osm" bag_filename:="$(ros2 pkg prefix autoware_static_centerline_generator --share)/test/data/bag_ego_trajectory_turn-right.db3" vehicle_model:=lexus
4+
5+
# ros2 launch autoware_static_centerline_generator static_centerline_generator.launch.xml centerline_source:="bag_ego_trajectory_base" mode:="GUI" lanelet2_input_file_path:="$HOME/autoware_map/sample_map/lanelet2_map.osm" bag_filename:="$(ros2 pkg prefix autoware_static_centerline_generator --share)/test/data/bag_ego_trajectory_turn-left-right.db3" vehicle_model:=lexus

0 commit comments

Comments
 (0)