feat(control_performance_analysis): add new functionalities to evalua…

…te the control modules(#567) Signed-off-by: Berkay <[email protected]> evaluation node updated(#567) message files arranged, structure changed(#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 <[email protected]> ci(pre-commit): autofix odom history interval param added (#567) Signed-off-by: Berkay <[email protected]> ci(pre-commit): autofix added steering stamp rate Signed-off-by: Berkay <[email protected]> added steering stamp state deleted min treshold of dt Signed-off-by: Berkay <[email protected]> ci(pre-commit): autofix fix build Signed-off-by: Berkay <[email protected]> added plot config file Signed-off-by: Berkay <[email protected]> ci(pre-commit): autofix readme updated Signed-off-by: Berkay <[email protected]> ci(pre-commit): autofix fix cmakelist and msg files Signed-off-by: Berkay <[email protected]> ci(pre-commit): autofix adjusted license Signed-off-by: Berkay <[email protected]> fix build error Signed-off-by: Berkay <[email protected]> fix lint_cmake error Signed-off-by: Berkay <[email protected]> ci(pre-commit): autofix fix cmakelist build error Signed-off-by: Berkay <[email protected]> cmake error fix Signed-off-by: Berkay <[email protected]> ci(pre-commit): autofix update readme Signed-off-by: Berkay <[email protected]> ci(pre-commit): autofix fix typesupport error Signed-off-by: Berkay <[email protected]> fix cmake Signed-off-by: Berkay <[email protected]> errors revised Signed-off-by: Berkay <[email protected]> update error msg Signed-off-by: Berkay <[email protected]> ci(pre-commit): autofix added interpolated velocity Signed-off-by: Berkay <[email protected]> ci(pre-commit): autofix structure changed Signed-off-by: Berkay <[email protected]> error accelerations added Signed-off-by: Berkay <[email protected]> fix error goes infinity issue Signed-off-by: Berkay <[email protected]> ci(pre-commit): autofix fixed bugs and changed params Signed-off-by: Berkay <[email protected]> ci(pre-commit): autofix fix start error Signed-off-by: Berkay <[email protected]> update readme Signed-off-by: Berkay <[email protected]> update readme Signed-off-by: Berkay <[email protected]> ci(pre-commit): autofix fix cpplint error Signed-off-by: Berkay <[email protected]> ci(pre-commit): autofix fix checklist error Signed-off-by: Berkay <[email protected]> fixed lint errors Signed-off-by: Berkay <[email protected]> ci(pre-commit): autofix
autowarefoundation · 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