Revert "feat: #389 set initial pose directly (#399)"

This reverts commit dc4240e.

Author: hayato-m126

.vscode/settings.json

@@ -35,12 +35,12 @@
     "${env:HOME}/ros_ws/awf/install/driving_log_replayer_msgs/local/lib/python3.10/dist-packages",
     "${env:HOME}/ros_ws/awf/install/tier4_debug_msgs/local/lib/python3.10/dist-packages",
     "${env:HOME}/ros_ws/awf/install/driving_log_replayer_analyzer/local/lib/python3.10/dist-packages",
+    "${env:HOME}/ros_ws/awf/install/autoware_adapi_v1_msgs/local/lib/python3.10/dist-packages",
     "${env:HOME}/ros_ws/awf/install/tier4_localization_msgs/local/lib/python3.10/dist-packages",
     "${env:HOME}/ros_ws/awf/install/tier4_api_msgs/local/lib/python3.10/dist-packages",
     "${env:HOME}/ros_ws/awf/install/tier4_perception_msgs/local/lib/python3.10/dist-packages",
-    "${env:HOME}/ros_ws/awf/install/lanelet2_extension_python/local/lib/python3.10/dist-packages",
-    "${env:HOME}/ros_ws/awf/install/autoware_common_msgs/local/lib/python3.10/dist-packages",
-    "${env:HOME}/ros_ws/awf/install/autoware_perception_msgs/local/lib/python3.10/dist-packages"
+    "${env:HOME}/ros_ws/awf/install/autoware_perception_msgs/local/lib/python3.10/dist-packages",
+    "${env:HOME}/ros_ws/awf/install/lanelet2_extension_python/local/lib/python3.10/dist-packages"
   ],
   "python.analysis.extraPaths": [
     "/opt/ros/humble/lib/python3.10/site-packages",
@@ -53,12 +53,12 @@
     "${env:HOME}/ros_ws/awf/install/driving_log_replayer_msgs/local/lib/python3.10/dist-packages",
     "${env:HOME}/ros_ws/awf/install/tier4_debug_msgs/local/lib/python3.10/dist-packages",
     "${env:HOME}/ros_ws/awf/install/driving_log_replayer_analyzer/local/lib/python3.10/dist-packages",
+    "${env:HOME}/ros_ws/awf/install/autoware_adapi_v1_msgs/local/lib/python3.10/dist-packages",
     "${env:HOME}/ros_ws/awf/install/tier4_localization_msgs/local/lib/python3.10/dist-packages",
     "${env:HOME}/ros_ws/awf/install/tier4_api_msgs/local/lib/python3.10/dist-packages",
     "${env:HOME}/ros_ws/awf/install/tier4_perception_msgs/local/lib/python3.10/dist-packages",
-    "${env:HOME}/ros_ws/awf/install/lanelet2_extension_python/local/lib/python3.10/dist-packages",
-    "${env:HOME}/ros_ws/awf/install/autoware_common_msgs/local/lib/python3.10/dist-packages",
-    "${env:HOME}/ros_ws/awf/install/autoware_perception_msgs/local/lib/python3.10/dist-packages"
+    "${env:HOME}/ros_ws/awf/install/autoware_perception_msgs/local/lib/python3.10/dist-packages",
+    "${env:HOME}/ros_ws/awf/install/lanelet2_extension_python/local/lib/python3.10/dist-packages"
   ],
   "files.associations": {
     "cctype": "cpp",

driving_log_replayer/driving_log_replayer/evaluator.py

@@ -18,6 +18,7 @@
 from typing import Any
 from typing import TYPE_CHECKING
 
+from autoware_adapi_v1_msgs.srv import InitializeLocalization
 from autoware_auto_perception_msgs.msg import ObjectClassification
 from builtin_interfaces.msg import Time as Stamp
 from geometry_msgs.msg import Point
@@ -43,7 +44,6 @@
 from tf2_ros import TransformException
 from tf2_ros import TransformListener
 from tf_transformations import euler_from_quaternion
-from tier4_localization_msgs.srv import InitializeLocalization
 from tier4_localization_msgs.srv import PoseWithCovarianceStamped as PoseWithCovarianceStampedSrv
 import yaml
 
@@ -53,7 +53,7 @@
 from driving_log_replayer.scenario import load_scenario
 
 if TYPE_CHECKING:
-    from autoware_common_msgs.msg import ResponseStatus
+    from autoware_adapi_v1_msgs.msg import ResponseStatus
 
 
 class DLREvaluator(Node):
@@ -107,10 +107,9 @@ def __init__(self, name: str, scenario_class: Callable, result_class: Callable)
         self._tf_buffer = Buffer()
         self._tf_listener = TransformListener(self._tf_buffer, self, spin_thread=True)
 
-        # initial pose estimation
         self._initial_pose_running: bool = False
         self._initial_pose_success: bool = False
-        self._initial_pose, self._initial_pose_method = self.set_initial_pose()
+        self._initial_pose = self.set_initial_pose()
         self.start_initialpose_service()
 
         self._current_time = Time().to_msg()
@@ -175,20 +174,17 @@ def start_initialpose_service(self) -> None:
             return
         self._initial_pose_client = self.create_client(
             InitializeLocalization,
-            "/localization/initialize",
+            "/api/localization/initialize",
         )
+        self._map_fit_client = self.create_client(
+            PoseWithCovarianceStampedSrv,
+            "/map/map_height_fitter/service",
+        )
+
         while not self._initial_pose_client.wait_for_service(timeout_sec=1.0):
             self.get_logger().warning("initial pose service not available, waiting again...")
-
-        if self._initial_pose_method == InitializeLocalization.Request.AUTO:
-            self._map_fit_client = self.create_client(
-                PoseWithCovarianceStampedSrv,
-                "/map/map_height_fitter/service",
-            )
-            while not self._map_fit_client.wait_for_service(timeout_sec=1.0):
-                self.get_logger().warning(
-                    "map height fitter service not available, waiting again...",
-                )
+        while not self._map_fit_client.wait_for_service(timeout_sec=1.0):
+            self.get_logger().warning("map height fitter service not available, waiting again...")
 
     def call_initialpose_service(self) -> None:
         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:
         )
         self._initial_pose_running = True
         self._initial_pose.header.stamp = self._current_time
