Simplify reduce_wheel_speed_until_steering_reached logic

[christophfroehlich]

Description

While checking #2392 I was confused by the code. I used the chance to simplify the logic a bit.

Is this user-facing behavior change?

Minimal, the scale is continuous now as cos(max_phi_delta) / cos(min_phi_delta) is not exactly 0.01.

Did you use Generative AI?

no

Additional Information

[codecov]

Codecov Report

✅ All modified and coverable lines are covered by tests.
✅ Project coverage is 85.60%. Comparing base (2850cc4) to head (bae945a).

Additional details and impacted files

@@            Coverage Diff             @@
##           master    #2396      +/-   ##
==========================================
- Coverage   85.60%   85.60%   -0.01%     
==========================================
  Files         148      148              
  Lines       15829    15828       -1     
  Branches     1339     1339              
==========================================
- Hits        13551    13550       -1     
  Misses       1792     1792              
  Partials      486      486              

Flag	Coverage Δ
unittests	85.60% <100.00%> (-0.01%)	⬇️

Flags with carried forward coverage won't be shown. Click here to find out more.

Files with missing lines	Coverage Δ
...ng_controllers_library/src/steering_kinematics.cpp	86.20% <100.00%> (-0.08%)	⬇️

🚀 New features to boost your workflow:

• ❄️ Test Analytics: Detect flaky tests, report on failures, and find test suite problems.

[christophfroehlich]

@joseph-ros2-dev what do you think about this?

[joseph-ros2-dev]

Hi @christophfroehlich,

Thank you for opening PR #2396 — the cos(std::min(...)) / cos(min_phi_delta) approach and the normalization insight are exactly right.

Reviewing both PRs together, I found that tricycle_controller.cpp has two separate issues, not just the M_PI_2 boundary addressed in #2392:

---

Issue 1: Discontinuity at the 30° lower boundary (unaddressed by either PR)

The current logic applies scale = cos(alpha_delta) directly (without normalization) in the 30°–90° range. Since cos(30°) ≈ 0.866 ≠ 1.0, Ws_write drops 13% instantaneously the moment alpha_delta crosses 30°. This is the same root cause your normalization in steering_kinematics.cpp correctly eliminates.

Numerical verification

alpha_delta	Old scale	Note
100°	0.0100	> M_PI_2 branch
90° (exact)	~6e-17	BUG: strict > fails, falls into cos() branch
75°	0.2588	cos(75°)
60°	0.5000	cos(60°)
45°	0.7071	cos(45°)
30°	0.8660	← JUMP lower edge
29.9°	1.0000	← JUMP upper edge: instantaneous +0.134
0°	1.0000	if branch

---

Proposed fix: std::max + normalization

const double min_alpha_delta = M_PI / 6.0;  // 30°
const double min_scale       = 0.01;         // explicit: 1% creep floor

if (alpha_delta < min_alpha_delta)
{
  scale = 1.0;
}
else
{
  scale = std::max(min_scale, cos(alpha_delta) / cos(min_alpha_delta));
}

Numerical verification

alpha_delta	Old scale	Proposed scale	Delta	Note
100°	0.0100	0.0100	—	cos < 0 → std::max clamps ✓
90° (exact)	~6e-17	0.0100	—	BUG fixed: std::max clamps ✓
89.43°	0.0100	0.0116	+0.002	cos(acos(0.01))/cos(30°)
75°	0.2588	0.2989	+0.040	cos(75°)/cos(30°)
60°	0.5000	0.5774	+0.077	cos(60°)/cos(30°)
45°	0.7071	0.8165	+0.109	cos(45°)/cos(30°)
30°	0.8660	1.0000	+0.134	JUMP fixed: cos(30°)/cos(30°) = 1 ✓
29.9°	1.0000	1.0000	—	if branch
0°	1.0000	1.0000	—	if branch

---

Summary

Property	Old code	PR #2392 (>=)	This proposal
Continuous at 30°	✗ jump to 0.866	✗ jump to 0.866	✓ stays at 1.000
Correct at 90° (exact)	✗ ~6e-17	✓ 0.01	✓ 0.01 via std::max
Needs exact boundary value	depends on M_PI_2	depends on M_PI_2	✓ not needed
Design intent explicit	✗	✗	✓ min_scale = 0.01
Aligned with steering_kinematics.cpp	✗	✗	✓ same normalization

Would it make sense to update PR #2392 with this approach to address both issues at once, aligned with the same design philosophy as your refactor in #2396?

[christophfroehlich]

Can we merge this PR here first? Please leave an approval in the github UI if you agree.
Then let's apply the same fix for tricycle_controller with #2392.

As a follow-up, let's implement a proper function instead, see the plots from @thedevmystic in #1695 (comment)
I would vote for the sigmoid or guassian function.

[joseph-ros2-dev]

LGTM. The normalization approach cleanly resolves the discontinuity.
Ready to proceed with applying the same fix to tricycle_controller in #2392.