Add DopplerVelocityLogSystem plugin #1804

hidmic · 2022-11-21T19:00:09Z

🎉 New feature

Summary

This patch adds a system plugin to pick DVL sensors. Needs gazebosim/gz-sensors#290.

Test it

TBD

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.

Signed-off-by: Michel Hidalgo <[email protected]>

hidmic · 2022-11-21T19:01:05Z

First iteration. Once we've agreed on gazebosim/gz-sensors#290 and connected PRs, I'll add integration tests here.

arjo129 · 2022-11-23T00:17:10Z

Check whitespaces.

Signed-off-by: Michel Hidalgo <[email protected]>

hidmic · 2022-12-09T14:17:35Z

@iche033 @caguero I'm getting:

[Err] [SceneManager.cc:217] Visual: [horizontal_plane] already exists
237: [Err] [SceneManager.cc:217] Visual: [tethys] already exists
237: [Err] [BaseStorage.hh:927] Another item already exists with name: sun

in tests. Isn't the rendering engine cleaned up on test fixture destruction?

iche033 · 2023-01-31T19:37:09Z

@osrf-jenkins run tests please

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

iche033 · 2023-02-02T22:29:49Z

Made a few changes in bdfe547 which should fix compilation.

There are a couple other issues left:

INTEGRATION_dvl_system crashes.
- if I run the tests individually, they do not crash but DVLTest.BottomTracking and DVLTest.WaterMassTracking fail
gz-sim core adds a new dependency on gz-sensors
- I'm not sure if we want to do that. To workaround the issue, we would need to have a generic EnvironmentalData struct in gz sim and provide a function to convert between gz-sim and gz-sensors EnvironmentalData structs

codecov · 2023-02-02T23:29:35Z

Codecov Report

Merging #1804 (1740385) into main (2d19f93) will increase coverage by 0.20%.
The diff coverage is 72.22%.

@@            Coverage Diff             @@
##             main    #1804      +/-   ##
==========================================
+ Coverage   64.26%   64.47%   +0.20%     
==========================================
  Files         340      341       +1     
  Lines       26962    27139     +177     
==========================================
+ Hits        17328    17498     +170     
- Misses       9634     9641       +7

Impacted Files	Coverage Δ
include/gz/sim/ServerConfig.hh	`100.00% <ø> (ø)`
include/gz/sim/SystemLoader.hh	`100.00% <ø> (ø)`
include/gz/sim/Util.hh	`100.00% <ø> (ø)`
include/gz/sim/components/DetachableJoint.hh	`100.00% <ø> (ø)`
...nclude/gz/sim/components/ExternalWorldWrenchCmd.hh	`100.00% <ø> (ø)`
include/gz/sim/components/LevelBuffer.hh	`100.00% <ø> (ø)`
include/gz/sim/components/LevelEntityNames.hh	`100.00% <ø> (ø)`
include/gz/sim/components/PerformerAffinity.hh	`100.00% <ø> (ø)`
include/gz/sim/components/PerformerLevels.hh	`100.00% <ø> (ø)`
include/gz/sim/rendering/RenderUtil.hh	`100.00% <ø> (ø)`
... and 79 more

Help us with your feedback. Take ten seconds to tell us how you rate us. Have a feature suggestion? Share it here.

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

iche033 · 2023-02-03T01:08:40Z

if I run the tests individually, they do not crash but DVLTest.BottomTracking and DVLTest.WaterMassTracking fail

As of 0cb2e4d:

Worked around crash by splitting up tests into individual files. Currently rendering does not cleanup properly in between tests.
Also fixed DVLTest.BottomTracking test
DVLTest.WaterMassTracking is still failing. I'll probably need help from someone with knowledge on DVL for this. Test failure from running /build/gz-sim8/bin/INTEGRATION_dvl_system_water_mass_tracking:

