diff --git a/control/control_performance_analysis/README.md b/control/control_performance_analysis/README.md index 72e076e32e2fd..9f36ae5c04ae8 100644 --- a/control/control_performance_analysis/README.md +++ b/control/control_performance_analysis/README.md @@ -11,7 +11,7 @@ Based on the various input from planning, control, and vehicle, it publishes the 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. ` +`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`. @@ -19,8 +19,6 @@ Error acceleration calculations are made based on the velocity calculations abov ![CodeCogsEqn](https://user-images.githubusercontent.com/45468306/169027099-ef15b306-2868-4084-a350-0e2b652c310f.png) - - ## Input / Output ### Input topics @@ -52,35 +50,34 @@ Error acceleration calculations are made based on the velocity calculations abov | `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] | +| 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 | +| 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 @@ -88,9 +85,11 @@ Error acceleration calculations are made based on the velocity calculations abov - 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/src/control_performance_analysis_core.cpp b/control/control_performance_analysis/src/control_performance_analysis_core.cpp index 96f2c9f4da65b..3456338c309c0 100644 --- a/control/control_performance_analysis/src/control_performance_analysis_core.cpp +++ b/control/control_performance_analysis/src/control_performance_analysis_core.cpp @@ -147,7 +147,6 @@ std::pair ControlPerformanceAnalysisCore::findClosestPrevWayPoint int32_t length_of_trajectory = std::distance(current_waypoints_ptr_->poses.cbegin(), current_waypoints_ptr_->poses.cend()); - return ((min_distance_ds <= acceptable_min_waypoint_distance_) && (*idx_prev_wp_ >= 0) && (*idx_prev_wp_ <= length_of_trajectory)) ? std::make_pair(true, *idx_prev_wp_)