Skip to content

Commit 40f39f3

Browse files
committed
Revert "feat: #389 set initial pose directly (#399)"
This reverts commit dc4240e.
1 parent 26e327b commit 40f39f3

File tree

5 files changed

+33
-93
lines changed

5 files changed

+33
-93
lines changed

.vscode/settings.json

+6-6
Original file line numberDiff line numberDiff line change
@@ -35,12 +35,12 @@
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",
41-
"${env:HOME}/ros_ws/awf/install/lanelet2_extension_python/local/lib/python3.10/dist-packages",
42-
"${env:HOME}/ros_ws/awf/install/autoware_common_msgs/local/lib/python3.10/dist-packages",
43-
"${env:HOME}/ros_ws/awf/install/autoware_perception_msgs/local/lib/python3.10/dist-packages"
42+
"${env:HOME}/ros_ws/awf/install/autoware_perception_msgs/local/lib/python3.10/dist-packages",
43+
"${env:HOME}/ros_ws/awf/install/lanelet2_extension_python/local/lib/python3.10/dist-packages"
4444
],
4545
"python.analysis.extraPaths": [
4646
"/opt/ros/humble/lib/python3.10/site-packages",
@@ -53,12 +53,12 @@
5353
"${env:HOME}/ros_ws/awf/install/driving_log_replayer_msgs/local/lib/python3.10/dist-packages",
5454
"${env:HOME}/ros_ws/awf/install/tier4_debug_msgs/local/lib/python3.10/dist-packages",
5555
"${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",
5657
"${env:HOME}/ros_ws/awf/install/tier4_localization_msgs/local/lib/python3.10/dist-packages",
5758
"${env:HOME}/ros_ws/awf/install/tier4_api_msgs/local/lib/python3.10/dist-packages",
5859
"${env:HOME}/ros_ws/awf/install/tier4_perception_msgs/local/lib/python3.10/dist-packages",
59-
"${env:HOME}/ros_ws/awf/install/lanelet2_extension_python/local/lib/python3.10/dist-packages",
60-
"${env:HOME}/ros_ws/awf/install/autoware_common_msgs/local/lib/python3.10/dist-packages",
61-
"${env:HOME}/ros_ws/awf/install/autoware_perception_msgs/local/lib/python3.10/dist-packages"
60+
"${env:HOME}/ros_ws/awf/install/autoware_perception_msgs/local/lib/python3.10/dist-packages",
61+
"${env:HOME}/ros_ws/awf/install/lanelet2_extension_python/local/lib/python3.10/dist-packages"
6262
],
6363
"files.associations": {
6464
"cctype": "cpp",

driving_log_replayer/driving_log_replayer/evaluator.py

+23-44
Original file line numberDiff line numberDiff line change
@@ -18,6 +18,7 @@
1818
from typing import Any
1919
from typing import TYPE_CHECKING
2020

21+
from autoware_adapi_v1_msgs.srv import InitializeLocalization
2122
from autoware_auto_perception_msgs.msg import ObjectClassification
2223
from builtin_interfaces.msg import Time as Stamp
2324
from geometry_msgs.msg import Point
@@ -43,7 +44,6 @@
4344
from tf2_ros import TransformException
4445
from tf2_ros import TransformListener
4546
from tf_transformations import euler_from_quaternion
46-
from tier4_localization_msgs.srv import InitializeLocalization
4747
from tier4_localization_msgs.srv import PoseWithCovarianceStamped as PoseWithCovarianceStampedSrv
4848
import yaml
4949

@@ -53,7 +53,7 @@
5353
from driving_log_replayer.scenario import load_scenario
5454

5555
if TYPE_CHECKING:
56-
from autoware_common_msgs.msg import ResponseStatus
56+
from autoware_adapi_v1_msgs.msg import ResponseStatus
5757

5858

5959
class DLREvaluator(Node):
@@ -107,10 +107,9 @@ def __init__(self, name: str, scenario_class: Callable, result_class: Callable)
107107
self._tf_buffer = Buffer()
108108
self._tf_listener = TransformListener(self._tf_buffer, self, spin_thread=True)
109109

110-
# initial pose estimation
111110
self._initial_pose_running: bool = False
112111
self._initial_pose_success: bool = False
113-
self._initial_pose, self._initial_pose_method = self.set_initial_pose()
112+
self._initial_pose = self.set_initial_pose()
114113
self.start_initialpose_service()
115114

116115
self._current_time = Time().to_msg()
@@ -175,20 +174,17 @@ def start_initialpose_service(self) -> None:
175174
return
176175
self._initial_pose_client = self.create_client(
177176
InitializeLocalization,
178-
"/localization/initialize",
177+
"/api/localization/initialize",
179178
)
179+
self._map_fit_client = self.create_client(
180+
PoseWithCovarianceStampedSrv,
181+
"/map/map_height_fitter/service",
182+
)
183+
180184
while not self._initial_pose_client.wait_for_service(timeout_sec=1.0):
181185
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-
)
186+
while not self._map_fit_client.wait_for_service(timeout_sec=1.0):
187+
self.get_logger().warning("map height fitter service not available, waiting again...")
192188

