MecanumDriveOdometry to handle odometry estimation of Mecanum wheeled models

[danilogsch]

🎉 New feature

Closes #683

Summary

Math Class to handle odometry estimation of Mecanum wheeled models.
Observation: Aproximation when the robot is given both linear, lateral and angular velocity commands. Exact pose integration or Runge Kuffa second order integration happens if one the velocity straight commands (linear or lateral) is 0.

Test it

run mecanum_drive.sdf and listen the odometry and tf topics.

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

[ahcorde]

Please check

• linters https://github.com/gazebosim/gz-math/runs/8020312206?check_suite_focus=true
• Add tests
• Sign the commit https://github.com/gazebosim/gz-math/pull/486/checks?check_run_id=8020243561

We are also adding python interfaces to new classes, if you don't have time to do it, please open an issue about adding python interfaces.

[danilogsch]

depends on #1664 of gz-sim

[ahcorde]

linters https://github.com/gazebosim/gz-math/actions/runs/3534393561/jobs/5931205749

  /github/workspace/include/ignition/math/MecanumDriveOdometry.hh:81:  Lines should be <= 80 characters long  [whitespace/line_length] [2]
  /github/workspace/src/MecanumDriveOdometry.cc:36:  Line ends in whitespace.  Consider deleting these extra spaces.  [whitespace/end_of_line] [4]
  /github/workspace/src/MecanumDriveOdometry.cc:41:  Lines should be <= 80 characters long  [whitespace/line_length] [2]
  /github/workspace/src/MecanumDriveOdometry.cc:144:  Lines should be <= 80 characters long  [whitespace/line_length] [2]
  /github/workspace/src/MecanumDriveOdometry.cc:152:  Lines should be <= 80 characters long  [whitespace/line_length] [2]
  /github/workspace/src/MecanumDriveOdometry.cc:153:  Lines should be <= 80 characters long  [whitespace/line_length] [2]
  /github/workspace/src/MecanumDriveOdometry.cc:154:  Lines should be <= 80 characters long  [whitespace/line_length] [2]
  /github/workspace/src/MecanumDriveOdometry.cc:155:  Lines should be <= 80 characters long  [whitespace/line_length] [2]
  /github/workspace/src/MecanumDriveOdometry.cc:176:  Lines should be <= 80 characters long  [whitespace/line_length] [2]
  /github/workspace/src/MecanumDriveOdometry.cc:177:  Lines should be <= 80 characters long  [whitespace/line_length] [2]
  /github/workspace/src/MecanumDriveOdometry.cc:180:  Lines should be <= 80 characters long  [whitespace/line_length] [2]
  /github/workspace/src/MecanumDriveOdometry.cc:181:  Lines should be <= 80 characters long  [whitespace/line_length] [2]
  /github/workspace/src/MecanumDriveOdometry.cc:182:  Lines should be <= 80 characters long  [whitespace/line_length] [2]
  /github/workspace/src/MecanumDriveOdometry.cc:206:  Lines should be <= 80 characters long  [whitespace/line_length] [2]
  /github/workspace/src/MecanumDriveOdometry.cc:272:  Lines should be <= 80 characters long  [whitespace/line_length] [2]
  /github/workspace/src/MecanumDriveOdometry.cc:285:  Missing username in TODO; it should look like "// TODO(my_username): Stuff."  [readability/todo] [2]
  /github/workspace/src/MecanumDriveOdometry.cc:286:  Lines should be <= 80 characters long  [whitespace/line_length] [2]
  /github/workspace/src/MecanumDriveOdometry.cc:287:  Lines should be <= 80 characters long  [whitespace/line_length] [2]
  Total errors found: 18

[ahcorde]

linters https://github.com/gazebosim/gz-math/actions/runs/3583238905/jobs/6034063390

[azeey]

@danilogsch Do you think you will have time to fix the linter issues and address @ahcorde's feedback ?

[mjcarroll]

@danilogsch friendly ping

[danilogsch]

hey, sorry that I haven't reached here in a while... Is there anything else you need me to do here?

[Bi0T1N]

Looks like linter errors are resolved, only Ubuntu Bionic lists

/github/workspace/include/ignition/math/MecanumDriveOdometry.hh:81:  Line ends in whitespace.  Consider deleting these extra spaces.  [whitespace/end_of_line] [4]
/github/workspace/include/ignition/math/MecanumDriveOdometry.hh:81:  Lines should be <= 80 characters long  [whitespace/line_length] [2]

but this could probably be neglected since Focal/Jammy don't complain.

However, you have to add tests - probably similar to the ones in DiffDriveOdometry_TEST.cc.
Also your last commit isn't signed but one option could be to amend, sign and force-pushing the new commit to your branch.

[danilogsch]

@Bi0T1N , thanks for the insight... I had no idea on how to deal with unsigned commits.
Anyway, I have time this week and I will work on the tests.

[muttistefano]

Hi, is there anything I can do to help here?
I have been waiting for this to be merged for a while, so if I can help, I would be glad.
Thanks

[danilogsch]

Hi, is there anything I can do to help here? I have been waiting for this to be merged for a while, so if I can help, I would be glad. Thanks

Hey, thanks for asking... could you help me with the tests? I've been using Garden in my research, I did not have time to look into the DiffDriveOdometry_TEST.cc and to mimic that just yet

[muttistefano]

ok,i can take a look into that.
I have a question regarding the pull, I see it targets ign-math6, this means that after that we will have to do another one into the gz-math7 branch ?
Thanks

[Bi0T1N]

I have a question regarding the pull, I see it targets ign-math6, this means that after that we will have to do another one into the gz-math7 branch ? Thanks

Yes, as ign-math6 is only for Citadel and Fortress. The merge into gz-math7 should be quite simple as it doesn't touch many other files but it should be rebased on the current HEAD due to the ignition to gz renaming.

[danilogsch]

ok,i can take a look into that. I have a question regarding the pull, I see it targets ign-math6, this means that after that we will have to do another one into the gz-math7 branch ? Thanks

I made this two branches when Garden was released:
gz-sim7
gz-math7

basic the same as this PR, feel free to use it

[mjcarroll]

I just addressed the linting issues here and pulled in @muttistefano's test from #537

[azeey]

I haven't verified with gz-sim, but I've looked at the equations and the referenced website and it all looks good to me. I'd say the assumption of a 1.0 wheel radius should be explicitly stated in the class header and in the gz-sim system that uses this.