fix: initial orientation

[planthaber]

Looks like the initial orientation was overwritten before

[chhtz]

Sorry for having missed this.
I think the main question is what to expect for different inputs. Should the first odometry orientation perfectly match the start_pose? Often it may be better to just take the yaw from the start_pose.

If the the orientation shall match the start_pose exactly, one could compute in the start_hook (or when the first imu_orientation is received) orientation_offset = start_pose.orientation * imu_orientation.inverse() and later multiply all orientations by that offset.

If just the yaw shall be taken, then (I think) one could delete the x,y parts of orientation_offset and renormalize the quaternion (untested ...)

[Rauldg]

The name of the property "start_pose" to me implies that the aim is to set odometry to start at that pose, so that when you check the odometry right after, before having performed any movement, just after processing the initial sensor measurements, the odometry is providing that as current pose estimation. So my interpretation matches what @chhtz describes in his answer to his question and in my opinion, that is what this pull request should be adapted towards.

The currently proposed "start_pose" sets a pose that is right away modified by whatever orientation is coming from the IMU. This makes a practical application in the context of setting the initial pose of the rover in a map difficult, because it is needed to know in advance what orientation the imu will give, invert that value, multiply it by the pursued initial_pose and then set that result as initial pose. Furthermore, if the next time you start the rover software with the start_pose set after the described process but rover imu starts measuring a different orientation (e.g. due to drift), the pose will again not be set as intended.