Merge pull request #730 from tier4/sync-upstream

chore: sync upstream

.cspell-partial.json

@@ -3,6 +3,7 @@
     "**/perception/**",
     "**/planning/**",
     "**/sensing/**",
+    "**/control/**",
     "perception/bytetrack/lib/**"
   ],
   "ignoreRegExpList": [],

control/control_validator/CMakeLists.txt

@@ -0,0 +1,46 @@
+cmake_minimum_required(VERSION 3.22)
+project(control_validator)
+
+find_package(autoware_cmake REQUIRED)
+autoware_package()
+
+ament_auto_add_library(control_validator_helpers SHARED
+  src/utils.cpp
+  src/debug_marker.cpp
+)
+
+# control validator
+ament_auto_add_library(control_validator_component SHARED
+  include/control_validator/control_validator.hpp
+  src/control_validator.cpp
+)
+target_link_libraries(control_validator_component control_validator_helpers)
+rclcpp_components_register_node(control_validator_component
+  PLUGIN "control_validator::ControlValidator"
+  EXECUTABLE control_validator_node
+)
+
+rosidl_generate_interfaces(
+  ${PROJECT_NAME}
+  "msg/ControlValidatorStatus.msg"
+  DEPENDENCIES builtin_interfaces
+)
+
+# to use a message defined in the same package
+if(${rosidl_cmake_VERSION} VERSION_LESS 2.5.0)
+  rosidl_target_interfaces(control_validator_component
+    ${PROJECT_NAME} "rosidl_typesupport_cpp")
+else()
+  rosidl_get_typesupport_target(
+    cpp_typesupport_target ${PROJECT_NAME} "rosidl_typesupport_cpp")
+  target_link_libraries(control_validator_component "${cpp_typesupport_target}")
+endif()
+
+# if(BUILD_TESTING)
+# endif()
+
+ament_auto_package(
+  INSTALL_TO_SHARE
+  config
+  launch
+)

control/control_validator/README.md

@@ -0,0 +1,58 @@
+# Control Validator
+
+The `control_validator` is a module that checks the validity of the output of the control component. The status of the validation can be viewed in the `/diagnostics` topic.
+
+![control_validator](./image/control_validator.drawio.svg)
+
+## Supported features
+
+The following features are supported for the validation and can have thresholds set by parameters:
+
+- **Deviation check between reference trajectory and predicted trajectory** : invalid when the largest deviation between the predicted trajectory and reference trajectory is greater than the given threshold.
+
+![trajectory_deviation](./image/trajectory_deviation.drawio.svg)
+
+Other features are to be implemented.
+
+## Inputs/Outputs
+
+### Inputs
+
+The `control_validator` takes in the following inputs:
+
+| Name                           | Type                                  | Description                                                                    |
+| ------------------------------ | ------------------------------------- | ------------------------------------------------------------------------------ |
+| `~/input/kinematics`           | nav_msgs/Odometry                     | ego pose and twist                                                             |
+| `~/input/reference_trajectory` | autoware_auto_control_msgs/Trajectory | reference trajectory which is outputted from planning module to to be followed |
+| `~/input/predicted_trajectory` | autoware_auto_control_msgs/Trajectory | predicted trajectory which is outputted from control module                    |
+
+### Outputs
+
+It outputs the following:
+
+| Name                         | Type                                     | Description                                                               |
+| ---------------------------- | ---------------------------------------- | ------------------------------------------------------------------------- |
+| `~/output/validation_status` | control_validator/ControlValidatorStatus | validator status to inform the reason why the trajectory is valid/invalid |
+| `/diagnostics`               | diagnostic_msgs/DiagnosticStatus         | diagnostics to report errors                                              |
+
+## Parameters
+
+The following parameters can be set for the `control_validator`:
+
+### System parameters
+
+| Name                         | Type | Description                                                                                                                                                                                                                                | Default value |
+| :--------------------------- | :--- | :----------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------- | :------------ |
+| `publish_diag`               | bool | if true, diagnostics msg is published.                                                                                                                                                                                                     | true          |
+| `diag_error_count_threshold` | int  | the Diag will be set to ERROR when the number of consecutive invalid trajectory exceeds this threshold. (For example, threshold = 1 means, even if the trajectory is invalid, the Diag will not be ERROR if the next trajectory is valid.) | true          |
+| `display_on_terminal`        | bool | show error msg on terminal                                                                                                                                                                                                                 | true          |
+
+### Algorithm parameters
+
+#### Thresholds
+
+The input trajectory is detected as invalid if the index exceeds the following thresholds.
+
+| Name                                | Type   | Description                                                                                                 | Default value |
+| :---------------------------------- | :----- | :---------------------------------------------------------------------------------------------------------- | :------------ |
+| `thresholds.max_distance_deviation` | double | invalid threshold of the max distance deviation between the predicted path and the reference trajectory [m] | 1.0           |

control/control_validator/config/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 trajectory exceeds this threshold, the Diag will be set to ERROR.
+    # (For example, threshold = 1 means, even if the trajectory is invalid, Diag will not be ERROR if
+    #  the next trajectory is valid.)
+    diag_error_count_threshold: 0
+
+    display_on_terminal: true # show error msg on terminal
+
+    thresholds:
+      max_distance_deviation: 1.0