43
43
from tf2_ros import TransformException
44
44
from tf2_ros import TransformListener
45
45
from tf_transformations import euler_from_quaternion
46
- from tier4_localization_msgs .srv import InitializeLocalization
47
46
from tier4_localization_msgs .srv import PoseWithCovarianceStamped as PoseWithCovarianceStampedSrv
48
47
import yaml
49
48
53
52
from driving_log_replayer .scenario import load_scenario
54
53
55
54
if TYPE_CHECKING :
56
- from autoware_common_msgs .msg import ResponseStatus
55
+ from autoware_adapi_v1_msgs .msg import ResponseStatus
57
56
58
57
59
58
class DLREvaluator (Node ):
@@ -107,10 +106,9 @@ def __init__(self, name: str, scenario_class: Callable, result_class: Callable)
107
106
self ._tf_buffer = Buffer ()
108
107
self ._tf_listener = TransformListener (self ._tf_buffer , self , spin_thread = True )
109
108
110
- # initial pose estimation
111
109
self ._initial_pose_running : bool = False
112
110
self ._initial_pose_success : bool = False
113
- self ._initial_pose , self . _initial_pose_method = self .set_initial_pose ()
111
+ self ._initial_pose = self .set_initial_pose ()
114
112
self .start_initialpose_service ()
115
113
116
114
self ._current_time = Time ().to_msg ()
@@ -175,20 +173,17 @@ def start_initialpose_service(self) -> None:
175
173
return
176
174
self ._initial_pose_client = self .create_client (
177
175
InitializeLocalization ,
178
- "/localization/initialize" ,
176
+ "/api/ localization/initialize" ,
179
177
)
178
+ self ._map_fit_client = self .create_client (
179
+ PoseWithCovarianceStampedSrv ,
180
+ "/map/map_height_fitter/service" ,
181
+ )
182
+
180
183
while not self ._initial_pose_client .wait_for_service (timeout_sec = 1.0 ):
181
184
self .get_logger ().warning ("initial pose service not available, waiting again..." )
182
-
183
- if self ._initial_pose_method == InitializeLocalization .Request .AUTO :
184
- self ._map_fit_client = self .create_client (
185
- PoseWithCovarianceStampedSrv ,
186
- "/map/map_height_fitter/service" ,
187
- )
188
- while not self ._map_fit_client .wait_for_service (timeout_sec = 1.0 ):
189
- self .get_logger ().warning (
190
- "map height fitter service not available, waiting again..." ,
191
- )
185
+ while not self ._map_fit_client .wait_for_service (timeout_sec = 1.0 ):
186
+ self .get_logger ().warning ("map height fitter service not available, waiting again..." )
192
187
193
188
def call_initialpose_service (self ) -> None :
194
189
if self ._initial_pose is None or self ._initial_pose_success or self ._initial_pose_running :
@@ -198,29 +193,17 @@ def call_initialpose_service(self) -> None:
198
193
)
199
194
self ._initial_pose_running = True
200
195
self ._initial_pose .header .stamp = self ._current_time
201
- if self ._initial_pose_method == InitializeLocalization .Request .AUTO :
202
- future_map_fit = self ._map_fit_client .call_async (
203
- PoseWithCovarianceStampedSrv .Request (pose_with_covariance = self ._initial_pose ),
204
- )
205
- future_map_fit .add_done_callback (self .map_fit_cb )
206
- else :
207
- future_direct_init_pose = self ._initial_pose_client .call_async (
208
- InitializeLocalization .Request (
209
- method = self ._initial_pose_method ,
210
- pose_with_covariance = [self ._initial_pose ],
211
- ),
212
- )
213
- future_direct_init_pose .add_done_callback (self .initial_pose_cb )
196
+ future_map_fit = self ._map_fit_client .call_async (
197
+ PoseWithCovarianceStampedSrv .Request (pose_with_covariance = self ._initial_pose ),
198
+ )
199
+ future_map_fit .add_done_callback (self .map_fit_cb )
214
200
215
201
def map_fit_cb (self , future : Future ) -> None :
216
202
result = future .result ()
217
203
if result is not None :
218
204
if result .success :
219
205
future_init_pose = self ._initial_pose_client .call_async (
220
- InitializeLocalization .Request (
221
- method = self ._initial_pose_method ,
222
- pose_with_covariance = [result .pose_with_covariance ],
223
- ),
206
+ InitializeLocalization .Request (pose = [result .pose_with_covariance ]),
224
207
)
225
208
future_init_pose .add_done_callback (self .initial_pose_cb )
226
209
else :
@@ -283,17 +266,12 @@ def transform_stamped_with_euler_angle(cls, transform_stamped: TransformStamped)
283
266
}
284
267
return tf_euler
285
268
286
- def set_initial_pose (self ) -> tuple [PoseWithCovarianceStamped | None , int | None ]:
287
- auto_pose = getattr (self ._scenario .Evaluation , "InitialPose" , None )
288
- direct_pose = getattr (self ._scenario .Evaluation , "DirectInitialPose" , None )
289
- if auto_pose is None and direct_pose is None :
290
- return None , None
291
- if auto_pose is not None :
292
- initial_pose : InitialPose = auto_pose
293
- pose_method : int = InitializeLocalization .Request .AUTO
294
- if direct_pose is not None :
295
- initial_pose : InitialPose = direct_pose
296
- pose_method : int = InitializeLocalization .Request .DIRECT
269
+ def set_initial_pose (self ) -> PoseWithCovarianceStamped | None :
270
+ if not hasattr (self ._scenario .Evaluation , "InitialPose" ):
271
+ return None
272
+ if self ._scenario .Evaluation .InitialPose is None :
273
+ return None
274
+ initial_pose : InitialPose = self ._scenario .Evaluation .InitialPose
297
275
covariance = np .array (
298
276
[
299
277
0.25 ,
@@ -341,7 +319,7 @@ def set_initial_pose(self) -> tuple[PoseWithCovarianceStamped | None, int | None
341
319
),
342
320
covariance = covariance ,
343
321
)
344
- return PoseWithCovarianceStamped (header = Header (frame_id = "map" ), pose = pose ), pose_method
322
+ return PoseWithCovarianceStamped (header = Header (frame_id = "map" ), pose = pose )
345
323
346
324
@classmethod
347
325
def get_perception_label_str (cls , classification : ObjectClassification ) -> str :
0 commit comments