From d2ede12c41b13633253e416acb80f22ad2d3b8a0 Mon Sep 17 00:00:00 2001 From: Lewie Donckers Date: Wed, 9 Feb 2022 16:31:02 +0000 Subject: [PATCH] Replaced member variable with local. --- include/path_tracking_pid/controller.hpp | 3 --- src/controller.cpp | 24 ++++++++++++------------ 2 files changed, 12 insertions(+), 15 deletions(-) diff --git a/include/path_tracking_pid/controller.hpp b/include/path_tracking_pid/controller.hpp index 82a8713a..ee57650f 100644 --- a/include/path_tracking_pid/controller.hpp +++ b/include/path_tracking_pid/controller.hpp @@ -326,9 +326,6 @@ class Controller : private details::NoCopyNoMove double c_lat_ = 1.; double c_ang_ = 1.; - // Used to check for tan(0)==>NaN in the filter calculation - double tan_filt_ = 1.; - // MPC settings int mpc_max_fwd_iter_; // Define # of steps that you look into the future with MPC [-] int mpc_max_vel_optimization_iter_; // Set maximum # of velocity bisection iterations diff --git a/src/controller.cpp b/src/controller.cpp index 74418984..b3c46910 100644 --- a/src/controller.cpp +++ b/src/controller.cpp @@ -515,28 +515,28 @@ geometry_msgs::Twist Controller::update(const double target_x_vel, if (cutoff_frequency_lat_ != -1) { // Check if tan(_) is really small, could cause c = NaN - tan_filt_ = tan((cutoff_frequency_lat_ * 6.2832) * dt.toSec() / 2); + auto tan_filt = tan((cutoff_frequency_lat_ * 6.2832) * dt.toSec() / 2); // Avoid tan(0) ==> NaN - if ((tan_filt_ <= 0.) && (tan_filt_ > -0.01)) - tan_filt_ = -0.01; - if ((tan_filt_ >= 0.) && (tan_filt_ < 0.01)) - tan_filt_ = 0.01; + if ((tan_filt <= 0.) && (tan_filt > -0.01)) + tan_filt = -0.01; + if ((tan_filt >= 0.) && (tan_filt < 0.01)) + tan_filt = 0.01; - c_lat_ = 1 / tan_filt_; + c_lat_ = 1 / tan_filt; } if (cutoff_frequency_ang_ != -1) { // Check if tan(_) is really small, could cause c = NaN - tan_filt_ = tan((cutoff_frequency_ang_ * 6.2832) * dt.toSec() / 2); + auto tan_filt = tan((cutoff_frequency_ang_ * 6.2832) * dt.toSec() / 2); // Avoid tan(0) ==> NaN - if ((tan_filt_ <= 0.) && (tan_filt_ > -0.01)) - tan_filt_ = -0.01; - if ((tan_filt_ >= 0.) && (tan_filt_ < 0.01)) - tan_filt_ = 0.01; + if ((tan_filt <= 0.) && (tan_filt > -0.01)) + tan_filt = -0.01; + if ((tan_filt >= 0.) && (tan_filt < 0.01)) + tan_filt = 0.01; - c_ang_ = 1 / tan_filt_; + c_ang_ = 1 / tan_filt; } controller_state_.filtered_error_lat.at(2) = controller_state_.filtered_error_lat.at(1);