From 1388dfd9e47b8c9ddc8c5b09ee9941e17537ed1f Mon Sep 17 00:00:00 2001 From: Berkay Karaman Date: Tue, 21 Jun 2022 14:26:35 +0300 Subject: [PATCH] feat(control_performance_analysis): add low pass filter to control_performance_analysis tool (#1099) * feat(control_performance_analysis): add low pass filter to control_performance_analysis tool Signed-off-by: Berkay * ci(pre-commit): autofix * Member variables are suffixed by an underscore Signed-off-by: Berkay Co-authored-by: Berkay Co-authored-by: pre-commit-ci[bot] <66853113+pre-commit-ci[bot]@users.noreply.github.com> --- .../control_performance_analysis.param.yaml | 3 +- .../control_performance_analysis_core.hpp | 4 +- .../control_performance_analysis_node.hpp | 1 + .../src/control_performance_analysis_core.cpp | 76 ++++++++++++++++++- .../src/control_performance_analysis_node.cpp | 3 +- 5 files changed, 82 insertions(+), 5 deletions(-) diff --git a/control/control_performance_analysis/config/control_performance_analysis.param.yaml b/control/control_performance_analysis/config/control_performance_analysis.param.yaml index cca4a02653690..6ff3b82dc0925 100644 --- a/control/control_performance_analysis/config/control_performance_analysis.param.yaml +++ b/control/control_performance_analysis/config/control_performance_analysis.param.yaml @@ -3,5 +3,6 @@ # -- publishing period -- curvature_interval_length_: 5.0 prevent_zero_division_value: 0.001 - odom_interval_: 3 + odom_interval_: 1 acceptable_min_waypoint_distance: 2.0 + low_pass_filter_gain: 0.95 diff --git a/control/control_performance_analysis/include/control_performance_analysis/control_performance_analysis_core.hpp b/control/control_performance_analysis/include/control_performance_analysis/control_performance_analysis_core.hpp index 711301271cb53..e74c7e215fae8 100644 --- a/control/control_performance_analysis/include/control_performance_analysis/control_performance_analysis_core.hpp +++ b/control/control_performance_analysis/include/control_performance_analysis/control_performance_analysis_core.hpp @@ -56,7 +56,8 @@ class ControlPerformanceAnalysisCore ControlPerformanceAnalysisCore(); ControlPerformanceAnalysisCore( double wheelbase, double curvature_interval_length, uint odom_interval, - double acceptable_min_waypoint_distance, double prevent_zero_division_value); + double acceptable_min_waypoint_distance, double prevent_zero_division_value, + double lpf_gain_val); // Setters void setCurrentPose(const Pose & msg); @@ -89,6 +90,7 @@ class ControlPerformanceAnalysisCore uint odom_interval_; double acceptable_min_waypoint_distance_; double prevent_zero_division_value_; + double lpf_gain_; // Variables Received Outside std::shared_ptr current_waypoints_ptr_; diff --git a/control/control_performance_analysis/include/control_performance_analysis/control_performance_analysis_node.hpp b/control/control_performance_analysis/include/control_performance_analysis/control_performance_analysis_node.hpp index a47ebc45b3a74..ff3f75ebf1475 100644 --- a/control/control_performance_analysis/include/control_performance_analysis/control_performance_analysis_node.hpp +++ b/control/control_performance_analysis/include/control_performance_analysis/control_performance_analysis_node.hpp @@ -52,6 +52,7 @@ struct Param double curvature_interval_length; double prevent_zero_division_value; uint odom_interval; // Increase it for smoother curve + double lpf_gain; // How far the next waypoint can be ahead of the vehicle direction. 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 79e0ccd86dd26..3fe0752905cc4 100644 --- a/control/control_performance_analysis/src/control_performance_analysis_core.cpp +++ b/control/control_performance_analysis/src/control_performance_analysis_core.cpp @@ -33,16 +33,18 @@ ControlPerformanceAnalysisCore::ControlPerformanceAnalysisCore() : wheelbase_{2. curvature_interval_length_ = 10.0; acceptable_min_waypoint_distance_ = 2.0; prevent_zero_division_value_ = 0.001; + lpf_gain_ = 0.8; } ControlPerformanceAnalysisCore::ControlPerformanceAnalysisCore( double wheelbase, double curvature_interval_length, uint odom_interval, - double acceptable_min_waypoint_distance, double prevent_zero_division_value) + double acceptable_min_waypoint_distance, double prevent_zero_division_value, double lpf_gain_val) : wheelbase_{wheelbase}, curvature_interval_length_{curvature_interval_length}, odom_interval_{odom_interval}, acceptable_min_waypoint_distance_{acceptable_min_waypoint_distance}, - prevent_zero_division_value_{prevent_zero_division_value} + prevent_zero_division_value_{prevent_zero_division_value}, + lpf_gain_{lpf_gain_val} { // prepare control performance struct prev_target_vars_ = std::make_unique(); @@ -312,6 +314,53 @@ bool ControlPerformanceAnalysisCore::calculateErrorVars() (std::fabs(curvature_est - prev_target_vars_->error.curvature_estimate)) / (1 + std::fabs(lateral_error - prev_target_vars_->error.lateral_error)); + if (prev_target_vars_) { + // LPF for error vars + + error_vars.error.curvature_estimate = lpf_gain_ * prev_target_vars_->error.curvature_estimate + + (1 - lpf_gain_) * error_vars.error.curvature_estimate; + + error_vars.error.curvature_estimate_pp = + lpf_gain_ * prev_target_vars_->error.curvature_estimate_pp + + (1 - lpf_gain_) * error_vars.error.curvature_estimate_pp; + + error_vars.error.lateral_error = lpf_gain_ * prev_target_vars_->error.lateral_error + + (1 - lpf_gain_) * error_vars.error.lateral_error; + + error_vars.error.lateral_error_velocity = + lpf_gain_ * prev_target_vars_->error.lateral_error_velocity + + (1 - lpf_gain_) * error_vars.error.lateral_error_velocity; + + error_vars.error.lateral_error_acceleration = + lpf_gain_ * prev_target_vars_->error.lateral_error_acceleration + + (1 - lpf_gain_) * error_vars.error.lateral_error_acceleration; + + error_vars.error.longitudinal_error = lpf_gain_ * prev_target_vars_->error.longitudinal_error + + (1 - lpf_gain_) * error_vars.error.longitudinal_error; + + error_vars.error.longitudinal_error_velocity = + lpf_gain_ * prev_target_vars_->error.longitudinal_error_velocity + + (1 - lpf_gain_) * error_vars.error.longitudinal_error_velocity; + + error_vars.error.longitudinal_error_acceleration = + lpf_gain_ * prev_target_vars_->error.longitudinal_error_acceleration + + (1 - lpf_gain_) * error_vars.error.longitudinal_error_acceleration; + + error_vars.error.heading_error = lpf_gain_ * prev_target_vars_->error.heading_error + + (1 - lpf_gain_) * error_vars.error.heading_error; + + error_vars.error.heading_error_velocity = + lpf_gain_ * prev_target_vars_->error.heading_error_velocity + + (1 - lpf_gain_) * error_vars.error.heading_error_velocity; + + error_vars.error.control_effort_energy = + lpf_gain_ * prev_target_vars_->error.control_effort_energy + + (1 - lpf_gain_) * error_vars.error.control_effort_energy; + + error_vars.error.error_energy = lpf_gain_ * prev_target_vars_->error.error_energy + + (1 - lpf_gain_) * error_vars.error.error_energy; + } + prev_target_vars_ = std::make_unique(error_vars); return true; @@ -373,6 +422,29 @@ bool ControlPerformanceAnalysisCore::calculateDrivingVars() rclcpp::Time(prev_driving_vars_->longitudinal_acceleration.header.stamp) + duration * 0.5); // Time stamp of jerk data } + if (prev_driving_vars_) { + // LPF for driving status vars + + driving_status_vars.longitudinal_acceleration.data = + lpf_gain_ * prev_driving_vars_->longitudinal_acceleration.data + + (1 - lpf_gain_) * driving_status_vars.longitudinal_acceleration.data; + + driving_status_vars.lateral_acceleration.data = + lpf_gain_ * prev_driving_vars_->lateral_acceleration.data + + (1 - lpf_gain_) * driving_status_vars.lateral_acceleration.data; + + driving_status_vars.lateral_jerk.data = + lpf_gain_ * prev_driving_vars_->lateral_jerk.data + + (1 - lpf_gain_) * driving_status_vars.lateral_jerk.data; + + driving_status_vars.longitudinal_jerk.data = + lpf_gain_ * prev_driving_vars_->longitudinal_jerk.data + + (1 - lpf_gain_) * driving_status_vars.longitudinal_jerk.data; + + driving_status_vars.controller_processing_time.data = + lpf_gain_ * prev_driving_vars_->controller_processing_time.data + + (1 - lpf_gain_) * driving_status_vars.controller_processing_time.data; + } prev_driving_vars_ = std::move(std::make_unique(driving_status_vars)); diff --git a/control/control_performance_analysis/src/control_performance_analysis_node.cpp b/control/control_performance_analysis/src/control_performance_analysis_node.cpp index 58c56fea90a47..ef95d6ed3d314 100644 --- a/control/control_performance_analysis/src/control_performance_analysis_node.cpp +++ b/control/control_performance_analysis/src/control_performance_analysis_node.cpp @@ -50,11 +50,12 @@ ControlPerformanceAnalysisNode::ControlPerformanceAnalysisNode( param_.odom_interval = declare_parameter("odom_interval", 2); param_.acceptable_min_waypoint_distance = declare_parameter("acceptable_min_waypoint_distance", 2.0); + param_.lpf_gain = declare_parameter("low_pass_filter_gain", 0.8); // Prepare error computation class with the wheelbase parameter. control_performance_core_ptr_ = std::make_unique( param_.wheel_base, param_.curvature_interval_length, param_.odom_interval, - param_.acceptable_min_waypoint_distance, param_.prevent_zero_division_value); + param_.acceptable_min_waypoint_distance, param_.prevent_zero_division_value, param_.lpf_gain); // Subscribers. sub_trajectory_ = create_subscription(