/home/ian/code/gz_h_ws/src/gz-sim/test/integration/dvl_system_water_mass_tracking.cc:85: Failure
The difference between linearVelocityEstimate.X() and -waterCurrentVelocity.X() is 2.0003976831499872, which exceeds kVelocityTolerance, where
linearVelocityEstimate.X() evaluates to -1.0003976831499872,
-waterCurrentVelocity.X() evaluates to 1, and
kVelocityTolerance evaluates to 0.10000000000000001.
/home/ian/code/gz_h_ws/src/gz-sim/test/integration/dvl_system_water_mass_tracking.cc:87: Failure
The difference between linearVelocityEstimate.Y() and -waterCurrentVelocity.Y() is 0.99920384168009524, which exceeds kVelocityTolerance, where
linearVelocityEstimate.Y() evaluates to 0.49920384168009518,
-waterCurrentVelocity.Y() evaluates to -0.5, and
kVelocityTolerance evaluates to 0.10000000000000001.
/home/ian/code/gz_h_ws/src/gz-sim/test/integration/dvl_system_water_mass_tracking.cc:133: Failure
The difference between expectedLinearVelocityEstimate.X() and linearVelocityEstimate.X() is 0.50119416839480213, which exceeds kVelocityTolerance, where
expectedLinearVelocityEstimate.X() evaluates to -0.49920351475518465,
linearVelocityEstimate.X() evaluates to -1.0003976831499868, and
kVelocityTolerance evaluates to 0.10000000000000001.
/home/ian/code/gz_h_ws/src/gz-sim/test/integration/dvl_system_water_mass_tracking.cc:135: Failure
The difference between expectedLinearVelocityEstimate.Y() and linearVelocityEstimate.Y() is 1.4996016879686576, which exceeds kVelocityTolerance, where
expectedLinearVelocityEstimate.Y() evaluates to -1.0003978462872014,
linearVelocityEstimate.Y() evaluates to 0.49920384168145615, and
kVelocityTolerance evaluates to 0.10000000000000001.

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

iche033 · 2023-02-08T00:35:13Z

gz-sim core adds a new dependency on gz-sensors

removed dependency on gz-sensors in core edf0959. One todo suggestion is to create the generic EnvironmentalData structure in sdformat and make gz-sensors and gz-sim use that data structure.

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

iche033 · 2023-02-15T00:42:40Z

Made more fixes in fbd8783 and 2070591

Fixed applying linear/angular velocity commands in body fixed frame (instead of world frame which does not have any effect) in Testfixture's ModelManipulator.
Fixed DVL tests by computing the expected linear velocity in the correct reference frame
Disabled applying angular velocities to the AUV in water mass tracking integration test
- If angular velocities were applied, the linear velocity estimates produced by the DVL sensor were found to be different from the expected values. I don't know if it's an issue with the test or an actual bug in the DVL sensor implementationin gz-sensors. The tests in gz-sensors also do not test this scenario. I added a todo note for this.

The tests are passing now with the caveat mentioned in the last bullet point.

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

iche033 · 2023-02-16T23:36:27Z

ok I'm at the point that I think I could use a hand from people who are familiar with the DVL sensor implementation.

If I enable setting angular velocity on the AUV in the water mass tracking test by uncommenting this line, the test fails:

/home/ian/code/gz_h_ws/src/gz-sim/test/integration/dvl_system_water_mass_tracking.cc:182: Failure
The difference between expectedLinearVelocityEstimate.X() and linearVelocityEstimate.X() is 0.31949900784986629, which exceeds kVelocityTolerance, where
expectedLinearVelocityEstimate.X() evaluates to -1.7163285450741428,
linearVelocityEstimate.X() evaluates to -1.3968295372242765, and
kVelocityTolerance evaluates to 0.10000000000000001.
/home/ian/code/gz_h_ws/src/gz-sim/test/integration/dvl_system_water_mass_tracking.cc:184: Failure
The difference between expectedLinearVelocityEstimate.Y() and linearVelocityEstimate.Y() is 1.023310004056881, which exceeds kVelocityTolerance, where
expectedLinearVelocityEstimate.Y() evaluates to -0.78063882907783577,
linearVelocityEstimate.Y() evaluates to -1.8039488331347169, and
kVelocityTolerance evaluates to 0.10000000000000001.

Maybe @arjo129 or @hidmic can shed some light on where or what the problem could be?

hidmic

@iche033 thank you for picking this up

hidmic · 2023-02-17T12:16:08Z

test/helpers/ModelManipulator.hh

+        velocity->Data() = this->angularVelocityRequest.value();
+      }
+    }
+  }


@iche033 why are requests not reset once applied?

if reset, the velocities are only applied in that one iteration and moves the AUV only a little bit. I noticed that in the test comment, it is intended to make the AUV go in a circle so I removed the reset logic here in order to continuously apply the same velocities to the AUV.

