feat(control_validator): measure predicted path deviation from trajec…

…tory (autowarefoundation#4549) * add feature for getting predicted path deviation from trajectory Signed-off-by: kyoichi-sugahara <kyoichi.sugahara@tier4.jp> * fix for build success Signed-off-by: kyoichi-sugahara <kyoichi.sugahara@tier4.jp> * fix topic name Signed-off-by: kyoichi-sugahara <kyoichi.sugahara@tier4.jp> * temp Signed-off-by: kyoichi-sugahara <kyoichi.sugahara@tier4.jp> * temp Signed-off-by: kyoichi-sugahara <kyoichi.sugahara@tier4.jp> * cut off extra length on predicted path Signed-off-by: kyoichi-sugahara <kyoichi.sugahara@tier4.jp> * cut off extra length on predicted path Signed-off-by: kyoichi-sugahara <kyoichi.sugahara@tier4.jp> * style(pre-commit): autofix * minor refactor Signed-off-by: kyoichi-sugahara <kyoichi.sugahara@tier4.jp> * change function name Signed-off-by: kyoichi-sugahara <kyoichi.sugahara@tier4.jp> * add control validator Signed-off-by: kyoichi-sugahara <kyoichi.sugahara@tier4.jp> * style(pre-commit): autofix * add max_deviation calculation Signed-off-by: kyoichi-sugahara <kyoichi.sugahara@tier4.jp> * refactor Signed-off-by: kyoichi-sugahara <kyoichi.sugahara@tier4.jp> * style(pre-commit): autofix * update launch Signed-off-by: kyoichi-sugahara <kyoichi.sugahara@tier4.jp> * style(pre-commit): autofix * change maintainer Signed-off-by: kyoichi-sugahara <kyoichi.sugahara@tier4.jp> * refactor Signed-off-by: kyoichi-sugahara <kyoichi.sugahara@tier4.jp> * style(pre-commit): autofix * feat(dynamic_avoidance): object polygon based drivable area generation (autowarefoundation#4598) Signed-off-by: Takayuki Murooka <takayuki5168@gmail.com> * update Signed-off-by: kyoichi-sugahara <kyoichi.sugahara@tier4.jp> * update README Signed-off-by: kyoichi-sugahara <kyoichi.sugahara@tier4.jp> * fix typo Signed-off-by: kyoichi-sugahara <kyoichi.sugahara@tier4.jp> * apply clang-tidy check Signed-off-by: kyoichi-sugahara <kyoichi.sugahara@tier4.jp> * Update control/control_validator/include/control_validator/utils.hpp Co-authored-by: Takamasa Horibe <horibe.takamasa@gmail.com> * remove debug code Signed-off-by: kyoichi-sugahara <kyoichi.sugahara@tier4.jp> * add maintainer Signed-off-by: kyoichi-sugahara <kyoichi.sugahara@tier4.jp> --------- Signed-off-by: kyoichi-sugahara <kyoichi.sugahara@tier4.jp> Signed-off-by: Takayuki Murooka <takayuki5168@gmail.com> Co-authored-by: pre-commit-ci[bot] <66853113+pre-commit-ci[bot]@users.noreply.github.com> Co-authored-by: Takayuki Murooka <takayuki5168@gmail.com> Co-authored-by: Takamasa Horibe <horibe.takamasa@gmail.com>
kminoda · Aug 21, 2023 · 7caa3f5 · 7caa3f5
1 parent c418a70
commit 7caa3f5
Show file tree

Hide file tree

Showing 15 changed files with 1,165 additions and 0 deletions.
diff --git a/control/control_validator/CMakeLists.txt b/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
+)
diff --git a/control/control_validator/README.md b/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           |
diff --git a/control/control_validator/config/control_validator.param.yaml b/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
diff --git a/control/control_validator/image/control_validator.drawio.svg b/control/control_validator/image/control_validator.drawio.svg