Odometry Twist Covariances not populated properly

[Aposhian]

Describe the bug
On the odometry map message, the twist covariance is wrong. The NED velocity uncertainties are being put in the wrong elements (21, 28, and 35, when the should be 0, 7, and 14), and the angular rate uncertainties are not being populated at all.

The proper form is documented here: https://github.com/ros2/common_interfaces/blob/rolling/geometry_msgs/msg/TwistWithCovariance.msg

To Reproduce
Steps to reproduce the behavior:

1. Enable odometry map message
2. Observe that not all diagonals are populated

Expected behavior
All diagonal entries should be populated on the odometry map twist covariance

Environment (please complete the following information):

• OS: Ubuntu
• Architecture: N/A
• ROS Version: humble
• Version: 4.2.0
• Sensor(s): 3DM-GQ7

Modifications
No

Launch Parameters
N/A

Additional context