From 01c14b0886845e77fe43f7494e6b57f41d06158d Mon Sep 17 00:00:00 2001
From: Kyoichi Sugahara <kyoichi.sugahara@tier4.jp>
Date: Tue, 15 Aug 2023 19:31:20 +0900
Subject: [PATCH] feat(control_validator): measure predicted path deviation
 from trajectory (#509)

* update launcher

Signed-off-by: kyoichi-sugahara <kyoichi.sugahara@tier4.jp>

* add config and modify launch file

Signed-off-by: kyoichi-sugahara <kyoichi.sugahara@tier4.jp>

* style(pre-commit): autofix

* feat(lane_departure_checker): add road_border departure checker (#511)

add param

Signed-off-by: kyoichi-sugahara <kyoichi.sugahara@tier4.jp>

* feat(system_error_monitor): check lateral deviation in sim (#516)

Signed-off-by: satoshi-ota <satoshi.ota928@gmail.com>

* restore rviz config change

Signed-off-by: kyoichi-sugahara <kyoichi.sugahara@tier4.jp>

---------

Signed-off-by: kyoichi-sugahara <kyoichi.sugahara@tier4.jp>
Signed-off-by: satoshi-ota <satoshi.ota928@gmail.com>
Co-authored-by: pre-commit-ci[bot] <66853113+pre-commit-ci[bot]@users.noreply.github.com>
Co-authored-by: Satoshi OTA <44889564+satoshi-ota@users.noreply.github.com>
---
 .../control_validator/control_validator.param.yaml | 14 ++++++++++++++
 .../components/tier4_control_component.launch.xml  |  1 +
 2 files changed, 15 insertions(+)
 create mode 100644 autoware_launch/config/control/control_validator/control_validator.param.yaml

diff --git a/autoware_launch/config/control/control_validator/control_validator.param.yaml b/autoware_launch/config/control/control_validator/control_validator.param.yaml
new file mode 100644
index 0000000000..9ce677b2c2
--- /dev/null
+++ b/autoware_launch/config/control/control_validator/control_validator.param.yaml
@@ -0,0 +1,14 @@
+/**:
+  ros__parameters:
+
+    publish_diag: true  # if true, diagnostic msg is published
+
+    # If the number of consecutive invalid predicted_path exceeds this threshold, the Diag will be set to ERROR.
+    # (For example, threshold = 1 means, even if the predicted_path is invalid, Diag will not be ERROR if
+    #  the next predicted_path is valid.)
+    diag_error_count_threshold: 0
+
+    display_on_terminal: true # show error msg on terminal
+
+    thresholds:
+      max_distance_deviation: 1.0
diff --git a/autoware_launch/launch/components/tier4_control_component.launch.xml b/autoware_launch/launch/components/tier4_control_component.launch.xml
index 7b3169eacd..8b29135048 100644
--- a/autoware_launch/launch/components/tier4_control_component.launch.xml
+++ b/autoware_launch/launch/components/tier4_control_component.launch.xml
@@ -35,6 +35,7 @@
     <arg name="lon_controller_param_path" value="$(var lon_controller_param_path)"/>
     <arg name="vehicle_cmd_gate_param_path" value="$(find-pkg-share autoware_launch)/config/control/vehicle_cmd_gate/vehicle_cmd_gate.param.yaml"/>
     <arg name="lane_departure_checker_param_path" value="$(find-pkg-share autoware_launch)/config/control/lane_departure_checker/lane_departure_checker.param.yaml"/>
+    <arg name="control_validator_param_path" value="$(find-pkg-share autoware_launch)/config/control/control_validator/control_validator.param.yaml"/>
     <arg name="operation_mode_transition_manager_param_path" value="$(find-pkg-share autoware_launch)/config/control/operation_mode_transition_manager/operation_mode_transition_manager.param.yaml"/>
     <arg name="shift_decider_param_path" value="$(find-pkg-share autoware_launch)/config/control/shift_decider/shift_decider.param.yaml"/>
     <arg name="obstacle_collision_checker_param_path" value="$(find-pkg-share autoware_launch)/config/control/obstacle_collision_checker/obstacle_collision_checker.param.yaml"/>