Add GNSS rotation heading bootstrap

[jkaflik]

Summary

This adds a GNSS rotation-based heading bootstrap for setups where the GPS/GNSS antenna is not mounted at base_link.

In my OpenMower setup the antenna has a lever arm relative to base_link, so robot rotation makes the antenna trace a small arc. The observed GNSS displacement, relative yaw from odometry/gyro, and configured lever arm can infer absolute heading before normal track-based heading is reliable.

Additional change:

  // Minimum forward speed to count as real motion
  // Below this threshold: could be GPS jitter, spinning in place, or sliding
  const double MIN_SPEED = 0.2;  // m/s

  // Maximum yaw rate: if spinning fast, heading is not observable from track
  const double MAX_YAW_RATE = 0.3;  // rad/s (~17 deg/s)

Is extracted into parameter. In my case, with RTK, GPS jitter is much less, so min speed threshold can be lowered down. Happy to extract it into a separate PR.

The PR also includes small ROS compatibility fixes needed in Jazzy. (should not be a part of this PR)

Status

This is WIP.

I have only done a quick/superficial validation in simulation so far. I have not yet tested this on real hardware in an outdoor environment. I plan to do that in the coming days.

[jkaflik]

@manankharwar, any reason for closing this?

[manankharwar]

Hey @jkaflik, honestly no good reason, and closing it without a comment was poor form on my part. Sorry about that.
Reopening. I'd like to get this in. Easiest path I think is probably to split it: the GNSS rotation heading bootstrap as its own focused PR, and the param extraction as a quick separate one. I'm happy to help rebase against current main so it's not all on you. And whenever you get the outdoor hardware run done, I'd genuinely love to see how it behaves on the real antenna arc.

[jkaflik]

Thanks for your response.
I had a quick run on real hardware. It seemed to require some parameter tuning, so I didn't reply here.
Maybe I can give it another spin in upcoming days.

[manankharwar]

No rush at all, and thanks for actually taking it to hardware, that's the part I care most about.

Two things to make this easier on you:

1. I'll rebase this against current main and resolve the conflicts myself, so that's off your plate.
2. If you're up for it, could you share a rosbag from that outdoor run (GPS + IMU + wheel odom + tf)? Then I can reproduce the tuning problem on my side and debug it offline instead of you burning field time on it. Honestly, real RTK data from an actual mower is more useful to me than anything I can generate myself.

Also curious what exactly it did wrong, and which params you had to move. Heading converging to the wrong value, oscillating, or just not converging? If it needs hand-tuning to work at all, that's my bug to fix, the whole point is that it should behave with sane defaults.

[manankharwar]

@jkaflik I did the rebase I promised. It's on pr67-on-main in this repo:

https://github.com/manankharwar/fusioncore/tree/pr67-on-main

Your branch was 68 commits behind and conflicted in 4 files, mostly because main picked up magnetometer support, gnss.max_vdop, a GNSS reject-reason field, and an inter-sensor clock-skew fix since May. All of that is preserved; wherever your branch and main touched the same code I kept main's newer version. GPS_ROTATION is enum 5 now because MAGNETOMETER took 4. It builds and 87 core plus 9 ROS tests pass, including your six new ones, so the bootstrap works on current main.

I did not force-push to your branch, that would throw away your commits and anything local you have. Pull it however suits you:

git remote add upstream https://github.com/manankharwar/fusioncore.git
git fetch upstream
git checkout openmower/gnss-rotation-heading-bootstrap
git reset --hard upstream/pr67-on-main
git push --force-with-lease

On the feature itself: I really like it. Getting absolute heading out of the antenna arc is a genuinely clever way around the "drive 5 m straight before heading is observable" problem, and it needs no extra hardware. I want this in.

The one thing holding me back from merging is that it changes the lever arm for everyone, not just users of the new feature. You moved the antenna offset out of the measurement function and onto the measurement itself, and I think you're right that applying it in the measurement function turns every position fix into a competing yaw observation. But that touches every existing GPS user, and I'm not comfortable merging a filter behaviour change that hasn't been seen working on a real robot.

So: would you be up for doing the outdoor run and reporting back? Trajectory and heading plots, or just a rosbag I can dig through, either is fine. If it looks good on your mower I'd love to land it. My own rover can't test this one, my antenna sits directly above the rotation centre so it traces no arc when it spins.

Two things that should make that run go better than your last one:

Check your sensor clocks first. 0.3.4 fixed a bug where sensors stamped from different clocks made the filter fuse measurements at the wrong times. It presents exactly as flaky, tuning-sensitive behaviour rather than an obvious failure, which is what your "seemed to require some parameter tuning" sounds like. OpenMower is a good candidate given the mainboard and Pi:

ros2 topic echo /your/gps/topic --field header.stamp --once
ros2 topic echo /your/imu/topic --field header.stamp --once
ros2 topic echo /your/odom/topic --field header.stamp --once

Those should agree within milliseconds. If they don't, that was likely what you were fighting, and 0.3.4 now warns at startup instead of failing quietly.

The param extraction half is already done. gps_track_heading_min_speed and gps_track_heading_max_yaw_rate landed in main a while back and are exposed as ROS params, so you can drop that from this PR. Your gps_track_heading_max_yaw_delta is new and I kept it.

No rush at all, and if it needed hand-tuning to converge I'd still call that my bug to fix, not yours. Sane defaults are the whole point. Let me know.