populate the covariance fields using the noise models #333

ejalaa12 · 2023-03-15T19:43:36Z

🎉 New feature

This PR fills the imu covariance field

Summary

This PR depends on this one: gazebosim/gz-msgs#333

The published imu message is missing the covariance fields for linear_acceleration, angular_velocity and orientation_covariance. Those can be very useful when bridging between gz and ros.

Note: a Pr on ros_gz to update the convertion to ros is coming

Checklist

Signed all commits for DCO
Added tests
Added example and/or tutorial
Updated documentation (as needed)
Updated migration guide (as needed)
Consider updating Python bindings (if the library has them)
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.

src/ImuSensor.cc

ahcorde · 2023-03-20T19:14:35Z

src/ImuSensor.cc

@@ -255,6 +256,41 @@ bool ImuSensor::Update(const std::chrono::steady_clock::duration &_now)
  frame->set_key("frame_id");
  frame->add_value(this->FrameId());

+  // Populate covariance
+  for (int i = 0; i < 9; ++i) {


number 9 as a const ?

I don't see a problem, but wouldn't that be less easy to read ?

const int matrix_3x3_size = 9; for (int i = 0; i < matrix_3x3_size; ++i) ...

azeey · 2023-04-10T18:34:44Z

@ejalaa12 Have you had a chance to look at the PR reviews?

ejalaa12 · 2023-04-11T15:28:32Z

@ejalaa12 Have you had a chance to look at the PR reviews?

@azeey Sorry for the long time to answer, was busy on other projects.
I just accepted/answered to the review.

Btw, this branch is out-of-date. I could do a rebase on the most recent gz-sensors7 branch, if that is ok with you.

azeey · 2023-04-11T15:39:24Z

@ejalaa12 Have you had a chance to look at the PR reviews?

@azeey Sorry for the long time to answer, was busy on other projects. I just accepted/answered to the review.

No problem.

Btw, this branch is out-of-date. I could do a rebase on the most recent gz-sensors7 branch, if that is ok with you.

Since we do squash merged, you can just hit the "Update branch" button to merge from gz-sensors7. I don't recommend rebasing since it usually causes github comments to disappear from the context of their files after the rebase.

ejalaa12 · 2023-04-11T16:32:40Z

Awesome !
Just did the update branch :)

ejalaa12 · 2023-08-04T16:25:43Z

@mjcarroll I'm thinking that since this PR was merged on main, how should we handler it here? Shall we target main also ? That might explain why the CI is not passing no?

iche033 · 2023-08-07T22:30:15Z

yes can you retarget the changes to main, thanks.

ejalaa12 · 2023-08-09T07:45:53Z

We did two merge of gz-sensors7 into this PR, which introduce some commits that are not on main yet.
I can clean this up by rebasing only my changes onto main and then doing a force-push if that's ok for you @iche033

ejalaa12 · 2023-08-09T08:21:33Z

I tried just removing the latest branch merge.
Now the DCO is failing because one of the previous merge-commit does not contain a 'sign-off' message.
I'll wait for your confirmation on which method you prefer (rebase only relevant commis on top of main VS leaving it as it is)

azeey · 2023-08-09T15:37:01Z

I think doing git rebase --signoff main (assuming main is up-to-date) would help.

Signed-off-by: Alaa El Jawad <[email protected]>

Co-authored-by: Alejandro Hernández Cordero <[email protected]> Signed-off-by: Alaa El Jawad <[email protected]>

ejalaa12 · 2023-08-10T11:03:16Z

@azeey thanks, I know that you preferred to avoid rebasing so that's why I was waiting for this :).
Done !

iche033

looks good to me! Just some minor comments.

src/ImuSensor.cc

Signed-off-by: Alaa El Jawad <[email protected]>

ejalaa12 · 2023-08-11T09:30:29Z

@iche033 thanks for the feedback, I just addressed them in my last commit

iche033 · 2023-08-11T17:17:29Z

src/ImuSensor.cc

+        return static_cast<float>(gaussian->StdDev() * gaussian->StdDev());
+      }
+    }
+    return 0.001f;


is 0.001f a good default value to return? or 0?

It's good question. I'm not entirely sure here.
According to ros imu message definition:

if the covariance is known it should be filled in

if the covariance is unknown we should set it to 0

if we're missing an estimate for one of those values, we should set it to -1.

In this case, I guess 0 is a good default, that is if we want to follow ros conventions in gz-sensors as well.

ok let's keep it consistent with ROS and set it to 0 here. Can you also do that for the orientation_covariance below?

Friendly ping @ejalaa12, I would like to get this in for Harmonic.

Hey sorry for the late reply.
I just updated the PR and set the default to 0.
Concerning the orientation_covariance, setting it 0. It is important to note that this implies (according ros convention) the noise is always unknown, while in fact, we are just not applying any.
This is fine for robot_localization for example (a small offset will be added), but I wonder what this will do for other localization nodes... Thankfully, this will not break anything as the behavior would be unchanged with this PR :)

On a different tangent, should we consider applying noise to the orientation values (probably a separate PR). That would mean addind a new SensorNoiseType

Noising up orientation can be tricky as an IMU isn't really directly measuring that, but instead running it through some sort of filter. You end up with things that aren't linear and uncorrelated in a tidy way. I think we can open a ticket to follow up on that though, if you would like.

codecov · 2023-08-11T17:21:22Z

Codecov Report

Merging #333 (996893c) into main (83f0e1f) will increase coverage by 0.11%.
The diff coverage is 100.00%.

