feat(control_performance_analysis): add new functionalities to evalua…

…te the control modules(autowarefoundation#567) Signed-off-by: Berkay <berkay@leodrive.ai> evaluation node updated(autowarefoundation#567) message files arranged, structure changed(autowarefoundation#567) ci(pre-commit): autofix added another condition added velocity history information to estimate acceleration fix new message file added monitor variables ready fix can build runtime errors works works fix Signed-off-by: Berkay <berkay@leodrive.ai> ci(pre-commit): autofix odom history interval param added (autowarefoundation#567) Signed-off-by: Berkay <berkay@leodrive.ai> ci(pre-commit): autofix added steering stamp rate Signed-off-by: Berkay <berkay@leodrive.ai> added steering stamp state deleted min treshold of dt Signed-off-by: Berkay <berkay@leodrive.ai> ci(pre-commit): autofix fix build Signed-off-by: Berkay <berkay@leodrive.ai> added plot config file Signed-off-by: Berkay <berkay@leodrive.ai> ci(pre-commit): autofix readme updated Signed-off-by: Berkay <berkay@leodrive.ai> ci(pre-commit): autofix fix cmakelist and msg files Signed-off-by: Berkay <berkay@leodrive.ai> ci(pre-commit): autofix adjusted license Signed-off-by: Berkay <berkay@leodrive.ai> fix build error Signed-off-by: Berkay <berkay@leodrive.ai> fix lint_cmake error Signed-off-by: Berkay <berkay@leodrive.ai> ci(pre-commit): autofix fix cmakelist build error Signed-off-by: Berkay <berkay@leodrive.ai> cmake error fix Signed-off-by: Berkay <berkay@leodrive.ai> ci(pre-commit): autofix update readme Signed-off-by: Berkay <berkay@leodrive.ai> ci(pre-commit): autofix fix typesupport error Signed-off-by: Berkay <berkay@leodrive.ai> fix cmake Signed-off-by: Berkay <berkay@leodrive.ai> errors revised Signed-off-by: Berkay <berkay@leodrive.ai> update error msg Signed-off-by: Berkay <berkay@leodrive.ai> ci(pre-commit): autofix added interpolated velocity Signed-off-by: Berkay <berkay@leodrive.ai> ci(pre-commit): autofix structure changed Signed-off-by: Berkay <berkay@leodrive.ai> error accelerations added Signed-off-by: Berkay <berkay@leodrive.ai> fix error goes infinity issue Signed-off-by: Berkay <berkay@leodrive.ai> ci(pre-commit): autofix fixed bugs and changed params Signed-off-by: Berkay <berkay@leodrive.ai> ci(pre-commit): autofix fix start error Signed-off-by: Berkay <berkay@leodrive.ai> update readme Signed-off-by: Berkay <berkay@leodrive.ai> update readme Signed-off-by: Berkay <berkay@leodrive.ai> ci(pre-commit): autofix fix cpplint error Signed-off-by: Berkay <berkay@leodrive.ai> ci(pre-commit): autofix fix checklist error Signed-off-by: Berkay <berkay@leodrive.ai> fixed lint errors Signed-off-by: Berkay <berkay@leodrive.ai> ci(pre-commit): autofix
brkay54 · May 24, 2022 · c91b8a5 · c91b8a5
1 parent 003c476
commit c91b8a5
Show file tree

Hide file tree

Showing 15 changed files with 1,134 additions and 250 deletions.
diff --git a/control/control_performance_analysis/CMakeLists.txt b/control/control_performance_analysis/CMakeLists.txt
@@ -4,47 +4,62 @@ project(control_performance_analysis)
 find_package(autoware_cmake REQUIRED)
 autoware_package()
 
+
 find_package(Boost REQUIRED)
 
 find_package(eigen3_cmake_module REQUIRED)
 find_package(Eigen3 REQUIRED)
 
-rosidl_generate_interfaces(control_performance_analysis_msgs
-  msg/Error.msg
-  msg/ErrorStamped.msg
-  DEPENDENCIES
-    std_msgs
+find_package(builtin_interfaces REQUIRED)
+find_package(rosidl_default_generators REQUIRED)
+find_package(std_msgs REQUIRED)
+
+rosidl_generate_interfaces(
+        control_performance_analysis
+        "msg/Error.msg"
+        "msg/ErrorStamped.msg"
+        "msg/DrivingMonitorStamped.msg"
+        "msg/FloatStamped.msg"
+        DEPENDENCIES builtin_interfaces std_msgs
 )
 
-ament_auto_add_library(control_performance_analysis_core SHARED
-  src/control_performance_analysis_utils.cpp
-  src/control_performance_analysis_core.cpp
+ament_auto_add_library(
+        control_performance_analysis_core SHARED
+        src/control_performance_analysis_utils.cpp
+        src/control_performance_analysis_core.cpp
 )
 
-ament_auto_add_library(control_performance_analysis_node SHARED
-  src/control_performance_analysis_node.cpp
+ament_auto_add_library(
+        control_performance_analysis_node SHARED
+        src/control_performance_analysis_node.cpp
 )
 
 if(${rosidl_cmake_VERSION} VERSION_LESS 2.5.0)
-  rosidl_target_interfaces(control_performance_analysis_node
-    control_performance_analysis_msgs "rosidl_typesupport_cpp")
+    rosidl_target_interfaces(control_performance_analysis_node
+            control_performance_analysis "rosidl_typesupport_cpp")
+    rosidl_target_interfaces(control_performance_analysis_core
+            control_performance_analysis "rosidl_typesupport_cpp")
 else()
-  rosidl_get_typesupport_target(
-    cpp_typesupport_target control_performance_analysis_msgs "rosidl_typesupport_cpp")
-  target_link_libraries(control_performance_analysis_node "${cpp_typesupport_target}")
+    rosidl_get_typesupport_target(
+            cpp_typesupport_target control_performance_analysis "rosidl_typesupport_cpp")
+    target_link_libraries(control_performance_analysis_node "${cpp_typesupport_target}")
+    target_link_libraries(control_performance_analysis_core "${cpp_typesupport_target}")
+
 endif()
 
-target_link_libraries(control_performance_analysis_node
-  control_performance_analysis_core
+target_link_libraries(
+        control_performance_analysis_node
+        control_performance_analysis_core
 )
 
-rclcpp_components_register_node(control_performance_analysis_node
-  PLUGIN "control_performance_analysis::ControlPerformanceAnalysisNode"
-  EXECUTABLE control_performance_analysis
+rclcpp_components_register_node(
+        control_performance_analysis_node
+        PLUGIN "control_performance_analysis::ControlPerformanceAnalysisNode"
+        EXECUTABLE control_performance_analysis_exe
 )
 
 ament_auto_package(
-  INSTALL_TO_SHARE
-    launch
-    config
+        INSTALL_TO_SHARE
+        launch
+        config
 )
diff --git a/control/control_performance_analysis/README.md b/control/control_performance_analysis/README.md
@@ -2,27 +2,92 @@
 
 ## Purpose
 
-`control_performance_analysis` is the package to analyze the tracking performance of a control module.
+`control_performance_analysis` is the package to analyze the tracking performance of a control module and monitor the driving status of the vehicle.
 
 This package is used as a tool to quantify the results of the control module.
 That's why it doesn't interfere with the core logic of autonomous driving.
 
 Based on the various input from planning, control, and vehicle, it publishes the result of analysis as `control_performance_analysis::msg::ErrorStamped` defined in this package.
 
+All results in `ErrorStamped` message are calculated in Frenet Frame of curve. Errors and velocity errors are calculated by using paper below.
+
+`Werling, Moritz & Groell, Lutz & Bretthauer, Georg. (2010). Invariant Trajectory Tracking With a Full-Size Autonomous Road Vehicle. Robotics, IEEE Transactions on. 26. 758 - 765. 10.1109/TRO.2010.2052325.`
+
+If you are interested in calculations, you can see the error and error velocity calculations in section `C. Asymptotical Trajectory Tracking With Orientation Control`.
+
+Error acceleration calculations are made based on the velocity calculations above. You can see below the calculation of error acceleration.
+
+![CodeCogsEqn](https://user-images.githubusercontent.com/45468306/169027099-ef15b306-2868-4084-a350-0e2b652c310f.png)
+
 ## Input / Output
 
 ### Input topics
 
-| Name                                               | Type                                                     | Description                                         |
-| -------------------------------------------------- | -------------------------------------------------------- | --------------------------------------------------- |
-| `/planning/scenario_planning/trajectory`           | autoware_auto_planning_msgs::msg::Trajectory             | Output trajectory from planning module.             |
-| `/control/trajectory_follower/lateral/control_cmd` | autoware_auto_control_msgs::msg::AckermannLateralCommand | Output lateral control command from control module. |
-| `/vehicle/status/steering_status`                  | autoware_auto_vehicle_msgs::msg::SteeringReport          | Steering information from vehicle.                  |
-| `/localization/kinematic_state`                    | nav_msgs::msg::Odometry                                  | Use twist from odometry.                            |
-| `/tf`                                              | tf2_msgs::msg::TFMessage                                 | Extract ego pose from tf.                           |
+| Name                                     | Type                                                     | Description                                 |
+| ---------------------------------------- | -------------------------------------------------------- | ------------------------------------------- |
+| `/planning/scenario_planning/trajectory` | autoware_auto_planning_msgs::msg::Trajectory             | Output trajectory from planning module.     |
+| `/control/command/control_cmd`           | autoware_auto_control_msgs::msg::AckermannControlCommand | Output control command from control module. |
+| `/vehicle/status/steering_status`        | autoware_auto_vehicle_msgs::msg::SteeringReport          | Steering information from vehicle.          |
+| `/localization/kinematic_state`          | nav_msgs::msg::Odometry                                  | Use twist from odometry.                    |
+| `/tf`                                    | tf2_msgs::msg::TFMessage                                 | Extract ego pose from tf.                   |
 
 ### Output topics
 
-| Name                                    | Type                                            | Description                             |
-| --------------------------------------- | ----------------------------------------------- | --------------------------------------- |
-| `/control_performance/performance_vars` | control_performance_analysis::msg::ErrorStamped | The result of the performance analysis. |
+| Name                                    | Type                                                     | Description                                         |
+| --------------------------------------- | -------------------------------------------------------- | --------------------------------------------------- |
+| `/control_performance/performance_vars` | control_performance_analysis::msg::ErrorStamped          | The result of the performance analysis.             |
+| `/control_performance/driving_status`   | control_performance_analysis::msg::DrivingMonitorStamped | Driving status (acceleration, jerk etc.) monitoring |
+
+### Outputs
+
+#### control_performance_analysis::msg::DrivingMonitorStamped
+
+| Name                         | Type  | Description                                              |
+| ---------------------------- | ----- | -------------------------------------------------------- |
+| `longitudinal_acceleration`  | float | [m / s^2]                                                |
+| `longitudinal_jerk`          | float | [m / s^3]                                                |
+| `lateral_acceleration`       | float | [m / s^2]                                                |
+| `lateral_jerk`               | float | [m / s^3]                                                |
+| `controller_processing_time` | float | Timestamp between last two control command messages [ms] |
+
+#### control_performance_analysis::msg::ErrorStamped
+
+| Name                                       | Type  | Description                                                                                                       |
+| ------------------------------------------ | ----- | ----------------------------------------------------------------------------------------------------------------- |
+| `lateral_error`                            | float | [m]                                                                                                               |
+| `lateral_error_velocity`                   | float | [m / s]                                                                                                           |
+| `lateral_error_acceleration`               | float | [m / s^2]                                                                                                         |
+| `longitudinal_error`                       | float | [m]                                                                                                               |
+| `longitudinal_error_velocity`              | float | [m / s]                                                                                                           |
+| `longitudinal_error_acceleration`          | float | [m / s^2]                                                                                                         |
+| `heading_error`                            | float | [rad]                                                                                                             |
+| `heading_error_velocity`                   | float | [rad / s]                                                                                                         |
+| `control_effort_energy`                    | float | [u * R * u^T]                                                                                                     |
+| `error_energy`                             | float | lateral_error^2 + heading_error^2                                                                                 |
+| `value_approximation`                      | float | V = xPx' ; Value function from DARE Lyap matrix P                                                                 |
+| `curvature_estimate`                       | float | [1 / m]                                                                                                           |
+| `curvature_estimate_pp`                    | float | [1 / m]                                                                                                           |
+| `vehicle_velocity_error`                   | float | [m / s]                                                                                                           |
+| `tracking_curvature_discontinuity_ability` | float | Measures the ability to tracking the curvature changes [`abs(delta(curvature)) / (1 + abs(delta(lateral_error))`] |
+
+## Parameters
+
+| Name                               | Type             | Description                                                     |
+| ---------------------------------- | ---------------- | --------------------------------------------------------------- |
+| `curvature_interval_length`        | double           | Used for estimating current curvature                           |
+| `prevent_zero_division_value`      | double           | Value to avoid zero division. Default is `0.001`                |
+| `odom_interval`                    | unsigned integer | Interval between odom messages, increase it for smoother curve. |
+| `acceptable_min_waypoint_distance` | double           | How far the next waypoint can be ahead of the vehicle direction |
+
+## Usage
+
+- After launched simulation and control module, launch the `control_performance_analysis.launch.xml`.
+- You should be able to see the driving monitor and error variables in topics.
+- If you want to visualize the results, you can use `Plotjuggler` and use `config/controller_monitor.xml` as layout.
+- After import the layout, please specify the topics that are listed below.
+
+> - /localization/kinematic_state
+> - /control_performance/driving_status
+> - /control_performance/performance_vars
+
+- In `Plotjuggler` you can export the statistic (max, min, average) values as csv file. Use that statistics to compare the control modules.
diff --git a/control/control_performance_analysis/config/control_performance_analysis.param.yaml b/control/control_performance_analysis/config/control_performance_analysis.param.yaml
@@ -1,5 +1,7 @@
 /**:
   ros__parameters:
     # -- publishing period --
-    control_period: 0.033
-    double curvature_interval_length_: 5.0
+    curvature_interval_length_: 5.0
+    prevent_zero_division_value: 0.001
+    odom_interval_: 3
+    acceptable_min_waypoint_distance: 2.0