Oh, I see. So the command component themselves are reset on every iteration? That's good to know.

hidmic · 2023-02-17T12:22:24Z

test/helpers/ModelManipulator.hh

+    {
+      sim::World world(sim::worldEntity(_ecm));
+      sim::Model model(world.ModelByName(_ecm, this->modelName));
+      using LinearVelocityCmd = sim::components::LinearVelocityCmd;


@iche033 I see the component was changed. For posterity, mind to clarify which are the reference and observation frames here? I understood (and I may be wrong) that a WorldLinearVelocityCmd sets a model's linear velocity w.r.t. to the world frame as observed in the world frame, whereas LinearVelocityCmd does so w.r.t. to the world frame as observed in the model frame.

I looked through the physics system and noticed that WorldLinearVelocityCmd is not being used at all. The LinearVelocityCmd applies the velocity in model frame, which I think it's what's intended after looking at the comment in the test. In order to move the AUV in circle, it needs to continuously apply x linear vel and z angular vel in the model frame

hidmic · 2023-02-17T12:24:45Z

include/gz/sim/components/Environment.hh

+      data->units = _units;
+      data->staticTime = _ignoreTimeStep;
+      return data;
+    }


@iche033 hmm, so we are duplicating EnvironmentalData definition, in gz-sensors and here, to avoid the dependency?

this introduces gz-sensors dependency in the core component of gz-sim. So far, only the gz systems (plugins) have dependency on gz-sensors, which are are loaded at run time

The general approach we've been taking is to add the common data structure in sdformat in use them in other gz libraries. It's possible to add this extra dependency but I think it needs some discussion. I made the change to try and get the PR in first.

hidmic · 2023-02-17T13:06:32Z

test/integration/dvl_system_water_mass_tracking.cc

+        angularVelocityRefFrame.Cross(sensorPositionInSFMFrame);
+    // calculate the final linear velocity estimate in reference frame
+    math::Vector3d expectedLinearVelocityEstimate =
+        linearVelocityRefFrame + tangentialVelocityRefFrame;


@iche033 this seems about right (thanks for the fix BTW, frames for angular velocity contributions were off). If the velocity commands are correct (see the request for clarification above), maybe sensor linear velocities w.r.t. the world frame as observed in the world frame back in the DVL are incorrect? This code would be wrong if the reported velocity were not accounting for the transform between body and sensor frame.

thanks for the pointers! I'll take a look at the dvl code on the gz-sensors side and see I can figure out what's wrong

…led before render to guarantee correct timestamp Signed-off-by: Ian Chen <[email protected]>

iche033 · 2023-02-18T02:30:47Z

ok tests are now passing after changes in a91ff9e!

The maths are correct on the gz-sensors side. The problems were:

Test computes the expected velocities using values from the wrong timestep. I updated the test to retrieve all model properties at the time that corresponds to the timestamp on the dvl message
Ocassionally due to race condition between rendering and physics, there DVL system was passing a world state with incorrect timestamp to gz-sensors. Fixed by making sure SetWorldState is called right before render update.

This PR is ready.

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

iche033 · 2023-02-18T03:51:57Z

CIs may fail because infra is being updated to use the latest harmonic deps: fuel-tools9, msgs10, and transport13.

Depends on: #1892

iche033 · 2023-03-08T22:50:29Z

CI is now working after mernging with main. Builds look good, merging!

Add DopplerVelocityLogSystem plugin

da0b9ac

Signed-off-by: Michel Hidalgo <[email protected]>

hidmic added 🌱 garden Ignition Garden MBARI-LRAUV Sponsored by MBARI-LRAUV project: https://github.com/osrf/lrauv labels Nov 21, 2022

hidmic requested a review from mjcarroll as a code owner November 21, 2022 19:00

hidmic requested a review from caguero November 21, 2022 19:00

hidmic mentioned this pull request Nov 22, 2022

Add DopplerVelocityLog sensor gazebosim/gz-sensors#290

Merged

9 tasks

Please cppcheck

cd16bc8

Signed-off-by: Michel Hidalgo <[email protected]>

hidmic mentioned this pull request Nov 23, 2022

2022 M4 meta-issue osrf/lrauv#251

Closed

38 tasks

mjcarroll added the needs upstream release Blocked by a release of an upstream library label Nov 28, 2022