❗ Current head 996893c differs from pull request most recent head de0e49b. Consider uploading reports for the commit de0e49b to get more accurate results

@@            Coverage Diff             @@
##             main     #333      +/-   ##
==========================================
+ Coverage   72.62%   72.74%   +0.11%     
==========================================
  Files          36       36              
  Lines        4947     4968      +21     
==========================================
+ Hits         3593     3614      +21     
  Misses       1354     1354

Files Changed	Coverage Δ
src/ImuSensor.cc	`86.84% <100.00%> (+1.63%)`	⬆️

Signed-off-by: Alaa El Jawad <[email protected]>

ejalaa12 requested a review from iche033 as a code owner March 15, 2023 19:43

github-actions bot added the 🌱 garden Ignition Garden label Mar 15, 2023

ejalaa12 mentioned this pull request Mar 15, 2023

populate imu covariances when converting gazebosim/ros_gz#375

Merged

9 tasks

azeey requested a review from mjcarroll March 20, 2023 18:28

ahcorde requested changes Mar 20, 2023

View reviewed changes

azeey added the needs upstream release Blocked by a release of an upstream library label Apr 11, 2023

azeey added the beta Targeting beta release of upcoming collection label Jul 31, 2023

ejalaa12 changed the base branch from gz-sensors7 to main August 9, 2023 07:40

ejalaa12 force-pushed the gz-sensors7 branch 2 times, most recently from 9824009 to c99f13d Compare August 9, 2023 08:01

ejalaa12 and others added 2 commits August 10, 2023 13:02

populate the covariance fields using the noise models

91740c1

Signed-off-by: Alaa El Jawad <[email protected]>

Update src/ImuSensor.cc

f9885a8

Co-authored-by: Alejandro Hernández Cordero <[email protected]> Signed-off-by: Alaa El Jawad <[email protected]>

ejalaa12 force-pushed the gz-sensors7 branch from c99f13d to f9885a8 Compare August 10, 2023 11:02

iche033 reviewed Aug 10, 2023

View reviewed changes

src/ImuSensor.cc Outdated Show resolved Hide resolved

src/ImuSensor.cc Show resolved Hide resolved

use modern casting

a0a18e7

Signed-off-by: Alaa El Jawad <[email protected]>

ejalaa12 force-pushed the gz-sensors7 branch from 35fc06f to a0a18e7 Compare August 11, 2023 09:29

iche033 reviewed Aug 11, 2023

View reviewed changes

mjcarroll and others added 2 commits August 22, 2023 09:02

Merge branch 'main' into gz-sensors7

8a0fa77

set covariance to 0 by default.

de0e49b

Signed-off-by: Alaa El Jawad <[email protected]>

mjcarroll merged commit 67e0dea into gazebosim:main Aug 23, 2023
6 of 7 checks passed

Provide feedback

Saved searches

Use saved searches to filter your results more quickly

populate the covariance fields using the noise models #333

populate the covariance fields using the noise models #333

ejalaa12 commented Mar 15, 2023

ahcorde Mar 20, 2023

ejalaa12 Apr 11, 2023

azeey commented Apr 10, 2023

ejalaa12 commented Apr 11, 2023

azeey commented Apr 11, 2023

ejalaa12 commented Apr 11, 2023

ejalaa12 commented Aug 4, 2023

iche033 commented Aug 7, 2023

ejalaa12 commented Aug 9, 2023

ejalaa12 commented Aug 9, 2023

azeey commented Aug 9, 2023

ejalaa12 commented Aug 10, 2023

iche033 left a comment

ejalaa12 commented Aug 11, 2023

iche033 Aug 11, 2023

ejalaa12 Aug 16, 2023

iche033 Aug 16, 2023 •

edited

Loading

mjcarroll Aug 22, 2023

ejalaa12 Aug 23, 2023

mjcarroll Aug 23, 2023

codecov bot commented Aug 11, 2023 •

edited

Loading

populate the covariance fields using the noise models #333

populate the covariance fields using the noise models #333

Conversation

ejalaa12 commented Mar 15, 2023

🎉 New feature

Summary

Checklist

ahcorde Mar 20, 2023

Choose a reason for hiding this comment

ejalaa12 Apr 11, 2023

Choose a reason for hiding this comment

azeey commented Apr 10, 2023

ejalaa12 commented Apr 11, 2023

azeey commented Apr 11, 2023

ejalaa12 commented Apr 11, 2023

ejalaa12 commented Aug 4, 2023

iche033 commented Aug 7, 2023

ejalaa12 commented Aug 9, 2023

ejalaa12 commented Aug 9, 2023

azeey commented Aug 9, 2023

ejalaa12 commented Aug 10, 2023

iche033 left a comment

Choose a reason for hiding this comment

ejalaa12 commented Aug 11, 2023

iche033 Aug 11, 2023

Choose a reason for hiding this comment

ejalaa12 Aug 16, 2023

Choose a reason for hiding this comment

iche033 Aug 16, 2023 • edited Loading

Choose a reason for hiding this comment

mjcarroll Aug 22, 2023

Choose a reason for hiding this comment

ejalaa12 Aug 23, 2023

Choose a reason for hiding this comment

mjcarroll Aug 23, 2023

Choose a reason for hiding this comment

codecov bot commented Aug 11, 2023 • edited Loading

Codecov Report

iche033 Aug 16, 2023 •

edited

Loading

codecov bot commented Aug 11, 2023 •

edited

Loading