From dfa07a19bfb02174aa6421155e3a2e0036841b76 Mon Sep 17 00:00:00 2001 From: Rein Appeldoorn Date: Tue, 10 Sep 2024 16:37:02 +0200 Subject: [PATCH 1/3] fix(steering-odometry): handle infinite turning radius properly --- .../src/steering_odometry.cpp | 14 ++++++++++---- 1 file changed, 10 insertions(+), 4 deletions(-) diff --git a/steering_controllers_library/src/steering_odometry.cpp b/steering_controllers_library/src/steering_odometry.cpp index ba431faf33..007cc27edc 100644 --- a/steering_controllers_library/src/steering_odometry.cpp +++ b/steering_controllers_library/src/steering_odometry.cpp @@ -133,11 +133,17 @@ double SteeringOdometry::get_linear_velocity_double_traction_axle( const double steer_pos) { double turning_radius = wheelbase_ / std::tan(steer_pos); + double vel_wheel_r = right_traction_wheel_vel * wheel_radius_; + double vel_wheel_l = left_traction_wheel_vel * wheel_radius_; + + if (std::isinf(turning_radius)) + { + return (vel_wheel_r + vel_wheel_l) * 0.5; + } + // overdetermined, we take the average - double vel_r = right_traction_wheel_vel * wheel_radius_ * turning_radius / - (turning_radius + wheel_track_ * 0.5); - double vel_l = left_traction_wheel_vel * wheel_radius_ * turning_radius / - (turning_radius - wheel_track_ * 0.5); + double vel_r = vel_wheel_r * turning_radius / (turning_radius + wheel_track_ * 0.5); + double vel_l = vel_wheel_l * turning_radius / (turning_radius - wheel_track_ * 0.5); return (vel_r + vel_l) * 0.5; } From fdcfe1ebbaa2da49fdff38f550c4c716f364b6e7 Mon Sep 17 00:00:00 2001 From: Rein Appeldoorn Date: Wed, 11 Sep 2024 08:38:15 +0200 Subject: [PATCH 2/3] Update steering_controllers_library/src/steering_odometry.cpp Co-authored-by: Sai Kishor Kothakota --- steering_controllers_library/src/steering_odometry.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/steering_controllers_library/src/steering_odometry.cpp b/steering_controllers_library/src/steering_odometry.cpp index 007cc27edc..6100914aac 100644 --- a/steering_controllers_library/src/steering_odometry.cpp +++ b/steering_controllers_library/src/steering_odometry.cpp @@ -133,8 +133,8 @@ double SteeringOdometry::get_linear_velocity_double_traction_axle( const double steer_pos) { double turning_radius = wheelbase_ / std::tan(steer_pos); - double vel_wheel_r = right_traction_wheel_vel * wheel_radius_; - double vel_wheel_l = left_traction_wheel_vel * wheel_radius_; + const double vel_wheel_r = right_traction_wheel_vel * wheel_radius_; + const double vel_wheel_l = left_traction_wheel_vel * wheel_radius_; if (std::isinf(turning_radius)) { From 7eda2e765ce4762983677585d54a8c5ebeee4293 Mon Sep 17 00:00:00 2001 From: Rein Appeldoorn Date: Wed, 11 Sep 2024 08:38:28 +0200 Subject: [PATCH 3/3] Update steering_controllers_library/src/steering_odometry.cpp Co-authored-by: Sai Kishor Kothakota --- steering_controllers_library/src/steering_odometry.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/steering_controllers_library/src/steering_odometry.cpp b/steering_controllers_library/src/steering_odometry.cpp index 6100914aac..824ec86f59 100644 --- a/steering_controllers_library/src/steering_odometry.cpp +++ b/steering_controllers_library/src/steering_odometry.cpp @@ -142,8 +142,8 @@ double SteeringOdometry::get_linear_velocity_double_traction_axle( } // overdetermined, we take the average - double vel_r = vel_wheel_r * turning_radius / (turning_radius + wheel_track_ * 0.5); - double vel_l = vel_wheel_l * turning_radius / (turning_radius - wheel_track_ * 0.5); + const double vel_r = vel_wheel_r * turning_radius / (turning_radius + wheel_track_ * 0.5); + const double vel_l = vel_wheel_l * turning_radius / (turning_radius - wheel_track_ * 0.5); return (vel_r + vel_l) * 0.5; }