feat(control_validator, tier4_diagnostics): sync upstream (#838)

* feat(control_validator): add diag to check control component latency (autowarefoundation#1349)

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

* feat(control_validator)!: add overrun_stop_point_dist parameter (autowarefoundation#1346)

Signed-off-by: yuki-takagi-66 <yuki.takagi@tier4.jp>

* feat(control_validator)!: add acceleration check (autowarefoundation#1375)

Signed-off-by: yuki-takagi-66 <yuki.takagi@tier4.jp>

* feat(diagnostics): add control diag (#803)

Signed-off-by: yuki-takagi-66 <yuki.takagi@tier4.jp>

* fix(tier4_diagnostics): remove lane departure trajectory deviation

* feat(control_validator): add over run estimation feature (autowarefoundation#1394)

Signed-off-by: yuki-takagi-66 <yuki.takagi@tier4.jp>

* feat(tier4_diagnostics): replace overrun estimation diag (#836)

Signed-off-by: yuki-takagi-66 <yuki.takagi@tier4.jp>

---------

Signed-off-by: satoshi-ota <satoshi.ota928@gmail.com>
Signed-off-by: yuki-takagi-66 <yuki.takagi@tier4.jp>
Co-authored-by: Satoshi OTA <44889564+satoshi-ota@users.noreply.github.com>
Co-authored-by: Zulfaqar Azmi <zulfaqar.azmi@tier4.jp>

Author: 3 people

autoware_launch/config/control/control_validator/control_validator.param.yaml

@@ -9,9 +9,17 @@
 
     thresholds:
       max_distance_deviation: 1.0
+      acc_error_offset: 0.8
+      acc_error_scale: 0.2
       rolling_back_velocity: 0.5
       over_velocity_offset: 2.0
       over_velocity_ratio: 0.2
+      overrun_stop_point_dist: 0.8
+      will_overrun_stop_point_dist: 1.0
+      assumed_limit_acc: 5.0
+      assumed_delay_time: 0.2
+      nominal_latency: 0.01
 
+    acc_lpf_gain: 0.97 # Time constant 1.11s
     vel_lpf_gain: 0.9 # Time constant 0.33
     hold_velocity_error_until_stop: true

autoware_launch/config/system/tier4_diagnostics/control.yaml

@@ -25,13 +25,15 @@ units:
     list:
       - { type: link, link: /control/001-topic_status/control_command-error }
       - { type: link, link: /control/004-lane_departure-error }
-      - { type: link, link: /control/005-trajectory_deviation-error }
       - { type: link, link: /control/009-aeb_emergency_stop }
       # - { type: link, link: /control/010-max_distance_deviation-error }
       # - { type: link, link: /control/011-slip_detection }
       - { type: link, link: /control/collision_detector }
+      # - { type: link, link: /control/acceleration_error }
       - { type: link, link: /control/rolling_back }
       - { type: link, link: /control/over_velocity }
+      - { type: link, link: /control/overrun_stop_point }
+      - { type: link, link: /control/will_overrun_stop_point }
 
   - path: /control/comfortable_stop
     type: and
@@ -59,12 +61,6 @@ units:
       type: link
       link: /control/004-lane_departure
 
-  - path: /control/005-trajectory_deviation-error
-    type: warn-to-ok
-    item:
-      type: link
-      link: /control/005-trajectory_deviation
-
   - path: /control/010-max_distance_deviation-error
     type: warn-to-ok
     item:
@@ -89,12 +85,6 @@ units:
     name: lane_departure
     timeout: 1.0
 
-  - path: /control/005-trajectory_deviation
-    type: diag
-    node: lane_departure_checker_node
-    name: trajectory_deviation
-    timeout: 1.0
-
   - path: /control/007-external_command_converter_heartbeat
     type: diag
     node: external_cmd_converter
@@ -130,6 +120,11 @@ units:
     node: collision_detector
     name: collision_detect
 
+  - path: /control/acceleration_error
+    type: diag
+    node: control_validator
+    name: control_validation_acceleration_error
+
   - path: /control/rolling_back
     type: diag
     node: control_validator
@@ -139,3 +134,13 @@ units:
     type: diag
     node: control_validator
     name: control_validation_over_velocity
+
+  - path: /control/overrun_stop_point
+    type: diag
+    node: control_validator
+    name: control_validation_overrun_stop_point
+
+  - path: /control/will_overrun_stop_point
+    type: diag
+    node: control_validator
+    name: control_validation_will_overrun_stop_point