-
Notifications
You must be signed in to change notification settings - Fork 58
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
populate the covariance fields using the noise models #333
Conversation
@@ -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) { |
There was a problem hiding this comment.
Choose a reason for hiding this comment
The reason will be displayed to describe this comment to others. Learn more.
number 9 as a const ?
There was a problem hiding this comment.
Choose a reason for hiding this comment
The reason will be displayed to describe this comment to others. Learn more.
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)
...
@ejalaa12 Have you had a chance to look at the PR reviews? |
No problem.
Since we do squash merged, you can just hit the "Update branch" button to merge from |
Awesome ! |
@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? |
yes can you retarget the changes to |
We did two merge of gz-sensors7 into this PR, which introduce some commits that are not on main yet. |
9824009
to
c99f13d
Compare
I tried just removing the latest branch merge. |
I think doing |
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]>
@azeey thanks, I know that you preferred to avoid rebasing so that's why I was waiting for this :). |
There was a problem hiding this comment.
Choose a reason for hiding this comment
The reason will be displayed to describe this comment to others. Learn more.
looks good to me! Just some minor comments.
Signed-off-by: Alaa El Jawad <[email protected]>
@iche033 thanks for the feedback, I just addressed them in my last commit |
src/ImuSensor.cc
Outdated
return static_cast<float>(gaussian->StdDev() * gaussian->StdDev()); | ||
} | ||
} | ||
return 0.001f; |
There was a problem hiding this comment.
Choose a reason for hiding this comment
The reason will be displayed to describe this comment to others. Learn more.
is 0.001f
a good default value to return? or 0
?
There was a problem hiding this comment.
Choose a reason for hiding this comment
The reason will be displayed to describe this comment to others. Learn more.
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.
There was a problem hiding this comment.
Choose a reason for hiding this comment
The reason will be displayed to describe this comment to others. Learn more.
ok let's keep it consistent with ROS and set it to 0 here. Can you also do that for the orientation_covariance
below?
There was a problem hiding this comment.
Choose a reason for hiding this comment
The reason will be displayed to describe this comment to others. Learn more.
Friendly ping @ejalaa12, I would like to get this in for Harmonic.
There was a problem hiding this comment.
Choose a reason for hiding this comment
The reason will be displayed to describe this comment to others. Learn more.
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
There was a problem hiding this comment.
Choose a reason for hiding this comment
The reason will be displayed to describe this comment to others. Learn more.
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 Report
@@ 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
|
Signed-off-by: Alaa El Jawad <[email protected]>
🎉 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
codecheck
passed (See contributing)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.