-        if self._initial_pose_method == InitializeLocalization.Request.AUTO:
-            future_map_fit = self._map_fit_client.call_async(
-                PoseWithCovarianceStampedSrv.Request(pose_with_covariance=self._initial_pose),
-            )
-            future_map_fit.add_done_callback(self.map_fit_cb)
-        else:
-            future_direct_init_pose = self._initial_pose_client.call_async(
-                InitializeLocalization.Request(
-                    method=self._initial_pose_method,
-                    pose_with_covariance=[self._initial_pose],
-                ),
-            )
-            future_direct_init_pose.add_done_callback(self.initial_pose_cb)
+        future_map_fit = self._map_fit_client.call_async(
+            PoseWithCovarianceStampedSrv.Request(pose_with_covariance=self._initial_pose),
+        )
+        future_map_fit.add_done_callback(self.map_fit_cb)
 
     def map_fit_cb(self, future: Future) -> None:
         result = future.result()
         if result is not None:
             if result.success:
                 future_init_pose = self._initial_pose_client.call_async(
-                    InitializeLocalization.Request(
-                        method=self._initial_pose_method,
-                        pose_with_covariance=[result.pose_with_covariance],
-                    ),
+                    InitializeLocalization.Request(pose=[result.pose_with_covariance]),
                 )
                 future_init_pose.add_done_callback(self.initial_pose_cb)
             else:
@@ -283,17 +267,12 @@ def transform_stamped_with_euler_angle(cls, transform_stamped: TransformStamped)
         }
         return tf_euler
 
-    def set_initial_pose(self) -> tuple[PoseWithCovarianceStamped | None, int | None]:
-        auto_pose = getattr(self._scenario.Evaluation, "InitialPose", None)
-        direct_pose = getattr(self._scenario.Evaluation, "DirectInitialPose", None)
-        if auto_pose is None and direct_pose is None:
-            return None, None
-        if auto_pose is not None:
-            initial_pose: InitialPose = auto_pose
-            pose_method: int = InitializeLocalization.Request.AUTO
-        if direct_pose is not None:
-            initial_pose: InitialPose = direct_pose
-            pose_method: int = InitializeLocalization.Request.DIRECT
+    def set_initial_pose(self) -> PoseWithCovarianceStamped | None:
+        if not hasattr(self._scenario.Evaluation, "InitialPose"):
+            return None
+        if self._scenario.Evaluation.InitialPose is None:
+            return None
+        initial_pose: InitialPose = self._scenario.Evaluation.InitialPose
         covariance = np.array(
             [
                 0.25,
@@ -341,7 +320,7 @@ def set_initial_pose(self) -> tuple[PoseWithCovarianceStamped | None, int | None
             ),
             covariance=covariance,
         )
-        return PoseWithCovarianceStamped(header=Header(frame_id="map"), pose=pose), pose_method
+        return PoseWithCovarianceStamped(header=Header(frame_id="map"), pose=pose)
 
     @classmethod
     def get_perception_label_str(cls, classification: ObjectClassification) -> str:

driving_log_replayer/driving_log_replayer/localization.py

@@ -24,14 +24,13 @@
 from geometry_msgs.msg import PoseStamped
 import numpy as np
 from pydantic import BaseModel
-from pydantic import model_validator
 from rosidl_runtime_py import message_to_ordereddict
 from tier4_debug_msgs.msg import Float32Stamped
 from tier4_debug_msgs.msg import Int32Stamped
 
 from driving_log_replayer.result import EvaluationItem
 from driving_log_replayer.result import ResultBase
-from driving_log_replayer.scenario import InitialPose as InitialPoseModel
+from driving_log_replayer.scenario import InitialPose
 from driving_log_replayer.scenario import number
 from driving_log_replayer.scenario import Scenario
 
@@ -66,17 +65,9 @@ class Conditions(BaseModel):
 
 class Evaluation(BaseModel):
     UseCaseName: Literal["localization"]
-    UseCaseFormatVersion: Literal["1.2.0", "1.3.0"]
+    UseCaseFormatVersion: Literal["1.2.0"]
     Conditions: Conditions
-    InitialPose: InitialPoseModel | None = None
-    DirectInitialPose: InitialPoseModel | None = None
-
-    @model_validator(mode="after")
-    def mutually_exclusive(self) -> "Evaluation":
-        if self.InitialPose is not None and self.DirectInitialPose is not None:
-            err_msg = "Expected only one, either `InitialPose` or `DirectInitialPose`, not together"
-            raise ValueError(err_msg)
-        return self
+    InitialPose: InitialPose | None
 
 
 class LocalizationScenario(Scenario):

driving_log_replayer/package.xml

@@ -16,9 +16,9 @@
 
   <build_depend>autoware_cmake</build_depend>
 
+  <depend>autoware_adapi_v1_msgs</depend>
   <depend>autoware_auto_mapping_msgs</depend>
   <depend>autoware_auto_perception_msgs</depend>
-  <depend>autoware_common_msgs</depend>
   <depend>autoware_perception_msgs</depend>
   <depend>diagnostic_msgs</depend>
   <depend>driving_log_replayer_msgs</depend>