Revert "feat: planning control metric msg (#43)"

This reverts commit 8abe1d9.

Signed-off-by: Hayato Mizushima <[email protected]>

.vscode/settings.json

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

docs/use_case/planning_control.en.md

@@ -1,27 +1,26 @@
 # Evaluate Planning Control
 
-Evaluate Metrics are output at specified times and conditions
+Evaluate whether Planning / Control 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 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.
+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.
 
 ## Evaluation Result
 
-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.
+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.
 If the conditions for evaluation are not met, no log is output.
 
 ### Normal
 
-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.
+Normal if status[0].values[0].value matches the decision in the scenario.
 If kinematic_condition is specified, additionally, kinematic_state must meet the condition.
 
 ### Error
@@ -32,10 +31,10 @@ When the normal condition is not met
 
 Subscribed topics:
 
-| Topic name                                    | Data type                            |
-| --------------------------------------------- | ------------------------------------ |
-| /control/control_evaluator/metrics            | tier4_metric_msg/msg/MetricArray     |
-| /control/autonomous_emergency_braking/metrics | tier4_metric_msg/msg/DiagnosticArray |
+| Topic name                           | Data type                             |
+| ------------------------------------ | ------------------------------------- |
+| /control/control_evaluator/metrics   | diagnostic_msgs/msg/DiagnosticArray |
+| /planning/planning_evaluator/metrics | diagnostic_msgs/msg/DiagnosticArray |
 
 Published topics:
 

docs/use_case/planning_control.ja.md

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

driving_log_replayer_v2/driving_log_replayer_v2/planning_control.py

@@ -16,11 +16,12 @@
 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
@@ -53,12 +54,7 @@ class LaneInfo(BaseModel):
     s: float | None = None
     t: LeftRight | None = None
 
-    def match_condition(
-        self,
-        lane_info: tuple[int, float, float],
-        *,
-        start_condition: bool = False,
-    ) -> bool:
+    def match_condition(self, lane_info: tuple, *, start_condition: bool = False) -> bool:
         lane_id, s, t = lane_info
         if self.id != lane_id:
             return False
@@ -76,26 +72,25 @@ class LaneCondition(BaseModel):
     ended: bool = False
 
     @classmethod
-    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)
+    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)
         return (lane_id, s, t)
 
-    def is_started(self, lane_info_tuple: tuple[int, float, float]) -> bool:
+    def is_started(self, lane_info_tuple: tuple[float, 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[int, float, float]) -> bool:
+    def is_ended(self, lane_info_tuple: tuple[float, float, float]) -> bool:
         # Once True, do not change.
         if not self.ended:
             self.ended = self.end.match_condition(lane_info_tuple)
@@ -108,16 +103,16 @@ class KinematicCondition(BaseModel):
     jerk: MinMax | None = None
 
     @classmethod
-    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)
+    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)
         return (vel, acc, jerk)
 
     def match_condition(self, kinematic_state_tuple: tuple[float, float, float]) -> bool:
@@ -131,22 +126,22 @@ def match_condition(self, kinematic_state_tuple: tuple[float, float, float]) ->
         return True
 
 
-class MetricCondition(BaseModel):
-    topic: str
-    name: str
-    value: str
+class PlanningControlCondition(BaseModel):
+    module: str
+    decision: str
     condition_type: Literal["any_of", "all_of"]
     lane_condition: LaneCondition | None = None
     kinematic_condition: KinematicCondition | None = None
 
 
 class Conditions(BaseModel):
-    MetricConditions: list[MetricCondition] = []
+    ControlConditions: list[PlanningControlCondition] = []
+    PlanningConditions: list[PlanningControlCondition] = []
 
 
 class Evaluation(BaseModel):
     UseCaseName: Literal["planning_control"]
-    UseCaseFormatVersion: Literal["1.0.0"]
+    UseCaseFormatVersion: Literal["0.1.0"]
     Conditions: Conditions
     Datasets: list[dict]
 
@@ -158,12 +153,53 @@ class PlanningControlScenario(Scenario):
 @dataclass
 class Metrics(EvaluationItem):
     def __post_init__(self) -> None:
-        self.condition: MetricCondition
+        self.condition: PlanningControlCondition
         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: MetricArray, control_metrics: MetricArray) -> dict | None:
-        lane_info_tuple = LaneCondition.metric_lane_info(control_metrics)
+    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
+
         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)
@@ -182,12 +218,8 @@ def set_frame(self, msg: MetricArray, control_metrics: MetricArray) -> dict | No
 
         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.value == aeb_value:
+        if self.condition.decision == status0.values[0].value:
             if self.use_kinematic_condition:
                 if self.condition.kinematic_condition.match_condition(kinematic_state_tuple):
                     frame_success = "Success"
@@ -205,23 +237,23 @@ def set_frame(self, msg: MetricArray, control_metrics: MetricArray) -> dict | No
             "Result": {"Total": self.success_str(), "Frame": frame_success},
             "Info": {
                 "TotalPassed": self.passed,
-                "Decision": aeb_value,
+                "Decision": status0.values[0].value,
                 "LaneInfo": lane_info_tuple,
                 "KinematicState": kinematic_state_tuple,
             },
         }
 
 
 class MetricsClassContainer:
-    def __init__(self, conditions: list[MetricCondition]) -> None:
+    def __init__(self, conditions: list[PlanningControlCondition], module: str) -> None:
         self.__container: list[Metrics] = []
-        for i, cond in enumerate(conditions):
-            self.__container.append(Metrics(f"Condition_{i}", cond))
+        for i, module_cond in enumerate(conditions):
+            self.__container.append(Metrics(f"{module}_{i}", module_cond))
 
-    def set_frame(self, msg: MetricArray, control_metrics: MetricArray) -> dict:
+    def set_frame(self, msg: DiagnosticArray) -> dict:
         frame_result: dict[int, dict] = {}
         for evaluation_item in self.__container:
-            result_i = evaluation_item.set_frame(msg, control_metrics)
+            result_i = evaluation_item.set_frame(msg)
             if result_i is not None:
                 frame_result[f"{evaluation_item.name}"] = result_i
         return frame_result
@@ -243,11 +275,24 @@ def update(self) -> tuple[bool, str]:
 class PlanningControlResult(ResultBase):
     def __init__(self, condition: Conditions) -> None:
         super().__init__()
-        self.__metrics_container = MetricsClassContainer(condition.MetricConditions)
+        self.__control_container = MetricsClassContainer(
+            condition.ControlConditions,
+            "control",
+        )
+        self.__planning_container = MetricsClassContainer(
+            condition.PlanningConditions,
+            "planning",
+        )
 
     def update(self) -> None:
-        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)
+        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.update()

driving_log_replayer_v2/package.xml

@@ -31,7 +31,6 @@
   <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>map_height_fitter</exec_depend>