hidmic requested a review from marcoag December 8, 2022 17:03

Add DopplerVelocityLogSystem plugin tests

f9f4213

Signed-off-by: Michel Hidalgo <[email protected]>

hidmic changed the base branch from gz-sim7 to main December 9, 2022 17:49

azeey removed needs upstream release Blocked by a release of an upstream library 🌱 garden Ignition Garden labels Jan 31, 2023

iche033 added 2 commits February 2, 2023 12:54

Merge branch 'main' into hidmic/dvl

4b08667

fix build and doxygen generation. Update doxygen, style

bdfe547

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

split tests, fix Bottom tracking test

0cb2e4d

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

iche033 added 4 commits February 2, 2023 17:13

remove unneeded code

2df4a98

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

more style fixes, fixing windows

fae98b1

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

fixing windows, disable tests on macos

4685c4f

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

remove gz-sensors dep in core

edf0959

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

iche033 added 3 commits February 14, 2023 15:17

fix model manipulator, model observer, dvl tests

fbd8783

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

add include

ef04eb8

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

remove unused var

ce79e44

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

iche033 added 2 commits February 14, 2023 20:51

fix ang vel for bottom tracking

2070591

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

update todo

c0ef18f

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

hidmic commented Feb 17, 2023

View reviewed changes

fix retrieving model properties in test, ensure setworldstates is cal…

a91ff9e

…led before render to guarantee correct timestamp Signed-off-by: Ian Chen <[email protected]>

doxy

5112ee9

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

Merge branch 'main' into hidmic/dvl

1740385

iche033 added the 🎵 harmonic Gazebo Harmonic label Mar 8, 2023

iche033 approved these changes Mar 8, 2023

View reviewed changes

iche033 merged commit cf07b19 into main Mar 8, 2023

iche033 deleted the hidmic/dvl branch March 8, 2023 22:51

Provide feedback

Saved searches

Use saved searches to filter your results more quickly

Add DopplerVelocityLogSystem plugin #1804

Add DopplerVelocityLogSystem plugin #1804

hidmic commented Nov 21, 2022

hidmic commented Nov 21, 2022 •

edited

Loading

arjo129 commented Nov 23, 2022

hidmic commented Dec 9, 2022

iche033 commented Jan 31, 2023

iche033 commented Feb 2, 2023 •

edited

Loading

codecov bot commented Feb 2, 2023 •

edited

Loading

iche033 commented Feb 3, 2023 •

edited

Loading

iche033 commented Feb 8, 2023

iche033 commented Feb 15, 2023 •

edited

Loading

iche033 commented Feb 16, 2023

hidmic left a comment

hidmic Feb 17, 2023

iche033 Feb 17, 2023

hidmic Feb 17, 2023

hidmic Feb 17, 2023

iche033 Feb 17, 2023

hidmic Feb 17, 2023

iche033 Feb 17, 2023

hidmic Feb 17, 2023

iche033 Feb 17, 2023

iche033 commented Feb 18, 2023

iche033 commented Feb 18, 2023 •

edited

Loading

iche033 commented Mar 8, 2023

Add DopplerVelocityLogSystem plugin #1804

Add DopplerVelocityLogSystem plugin #1804

Conversation

hidmic commented Nov 21, 2022

🎉 New feature

Summary

Test it

Checklist

hidmic commented Nov 21, 2022 • edited Loading

arjo129 commented Nov 23, 2022

hidmic commented Dec 9, 2022

iche033 commented Jan 31, 2023

iche033 commented Feb 2, 2023 • edited Loading

codecov bot commented Feb 2, 2023 • edited Loading

Codecov Report

iche033 commented Feb 3, 2023 • edited Loading

iche033 commented Feb 8, 2023

iche033 commented Feb 15, 2023 • edited Loading

iche033 commented Feb 16, 2023

hidmic left a comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

iche033 commented Feb 18, 2023

iche033 commented Feb 18, 2023 • edited Loading

iche033 commented Mar 8, 2023

hidmic commented Nov 21, 2022 •

edited

Loading

iche033 commented Feb 2, 2023 •

edited

Loading

codecov bot commented Feb 2, 2023 •

edited

Loading

iche033 commented Feb 3, 2023 •

edited

Loading

iche033 commented Feb 15, 2023 •

edited

Loading

iche033 commented Feb 18, 2023 •

edited

Loading