Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Add Gaussian noise to Odometry Publisher #1393

Merged
merged 17 commits into from
Apr 5, 2022

Conversation

adityapande-1995
Copy link
Contributor

@adityapande-1995 adityapande-1995 commented Mar 16, 2022

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

🎉 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.

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

@github-actions github-actions bot added the 🏯 fortress Ignition Fortress label Mar 16, 2022
@adityapande-1995
Copy link
Contributor Author

adityapande-1995 commented Mar 16, 2022

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
Copy link
Contributor

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.

Signed-off-by: Aditya <[email protected]>
@adityapande-1995 adityapande-1995 marked this pull request as ready for review March 16, 2022 20:06
@adityapande-1995 adityapande-1995 added the needs upstream release Blocked by a release of an upstream library label Mar 16, 2022
@adityapande-1995 adityapande-1995 changed the title Adds Gaussian noise to Odometry Publisher Add Gaussian noise to Odometry Publisher Mar 16, 2022
@adityapande-1995
Copy link
Contributor Author

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

@nuclearsandwich
Copy link

@osrf-jenkins retest this please

Signed-off-by: Aditya <[email protected]>
@chapulina chapulina removed the needs upstream release Blocked by a release of an upstream library label Mar 30, 2022
Signed-off-by: Louise Poubel <[email protected]>
Signed-off-by: Louise Poubel <[email protected]>
Copy link
Contributor

@chapulina chapulina left a comment

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

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
Copy link

codecov bot commented Apr 4, 2022

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
Copy link
Contributor Author

The test failing in focal is a flaky one.

