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

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/autoware_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/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"
   ],
   "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/autoware_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/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"
   ],
   "files.associations": {
     "cctype": "cpp",

driving_log_replayer/driving_log_replayer/evaluator.py

@@ -18,7 +18,6 @@
 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
@@ -44,6 +43,7 @@
 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_adapi_v1_msgs.msg import ResponseStatus
+    from autoware_common_msgs.msg import ResponseStatus
 
 
 class DLREvaluator(Node):
@@ -107,9 +107,10 @@ 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.set_initial_pose()
+        self._initial_pose, self._initial_pose_method = self.set_initial_pose()
         self.start_initialpose_service()
 
         self._current_time = Time().to_msg()
@@ -174,17 +175,20 @@ def start_initialpose_service(self) -> None:
             return
         self._initial_pose_client = self.create_client(
             InitializeLocalization,
-            "/api/localization/initialize",
+            "/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...")
-        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...")
+
+        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...",
+                )
 
     def call_initialpose_service(self) -> None:
         if self._initial_pose is None or self._initial_pose_success or self._initial_pose_running:
@@ -194,17 +198,29 @@ def call_initialpose_service(self) -> None:
         )
         self._initial_pose_running = True
         self._initial_pose.header.stamp = self._current_time
-        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)
+        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)
 
     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(pose=[result.pose_with_covariance]),
+                    InitializeLocalization.Request(
+                        method=self._initial_pose_method,
+                        pose_with_covariance=[result.pose_with_covariance],
+                    ),
                 )
                 future_init_pose.add_done_callback(self.initial_pose_cb)
             else:
@@ -267,12 +283,17 @@ def transform_stamped_with_euler_angle(cls, transform_stamped: TransformStamped)
         }
         return tf_euler
 
-    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
+    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
         covariance = np.array(
             [
                 0.25,
@@ -320,7 +341,7 @@ def set_initial_pose(self) -> PoseWithCovarianceStamped | None:
             ),
             covariance=covariance,
         )
-        return PoseWithCovarianceStamped(header=Header(frame_id="map"), pose=pose)
+        return PoseWithCovarianceStamped(header=Header(frame_id="map"), pose=pose), pose_method
 
     @classmethod
     def get_perception_label_str(cls, classification: ObjectClassification) -> str:

driving_log_replayer/driving_log_replayer/localization.py

@@ -24,13 +24,14 @@
 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
+from driving_log_replayer.scenario import InitialPose as InitialPoseModel
 from driving_log_replayer.scenario import number
 from driving_log_replayer.scenario import Scenario
 
@@ -65,9 +66,17 @@ class Conditions(BaseModel):
 
 class Evaluation(BaseModel):
     UseCaseName: Literal["localization"]
-    UseCaseFormatVersion: Literal["1.2.0"]
+    UseCaseFormatVersion: Literal["1.2.0", "1.3.0"]
     Conditions: Conditions
-    InitialPose: InitialPose | None
+    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
 
 
 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>

sample/localization/scenario_direct_initialpose.yaml

@@ -0,0 +1,30 @@
+ScenarioFormatVersion: 2.2.0
+ScenarioName: sample_localization
+ScenarioDescription: sample_localization
+SensorModel: sample_sensor_kit
+VehicleModel: sample_vehicle
+VehicleId: default
+LocalMapPath: $HOME/autoware_map/sample-map-planning
+Evaluation:
+  UseCaseName: localization
+  UseCaseFormatVersion: 1.3.0
+  Conditions:
+    Convergence:
+      AllowableDistance: 0.2 # Lateral distance to be considered convergence
+      AllowableExeTimeMs: 100.0 # If the NDT computation time is less than or equal to this value, it is considered successful.
+      AllowableIterationNum: 30 # If the number of NDT calculations is less than or equal to this value, it is considered a success.
+      PassRate: 95.0 # How much (%) of the evaluation attempts are considered successful.
+    Reliability:
+      Method: NVTL # NVTL or TP which method to use for evaluation
+      AllowableLikelihood: 2.3 # If above this value, the localization reliability value is considered normal.
+      NGCount: 10 # If the reliability value is lower than the threshold value for more than this number in the sequence. the evaluation is considered to have failed.
+  DirectInitialPose:
+    position:
+      x: 3839.340576171875
+      y: 73731.3359375
+      z: 19.639482498168945
+    orientation:
+      x: -0.023971009814459248
+      y: 0.005891999600528019
+      z: -0.9709848597542231
+      w: 0.237863568369043