Skip to content

Commit

Permalink
ci(pre-commit): autofix
Browse files Browse the repository at this point in the history
  • Loading branch information
@pre-commit-ci @brkay54
pre-commit-ci[bot] authored and brkay54 committed May 20, 2022
1 parent f84807d commit f4168cf
Show file tree
Hide file tree
Showing 2 changed files with 25 additions and 27 deletions.
51 changes: 25 additions & 26 deletions control/control_performance_analysis/README.md
View file
Original file line number Diff line number Diff line change
Expand Up @@ -11,16 +11,14 @@ 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`.

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
Expand Down Expand Up @@ -52,45 +50,46 @@ 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

- 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.
1 change: 0 additions & 1 deletion control/control_performance_analysis/src/control_performance_analysis_core.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -147,7 +147,6 @@ std::pair<bool, int32_t> 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_)
Expand Down

0 comments on commit f4168cf

Please sign in to comment.