20
20
21
21
from PyQt5 import QtCore
22
22
from PyQt5 .QtWidgets import QApplication
23
- from PyQt5 .QtWidgets import QGridLayout
23
+ from PyQt5 .QtWidgets import QGroupBox
24
24
from PyQt5 .QtWidgets import QMainWindow
25
25
from PyQt5 .QtWidgets import QPushButton
26
26
from PyQt5 .QtWidgets import QSizePolicy
27
27
from PyQt5 .QtWidgets import QSlider
28
+ from PyQt5 .QtWidgets import QVBoxLayout
28
29
from PyQt5 .QtWidgets import QWidget
29
30
from autoware_planning_msgs .msg import Trajectory
30
31
import rclpy
31
32
from rclpy .node import Node
32
33
from rclpy .qos import QoSDurabilityPolicy
33
34
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
35
37
from std_msgs .msg import Int32
36
38
37
39
@@ -46,95 +48,69 @@ def setupUI(self):
46
48
self .setWindowFlags (QtCore .Qt .WindowStaysOnTopHint )
47
49
48
50
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 )
52
52
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 )
68
53
69
54
# slide of the trajectory's start and end index
70
55
self .traj_start_index_slider = QSlider (QtCore .Qt .Horizontal )
71
56
self .traj_end_index_slider = QSlider (QtCore .Qt .Horizontal )
72
57
self .min_traj_start_index = 0
73
58
self .max_traj_end_index = None
74
59
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
99
81
)
100
82
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" ))
104
84
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 )
108
90
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 )
111
95
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 )
127
100
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 )
130
102
131
- self .initializeSlider ()
103
+ def initWithEndIndex (self , max_traj_end_index ):
104
+ self .max_traj_end_index = max_traj_end_index
132
105
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 )
135
110
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 )
138
114
139
115
140
116
class CenterlineUpdaterHelper (Node ):
@@ -144,18 +120,30 @@ def __init__(self):
144
120
self .widget = CenterlineUpdaterWidget ()
145
121
self .widget .show ()
146
122
self .widget .save_map_button .clicked .connect (self .onSaveMapButtonPushed )
123
+ self .widget .validate_button .clicked .connect (self .onValidateButtonPushed )
147
124
self .widget .traj_start_index_slider .valueChanged .connect (self .onStartIndexChanged )
148
125
self .widget .traj_end_index_slider .valueChanged .connect (self .onEndIndexChanged )
126
+ self .widget .road_boundary_lateral_margin_slider .valueChanged .connect (
127
+ self .onRoadBoundaryLateralMargin
128
+ )
149
129
150
130
# 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
+ )
154
142
155
143
transient_local_qos = QoSProfile (depth = 1 , durability = QoSDurabilityPolicy .TRANSIENT_LOCAL )
156
144
self .sub_whole_centerline = self .create_subscription (
157
145
Trajectory ,
158
- "/static_centerline_generator/output_whole_centerline " ,
146
+ "/static_centerline_generator/output/whole_centerline " ,
159
147
self .onWholeCenterline ,
160
148
transient_local_qos ,
161
149
)
@@ -168,20 +156,31 @@ def onWholeCenterline(self, whole_centerline):
168
156
self .widget .initWithEndIndex (len (whole_centerline .points ) - 1 )
169
157
170
158
def onSaveMapButtonPushed (self , event ):
171
- msg = Bool ()
172
- msg .data = True
159
+ msg = Empty ()
173
160
self .pub_save_map .publish (msg )
174
161
162
+ def onValidateButtonPushed (self , event ):
163
+ msg = Empty ()
164
+ self .pub_validate .publish (msg )
165
+
175
166
def onStartIndexChanged (self , event ):
176
167
msg = Int32 ()
177
- msg .data = self .widget .getCurrentStartIndex ()
168
+ msg .data = self .widget .traj_start_index_slider . value ()
178
169
self .pub_traj_start_index .publish (msg )
179
170
180
171
def onEndIndexChanged (self , event ):
181
172
msg = Int32 ()
182
- msg .data = self .widget .getCurrentEndIndex ()
173
+ msg .data = self .widget .traj_end_index_slider . value ()
183
174
self .pub_traj_end_index .publish (msg )
184
175
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
+
185
184
186
185
def main (args = None ):
187
186
app = QApplication (sys .argv )
0 commit comments