Skip to content

Commit cd1382f

Browse files
committed
Revert "feat: #389 set initial pose directly (#399)"
This reverts commit dc4240e.
1 parent 206e0fb commit cd1382f

File tree

4 files changed

+27
-86
lines changed

4 files changed

+27
-86
lines changed

.vscode/settings.json

+2
Original file line numberDiff line numberDiff line change
@@ -35,6 +35,7 @@
3535
"${env:HOME}/ros_ws/awf/install/driving_log_replayer_msgs/local/lib/python3.10/dist-packages",
3636
"${env:HOME}/ros_ws/awf/install/tier4_debug_msgs/local/lib/python3.10/dist-packages",
3737
"${env:HOME}/ros_ws/awf/install/driving_log_replayer_analyzer/local/lib/python3.10/dist-packages",
38+
"${env:HOME}/ros_ws/awf/install/autoware_adapi_v1_msgs/local/lib/python3.10/dist-packages",
3839
"${env:HOME}/ros_ws/awf/install/tier4_localization_msgs/local/lib/python3.10/dist-packages",
3940
"${env:HOME}/ros_ws/awf/install/tier4_api_msgs/local/lib/python3.10/dist-packages",
4041
"${env:HOME}/ros_ws/awf/install/tier4_perception_msgs/local/lib/python3.10/dist-packages",
@@ -52,6 +53,7 @@
5253
"${env:HOME}/ros_ws/awf/install/driving_log_replayer_msgs/local/lib/python3.10/dist-packages",
5354
"${env:HOME}/ros_ws/awf/install/tier4_debug_msgs/local/lib/python3.10/dist-packages",
5455
"${env:HOME}/ros_ws/awf/install/driving_log_replayer_analyzer/local/lib/python3.10/dist-packages",
56+
"${env:HOME}/ros_ws/awf/install/autoware_adapi_v1_msgs/local/lib/python3.10/dist-packages",
5557
"${env:HOME}/ros_ws/awf/install/tier4_localization_msgs/local/lib/python3.10/dist-packages",
5658
"${env:HOME}/ros_ws/awf/install/tier4_api_msgs/local/lib/python3.10/dist-packages",
5759
"${env:HOME}/ros_ws/awf/install/tier4_perception_msgs/local/lib/python3.10/dist-packages",

driving_log_replayer/driving_log_replayer/evaluator.py

+22-44
Original file line numberDiff line numberDiff line change
@@ -43,7 +43,6 @@
4343
from tf2_ros import TransformException
4444
from tf2_ros import TransformListener
4545
from tf_transformations import euler_from_quaternion
46-
from tier4_localization_msgs.srv import InitializeLocalization
4746
from tier4_localization_msgs.srv import PoseWithCovarianceStamped as PoseWithCovarianceStampedSrv
4847
import yaml
4948

@@ -53,7 +52,7 @@
5352
from driving_log_replayer.scenario import load_scenario
5453

5554
if TYPE_CHECKING:
56-
from autoware_common_msgs.msg import ResponseStatus
55+
from autoware_adapi_v1_msgs.msg import ResponseStatus
5756

5857

5958
class DLREvaluator(Node):
@@ -107,10 +106,9 @@ def __init__(self, name: str, scenario_class: Callable, result_class: Callable)
107106
self._tf_buffer = Buffer()
108107
self._tf_listener = TransformListener(self._tf_buffer, self, spin_thread=True)
109108

110-
# initial pose estimation
111109
self._initial_pose_running: bool = False
112110
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()
114112
self.start_initialpose_service()
115113

116114
self._current_time = Time().to_msg()
@@ -175,20 +173,17 @@ def start_initialpose_service(self) -> None:
175173
return
176174
self._initial_pose_client = self.create_client(
177175
InitializeLocalization,
178-
"/localization/initialize",
176+
"/api/localization/initialize",
179177
)
178+
self._map_fit_client = self.create_client(
179+
PoseWithCovarianceStampedSrv,
180+
"/map/map_height_fitter/service",
181+
)
182+
180183
while not self._initial_pose_client.wait_for_service(timeout_sec=1.0):
181184
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...")
192187

193188
def call_initialpose_service(self) -> None:
194189
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:
198193
)
199194
self._initial_pose_running = True
200195
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)
214200

215201
def map_fit_cb(self, future: Future) -> None:
216202
result = future.result()
217203
if result is not None:
218204
if result.success:
219205
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]),
224207
)
225208
future_init_pose.add_done_callback(self.initial_pose_cb)
226209
else:
@@ -283,17 +266,12 @@ def transform_stamped_with_euler_angle(cls, transform_stamped: TransformStamped)
283266
}
284267
return tf_euler
285268

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
297275
covariance = np.array(
298276
[
299277
0.25,
@@ -341,7 +319,7 @@ def set_initial_pose(self) -> tuple[PoseWithCovarianceStamped | None, int | None
341319
),
342320
covariance=covariance,
343321
)
344-
return PoseWithCovarianceStamped(header=Header(frame_id="map"), pose=pose), pose_method
322+
return PoseWithCovarianceStamped(header=Header(frame_id="map"), pose=pose)
345323

346324
@classmethod
347325
def get_perception_label_str(cls, classification: ObjectClassification) -> str:

driving_log_replayer/driving_log_replayer/localization.py

+3-12
Original file line numberDiff line numberDiff line change
@@ -24,14 +24,13 @@
2424
from geometry_msgs.msg import PoseStamped
2525
import numpy as np
2626
from pydantic import BaseModel
27-
from pydantic import model_validator
2827
from rosidl_runtime_py import message_to_ordereddict
2928
from tier4_debug_msgs.msg import Float32Stamped
3029
from tier4_debug_msgs.msg import Int32Stamped
3130

3231
from driving_log_replayer.result import EvaluationItem
3332
from driving_log_replayer.result import ResultBase
34-
from driving_log_replayer.scenario import InitialPose as InitialPoseModel
33+
from driving_log_replayer.scenario import InitialPose
3534
from driving_log_replayer.scenario import number
3635
from driving_log_replayer.scenario import Scenario
3736

@@ -66,17 +65,9 @@ class Conditions(BaseModel):
6665

6766
class Evaluation(BaseModel):
6867
UseCaseName: Literal["localization"]
69-
UseCaseFormatVersion: Literal["1.2.0", "1.3.0"]
68+
UseCaseFormatVersion: Literal["1.2.0"]
7069
Conditions: Conditions
71-
InitialPose: InitialPoseModel | None = None
72-
DirectInitialPose: InitialPoseModel | None = None
73-
74-
@model_validator(mode="after")
75-
def mutually_exclusive(self) -> "Evaluation":
76-
if self.InitialPose is not None and self.DirectInitialPose is not None:
77-
err_msg = "Expected only one, either `InitialPose` or `DirectInitialPose`, not together"
78-
raise ValueError(err_msg)
79-
return self
70+
InitialPose: InitialPose | None
8071

8172

8273
class LocalizationScenario(Scenario):

sample/localization/scenario_direct_initialpose.yaml

-30
This file was deleted.

0 commit comments

Comments
 (0)