193189
def call_initialpose_service(self) -> None:
194190
if self._initial_pose is None or self._initial_pose_success or self._initial_pose_running:
@@ -198,29 +194,17 @@ def call_initialpose_service(self) -> None:
198194
)
199195
self._initial_pose_running = True
200196
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)
197+
future_map_fit = self._map_fit_client.call_async(
198+
PoseWithCovarianceStampedSrv.Request(pose_with_covariance=self._initial_pose),
199+
)
200+
future_map_fit.add_done_callback(self.map_fit_cb)
214201

215202
def map_fit_cb(self, future: Future) -> None:
216203
result = future.result()
217204
if result is not None:
218205
if result.success:
219206
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-
),
207+
InitializeLocalization.Request(pose=[result.pose_with_covariance]),
224208
)
225209
future_init_pose.add_done_callback(self.initial_pose_cb)
226210
else:
@@ -283,17 +267,12 @@ def transform_stamped_with_euler_angle(cls, transform_stamped: TransformStamped)
283267
}
284268
return tf_euler
285269

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
270+
def set_initial_pose(self) -> PoseWithCovarianceStamped | None:
271+
if not hasattr(self._scenario.Evaluation, "InitialPose"):
272+
return None
273+
if self._scenario.Evaluation.InitialPose is None:
274+
return None
275+
initial_pose: InitialPose = self._scenario.Evaluation.InitialPose
297276
covariance = np.array(
298277
[
299278
0.25,
@@ -341,7 +320,7 @@ def set_initial_pose(self) -> tuple[PoseWithCovarianceStamped | None, int | None
341320
),
342321
covariance=covariance,
343322
)
344-
return PoseWithCovarianceStamped(header=Header(frame_id="map"), pose=pose), pose_method
323+
return PoseWithCovarianceStamped(header=Header(frame_id="map"), pose=pose)
345324

346325
@classmethod
347326
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):

driving_log_replayer/package.xml

+1-1
Original file line numberDiff line numberDiff line change
@@ -16,9 +16,9 @@
1616

1717
<build_depend>autoware_cmake</build_depend>
1818

19+
<depend>autoware_adapi_v1_msgs</depend>
1920
<depend>autoware_auto_mapping_msgs</depend>
2021
<depend>autoware_auto_perception_msgs</depend>
21-
<depend>autoware_common_msgs</depend>
2222
<depend>autoware_perception_msgs</depend>
2323
<depend>diagnostic_msgs</depend>
2424
<depend>driving_log_replayer_msgs</depend>

sample/localization/scenario_direct_initialpose.yaml

-30
This file was deleted.

0 commit comments

Comments
 (0)