Skip to content

Commit

Permalink
feat(control_performance_analysis): add new functionalities to evalua…
Browse files Browse the repository at this point in the history 
…te the control modules (tier4#659)

Signed-off-by: Berkay <[email protected]>

Co-authored-by: M. Fatih Cırıt <[email protected]>
  • Loading branch information
@brkay54 @xmfcx @boyali
2 people authored and boyali committed Oct 19, 2022
1 parent fa4b050 commit 155c552
Show file tree
Hide file tree
Showing 15 changed files with 1,133 additions and 250 deletions.
60 changes: 37 additions & 23 deletions control/control_performance_analysis/CMakeLists.txt
View file
Original file line number Diff line number Diff line change
Expand Up @@ -4,6 +4,7 @@ project(control_performance_analysis)
find_package(autoware_cmake REQUIRED)
autoware_package()


find_package(Boost REQUIRED)

find_package(eigen3_cmake_module REQUIRED)
Expand All @@ -13,43 +14,56 @@ include_directories(
SYSTEM
${EIGEN3_INCLUDE_DIR}
)
find_package(builtin_interfaces REQUIRED)
find_package(rosidl_default_generators REQUIRED)
find_package(std_msgs REQUIRED)

rosidl_generate_interfaces(control_performance_analysis_msgs
msg/Error.msg
msg/ErrorStamped.msg
DEPENDENCIES
std_msgs
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
)
87 changes: 76 additions & 11 deletions control/control_performance_analysis/README.md
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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.
6 changes: 4 additions & 2 deletions control/control_performance_analysis/config/control_performance_analysis.param.yaml
View file
Original file line number Diff line number Diff line change
@@ -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
Loading

0 comments on commit 155c552

Please sign in to comment.