Add Gaussian noise to Odometry Publisher

[adityapande-1995]

🔸🔸🔸🔸🔸🔸🔸🔸🔸🔸🔸🔸🔸🔸🔸🔸🔸🔸🔸🔸🔸🔸🔸🔸🔸🔸🔸🔸🔸🔸🔸🔸🔸🔸🔸🔸🔸

🎉 New feature

Summary

This PR adds Gaussian noise to the published odometry message, and a new publisher which publishes OdometryWithCovariance messages.

Depends on gazebosim/gz-msgs#224.
This PR in ros_ign makes it compatible with geometry_msgs::odometry : gazebosim/ros_gz#222

Test it

Added a test case that checks the noise statistically and the diagonal elements of the covariance matrix.

Checklist

• Signed all commits for DCO
• Added tests
• Added example and/or tutorial
• Updated documentation (as needed)
• Updated migration guide (as needed)
• codecheck passed (See contributing)
• All tests passed (See test coverage)
• While waiting for a review on your PR, please help review another open pull request to support the maintainers

Note to maintainers: Remember to use Squash-Merge and edit the commit message to match the pull request summary while retaining Signed-off-by messages.

🔸🔸🔸🔸🔸🔸🔸🔸🔸🔸🔸🔸🔸🔸🔸🔸🔸🔸🔸🔸🔸🔸🔸🔸🔸🔸🔸🔸🔸🔸🔸🔸🔸🔸🔸🔸🔸

[adityapande-1995]

Concerns to be addressed before moving ahead:

1. This PR adds a second publisher for OdometryWithCovariance along with the existing one, to maintain API. Is this okay or should this PR be based to ign-gazebo7 ?
2. This is trying to replicate the behavior in gazebo_ros_p3d plugin. Originally, the noise was added only to the twist and NOT the pose, yet the covariance matrix is populated with diagonal elements for both twist and pose. Should it be a zero matrix for pose ? I've replicated the same behavior here.
   Check this for the section of code which populates the covariance matrix : https://github.com/ros-simulation/gazebo_ros_pkgs/blob/dcda27f7ac710bdd6a8aba2692e3b3b3ac1d9c4b/gazebo_plugins/src/gazebo_ros_p3d.cpp#L268-L293

[mjcarroll]

This PR adds a second publisher for OdometryWithCovariance along with the existing one, to maintain API. Is this okay or should this PR be based to ign-gazebo7 ?

This should be fine and keeps the API consistent, since you are just adding functionality. If you were replacing the current publisher with a different message type, that would require targeting a different version (and probably a tick-tock).

This is trying to replicate the behavior in gazebo_ros_p3d plugin. Originally, the noise was added only to the twist and NOT the pose, yet the covariance matrix is populated with diagonal elements for both twist and pose. Should it be a zero matrix for pose ? I've replicated the same behavior here.

To me, adding noise to the twist portion is what makes the most sense. If this is simulating wheel odometry, all you can typically measure is the rotation of the left and right wheels (in the most simple model), which will have some gaussian noise in the measurement (ignoring quantization and bias). You can then compute the linear (x) velocity and angular (z) velocity when you know the wheelbase width and tire size.

For the pose portion, this is typically computed by integrating the velocities, and without an external correction source, uncertainty should grow without bound.

[adityapande-1995]

CI is unable to find the ign-msgs headers for the new messages odometry_with_covariance

[nuclearsandwich]

@osrf-jenkins retest this please

[chapulina]

LGTM! I took the liberty of making a few commits directly to this branch to get it through CI and into the upcoming release:

• 1d2033f: Require ign-msgs 8.3
• cf78741 and 17e77b0: style fixes that aren't caught by our linters (it may be worth it taking a look at them to keep this in mind for future PRs)

[codecov]

Codecov Report

Merging #1393 (af673d1) into ign-gazebo6 (eb38961) will increase coverage by 0.06%.
The diff coverage is 95.16%.

@@               Coverage Diff               @@
##           ign-gazebo6    #1393      +/-   ##
===============================================
+ Coverage        63.36%   63.42%   +0.06%     
===============================================
  Files              306      306              
  Lines            24715    24765      +50     
===============================================
+ Hits             15660    15708      +48     
- Misses            9055     9057       +2     

Impacted Files	Coverage Δ
...rc/systems/odometry_publisher/OdometryPublisher.hh	100.00% <ø> (ø)
...rc/systems/odometry_publisher/OdometryPublisher.cc	88.37% <95.16%> (+2.31%)	⬆️

---

Continue to review full report at Codecov.

Legend - Click here to learn more
Δ = absolute <relative> (impact), ø = not affected, ? = missing data
Powered by Codecov. Last update eb38961...af673d1. Read the comment docs.

[adityapande-1995]

The test failing in focal is a flaky one.

[osrf-triage]

This pull request has been mentioned on Gazebo Community. There might be relevant details there:

https://community.gazebosim.org/t/new-releases-2022-04-27-fortress-citadel/1389/1