@adityapande-1995 adityapande-1995 merged commit d01652c into ign-gazebo6 Apr 5, 2022
@adityapande-1995 adityapande-1995 deleted the aditya/odom_pub_noise branch April 5, 2022 01:45
chapulina added a commit that referenced this pull request Apr 13, 2022
* GzSceneManager: Prevent crash 💥 when inserted from menu (#1371)

Signed-off-by: Louise Poubel <[email protected]>
Signed-off-by: ahcorde <[email protected]>

Co-authored-by: ahcorde <[email protected]>

* Prepare version 6.7.0 (#1373)

* Prepare version 6.7.0

Signed-off-by: Jose Luis Rivero <[email protected]>

* Populate GUI plugins that are empty (#1375)

Signed-off-by: Louise Poubel <[email protected]>

* Fix visualization python tutorial (#1377)

Signed-off-by: ahcorde <[email protected]>

* Add xyz and rpy offset to published odometry pose (#1341)

* Added xyz and rpy offset to published pose

Signed-off-by: Aditya <[email protected]>

* Added headless rendering tutorial (#1386)

* Added headless rendering tutorial

Signed-off-by: Nate Koenig <[email protected]>

* Update tutorials/headless_rendering.md

Co-authored-by: Louise Poubel <[email protected]>
Signed-off-by: Nate Koenig <[email protected]>

* Update tutorials/headless_rendering.md

Co-authored-by: Louise Poubel <[email protected]>
Signed-off-by: Nate Koenig <[email protected]>

* Mention ogre2

Signed-off-by: Nate Koenig <[email protected]>

* Added note about software rendering

Signed-off-by: Nate Koenig <[email protected]>

* add 'on linux systems'

Signed-off-by: Nate Koenig <[email protected]>

* Add xyz and rpy offset to published odometry pose (#1341)

* Added xyz and rpy offset to published pose

Signed-off-by: Aditya <[email protected]>
Signed-off-by: Nate Koenig <[email protected]>

Co-authored-by: Nate Koenig <[email protected]>
Co-authored-by: Louise Poubel <[email protected]>
Co-authored-by: Aditya Pande <[email protected]>

* Allow to turn on/off lights (#1343)

Signed-off-by: ahcorde <[email protected]>
Co-authored-by: Louise Poubel <[email protected]>

* Add gazebo Entity id to rendering sensor's user data (#1381)

Adds the gazebo::Entity id to a rendering::Sensor's UserData object. The main use case is so that we can get back the corresponding gazebo Entity in the rendering thread when processing sensors.

Signed-off-by: Ian Chen <[email protected]>

Co-authored-by: Nate Koenig <[email protected]>
Co-authored-by: Alejandro Hernández Cordero <[email protected]>

* Don't mark entities with a ComponentState::NoChange component as modified (#1391)

Signed-off-by: Ashton Larkin <[email protected]>

* Disable ModelCommandAPI_TEST.RgbdCameraSensor on macOS (#1397)

Disabling test since it's very flaky.

Signed-off-by: Addisu Z. Taddese <[email protected]>

* Disable PeerTracker.PeerTrackerStale on macOS (#1398)

Signed-off-by: Addisu Z. Taddese <[email protected]>

* Toggle Light visuals (#1387)

Signed-off-by: ahcorde <[email protected]>

* Prevent hanging when world has only non-world plugins (#1383)

Signed-off-by: Louise Poubel <[email protected]>

Co-authored-by: Alejandro Hernández Cordero <[email protected]>

* Component inspector: refactor Pose3d C++ code into a separate class (#1400)

Signed-off-by: Louise Poubel <[email protected]>

* add initial_position param to joint controller system (#1406)

Similar to the <initial_position> parameter in the JointTrajectoryController system, this PR adds an <initial_position> parameter to the JointPositionController to let users specifies the initial target pos for a joint.

Signed-off-by: Ian Chen <[email protected]>

* Fix JointStatePublisher topic name for nested models (#1405)

The joint-state-publisher system currently assumes it's always attached to the top level model and hence generates an incorrect topic name for nested models. This PR updates it to generate the correct topic name.

Signed-off-by: Ian Chen <[email protected]>

* Added user command to set multiple entities (#1394)

Signed-off-by: ahcorde <[email protected]>
Signed-off-by: Louise Poubel <[email protected]>

Co-authored-by: Louise Poubel <[email protected]>

* Disable tests that are expected to fail on Windows (#1408)

Signed-off-by: Louise Poubel <[email protected]>

* Fortress: Install Ogre 2.2, simplify docker (#1395)

Signed-off-by: Louise Poubel <[email protected]>

* SceneBroadcaster: only send changed state information for change events (#1392)

Signed-off-by: Ashton Larkin <[email protected]>

Co-authored-by: Louise Poubel <[email protected]>

* Add wheel slip user command (#1241)

Signed-off-by: Ivan Santiago Paunovic <[email protected]>

* Distortion camera integration test (#1374)

Signed-off-by: William Lew <[email protected]>
Co-authored-by: Alejandro Hernández Cordero <[email protected]>
Co-authored-by: Louise Poubel <[email protected]>

* Add the Model Photo Shoot system, port of Modelpropshop plugin from Gazebo classic (#1331)

This system can be used to generate thumbnails of models. In comparison to the Modelpropshop plugin in Gazebo classic, this adds the ability to generate thumbnails after randomizing the joint positions of the model.

Signed-off-by: Marco A. Gutierrez <[email protected]>
Signed-off-by: Addisu Z. Taddese <[email protected]>

* Referring to Fuel assets within a heightmap (#1419)

Signed-off-by: Louise Poubel <[email protected]>

* Disable sensors in sensors system when battery is drained (#1385)

Added a new parameter <disable_on_drained_battery> to the sensors system. If set to true, sensors will be disabled if the model is out of battery.

It listens to the battery state of charge from the battery plugin, and if the charge reaches zero, all child sensors and nested sensors are set to be inactive.

Signed-off-by: Ian Chen <[email protected]>

* 🏁🎈 5.4.0 (#1420)

Signed-off-by: Louise Poubel <[email protected]>

* ServerConfig accepts an sdf::Root DOM object (#1333)

Signed-off-by: Nate Koenig <[email protected]>

Co-authored-by: Nate Koenig <[email protected]>
Co-authored-by: Michael Carroll <[email protected]>
Co-authored-by: Alejandro Hernández Cordero <[email protected]>

* Preparing 6.8.0 release (#1425)

* Prepareing 6.8.0 release

Signed-off-by: Jose Luis Rivero <[email protected]>

* Update changelog after merging forward port

Signed-off-by: Jose Luis Rivero <[email protected]>

* Add Gaussian noise to Odometry Publisher (#1393)

* Added gaussian noise and odometry with covariance publisher

Signed-off-by: Aditya <[email protected]>

Co-authored-by: Louise Poubel <[email protected]>

* Fix deprecation warnings for ModelPhotoShoot (#1437)

Signed-off-by: Louise Poubel <[email protected]>

Co-authored-by: ahcorde <[email protected]>
Co-authored-by: Jose Luis Rivero <[email protected]>
Co-authored-by: Aditya Pande <[email protected]>
Co-authored-by: Nate Koenig <[email protected]>
Co-authored-by: Nate Koenig <[email protected]>
Co-authored-by: Ian Chen <[email protected]>
Co-authored-by: Ashton Larkin <[email protected]>
Co-authored-by: Addisu Z. Taddese <[email protected]>
Co-authored-by: Ivan Santiago Paunovic <[email protected]>
Co-authored-by: William Lew <[email protected]>
Co-authored-by: Marco A. Gutiérrez <[email protected]>
Co-authored-by: Michael Carroll <[email protected]>
@osrf-triage
Copy link

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

Sign up for free to join this conversation on GitHub. Already have an account? Sign in to comment
Labels
🏯 fortress Ignition Fortress
Projects
None yet
Development

Successfully merging this pull request may close these issues.

6 participants