diff --git a/.vscode/settings.json b/.vscode/settings.json
index 93ecdb126..69606cadb 100644
--- a/.vscode/settings.json
+++ b/.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",
diff --git a/driving_log_replayer/driving_log_replayer/evaluator.py b/driving_log_replayer/driving_log_replayer/evaluator.py
index 31689fe7b..d13a15539 100644
--- a/driving_log_replayer/driving_log_replayer/evaluator.py
+++ b/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:
diff --git a/driving_log_replayer/driving_log_replayer/localization.py b/driving_log_replayer/driving_log_replayer/localization.py
index dd3b48947..1b9459d0f 100644
--- a/driving_log_replayer/driving_log_replayer/localization.py
+++ b/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):
diff --git a/driving_log_replayer/package.xml b/driving_log_replayer/package.xml
index e0fb221f5..0cd19e6ef 100644
--- a/driving_log_replayer/package.xml
+++ b/driving_log_replayer/package.xml
@@ -16,9 +16,9 @@
autoware_cmake
- autoware_adapi_v1_msgs
autoware_auto_mapping_msgs
autoware_auto_perception_msgs
+ autoware_common_msgs
autoware_perception_msgs
diagnostic_msgs
driving_log_replayer_msgs
diff --git a/sample/localization/scenario_direct_initialpose.yaml b/sample/localization/scenario_direct_initialpose.yaml
new file mode 100644
index 000000000..95bcd8a09
--- /dev/null
+++ b/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