feat(control_performance_analysis): add low pass filter to control_pe…

…rformance_analysis tool (autowarefoundation#1099) * feat(control_performance_analysis): add low pass filter to control_performance_analysis tool Signed-off-by: Berkay <[email protected]> * ci(pre-commit): autofix * Member variables are suffixed by an underscore Signed-off-by: Berkay <[email protected]> Co-authored-by: Berkay <[email protected]> Co-authored-by: pre-commit-ci[bot] <66853113+pre-commit-ci[bot]@users.noreply.github.com>
Interplai · Jun 21, 2022 · 1388dfd · 1388dfd
1 parent 1110bc4
commit 1388dfd
Show file tree

Hide file tree

Showing 5 changed files with 82 additions and 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
@@ -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/...mance_analysis/include/control_performance_analysis/control_performance_analysis_core.hpp b/...mance_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<PoseArray> current_waypoints_ptr_;

diff --git a/...mance_analysis/include/control_performance_analysis/control_performance_analysis_node.hpp b/...mance_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
@@ -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<msg::ErrorStamped>();
@@ -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<msg::ErrorStamped>(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<msg::DrivingMonitorStamped>(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
@@ -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<ControlPerformanceAnalysisCore>(
     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<Trajectory>(