From 04f4081d86f05d0ec5e98a4d8304ceaee86b236a Mon Sep 17 00:00:00 2001 From: Hayato Mizushima Date: Fri, 2 Aug 2024 17:10:04 +0900 Subject: [PATCH] feat: planning control evaluator (#2) * feat: planning control node * feat: update evaluation logic * feat: update evaluation logic * fix: cast string * fix: field * feat: add unit test * chore: add temporary logic * fix: lane condition * fix: lane s condition * chore: allow empty KinematicCondition * chore: add output decision * chore: update sample * chore: pre-commit * feat: add remap /localization/acceleration * docs: update * docs: update can msg * feat: support LaunchPerception and LaunchPlanning * feat: remap topic in bag * fix: pre-commit * chore: update status length check * fix: remap * docs: update document * docs: update sample * feat: update unit test * fix: local variable lane_info_tuple referenced before assignment * feat: update scenario format and enable/disable component * feat: update topic name https://github.com/autowarefoundation/autoware.universe/pull/8152 * fix: pre-commit --- docs/use_case/planning_control.en.md | 126 +++ docs/use_case/planning_control.ja.md | 125 +++ log_evaluator/CMakeLists.txt | 1 + .../config/planning_control/qos.yaml | 15 + log_evaluator/launch/log_evaluator.launch.py | 13 + log_evaluator/log_evaluator/launch_config.py | 19 + .../log_evaluator/planning_control.py | 287 +++++++ .../planning_control_evaluator_node.py | 58 ++ .../test/unittest/test_planning_control.py | 256 ++++++ sample/planning_control/control_diag.txt | 103 +++ sample/planning_control/result.json | 741 ++++++++++++++++++ sample/planning_control/scenario.yaml | 37 + 12 files changed, 1781 insertions(+) create mode 100644 docs/use_case/planning_control.en.md create mode 100644 docs/use_case/planning_control.ja.md create mode 100644 log_evaluator/config/planning_control/qos.yaml create mode 100644 log_evaluator/log_evaluator/planning_control.py create mode 100755 log_evaluator/scripts/planning_control_evaluator_node.py create mode 100644 log_evaluator/test/unittest/test_planning_control.py create mode 100644 sample/planning_control/control_diag.txt create mode 100644 sample/planning_control/result.json create mode 100644 sample/planning_control/scenario.yaml diff --git a/docs/use_case/planning_control.en.md b/docs/use_case/planning_control.en.md new file mode 100644 index 00000000..c622c4a0 --- /dev/null +++ b/docs/use_case/planning_control.en.md @@ -0,0 +1,126 @@ +# 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 `/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 + +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 | +| ------------------------------------ | ------------------------------------- | +| /control/control_evaluator/metrics | diagnostic_msgs::msg::DiagnosticArray | +| /planning/planning_evaluator/metrics | diagnostic_msgs::msg::DiagnosticArray | + +Published topics: + +| Topic name | Data type | +| ---------- | --------- | +| N/A | N/A | + +## Arguments passed to logging_simulator.launch + +- localization: false + +To use /sensing/lidar/concatenated/pointcloud in the bag, add sensing:=false to the launch argument. +If you want to use perception and planning from the bag as well, add “perception:=false planning:=false” to the “launch” argument. + +```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]" + } + } + } +} +``` diff --git a/docs/use_case/planning_control.ja.md b/docs/use_case/planning_control.ja.md new file mode 100644 index 00000000..0542a943 --- /dev/null +++ b/docs/use_case/planning_control.ja.md @@ -0,0 +1,125 @@ +# 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は `/planning/planning_evaluator/metrics` に controlは `/control/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 | +| ------------------------------------ | ------------------------------------- | +| /control/control_evaluator/metrics | diagnostic_msgs::msg::DiagnosticArray | +| /planning/planning_evaluator/metrics | diagnostic_msgs::msg::DiagnosticArray | + +Published topics: + +| Topic name | Data type | +| ---------- | --------- | +| N/A | N/A | + +## logging_simulator.launch に渡す引数 + +- localization: false + +bagの中に入っている、/sensing/lidar/concatenated/pointcloudを利用する場合は、launchの引数にsensing:=falseを追加する +perception、planningも同様にbagから出力する場合は、launchの引数にperception:=false 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]" + } + } + } +} +``` diff --git a/log_evaluator/CMakeLists.txt b/log_evaluator/CMakeLists.txt index 88dbd6ce..ce37dcef 100644 --- a/log_evaluator/CMakeLists.txt +++ b/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} ) diff --git a/log_evaluator/config/planning_control/qos.yaml b/log_evaluator/config/planning_control/qos.yaml new file mode 100644 index 00000000..efa1210d --- /dev/null +++ b/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 diff --git a/log_evaluator/launch/log_evaluator.launch.py b/log_evaluator/launch/log_evaluator.launch.py index a7f9f37c..0297db55 100644 --- a/log_evaluator/launch/log_evaluator.launch.py +++ b/log_evaluator/launch/log_evaluator.launch.py @@ -251,6 +251,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") diff --git a/log_evaluator/log_evaluator/launch_config.py b/log_evaluator/log_evaluator/launch_config.py index 0b93b991..65fbdad3 100644 --- a/log_evaluator/log_evaluator/launch_config.py +++ b/log_evaluator/log_evaluator/launch_config.py @@ -177,6 +177,19 @@ PERFORMANCE_DIAG_NODE_PARAMS = {} +PLANNING_CONTROL_RECORD_TOPIC = """^/tf$\ +|^/control/control_evaluator/metrics$\ +|^/planning/planning_evaluator/metrics$\ +""" + +PLANNING_CONTROL_AUTOWARE_DISABLE = { + "localization": "false", +} + +PLANNING_CONTROL_AUTOWARE_ARGS = {} + +PLANNING_CONTROL_NODE_PARAMS = {} + TRAFFIC_LIGHT_RECORD_TOPIC = """^/tf$\ |^/sensing/camera/camera[67]/image_raw/compressed$\ |^/perception/.*/traffic_signals$\ @@ -263,6 +276,12 @@ "autoware": PERFORMANCE_DIAG_AUTOWARE_ARGS, "node": PERFORMANCE_DIAG_NODE_PARAMS, }, + "planning_control": { + "record": PLANNING_CONTROL_RECORD_TOPIC, + "disable": PLANNING_CONTROL_AUTOWARE_DISABLE, + "autoware": PLANNING_CONTROL_AUTOWARE_ARGS, + "node": PLANNING_CONTROL_NODE_PARAMS, + }, "traffic_light": { "record": TRAFFIC_LIGHT_RECORD_TOPIC, "disable": TRAFFIC_LIGHT_AUTOWARE_DISABLE, diff --git a/log_evaluator/log_evaluator/planning_control.py b/log_evaluator/log_evaluator/planning_control.py new file mode 100644 index 00000000..8ff73385 --- /dev/null +++ b/log_evaluator/log_evaluator/planning_control.py @@ -0,0 +1,287 @@ +# Copyright (c) 2024 TIER IV.inc +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +from dataclasses import dataclass +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 log_evaluator.result import EvaluationItem +from log_evaluator.result import ResultBase +from log_evaluator.scenario import Scenario + + +class MinMax(BaseModel): + min: float = float_info.min + max: float = float_info.max + + @model_validator(mode="after") + def validate_min_max(self) -> "MinMax": + err_msg = "max must be a greater number than min" + + if self.max < self.min: + raise ValueError(err_msg) + return self + + +class LeftRight(BaseModel): + left: float = Field(float_info.max, gt=0.0) # + + right: float = Field(float_info.max, gt=0.0) # - + + def match_condition(self, t: float) -> bool: + if (-1.0) * self.right <= t <= self.left: + return True + return False + + +class LaneInfo(BaseModel): + id: int + s: float | None = None + t: LeftRight | None = None + + 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 + if self.s is not None and self.s >= s: # Start or end when exceeded + return False + if start_condition and self.t is not None and not self.t.match_condition(t): + return False + return True + + +class LaneCondition(BaseModel): + start: LaneInfo + end: LaneInfo + started: bool = False + 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) + return (lane_id, s, t) + + 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[float, float, float]) -> bool: + # Once True, do not change. + if not self.ended: + self.ended = self.end.match_condition(lane_info_tuple) + return self.ended + + +class KinematicCondition(BaseModel): + vel: MinMax | None = None + acc: MinMax | None = None + 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) + return (vel, acc, jerk) + + def match_condition(self, kinematic_state_tuple: tuple[float, float, float]) -> bool: + vel, acc, jerk = kinematic_state_tuple + if self.vel is not None and not self.vel.min <= vel <= self.vel.max: + return False + if self.acc is not None and not self.acc.min <= acc <= self.acc.max: + return False + if self.jerk is not None and not self.jerk.min <= jerk <= self.jerk.max: + return False + return True + + +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): + ControlConditions: list[PlanningControlCondition] = [] + PlanningConditions: list[PlanningControlCondition] = [] + + +class Evaluation(BaseModel): + UseCaseName: Literal["planning_control"] + UseCaseFormatVersion: Literal["0.1.0"] + Conditions: Conditions + Datasets: list[dict] + + +class PlanningControlScenario(Scenario): + Evaluation: Evaluation + + +@dataclass +class Metrics(EvaluationItem): + def __post_init__(self) -> None: + self.condition: PlanningControlCondition + self.use_lane_condition = self.condition.lane_condition is not None + self.use_kinetic_condition = self.condition.kinematic_condition is not None + + def set_frame(self, msg: DiagnosticArray) -> dict | None: # noqa + if 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 None + + lane_info_tuple = None + kinetic_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": + kinetic_state_tuple = KinematicCondition.diag_kinematic_state(status) + + if lane_info_tuple is None or kinetic_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) + if not (started and not ended): + """ + return { + "Error": { + "LaneInfo": lane_info_tuple, + "KinematicState": kinetic_state_tuple, + "started": started, + "ended": ended, + }, + } + """ + return None + + self.total += 1 + frame_success = "Fail" + # OK if decision matches and kinetic_state satisfies the condition + if self.condition.decision == status0.values[0].value: + if self.use_kinetic_condition: + if self.condition.kinematic_condition.match_condition(kinetic_state_tuple): + frame_success = "Success" + self.passed += 1 + else: + frame_success = "Success" + self.passed += 1 + # any_of would only need one, all_of would need all of them. + self.success = ( + self.passed > 0 + if self.condition.condition_type == "any_of" + else self.passed == self.total + ) + return { + "Result": {"Total": self.success_str(), "Frame": frame_success}, + "Info": { + "TotalPassed": self.passed, + "Decision": status0.values[0].value, + "LaneInfo": lane_info_tuple, + "KinematicState": kinetic_state_tuple, + }, + } + + +class MetricsClassContainer: + def __init__(self, conditions: list[PlanningControlCondition], module: str) -> None: + self.__container: list[Metrics] = [] + for i, module_cond in enumerate(conditions): + self.__container.append(Metrics(f"{module}_{i}", module_cond)) + + 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) + if result_i is not None: + frame_result[f"{evaluation_item.name}"] = result_i + return frame_result + + def update(self) -> tuple[bool, str]: + rtn_success = True + rtn_summary = [] if len(self.__container) != 0 else ["NotTestTarget"] + for evaluation_item in self.__container: + if not evaluation_item.success: + rtn_success = False + rtn_summary.append(f"{evaluation_item.name} (Fail)") + else: + rtn_summary.append(f"{evaluation_item.name} (Success)") + prefix_str = "Passed" if rtn_success else "Failed" + rtn_summary_str = prefix_str + ":" + ", ".join(rtn_summary) + return (rtn_success, rtn_summary_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", + ) + + 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.update() diff --git a/log_evaluator/scripts/planning_control_evaluator_node.py b/log_evaluator/scripts/planning_control_evaluator_node.py new file mode 100755 index 00000000..97d175c1 --- /dev/null +++ b/log_evaluator/scripts/planning_control_evaluator_node.py @@ -0,0 +1,58 @@ +#!/usr/bin/env python3 + +# Copyright (c) 2024 TIER IV.inc +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + + +from diagnostic_msgs.msg import DiagnosticArray + +from log_evaluator.evaluator import evaluator_main +from log_evaluator.evaluator import LogEvaluator +from log_evaluator.planning_control import PlanningControlResult +from log_evaluator.planning_control import PlanningControlScenario + + +class PlanningControlEvaluator(LogEvaluator): + def __init__(self, name: str) -> None: + super().__init__(name, PlanningControlScenario, PlanningControlResult) + self._scenario: PlanningControlScenario + self._result: PlanningControlResult + + self.__sub_planning_diagnostics = self.create_subscription( + DiagnosticArray, + "/planning/planning_evaluator/metrics", + lambda msg, module_type="planning": self.diagnostics_cb(msg, module_type), + 1, + ) + + self.__sub_control_diagnostics = self.create_subscription( + DiagnosticArray, + "/control/control_evaluator/metrics", + lambda msg, module_type="control": self.diagnostics_cb(msg, module_type), + 1, + ) + + def diagnostics_cb(self, msg: DiagnosticArray, module: str) -> None: + self._result.set_frame(msg, module) + if self._result.frame != {}: + self._result_writer.write_result(self._result) + + +@evaluator_main +def main() -> LogEvaluator: + return PlanningControlEvaluator("planning_control_evaluator") + + +if __name__ == "__main__": + main() diff --git a/log_evaluator/test/unittest/test_planning_control.py b/log_evaluator/test/unittest/test_planning_control.py new file mode 100644 index 00000000..2a2127f7 --- /dev/null +++ b/log_evaluator/test/unittest/test_planning_control.py @@ -0,0 +1,256 @@ +# Copyright (c) 2024 TIER IV.inc +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +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 ValidationError +import pytest + +from log_evaluator.planning_control import KinematicCondition +from log_evaluator.planning_control import LaneCondition +from log_evaluator.planning_control import LaneInfo +from log_evaluator.planning_control import LeftRight +from log_evaluator.planning_control import Metrics +from log_evaluator.planning_control import MinMax +from log_evaluator.planning_control import PlanningControlCondition +from log_evaluator.planning_control import PlanningControlScenario +from log_evaluator.scenario import load_sample_scenario + + +def test_scenario() -> None: + scenario: PlanningControlScenario = load_sample_scenario( + "planning_control", + PlanningControlScenario, + ) + assert scenario.ScenarioName == "sample_planning_control" + assert scenario.Evaluation.Conditions.ControlConditions[0].condition_type == "all_of" + + +def test_min_max_validation() -> None: + with pytest.raises(ValidationError): + MinMax(min=3.0, max=1.0) + + +def test_left_right_validation() -> None: + with pytest.raises(ValidationError): + LeftRight(left=-3.0, right=1.0) + + +def test_left_right_match_condition() -> None: + left_right = LeftRight(left=1.0, right=1.0) + assert left_right.match_condition(0.5) + assert left_right.match_condition(2.0) is False + assert left_right.match_condition(-2.0) is False + + +def test_lane_info_match_condition() -> None: + lane_info = LaneInfo(id=1, s=1.0, t=LeftRight(left=1.0, right=1.0)) + assert lane_info.match_condition((1, 2.0, 0.5), start_condition=True) + assert lane_info.match_condition((2, 2.0, 0.5), start_condition=True) is False + assert lane_info.match_condition((1, 0.5, 0.5), start_condition=True) is False + assert lane_info.match_condition((1, 2.0, 3.0), start_condition=True) is False + assert lane_info.match_condition((1, 2.0, 3.0)) # if goal t is not used + + +def test_lane_condition_started_ended() -> None: + lane_condition = LaneCondition( + start=LaneInfo(id=1, s=1.0, t=LeftRight(left=1.0, right=1.0)), + end=LaneInfo(id=2, s=2.0), + ) + assert lane_condition.started is False + assert lane_condition.ended is False + assert lane_condition.is_started((1, 0.5, 0.5)) is False + assert lane_condition.is_started((1, 2.0, 0.5)) + assert lane_condition.is_started((2, 2.0)) # Once True, do not change. + assert lane_condition.is_ended((2, 1.0, 1.0)) is False + assert lane_condition.is_ended((2, 3.0, 1.0)) + + +def test_kinematic_state_match_condition() -> None: + kinematic_condition = KinematicCondition( + vel=MinMax(min=0.0, max=1.0), + acc=MinMax(min=0.0, max=2.0), + jerk=MinMax(min=0.0, max=3.0), + ) + assert kinematic_condition.match_condition((0.5, 1.0, 2.0)) + assert kinematic_condition.match_condition((2.0, 1.0, 2.0)) is False + assert kinematic_condition.match_condition((0.5, -1.0, 2.0)) is False + assert kinematic_condition.match_condition((0.5, 1.0, -1.0)) is False + + +def create_condition(condition_type: Literal["any_of", "all_of"]) -> PlanningControlCondition: + return PlanningControlCondition( + module="autonomous_emergency_braking: aeb_emergency_stop", + decision="deceleration", + condition_type=condition_type, + lane_condition=LaneCondition( + start=LaneInfo(id=1, s=1.0, t=LeftRight(left=1.0, right=1.0)), + end=LaneInfo(id=2, s=2.0), + ), + kinematic_condition=KinematicCondition( + vel=MinMax(min=0.0, max=1.0), + acc=MinMax(min=0.0, max=2.0), + jerk=MinMax(min=0.0, max=3.0), + ), + ) + + +def create_diag_msg( + *, + match_module: bool = True, + match_decision: bool = True, + is_started: bool = True, + is_ended: bool = False, + match_kinematic_state: bool = True, +) -> DiagnosticArray: + module = DiagnosticStatus( + name=( + "autonomous_emergency_braking: aeb_emergency_stop" + if match_module + else "module_not_matched" + ), + values=[ + KeyValue(key="decision" if match_decision else "not_decision", value="deceleration"), + ], + ) + lane_info = DiagnosticStatus( + name="ego_lane_info", + values=[ + KeyValue(key="lane_id", value="1" if is_ended is False else "2"), + KeyValue(key="s", value="2.0" if is_started else "1.0"), + KeyValue(key="t", value="0.5"), + ], + ) + kinematic_state = DiagnosticStatus( + name="kinematic_state", + values=[ + KeyValue(key="vel", value="0.5" if match_kinematic_state else "2.0"), + KeyValue(key="acc", value="1.0"), + KeyValue(key="jerk", value="2.0"), + ], + ) + return DiagnosticArray(status=[module, lane_info, kinematic_state]) + + +def test_msg_has_not_required_fields() -> None: + condition = create_condition("any_of") + evaluation_item: Metrics = Metrics(name="control_0", condition=condition) + diag_msg = DiagnosticArray(status=[]) + assert evaluation_item.set_frame(diag_msg) is None + + +def test_metrics_not_match_module_name() -> None: + condition = create_condition("any_of") + evaluation_item: Metrics = Metrics(name="control_0", condition=condition) + diag_msg = create_diag_msg(match_module=False) + assert evaluation_item.set_frame(diag_msg) is None + + +def test_metrics_not_match_decision() -> None: + condition = create_condition("any_of") + evaluation_item: Metrics = Metrics(name="control_0", condition=condition) + diag_msg = create_diag_msg(match_decision=False) + assert evaluation_item.set_frame(diag_msg) is None + + +def test_metrics_not_started() -> None: + condition = create_condition("any_of") + evaluation_item: Metrics = Metrics(name="control_0", condition=condition) + diag_msg = create_diag_msg(is_started=False) + assert evaluation_item.set_frame(diag_msg) is None + + +def test_metrics_finished() -> None: + condition = create_condition("any_of") + evaluation_item: Metrics = Metrics(name="control_0", condition=condition) + diag_msg = create_diag_msg(is_ended=True) + assert evaluation_item.set_frame(diag_msg) is None + + +def test_metrics_success_any_of() -> None: + condition = create_condition("any_of") + evaluation_item: Metrics = Metrics(name="control_0", condition=condition) + diag_msg = create_diag_msg() + frame_dict = evaluation_item.set_frame(diag_msg) + assert evaluation_item.success is True + assert frame_dict == { + "Result": {"Total": "Success", "Frame": "Success"}, + "Info": { + "TotalPassed": 1, + "Decision": "deceleration", + "LaneInfo": (1, 2.0, 0.5), + "KinematicState": (0.5, 1.0, 2.0), + }, + } + diag_msg = create_diag_msg(match_kinematic_state=False) + frame_dict = evaluation_item.set_frame(diag_msg) # any_of is OK if one of them succeeds. + assert evaluation_item.success is True + assert frame_dict == { + "Result": {"Total": "Success", "Frame": "Fail"}, + "Info": { + "TotalPassed": 1, + "Decision": "deceleration", + "LaneInfo": (1, 2.0, 0.5), + "KinematicState": (2.0, 1.0, 2.0), + }, + } + + +def test_metrics_fail_any_of() -> None: + condition = create_condition("any_of") + evaluation_item: Metrics = Metrics(name="control_0", condition=condition) + diag_msg = create_diag_msg(match_kinematic_state=False) + frame_dict = evaluation_item.set_frame(diag_msg) # any_of is OK if one of them succeeds. + assert evaluation_item.success is False + assert frame_dict == { + "Result": {"Total": "Fail", "Frame": "Fail"}, + "Info": { + "TotalPassed": 0, + "Decision": "deceleration", + "LaneInfo": (1, 2.0, 0.5), + "KinematicState": (2.0, 1.0, 2.0), + }, + } + + +def test_metrics_fail_all_of() -> None: + condition = create_condition("all_of") + evaluation_item: Metrics = Metrics(name="control_0", condition=condition) + diag_msg = create_diag_msg() + frame_dict = evaluation_item.set_frame(diag_msg) + assert evaluation_item.success is True + assert frame_dict == { + "Result": {"Total": "Success", "Frame": "Success"}, + "Info": { + "TotalPassed": 1, + "Decision": "deceleration", + "LaneInfo": (1, 2.0, 0.5), + "KinematicState": (0.5, 1.0, 2.0), + }, + } + diag_msg = create_diag_msg(match_kinematic_state=False) + frame_dict = evaluation_item.set_frame(diag_msg) # ALL_OF is not allowed if even one fails. + assert evaluation_item.success is False + assert frame_dict == { + "Result": {"Total": "Fail", "Frame": "Fail"}, + "Info": { + "TotalPassed": 1, + "Decision": "deceleration", + "LaneInfo": (1, 2.0, 0.5), + "KinematicState": (2.0, 1.0, 2.0), + }, + } diff --git a/sample/planning_control/control_diag.txt b/sample/planning_control/control_diag.txt new file mode 100644 index 00000000..7790a640 --- /dev/null +++ b/sample/planning_control/control_diag.txt @@ -0,0 +1,103 @@ +~/ros_ws/planning_control dlr/test-platform* 5m 49s +❯ ros2 topic echo /control/control_evaluator/metrics +1721114673.647621 [77] ros2: determined eno1 (udp/10.0.55.137) as highest quality interface, selected for automatic interface. +--- +header: + stamp: + sec: 1709009638 + nanosec: 704405284 + frame_id: '' +status: +- level: "\0" + name: 'autonomous_emergency_braking: aeb_emergency_stop' + message: '' + hardware_id: '' + values: + - key: decision + value: none +- level: "\0" + name: lateral_deviation + message: '' + hardware_id: '' + values: + - key: metric_value + value: '0.002404' +- level: "\0" + name: yaw_deviation + message: '' + hardware_id: '' + values: + - key: metric_value + value: '0.109910' +- level: "\0" + name: ego_lane_info + message: '' + hardware_id: '' + values: + - key: lane_id + value: '176472' + - key: s + value: '34.271024' + - key: t + value: '-0.207536' +- level: "\0" + name: kinematic_state + message: '' + hardware_id: '' + values: + - key: vel + value: '3.201738' + - key: acc + value: '-0.424839' + - key: jerk + value: '-0.288387' +--- +header: + stamp: + sec: 1709009638 + nanosec: 804405072 + frame_id: '' +status: +- level: "\0" + name: 'autonomous_emergency_braking: aeb_emergency_stop' + message: '' + hardware_id: '' + values: + - key: decision + value: deceleration +- level: "\0" + name: lateral_deviation + message: '' + hardware_id: '' + values: + - key: metric_value + value: '0.038727' +- level: "\0" + name: yaw_deviation + message: '' + hardware_id: '' + values: + - key: metric_value + value: '0.139692' +- level: "\0" + name: ego_lane_info + message: '' + hardware_id: '' + values: + - key: lane_id + value: '176472' + - key: s + value: '34.610130' + - key: t + value: '-0.275624' +- level: "\0" + name: kinematic_state + message: '' + hardware_id: '' + values: + - key: vel + value: '3.164714' + - key: acc + value: '-0.400825' + - key: jerk + value: '0.217844' diff --git a/sample/planning_control/result.json b/sample/planning_control/result.json new file mode 100644 index 00000000..22987d57 --- /dev/null +++ b/sample/planning_control/result.json @@ -0,0 +1,741 @@ +[ + { + "Condition": { + "ControlConditions": [ + { + "module": "autonomous_emergency_braking: aeb_emergency_stop", + "decision": "deceleration", + "condition_type": "all_of", + "lane_condition": { + "start": { + "id": 1500, + "s": null, + "t": null + }, + "end": { + "id": 1500, + "s": 2.3, + "t": null + }, + "started": false, + "ended": false + }, + "kinematic_condition": { + "vel": { + "min": 2.5, + "max": 3 + }, + "acc": { + "min": -0.5, + "max": 0.5 + }, + "jerk": null + } + }, + { + "module": "autonomous_emergency_braking: aeb_emergency_stop", + "decision": "none", + "condition_type": "all_of", + "lane_condition": { + "start": { + "id": 1500, + "s": 3, + "t": null + }, + "end": { + "id": 1500, + "s": 10, + "t": null + }, + "started": false, + "ended": false + }, + "kinematic_condition": null + } + ], + "PlanningConditions": [] + } + }, + { + "Result": { + "Success": false, + "Summary": "NoData" + }, + "Stamp": { + "System": 1721276989.222324 + }, + "Frame": {} + }, + { + "Result": { + "Success": false, + "Summary": "Control: Failed:control_0 (Success), control_1 (Fail) Planning: Passed:NotTestTarget" + }, + "Stamp": { + "System": 1721277019.2600749, + "ROS": 1709009640.003256 + }, + "Frame": { + "control_0": { + "Result": { + "Total": "Success", + "Frame": "Success" + }, + "Info": { + "TotalPassed": 1, + "Decision": "deceleration", + "LaneInfo": [1500, 0.254821, -0.983676], + "KinematicState": [2.780202, -0.322109, 0.299525] + } + } + } + }, + { + "Result": { + "Success": false, + "Summary": "Control: Failed:control_0 (Success), control_1 (Fail) Planning: Passed:NotTestTarget" + }, + "Stamp": { + "System": 1721277019.3607328, + "ROS": 1709009640.1032677 + }, + "Frame": { + "control_0": { + "Result": { + "Total": "Success", + "Frame": "Success" + }, + "Info": { + "TotalPassed": 2, + "Decision": "deceleration", + "LaneInfo": [1500, 0.524374, -1.038678], + "KinematicState": [2.77897, -0.193682, 1.284198] + } + } + } + }, + { + "Result": { + "Success": false, + "Summary": "Control: Failed:control_0 (Success), control_1 (Fail) Planning: Passed:NotTestTarget" + }, + "Stamp": { + "System": 1721277019.4600265, + "ROS": 1709009640.2032669 + }, + "Frame": { + "control_0": { + "Result": { + "Total": "Success", + "Frame": "Success" + }, + "Info": { + "TotalPassed": 3, + "Decision": "deceleration", + "LaneInfo": [1500, 0.797716, -1.089461], + "KinematicState": [2.766703, -0.157067, 0.374411] + } + } + } + }, + { + "Result": { + "Success": false, + "Summary": "Control: Failed:control_0 (Success), control_1 (Fail) Planning: Passed:NotTestTarget" + }, + "Stamp": { + "System": 1721277019.560831, + "ROS": 1709009640.3032598 + }, + "Frame": { + "control_0": { + "Result": { + "Total": "Success", + "Frame": "Success" + }, + "Info": { + "TotalPassed": 4, + "Decision": "deceleration", + "LaneInfo": [1500, 1.07291, -1.137064], + "KinematicState": [2.774931, -0.06495, 0.919988] + } + } + } + }, + { + "Result": { + "Success": false, + "Summary": "Control: Failed:control_0 (Success), control_1 (Fail) Planning: Passed:NotTestTarget" + }, + "Stamp": { + "System": 1721277019.6612196, + "ROS": 1709009640.403535 + }, + "Frame": { + "control_0": { + "Result": { + "Total": "Success", + "Frame": "Success" + }, + "Info": { + "TotalPassed": 5, + "Decision": "deceleration", + "LaneInfo": [1500, 1.345795, -1.181963], + "KinematicState": [2.785044, 0.000635, 0.654367] + } + } + } + }, + { + "Result": { + "Success": false, + "Summary": "Control: Failed:control_0 (Success), control_1 (Fail) Planning: Passed:NotTestTarget" + }, + "Stamp": { + "System": 1721277019.7615428, + "ROS": 1709009640.5032952 + }, + "Frame": { + "control_0": { + "Result": { + "Total": "Success", + "Frame": "Success" + }, + "Info": { + "TotalPassed": 6, + "Decision": "deceleration", + "LaneInfo": [1500, 1.619427, -1.222165], + "KinematicState": [2.806327, 0.093698, 0.929944] + } + } + } + }, + { + "Result": { + "Success": false, + "Summary": "Control: Failed:control_0 (Success), control_1 (Fail) Planning: Passed:NotTestTarget" + }, + "Stamp": { + "System": 1721277019.8604515, + "ROS": 1709009640.6032755 + }, + "Frame": { + "control_0": { + "Result": { + "Total": "Success", + "Frame": "Success" + }, + "Info": { + "TotalPassed": 7, + "Decision": "deceleration", + "LaneInfo": [1500, 1.899189, -1.257947], + "KinematicState": [2.826436, 0.134591, 0.411654] + } + } + } + }, + { + "Result": { + "Success": false, + "Summary": "Control: Failed:control_0 (Success), control_1 (Fail) Planning: Passed:NotTestTarget" + }, + "Stamp": { + "System": 1721277019.9600542, + "ROS": 1709009640.7032664 + }, + "Frame": { + "control_0": { + "Result": { + "Total": "Success", + "Frame": "Success" + }, + "Info": { + "TotalPassed": 8, + "Decision": "deceleration", + "LaneInfo": [1500, 2.180388, -1.287927], + "KinematicState": [2.855254, 0.206083, 0.706992] + } + } + } + }, + { + "Result": { + "Success": true, + "Summary": "Control: Passed:control_0 (Success), control_1 (Success) Planning: Passed:NotTestTarget" + }, + "Stamp": { + "System": 1721277020.2599468, + "ROS": 1709009641.0032742 + }, + "Frame": { + "control_1": { + "Result": { + "Total": "Success", + "Frame": "Success" + }, + "Info": { + "TotalPassed": 1, + "Decision": "none", + "LaneInfo": [1500, 3.048498, -1.357804], + "KinematicState": [2.973334, 0.388577, 0.757059] + } + } + } + }, + { + "Result": { + "Success": true, + "Summary": "Control: Passed:control_0 (Success), control_1 (Success) Planning: Passed:NotTestTarget" + }, + "Stamp": { + "System": 1721277020.3603773, + "ROS": 1709009641.1032643 + }, + "Frame": { + "control_1": { + "Result": { + "Total": "Success", + "Frame": "Success" + }, + "Info": { + "TotalPassed": 2, + "Decision": "none", + "LaneInfo": [1500, 3.348355, -1.371031], + "KinematicState": [3.031645, 0.492628, 1.050884] + } + } + } + }, + { + "Result": { + "Success": true, + "Summary": "Control: Passed:control_0 (Success), control_1 (Success) Planning: Passed:NotTestTarget" + }, + "Stamp": { + "System": 1721277020.4598317, + "ROS": 1709009641.203264 + }, + "Frame": { + "control_1": { + "Result": { + "Total": "Success", + "Frame": "Success" + }, + "Info": { + "TotalPassed": 3, + "Decision": "none", + "LaneInfo": [1500, 3.654895, -1.378377], + "KinematicState": [3.088592, 0.52498, 0.322604] + } + } + } + }, + { + "Result": { + "Success": true, + "Summary": "Control: Passed:control_0 (Success), control_1 (Success) Planning: Passed:NotTestTarget" + }, + "Stamp": { + "System": 1721277020.5599344, + "ROS": 1709009641.3032653 + }, + "Frame": { + "control_1": { + "Result": { + "Total": "Success", + "Frame": "Success" + }, + "Info": { + "TotalPassed": 4, + "Decision": "none", + "LaneInfo": [1500, 3.967671, -1.380667], + "KinematicState": [3.152821, 0.58422, 0.590881] + } + } + } + }, + { + "Result": { + "Success": true, + "Summary": "Control: Passed:control_0 (Success), control_1 (Success) Planning: Passed:NotTestTarget" + }, + "Stamp": { + "System": 1721277020.6612742, + "ROS": 1709009641.4032772 + }, + "Frame": { + "control_1": { + "Result": { + "Total": "Success", + "Frame": "Success" + }, + "Info": { + "TotalPassed": 5, + "Decision": "none", + "LaneInfo": [1500, 4.286699, -1.37869], + "KinematicState": [3.220785, 0.65566, 0.710801] + } + } + } + }, + { + "Result": { + "Success": true, + "Summary": "Control: Passed:control_0 (Success), control_1 (Success) Planning: Passed:NotTestTarget" + }, + "Stamp": { + "System": 1721277020.7622206, + "ROS": 1709009641.5033162 + }, + "Frame": { + "control_1": { + "Result": { + "Total": "Success", + "Frame": "Success" + }, + "Info": { + "TotalPassed": 6, + "Decision": "none", + "LaneInfo": [1500, 4.614102, -1.371985], + "KinematicState": [3.287503, 0.687749, 0.320499] + } + } + } + }, + { + "Result": { + "Success": true, + "Summary": "Control: Passed:control_0 (Success), control_1 (Success) Planning: Passed:NotTestTarget" + }, + "Stamp": { + "System": 1721277020.8598855, + "ROS": 1709009641.603265 + }, + "Frame": { + "control_1": { + "Result": { + "Total": "Success", + "Frame": "Success" + }, + "Info": { + "TotalPassed": 7, + "Decision": "none", + "LaneInfo": [1500, 4.954561, -1.359024], + "KinematicState": [3.346353, 0.642912, -0.443885] + } + } + } + }, + { + "Result": { + "Success": true, + "Summary": "Control: Passed:control_0 (Success), control_1 (Success) Planning: Passed:NotTestTarget" + }, + "Stamp": { + "System": 1721277020.9627604, + "ROS": 1709009641.7032757 + }, + "Frame": { + "control_1": { + "Result": { + "Total": "Success", + "Frame": "Success" + }, + "Info": { + "TotalPassed": 8, + "Decision": "none", + "LaneInfo": [1500, 5.289516, -1.341692], + "KinematicState": [3.4079, 0.626983, -0.162374] + } + } + } + }, + { + "Result": { + "Success": true, + "Summary": "Control: Passed:control_0 (Success), control_1 (Success) Planning: Passed:NotTestTarget" + }, + "Stamp": { + "System": 1721277021.060883, + "ROS": 1709009641.8032632 + }, + "Frame": { + "control_1": { + "Result": { + "Total": "Success", + "Frame": "Success" + }, + "Info": { + "TotalPassed": 9, + "Decision": "none", + "LaneInfo": [1500, 5.636661, -1.319875], + "KinematicState": [3.47503, 0.605346, -0.217098] + } + } + } + }, + { + "Result": { + "Success": true, + "Summary": "Control: Passed:control_0 (Success), control_1 (Success) Planning: Passed:NotTestTarget" + }, + "Stamp": { + "System": 1721277021.1596808, + "ROS": 1709009641.9032614 + }, + "Frame": { + "control_1": { + "Result": { + "Total": "Success", + "Frame": "Success" + }, + "Info": { + "TotalPassed": 10, + "Decision": "none", + "LaneInfo": [1500, 5.98997, -1.294059], + "KinematicState": [3.537443, 0.65008, 0.445461] + } + } + } + }, + { + "Result": { + "Success": true, + "Summary": "Control: Passed:control_0 (Success), control_1 (Success) Planning: Passed:NotTestTarget" + }, + "Stamp": { + "System": 1721277021.2599158, + "ROS": 1709009642.0032642 + }, + "Frame": { + "control_1": { + "Result": { + "Total": "Success", + "Frame": "Success" + }, + "Info": { + "TotalPassed": 11, + "Decision": "none", + "LaneInfo": [1500, 6.348585, -1.265829], + "KinematicState": [3.609332, 0.664397, 0.140656] + } + } + } + }, + { + "Result": { + "Success": true, + "Summary": "Control: Passed:control_0 (Success), control_1 (Success) Planning: Passed:NotTestTarget" + }, + "Stamp": { + "System": 1721277021.3598413, + "ROS": 1709009642.1032693 + }, + "Frame": { + "control_1": { + "Result": { + "Total": "Success", + "Frame": "Success" + }, + "Info": { + "TotalPassed": 12, + "Decision": "none", + "LaneInfo": [1500, 6.715403, -1.232613], + "KinematicState": [3.693096, 0.754094, 0.908683] + } + } + } + }, + { + "Result": { + "Success": true, + "Summary": "Control: Passed:control_0 (Success), control_1 (Success) Planning: Passed:NotTestTarget" + }, + "Stamp": { + "System": 1721277021.4612176, + "ROS": 1709009642.2032835 + }, + "Frame": { + "control_1": { + "Result": { + "Total": "Success", + "Frame": "Success" + }, + "Info": { + "TotalPassed": 13, + "Decision": "none", + "LaneInfo": [1500, 7.089984, -1.197086], + "KinematicState": [3.774627, 0.811998, 0.580573] + } + } + } + }, + { + "Result": { + "Success": true, + "Summary": "Control: Passed:control_0 (Success), control_1 (Success) Planning: Passed:NotTestTarget" + }, + "Stamp": { + "System": 1721277021.5604, + "ROS": 1709009642.3032677 + }, + "Frame": { + "control_1": { + "Result": { + "Total": "Success", + "Frame": "Success" + }, + "Info": { + "TotalPassed": 14, + "Decision": "none", + "LaneInfo": [1500, 7.473814, -1.158361], + "KinematicState": [3.848118, 0.791756, -0.201294] + } + } + } + }, + { + "Result": { + "Success": true, + "Summary": "Control: Passed:control_0 (Success), control_1 (Success) Planning: Passed:NotTestTarget" + }, + "Stamp": { + "System": 1721277021.6613028, + "ROS": 1709009642.4032865 + }, + "Frame": { + "control_1": { + "Result": { + "Total": "Success", + "Frame": "Success" + }, + "Info": { + "TotalPassed": 15, + "Decision": "none", + "LaneInfo": [1500, 7.863955, -1.116921], + "KinematicState": [3.930269, 0.825974, 0.344808] + } + } + } + }, + { + "Result": { + "Success": true, + "Summary": "Control: Passed:control_0 (Success), control_1 (Success) Planning: Passed:NotTestTarget" + }, + "Stamp": { + "System": 1721277021.7604876, + "ROS": 1709009642.5035427 + }, + "Frame": { + "control_1": { + "Result": { + "Total": "Success", + "Frame": "Success" + }, + "Info": { + "TotalPassed": 16, + "Decision": "none", + "LaneInfo": [1500, 8.264956, -1.076771], + "KinematicState": [4.006945, 0.811871, -0.140155] + } + } + } + }, + { + "Result": { + "Success": true, + "Summary": "Control: Passed:control_0 (Success), control_1 (Success) Planning: Passed:NotTestTarget" + }, + "Stamp": { + "System": 1721277021.8623214, + "ROS": 1709009642.6032639 + }, + "Frame": { + "control_1": { + "Result": { + "Total": "Success", + "Frame": "Success" + }, + "Info": { + "TotalPassed": 17, + "Decision": "none", + "LaneInfo": [1500, 8.687818, -1.034342], + "KinematicState": [4.083466, 0.78916, -0.220425] + } + } + } + }, + { + "Result": { + "Success": true, + "Summary": "Control: Passed:control_0 (Success), control_1 (Success) Planning: Passed:NotTestTarget" + }, + "Stamp": { + "System": 1721277021.9597435, + "ROS": 1709009642.7032695 + }, + "Frame": { + "control_1": { + "Result": { + "Total": "Success", + "Frame": "Success" + }, + "Info": { + "TotalPassed": 18, + "Decision": "none", + "LaneInfo": [1500, 9.086423, -0.993832], + "KinematicState": [4.153996, 0.769382, -0.205345] + } + } + } + }, + { + "Result": { + "Success": true, + "Summary": "Control: Passed:control_0 (Success), control_1 (Success) Planning: Passed:NotTestTarget" + }, + "Stamp": { + "System": 1721277022.0604632, + "ROS": 1709009642.8032649 + }, + "Frame": { + "control_1": { + "Result": { + "Total": "Success", + "Frame": "Success" + }, + "Info": { + "TotalPassed": 19, + "Decision": "none", + "LaneInfo": [1500, 9.505482, -0.951828], + "KinematicState": [4.233492, 0.802354, 0.328948] + } + } + } + }, + { + "Result": { + "Success": true, + "Summary": "Control: Passed:control_0 (Success), control_1 (Success) Planning: Passed:NotTestTarget" + }, + "Stamp": { + "System": 1721277022.159759, + "ROS": 1709009642.9032645 + }, + "Frame": { + "control_1": { + "Result": { + "Total": "Success", + "Frame": "Success" + }, + "Info": { + "TotalPassed": 20, + "Decision": "none", + "LaneInfo": [1500, 9.943206, -0.906019], + "KinematicState": [4.281736, 0.657596, -1.443561] + } + } + } + } +] diff --git a/sample/planning_control/scenario.yaml b/sample/planning_control/scenario.yaml new file mode 100644 index 00000000..5c9f9304 --- /dev/null +++ b/sample/planning_control/scenario.yaml @@ -0,0 +1,37 @@ +ScenarioFormatVersion: 3.0.0 +ScenarioName: sample_planning_control +ScenarioDescription: sample_planning_control +SensorModel: aip_xx1 +VehicleModel: lexus +Evaluation: + UseCaseName: planning_control + UseCaseFormatVersion: 0.1.0 + Datasets: + - odaiba: + VehicleId: default + Conditions: + ControlConditions: + - module: "autonomous_emergency_braking: aeb_emergency_stop" + decision: deceleration # Specify key as key and value as value in KeyValue of module. + condition_type: all_of + lane_condition: + start: + id: 1500 + end: + id: 1500 + s: 2.3 + kinematic_condition: + vel: { min: 2.5, max: 3.0 } + acc: { min: -0.5, max: 0.5 } + jerk: { min: 0.2, max: 1.5 } + - module: "autonomous_emergency_braking: aeb_emergency_stop" + decision: none + condition_type: all_of + lane_condition: # optional + start: + id: 1500 + s: 3.0 + end: + id: 1500 + s: 10.0 + kinematic_condition: null # optional