From 9aae0a9b50ed8203b29415363f8e8e3813fec5ae Mon Sep 17 00:00:00 2001 From: kyoichi-sugahara Date: Mon, 16 Oct 2023 09:06:55 +0900 Subject: [PATCH 1/2] minor refactor for updateOffset Signed-off-by: kyoichi-sugahara --- .../steering_offset/steering_offset.hpp | 2 +- .../src/steering_offset/steering_offset.cpp | 11 +++++++---- 2 files changed, 8 insertions(+), 5 deletions(-) diff --git a/control/mpc_lateral_controller/include/mpc_lateral_controller/steering_offset/steering_offset.hpp b/control/mpc_lateral_controller/include/mpc_lateral_controller/steering_offset/steering_offset.hpp index 494961ef679d2..93ae096403629 100644 --- a/control/mpc_lateral_controller/include/mpc_lateral_controller/steering_offset/steering_offset.hpp +++ b/control/mpc_lateral_controller/include/mpc_lateral_controller/steering_offset/steering_offset.hpp @@ -30,7 +30,7 @@ class SteeringOffsetEstimator ~SteeringOffsetEstimator() = default; double getOffset() const; - void updateOffset(const geometry_msgs::msg::Twist & twist, const double steering); + void updateOffset(const geometry_msgs::msg::Twist & twist, const double measured_steering_angle); private: // parameters diff --git a/control/mpc_lateral_controller/src/steering_offset/steering_offset.cpp b/control/mpc_lateral_controller/src/steering_offset/steering_offset.cpp index 60d4dd7901394..701100eb7768e 100644 --- a/control/mpc_lateral_controller/src/steering_offset/steering_offset.cpp +++ b/control/mpc_lateral_controller/src/steering_offset/steering_offset.cpp @@ -30,16 +30,19 @@ SteeringOffsetEstimator::SteeringOffsetEstimator( } void SteeringOffsetEstimator::updateOffset( - const geometry_msgs::msg::Twist & twist, const double steering) + const geometry_msgs::msg::Twist & twist, const double measured_steering_angle) { const bool update_offset = (std::abs(twist.linear.x) > update_vel_threshold_ && - std::abs(steering) < update_steer_threshold_); + std::abs(measured_steering_angle) < update_steer_threshold_); if (!update_offset) return; - const auto steer_angvel = std::atan2(twist.angular.z * wheelbase_, twist.linear.x); - const auto steer_offset = steering - steer_angvel; + // calculate the steering angle needed to achieve the current rate of rotation given the current + // velocity along the x-axis of the vehicle, under the below assumptions + // (i.e., no slipping, no lag in the steering mechanism, etc.). + const auto expected_steering_angle = std::atan2(twist.angular.z * wheelbase_, twist.linear.x); + const auto steer_offset = measured_steering_angle - expected_steering_angle; steering_offset_storage_.push_back(steer_offset); if (steering_offset_storage_.size() > average_num_) { steering_offset_storage_.pop_front(); From 1290540803707d23088338072a54eec352c0b3d3 Mon Sep 17 00:00:00 2001 From: Kyoichi Sugahara Date: Wed, 18 Oct 2023 14:28:25 +0900 Subject: [PATCH 2/2] Update control/mpc_lateral_controller/src/steering_offset/steering_offset.cpp Co-authored-by: Takamasa Horibe --- .../src/steering_offset/steering_offset.cpp | 7 ++++--- 1 file changed, 4 insertions(+), 3 deletions(-) diff --git a/control/mpc_lateral_controller/src/steering_offset/steering_offset.cpp b/control/mpc_lateral_controller/src/steering_offset/steering_offset.cpp index 701100eb7768e..a4ecaea3de50e 100644 --- a/control/mpc_lateral_controller/src/steering_offset/steering_offset.cpp +++ b/control/mpc_lateral_controller/src/steering_offset/steering_offset.cpp @@ -38,9 +38,10 @@ void SteeringOffsetEstimator::updateOffset( if (!update_offset) return; - // calculate the steering angle needed to achieve the current rate of rotation given the current - // velocity along the x-axis of the vehicle, under the below assumptions - // (i.e., no slipping, no lag in the steering mechanism, etc.). + // Assuming the yaw rate and speed are sufficiently accurate, we calculate the expected steering + // angle for the current yaw rate and speed from the vehicle model. To ignore dynamic + // characteristics such as steering delay or slip, estimation is performed only when the vehicle + // is driving at high speed and the steering angle is close to zero position. const auto expected_steering_angle = std::atan2(twist.angular.z * wheelbase_, twist.linear.x); const auto steer_offset = measured_steering_angle - expected_steering_angle; steering_offset_storage_.push_back(steer_offset);