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