feat: planning control evaluator

docs/use_case/planning_control.en.md

@@ -0,0 +1,131 @@
+# Evaluate Planning Control
+
+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 input rosbag and the perception module performs recognition.
+3. Using the results of perception, Autoware output Metrics to `/diagnostic/planning_evaluator/metrics` for planning and `/diagnostic/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
+
+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 status[0].values[0].value matches the decision in the scenario.
+If kinematic_condition is specified, additionally, kinematic_state must meet the condition.
+
+### Error
+
+When the normal condition is not met
+
+## Topic name and data type used by evaluation node
+
+Subscribed topics:
+
+| Topic name                             | Data type                             |
+| -------------------------------------- | ------------------------------------- |
+| /diagnostic/control_evaluator/metrics  | diagnostic_msgs::msg::DiagnosticArray |
+| /diagnostic/planning_evaluator/metrics | diagnostic_msgs::msg::DiagnosticArray |
+
+Published topics:
+
+| Topic name | Data type |
+| ---------- | --------- |
+| N/A        | N/A       |
+
+## Arguments passed to logging_simulator.launch
+
+To make Autoware processing less resource-consuming, modules that are not relevant to evaluation are disabled by passing the `false` parameter as a launch argument.
+
+- localization: false
+
+### Arguments specified in the scenario or launch command
+
+- sensing: It can be disabled by specifying LaunchSensing: false in the scenario. Or specify sensing:=false in the launch command
+- perception: It can be disabled by specifying LaunchPerception: false in the scenario. Or specify perception:=false in the launch command
+- planning: It can be disabled by specifying LaunchPlanning: false in the scenario. Or specify planning:=false in the launch command
+
+```shell
+ros2 launch log_evaluator log_evaluator.launch.py scenario_path:=${planning_control_scenario_path} sensing:=false perception:=false planning:=false
+```
+
+## About simulation
+
+State the information required to run the simulation.
+
+### Topic to be included in the input rosbag
+
+| Topic name                             | Data type                                    |
+| -------------------------------------- | -------------------------------------------- |
+| /pacmod/from_can_bus                   | can_msgs/msg/Frame                           |
+| /localization/kinematic_state          | nav_msgs/msg/Odometry                        |
+| /localization/acceleration             | geometry_msgs/msg/AccelWithCovarianceStamped |
+| /sensing/lidar/concatenated/pointcloud | sensor_msgs/msg/PointCloud2                  |
+| /tf                                    | tf2_msgs/msg/TFMessage                       |
+| /planning/mission_planning/route       | autoware_planning_msgs/msg/LaneletRoute      |
+
+The vehicle topics can be included instead of CAN.
+
+| Topic name                             | Data type                                           |
+| -------------------------------------- | --------------------------------------------------- |
+| /localization/kinematic_state          | nav_msgs/msg/Odometry                               |
+| /localization/acceleration             | geometry_msgs/msg/AccelWithCovarianceStamped        |
+| /sensing/lidar/concatenated/pointcloud | sensor_msgs/msg/PointCloud2                         |
+| /tf                                    | tf2_msgs/msg/TFMessage                              |
+| /planning/mission_planning/route       | autoware_planning_msgs/msg/LaneletRoute             |
+| /vehicle/status/control_mode           | autoware_auto_vehicle_msgs/msg/ControlModeReport    |
+| /vehicle/status/gear_status            | autoware_auto_vehicle_msgs/msg/GearReport           |
+| /vehicle/status/steering_status        | autoware_auto_vehicle_msgs/SteeringReport           |
+| /vehicle/status/turn_indicators_status | autoware_auto_vehicle_msgs/msg/TurnIndicatorsReport |
+| /vehicle/status/velocity_status        | autoware_auto_vehicle_msgs/msg/VelocityReport       |
+
+### Topics that must not be included in the input rosbag
+
+| Topic name | Data type               |
+| ---------- | ----------------------- |
+| --------   | ----------------------- |
+| /clock     | rosgraph_msgs/msg/Clock |
+
+The clock is output by the --clock option of ros2 bag play, so if it is recorded in the bag itself, it is output twice, so it is not included in the bag.
+
+## About Evaluation
+
+State the information necessary for the evaluation.
+
+### Scenario Format
+
+See [sample](https://github.com/tier4/driving_log_replayer/blob/main/sample/planning_control/scenario.ja.yaml).
+
+### Evaluation Result Format
+
+See [sample](https://github.com/tier4/driving_log_replayer/blob/main/sample/planning_control/result.json).
+
+The result format is shown below.
+**NOTE: common part of the result file format, which has already been explained, is omitted.**
+
+Success is determined when all evaluation conditions set in planning and control are met.
+
+```json
+{
+  "Frame": {
+    "[Planning|Control]_CONDITION_INDEX": {
+      "Result": { "Total": "Success or Fail", "Frame": "Success or Fail" },
+      "Info": {
+        "TotalPassed": "Total number of topics that passed the evaluation criteria",
+        "Decision": "Decision of the acquired TOPIC",
+        "LaneInfo": "[lane_id, s, t]",
+        "KinematicState": "[vel, acc, jerk]"
+      }
+    }
+  }
+}
+```

docs/use_case/planning_control.ja.md

@@ -0,0 +1,130 @@
+# Planning Controlの評価
+
+Planning / ControlのMetricsが指定の条件で出力されているか評価する
+
+## 評価方法
+
+launch を立ち上げると以下のことが実行され、評価される。
+
+1. launch で評価ノード(`planning_control_evaluator_node`)と `logging_simulator.launch`、`ros2 bag play`コマンドを立ち上げる
+2. bag から出力されたセンサーデータを autoware が受け取って、perception モジュールが認識を行う
+3. perceptionの結果を使って、planningは `/diagnostic/planning_evaluator/metrics` に controlは `/diagnostic/control_evaluator/metrics`にMetricsを出力する
+4. 評価ノードが topic を subscribe して、各基準を満たしているかを判定して結果をファイルに記録する
+5. bag の再生が終了すると自動で launch が終了して評価が終了する
+
+## 評価結果
+
+topicのstatus[0].nameがシナリオで指定したモジュール名と一致し、且つ、status[0].value[0].keyがdecisionの場合に評価される。
+また、シナリオでレーン条件を記述した場合は、レーン条件も満たした場合に評価される。
+評価の条件を満たさない場合は、ログも出力されない。
+
+### 正常
+
+status[0].values[0].valueがシナリオのdecisionと一致した場合に正常となる。
+kinematic_conditionを指定した場合は追加で、kinematic_stateが条件を満たしている必要がある。
+
+### 異常
+
+正常の条件を満たさないとき
+
+## 評価ノードが使用する Topic 名とデータ型
+
+Subscribed topics:
+
+| Topic name                             | Data type                             |
+| -------------------------------------- | ------------------------------------- |
+| /diagnostic/control_evaluator/metrics  | diagnostic_msgs::msg::DiagnosticArray |
+| /diagnostic/planning_evaluator/metrics | diagnostic_msgs::msg::DiagnosticArray |
+
+Published topics:
+
+| Topic name | Data type |
+| ---------- | --------- |
+| N/A        | N/A       |
+
+## logging_simulator.launch に渡す引数
+
+autoware の処理を軽くするため、評価に関係のないモジュールは launch の引数に false を渡すことで無効化する。以下を設定している。
+
+- localization: false
+
+### シナリオまたはlaunchコマンドで指定する引数
+
+- sensing: シナリオにLaunchSensing: falseを指定することで無効化できる。またはlaunchコマンドでsensing:=falseを指定する
+- perception: シナリオにLaunchPerception: falseを指定することで無効化できる。またはlaunchコマンドでperception:=falseを指定する
+- planning: シナリオにLaunchPlanning: falseを指定することで無効化できる。またはlaunchコマンドでplanning:=falseを指定する
+
+```shell
+ros2 launch log_evaluator log_evaluator.launch.py scenario_path:=${planning_control_scenario_path} sensing:=false perception:=false planning:=false
+```
+
+## simulation
+
+シミュレーション実行に必要な情報を述べる。
+
+### 入力 rosbag に含まれるべき topic
+
+| topic 名                               | データ型                                     |
+| -------------------------------------- | -------------------------------------------- |
+| /pacmod/from_can_bus                   | can_msgs/msg/Frame                           |
+| /localization/kinematic_state          | nav_msgs/msg/Odometry                        |
+| /localization/acceleration             | geometry_msgs/msg/AccelWithCovarianceStamped |
+| /sensing/lidar/concatenated/pointcloud | sensor_msgs/msg/PointCloud2                  |
+| /tf                                    | tf2_msgs/msg/TFMessage                       |
+| /planning/mission_planning/route       | autoware_planning_msgs/msg/LaneletRoute      |
+
+CAN の代わりに vehicle の topic を含めても良い。
+
+| topic 名                               | データ型                                            |
+| -------------------------------------- | --------------------------------------------------- |
+| /localization/kinematic_state          | nav_msgs/msg/Odometry                               |
+| /localization/acceleration             | geometry_msgs/msg/AccelWithCovarianceStamped        |
+| /sensing/lidar/concatenated/pointcloud | sensor_msgs/msg/PointCloud2                         |
+| /tf                                    | tf2_msgs/msg/TFMessage                              |
+| /planning/mission_planning/route       | autoware_planning_msgs/msg/LaneletRoute             |
+| /vehicle/status/control_mode           | autoware_auto_vehicle_msgs/msg/ControlModeReport    |
+| /vehicle/status/gear_status            | autoware_auto_vehicle_msgs/msg/GearReport           |
+| /vehicle/status/steering_status        | autoware_auto_vehicle_msgs/SteeringReport           |
+| /vehicle/status/turn_indicators_status | autoware_auto_vehicle_msgs/msg/TurnIndicatorsReport |
+| /vehicle/status/velocity_status        | autoware_auto_vehicle_msgs/msg/VelocityReport       |
+
+### 入力 rosbag に含まれてはいけない topic
+
+| topic 名 | データ型                |
+| -------- | ----------------------- |
+| /clock   | rosgraph_msgs/msg/Clock |
+
+clock は、ros2 bag play の--clock オプションによって出力しているので、bag 自体に記録されていると 2 重に出力されてしまうので bag には含めない
+
+## evaluation
+
+評価に必要な情報を述べる。
+
+### シナリオフォーマット
+
+[サンプル](https://github.com/tier4/driving_log_replayer/blob/main/sample/planning_control/scenario.ja.yaml)参照
+
+### 評価結果フォーマット
+
+[サンプル](https://github.com/tier4/driving_log_replayer/blob/main/sample/planning_control/result.json)参照
+
+以下に、それぞれの評価の例を記述する。
+**注:結果ファイルフォーマットで解説済みの共通部分については省略する。**
+
+planning と controlで設定した全ての評価条件で成功している場合に成功と判定される。
+
+```json
+{
+  "Frame": {
+    "[Planning|Control]_CONDITION_INDEX": {
+      "Result": { "Total": "Success or Fail", "Frame": "Success or Fail" },
+      "Info": {
+        "TotalPassed": "評価条件をパスしたtopicの総数",
+        "Decision": "取得したtopicのdecision",
+        "LaneInfo": "[lane_id, s, t]",
+        "KinematicState": "[vel, acc, jerk]"
+      }
+    }
+  }
+}
+```

log_evaluator/CMakeLists.txt

@@ -30,6 +30,7 @@ install(PROGRAMS
   scripts/eagleye_evaluator_node.py
   scripts/ar_tag_based_localizer_evaluator_node.py
   scripts/annotationless_perception_evaluator_node.py
+  scripts/planning_control_evaluator_node.py
   DESTINATION lib/${PROJECT_NAME}
 )
 

log_evaluator/config/planning_control/qos.yaml

@@ -0,0 +1,15 @@
+/tf_static:
+  reliability: reliable
+  history: keep_last
+  depth: 1
+  durability: transient_local
+/planning/mission_planning/route:
+  reliability: reliable
+  history: keep_last
+  depth: 1
+  durability: transient_local
+/perception/obstacle_segmentation/pointcloud:
+  depth: 10
+  durability: volatile
+  history: keep_last
+  reliability: best_effort

log_evaluator/launch/log_evaluator.launch.py

@@ -13,7 +13,6 @@
 # limitations under the License.
 
 import datetime
-from os.path import expandvars
 from pathlib import Path
 
 from ament_index_python.packages import get_package_share_directory
@@ -109,18 +108,22 @@ def ensure_arg_compatibility(context: LaunchContext) -> list:
         dataset_index = int(idx_str)
     for k, v in datasets[dataset_index].items():
         dataset_path = dataset_dir.joinpath(k)
-        map_path_str: str | None = v.get("LocalMapPath")
         conf["vehicle_id"] = v["VehicleId"]
-        launch_sensing = yaml_obj["Evaluation"].get("LaunchSensing")
-        launch_localization = yaml_obj["Evaluation"].get("LaunchLocalization")
+        launch_sensing = v.get("LaunchSensing")
+        launch_localization = v.get("LaunchLocalization")
+        launch_perception = v.get("LaunchPerception")
+        launch_planning = v.get("LaunchPlanning")
         if launch_sensing is not None:
-            conf["sensing"] = str(launch_sensing)
+            # str(bool) だと True/Falseになって文字列比較で問題でるのでlowerで全小文字にする
+            conf["sensing"] = str(launch_sensing).lower()
         if launch_localization is not None:
-            conf["localization"] = str(launch_localization)
+            conf["localization"] = str(launch_localization).lower()
+        if launch_perception is not None:
+            conf["perception"] = str(launch_perception).lower()
+        if launch_planning is not None:
+            conf["planning"] = str(launch_planning).lower()
 
-    map_path = (
-        dataset_path.joinpath("map") if map_path_str is None else Path(expandvars(map_path_str))
-    )
+    map_path = dataset_path.joinpath("map")
     conf["map_path"] = map_path.as_posix()
     conf["vehicle_model"] = yaml_obj["VehicleModel"]
     conf["sensor_model"] = yaml_obj["SensorModel"]
@@ -254,6 +257,19 @@ def launch_bag_player(
         remap_list.append(
             "/localization/kinematic_state:=/unused/localization/kinematic_state",
         )
+        remap_list.append(
+            "/localization/acceleration:=/unused/localization/acceleration",
+        )
+    if conf.get("perception", "true") == "true":
+        # remap perception msgs in bag
+        remap_list.append(
+            "/perception/obstacle_segmentation/pointcloud:=/unused/perception/obstacle_segmentation/pointcloud",
+        )
+        remap_list.append(
+            "/perception/object_recognition/objects:=/unused/perception/object_recognition/objects",
+        )
+    if conf.get("planning", "true") == "true":
+        pass
     if len(remap_list) != 1:
         play_cmd.extend(remap_list)
     bag_player = ExecuteProcess(cmd=play_cmd, output="screen")

log_evaluator/log_evaluator/launch_config.py

@@ -203,6 +203,17 @@
     ),
 }
 
+PLANNING_CONTROL_RECORD_TOPIC = """^/tf$\
+|^/diagnostic/control_evaluator/metrics$\
+|^/diagnostic/planning_evaluator/metrics$\
+"""
+
+PLANNING_CONTROL_AUTOWARE_ARGS = {
+    "localization": "false",
+}
+
+PLANNING_CONTROL_NODE_PARAMS = {}
+
 log_evaluator_config = {
     "localization": {
         "record": LOCALIZATION_RECORD_TOPIC,
@@ -254,4 +265,9 @@
         "autoware": ANNOTATIONLESS_PERCEPTION_AUTOWARE_ARGS,
         "node": ANNOTATIONLESS_PERCEPTION_NODE_PARAMS,
     },
+    "planning_control": {
+        "record": PLANNING_CONTROL_RECORD_TOPIC,
+        "autoware": PLANNING_CONTROL_AUTOWARE_ARGS,
+        "node": PLANNING_CONTROL_NODE_PARAMS,
+    },
 }