feat: planning control metric msg

.vscode/settings.json

@@ -40,7 +40,8 @@
     "${workspaceFolder}/../../../install/tier4_perception_msgs/local/lib/python3.10/dist-packages",
     "${workspaceFolder}/../../../install/lanelet2_extension_python/local/lib/python3.10/dist-packages",
     "${workspaceFolder}/../../../install/autoware_common_msgs/local/lib/python3.10/dist-packages",
-    "${workspaceFolder}/../../../install/autoware_adapi_v1_msgs/local/lib/python3.10/dist-packages"
+    "${workspaceFolder}/../../../install/autoware_adapi_v1_msgs/local/lib/python3.10/dist-packages",
+    "${workspaceFolder}/../../../install/tier4_metric_msgs/local/lib/python3.10/dist-packages"
   ],
   "python.analysis.extraPaths": [
     "/opt/ros/humble/lib/python3.10/site-packages",
@@ -58,7 +59,8 @@
     "${workspaceFolder}/../../../install/tier4_perception_msgs/local/lib/python3.10/dist-packages",
     "${workspaceFolder}/../../../install/lanelet2_extension_python/local/lib/python3.10/dist-packages",
     "${workspaceFolder}/../../../install/autoware_common_msgs/local/lib/python3.10/dist-packages",
-    "${workspaceFolder}/../../../install/autoware_adapi_v1_msgs/local/lib/python3.10/dist-packages"
+    "${workspaceFolder}/../../../install/autoware_adapi_v1_msgs/local/lib/python3.10/dist-packages",
+    "${workspaceFolder}/../../../install/tier4_metric_msgs/local/lib/python3.10/dist-packages"
   ],
   "files.associations": {
     "cctype": "cpp",

docs/use_case/planning_control.en.md

@@ -1,26 +1,27 @@
 # Evaluate Planning Control
 
-Evaluate whether Planning / Control metrics are output at specified times and conditions
+Evaluate Metrics are output at specified times and conditions
 
 ## Evaluation Method
 
 Launching the file executes the following steps:
 
 1. Execute launch of evaluation node (`planning_control_evaluator_node`), `logging_simulator.launch` file and `ros2 bag play` command
-2. Autoware receives sensor data output from input rosbag and the perception module performs recognition.
-3. Using the results of perception, Autoware output Metrics to `/planning/planning_evaluator/metrics` for planning and `/control/control_evaluator/metrics` for control.
-4. The evaluation node subscribes to the topic and evaluates data. The result is dumped into a file.
-5. When the playback of the rosbag is finished, Autoware's launch is automatically terminated, and the evaluation is completed.
+2. Autoware receives sensor data output from bag and publishes a metrics type message
+3. The evaluation node subscribes to the topic and evaluates data. The result is dumped into a file.
+4. When the playback of the rosbag is finished, Autoware's launch is automatically terminated, and the evaluation is completed.
 
 ## Evaluation Result
 
-It is evaluated when status[0].name of the topic matches the module name specified in the scenario and status[0].value[0].key is a decision.
-If a lane condition is described in the scenario, it is evaluated when the lane condition is also satisfied.
+This node uses `/control/autonomous_emergency_braking/metrics` and `/control/control_evaluator/metrics`.
+Evaluate if `/control/autonomous_emergency_braking/metrics` is the value specified in the scenario.
+If a lane condition is described in the scenario, it is evaluated when a lane that can be obtained from `/control/control_evaluator/metrics` satisfies the condition.
 If the conditions for evaluation are not met, no log is output.
 
 ### Normal
 
-Normal if status[0].values[0].value matches the decision in the scenario.
+Normal if the value in `/control/control_evaluator/metrics` matches the value specified in the scenario.
+Though, if `none` is specified, it is judged as `none` if the metric_array of topic is an empty array.
 If kinematic_condition is specified, additionally, kinematic_state must meet the condition.
 
 ### Error
@@ -31,10 +32,10 @@ When the normal condition is not met
 
 Subscribed topics:
 
-| Topic name                           | Data type                             |
-| ------------------------------------ | ------------------------------------- |
-| /control/control_evaluator/metrics   | diagnostic_msgs::msg::DiagnosticArray |
-| /planning/planning_evaluator/metrics | diagnostic_msgs::msg::DiagnosticArray |
+| Topic name                                    | Data type                              |
+| --------------------------------------------- | -------------------------------------- |
+| /control/control_evaluator/metrics            | tier4_metric_msg::msg::MetricArray     |
+| /control/autonomous_emergency_braking/metrics | tier4_metric_msg::msg::DiagnosticArray |
 
 Published topics:
 

docs/use_case/planning_control.ja.md

@@ -1,26 +1,27 @@
 # Planning Controlの評価
 
-Planning / ControlのMetricsが指定の条件で出力されているか評価する
+Metricsが指定の条件で出力されているか評価する
 
 ## 評価方法
 
 launch を立ち上げると以下のことが実行され、評価される。
 
 1. launch で評価ノード(`planning_control_evaluator_node`)と `logging_simulator.launch`、`ros2 bag play`コマンドを立ち上げる
-2. bag から出力されたセンサーデータを autoware が受け取って、perception モジュールが認識を行う
-3. perceptionの結果を使って、planningは `/planning/planning_evaluator/metrics` に controlは `/control/control_evaluator/metrics`にMetricsを出力する
-4. 評価ノードが topic を subscribe して、各基準を満たしているかを判定して結果をファイルに記録する
-5. bag の再生が終了すると自動で launch が終了して評価が終了する
+2. bag から出力されたセンサーデータを autoware が受け取って、metrics型のメッセージを出力する
+3. 評価ノードが topic を subscribe して、各基準を満たしているかを判定して結果をファイルに記録する
+4. bag の再生が終了すると自動で launch が終了して評価が終了する
 
 ## 評価結果
 
-topicのstatus[0].nameがシナリオで指定したモジュール名と一致し、且つ、status[0].value[0].keyがdecisionの場合に評価される。
-また、シナリオでレーン条件を記述した場合は、レーン条件も満たした場合に評価される。
+`/control/autonomous_emergency_braking/metrics`と`/control/control_evaluator/metrics`を利用する。
+`/control/autonomous_emergency_braking/metrics`がシナリオで指定されたvalueになっているかを評価する。
+シナリオでレーン条件を記述した場合は、`/control/control_evaluator/metrics`から取得できるレーンが条件を満たした場合に評価される。
 評価の条件を満たさない場合は、ログも出力されない。
 
 ### 正常
 
-status[0].values[0].valueがシナリオのdecisionと一致した場合に正常となる。
+`/control/control_evaluator/metrics`のvalueがシナリオ指定の値と一致した場合に正常となる。
+ただし、`none`が指定された場合は、topicのmetric_arrayが空配列の場合にnoneと判断する。
 kinematic_conditionを指定した場合は追加で、kinematic_stateが条件を満たしている必要がある。
 
 ### 異常
@@ -31,10 +32,10 @@ kinematic_conditionを指定した場合は追加で、kinematic_stateが条件
 
 Subscribed topics:
 
-| Topic name                           | Data type                             |
-| ------------------------------------ | ------------------------------------- |
-| /control/control_evaluator/metrics   | diagnostic_msgs::msg::DiagnosticArray |
-| /planning/planning_evaluator/metrics | diagnostic_msgs::msg::DiagnosticArray |
+| Topic name                                    | Data type                              |
+| --------------------------------------------- | -------------------------------------- |
+| /control/control_evaluator/metrics            | tier4_metric_msg::msg::MetricArray     |
+| /control/autonomous_emergency_braking/metrics | tier4_metric_msg::msg::DiagnosticArray |
 
 Published topics:
 

driving_log_replayer_v2/driving_log_replayer_v2/planning_control.py

@@ -16,12 +16,11 @@
 from sys import float_info
 from typing import Literal
 
-from diagnostic_msgs.msg import DiagnosticArray
-from diagnostic_msgs.msg import DiagnosticStatus
-from diagnostic_msgs.msg import KeyValue
 from pydantic import BaseModel
 from pydantic import Field
 from pydantic import model_validator
+from tier4_metric_msgs.msg import Metric
+from tier4_metric_msgs.msg import MetricArray
 
 from driving_log_replayer_v2.result import EvaluationItem
 from driving_log_replayer_v2.result import ResultBase
@@ -54,7 +53,12 @@ class LaneInfo(BaseModel):
     s: float | None = None
     t: LeftRight | None = None
 
-    def match_condition(self, lane_info: tuple, *, start_condition: bool = False) -> bool:
+    def match_condition(
+        self,
+        lane_info: tuple[int, float, float],
+        *,
+        start_condition: bool = False,
+    ) -> bool:
         lane_id, s, t = lane_info
         if self.id != lane_id:
             return False
@@ -72,25 +76,26 @@ class LaneCondition(BaseModel):
     ended: bool = False
 
     @classmethod
-    def diag_lane_info(cls, lane_info: DiagnosticStatus) -> tuple[float, float, float]:
-        lane_id, s, t = None, None, None
-        for kv in lane_info.values:
-            kv: KeyValue
-            if kv.key == "lane_id":
-                lane_id = int(kv.value)
-            if kv.key == "s":
-                s = float(kv.value)
-            if kv.key == "t":
-                t = float(kv.value)
+    def metric_lane_info(cls, msg: MetricArray) -> tuple[int, float, float]:
+        # If conditions cannot be taken, return conditions that can never be met.
+        lane_id, s, t = -1, float_info.max, 0.0
+        for metric in msg.metric_array:
+            metric: Metric
+            if metric.name == "ego_lane_info/lane_id":
+                lane_id = int(metric.value)
+            if metric.name == "ego_lane_info/s":
+                s = float(metric.value)
+            if metric.name == "ego_lane_info/t":
+                t = float(metric.value)
         return (lane_id, s, t)
 
-    def is_started(self, lane_info_tuple: tuple[float, float, float]) -> bool:
+    def is_started(self, lane_info_tuple: tuple[int, float, float]) -> bool:
         # Once True, do not change.
         if not self.started:
             self.started = self.start.match_condition(lane_info_tuple, start_condition=True)
         return self.started
 
-    def is_ended(self, lane_info_tuple: tuple[float, float, float]) -> bool:
+    def is_ended(self, lane_info_tuple: tuple[int, float, float]) -> bool:
         # Once True, do not change.
         if not self.ended:
             self.ended = self.end.match_condition(lane_info_tuple)
@@ -103,16 +108,16 @@ class KinematicCondition(BaseModel):
     jerk: MinMax | None = None
 
     @classmethod
-    def diag_kinematic_state(cls, kinematic_state: DiagnosticStatus) -> tuple[float, float, float]:
-        vel, acc, jerk = None, None, None
-        for kv in kinematic_state.values:
-            kv: KeyValue
-            if kv.key == "vel":
-                vel = float(kv.value)
-            if kv.key == "acc":
-                acc = float(kv.value)
-            if kv.key == "jerk":
-                jerk = float(kv.value)
+    def metric_kinematic_state(cls, msg: MetricArray) -> tuple[float, float, float]:
+        vel, acc, jerk = 0.0, 0.0, 0.0
+        for metric in msg.metric_array:
+            metric: Metric
+            if metric.name == "kinematic_state/vel":
+                vel = float(metric.value)
+            if metric.name == "kinematic_state/acc":
+                acc = float(metric.value)
+            if metric.name == "kinematic_state/jerk":
+                jerk = float(metric.value)
         return (vel, acc, jerk)
 
     def match_condition(self, kinematic_state_tuple: tuple[float, float, float]) -> bool:
@@ -126,22 +131,22 @@ def match_condition(self, kinematic_state_tuple: tuple[float, float, float]) ->
         return True
 
 
-class PlanningControlCondition(BaseModel):
-    module: str
-    decision: str
+class MetricCondition(BaseModel):
+    topic: str
+    name: str
+    value: str
     condition_type: Literal["any_of", "all_of"]
     lane_condition: LaneCondition | None = None
     kinematic_condition: KinematicCondition | None = None
 
 
 class Conditions(BaseModel):
-    ControlConditions: list[PlanningControlCondition] = []
-    PlanningConditions: list[PlanningControlCondition] = []
+    MetricConditions: list[MetricCondition] = []
 
 
 class Evaluation(BaseModel):
     UseCaseName: Literal["planning_control"]
-    UseCaseFormatVersion: Literal["0.1.0"]
+    UseCaseFormatVersion: Literal["1.0.0"]
     Conditions: Conditions
     Datasets: list[dict]
 
@@ -153,53 +158,12 @@ class PlanningControlScenario(Scenario):
 @dataclass
 class Metrics(EvaluationItem):
     def __post_init__(self) -> None:
-        self.condition: PlanningControlCondition
+        self.condition: MetricCondition
         self.use_lane_condition = self.condition.lane_condition is not None
         self.use_kinematic_condition = self.condition.kinematic_condition is not None
 
-    def set_frame(self, msg: DiagnosticArray) -> dict | None:  # noqa
-        if len(msg.status) <= 1:
-            """
-            return {
-                "Error": "len(msg.status) <= 1",
-            }
-            """
-            return None
-
-        # key check
-        status0: DiagnosticStatus = msg.status[0]
-        if status0.name != self.condition.module:
-            """
-            return {
-                "Error": f"{status0.name=}, {self.condition.module=} module name is not matched",
-            }
-            """
-            return None
-        if status0.values[0].key != "decision":
-            """
-            return {
-                "Error": f"{status0.values[0].key=} is not decision",
-            }
-            """
-            return None
-
-        lane_info_tuple = None
-        kinematic_state_tuple = None
-
-        # get additional condition
-        for _, status in enumerate(msg.status, 1):
-            status: DiagnosticStatus
-            if status.name == "ego_lane_info":
-                lane_info_tuple = LaneCondition.diag_lane_info(status)
-            if status.name == "kinematic_state":
-                kinematic_state_tuple = KinematicCondition.diag_kinematic_state(status)
-
-        if lane_info_tuple is None or kinematic_state_tuple is None:
-            """
-            return {"Error": "lane_info_tuple or kinematic_state_tuple is None"}
-            """
-            return None
-
+    def set_frame(self, msg: MetricArray, control_metrics: MetricArray) -> dict | None:
+        lane_info_tuple = LaneCondition.metric_lane_info(control_metrics)
         if self.use_lane_condition:
             started = self.condition.lane_condition.is_started(lane_info_tuple)
             ended = self.condition.lane_condition.is_ended(lane_info_tuple)
@@ -218,8 +182,12 @@ def set_frame(self, msg: DiagnosticArray) -> dict | None:  # noqa
 
         self.total += 1
         frame_success = "Fail"
+
+        aeb_value: str = "none" if len(msg.metric_array) == 0 else msg.metric_array[0].value
+        kinematic_state_tuple = KinematicCondition.metric_kinematic_state(control_metrics)
+
         # OK if decision matches and kinematic_state satisfies the condition
-        if self.condition.decision == status0.values[0].value:
+        if self.condition.value == aeb_value:
             if self.use_kinematic_condition:
                 if self.condition.kinematic_condition.match_condition(kinematic_state_tuple):
                     frame_success = "Success"
@@ -237,23 +205,23 @@ def set_frame(self, msg: DiagnosticArray) -> dict | None:  # noqa
             "Result": {"Total": self.success_str(), "Frame": frame_success},
             "Info": {
                 "TotalPassed": self.passed,
-                "Decision": status0.values[0].value,
+                "Decision": aeb_value,
                 "LaneInfo": lane_info_tuple,
                 "KinematicState": kinematic_state_tuple,
             },
         }
 
 
 class MetricsClassContainer:
-    def __init__(self, conditions: list[PlanningControlCondition], module: str) -> None:
+    def __init__(self, conditions: list[MetricCondition]) -> None:
         self.__container: list[Metrics] = []
-        for i, module_cond in enumerate(conditions):
-            self.__container.append(Metrics(f"{module}_{i}", module_cond))
+        for i, cond in enumerate(conditions):
+            self.__container.append(Metrics(f"Condition_{i}", cond))
 
-    def set_frame(self, msg: DiagnosticArray) -> dict:
+    def set_frame(self, msg: MetricArray, control_metrics: MetricArray) -> dict:
         frame_result: dict[int, dict] = {}
         for evaluation_item in self.__container:
-            result_i = evaluation_item.set_frame(msg)
+            result_i = evaluation_item.set_frame(msg, control_metrics)
             if result_i is not None:
                 frame_result[f"{evaluation_item.name}"] = result_i
         return frame_result
@@ -275,24 +243,11 @@ def update(self) -> tuple[bool, str]:
 class PlanningControlResult(ResultBase):
     def __init__(self, condition: Conditions) -> None:
         super().__init__()
-        self.__control_container = MetricsClassContainer(
-            condition.ControlConditions,
-            "control",
-        )
-        self.__planning_container = MetricsClassContainer(
-            condition.PlanningConditions,
-            "planning",
-        )
+        self.__metrics_container = MetricsClassContainer(condition.MetricConditions)
 
     def update(self) -> None:
-        control_success, control_summary = self.__control_container.update()
-        planning_success, planning_summary = self.__planning_container.update()
-        self._success = control_success and planning_success
-        self._summary = "Control: " + control_summary + " Planning: " + planning_summary
-
-    def set_frame(self, msg: DiagnosticArray, module: str) -> None:
-        if module == "control":
-            self._frame = self.__control_container.set_frame(msg)
-        if module == "planning":
-            self._frame = self.__planning_container.set_frame(msg)
+        self._success, self._summary = self.__metrics_container.update()
+
+    def set_frame(self, msg: MetricArray, control_metrics: MetricArray) -> None:
+        self._frame = self.__metrics_container.set_frame(msg, control_metrics)
         self.update()

driving_log_replayer_v2/package.xml

@@ -31,6 +31,7 @@
   <depend>tf2_ros</depend>
   <depend>tier4_debug_msgs</depend>
   <depend>tier4_localization_msgs</depend>
+  <depend>tier4_metric_msgs</depend>
   <depend>visualization_msgs</depend>
 
   <exec_depend>autoware_map_height_fitter</exec_depend>