Regulated Pure Pursuit: approach the goal inside its tolerance

[fmrico]

Fixes #115

Near the goal, the controller stopped translating at the system's goal tolerance, the same threshold the system uses for success. With localization noise it switched between turning to the goal heading and tracking the path, spinning left and right in place with zero linear speed.

Now:

• Effective tolerance: the strictest of the controller's xy_goal_tolerance/yaw_goal_tolerance and the system's goal_tolerance.*, so its own parameters take effect again.
• Approach: it drives to half of that tolerance before turning to the goal heading.
• Hysteresis: once turning, it only goes back to tracking the path if the robot leaves the whole tolerance. A new goal resets it.
• Stopping: the condition is unchanged (inside the tolerance and the heading within its tolerance).

Validation

Summit warehouse, recorded cmd_vel:

• before: up to 35 s of ±0.8 rad/s spins with zero linear speed, finishing in 8–41 s;
• after: no turn reversals, finishing in 8.5–11.3 s.

Battery of the playgrounds that use RPP (before → after):

Playground	Time	Real goal error
Kobuki costmap RPP	8.3 → 11.5 s	0.18 → 0.03 m
Kobuki RPP safe	6.6 → 13.0 s	0.25 → 0.02 m
Kobuki RPP reflex	9.9 → 8.2 s	0.18 → 0.03 m
TIAGo costmap RPP	8.5 → 11.7 s	0.19 → 0.07 m
Summit warehouse	38.4 → 8.5 s	0.40 → 0.43 m
Summit excavation	4.0 → 4.0 s	0.60 → 0.57 m

In the Summit cases the remaining error is localization: the robot ends 0.17–0.32 m from the goal by its own estimate.

Tests

8 new tests (regulated_pp_goal_approach_tests):

• keeps driving inside the tolerance but outside half of it;
• turns within half;
• no oscillation with noise inside the tolerance;
• tracks again after leaving the tolerance;
• a new goal;
• stops at the goal;
• a stricter own tolerance;
• own tolerances without the system's.

4 of them fail on the previous code.