Adding MecanumDrive plugin with Odom and Tf, with Tests. #2297

muttistefano · 2024-01-24T10:04:31Z

🎉 New feature

Summary

Adding MecanumDrive plugin with Odom and Tf, with Tests.
Relative to #1665 , but targets gz-sim8, fixes an include error in the plugin and add tests.

Checklist

[ x] Signed all commits for DCO
[ x] 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.

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

azeey · 2024-02-02T17:57:48Z

Thanks for your contribution @muttistefano. Do you mind fixing the linter issues and signing off on your commit (see https://github.com/gazebosim/gz-sim/pull/2297/checks?check_run_id=20866188364).

muttistefano · 2024-02-09T07:34:18Z

Hi @azeey , thanks for the answer.
I signed the commits but I don't understand the linter issue.

azeey · 2024-02-12T18:48:56Z

There are some trailing white spaces in the code (see https://github.com/gazebosim/gz-sim/actions/runs/7765649621/job/21405089973?pr=2297).

Signed-off-by: Stefano Mutti <[email protected]>

will-44 · 2024-05-24T15:30:45Z

Hello !
I'm very interested in this PR (as I think a lot of people are). Do you think it could be merged into the main version?
Thank you very much for your help

muttistefano · 2024-05-29T12:51:39Z

If I'm not mistaken, the checks are fine, the only thing missing is the review from @mjcarroll .
Thanks :)

azeey

@muttistefano I think you've accidentally checked in a few files and directories (e.g. log, .vscode) to the pull request. Could you please remove them?

muttistefano · 2024-05-29T17:31:34Z

My bad; it should be fixed now.

azeey · 2024-06-18T19:36:08Z

Can you fix DCO?

Signed-off-by: Stefano Mutti <[email protected]>

test/integration/mecanum_drive_system.cc

azeey · 2024-08-08T21:50:39Z

test/integration/mecanum_drive_system.cc

+#include "gz/sim/components/Model.hh"
+#include "gz/sim/components/Pose.hh"
+#include "gz/sim/Server.hh"
+#include "gz/sim/SystemLoader.hh"


This header seems to be unused. Can you remove?

azeey · 2024-08-08T21:52:00Z

test/integration/mecanum_drive_system.cc

+      // EXPECT_NEAR(poses.back().Pos().X(), finalModelFramePose.Pos().X(), 1e-2);
+      // EXPECT_NEAR(poses.back().Pos().Y(), finalModelFramePose.Pos().Y(), 1e-2);


Is there a reason for commenting these out? finalModelFramePose is now an unused variable.

azeey · 2024-08-08T21:55:30Z

test/integration/mecanum_drive_system.cc

+/////////////////////////////////////////////////
+// See: https://github.com/gazebosim/gz-sim/issues/1175
+// See: https://github.com/gazebosim/gz-sim/issues/630
+TEST_P(MecanumDriveTest, GZ_UTILS_TEST_ENABLED_ONLY_ON_LINUX(PublishCmd))


I know this is copied from the DiffDrive test, but can we first try to enable it and see if it doesn't work on the other platforms?

glpuga · 2024-08-18T18:02:05Z

test/integration/mecanum_drive_system.cc

+  testCmdVel(true /*test forward movement*/);
+  testCmdVel(false /*test backward movement*/);


I was unaware that this MR existed and basically did the exact same work two weeks ago. I also refactored the TestPublishCmd function to also test the sideways, diagonal and rotational movement of the drive instead of only forward and backward, which is what the diff drive test does.

Feel free to grab the function code from my changes in here to improve the coverage.

glpuga · 2024-08-18T18:09:51Z

test/integration/mecanum_drive_system.cc

+      // Odometry calculates the pose of a point that is located half way
+      // between the two wheels, not the origin of the model. For example,
+      // if the vehicle is commanded to rotate in place, the vehicle will
+      // rotate about the point half way between the two wheels, thus,
+      // the odometry position will remain zero.
+      // However, since the model origin is offset, the model position will
+      // change. To find the final pose of the model, we have to do the
+      // following similarity transformation
+
+      math::Pose3d tOdomModel(0.554283,0,-0.325,0,0,0);


This transform is correct for the differential drive model, but not for the mecanum one. The mecanum math derivation places the origin at the center of the four wheels, which for the model in the xml is at math::Pose3d tOdomModel(-0.2, 0., 0., 0., 0., 0.);

This is not noticed in the test probably because no rotation is being applied to the drive.

garyservin · 2024-11-08T14:41:29Z

any updates on this? It would be really great to have this implemented! Thanks!

ahcorde · 2024-11-08T14:47:28Z

src/systems/mecanum_drive/MecanumDrive.cc

+    return;
+  }
+
+  if (this->frontLeftJoints.empty() || this->frontRightJoints.empty() || this->backLeftJoints.empty() || this->backRightJoints.empty())


please use lines with less than 100 characters

ahcorde · 2024-11-08T14:47:38Z

src/systems/mecanum_drive/MecanumDrive.cc

+    return;
+  }
+
+  this->odom.Update(frontLeftPos->Data()[0],frontRightPos->Data()[0],backLeftPos->Data()[0],backRightPos->Data()[0],std::chrono::steady_clock::time_point(_info.simTime)); 


include <chrono> and lines < 100

ahcorde · 2024-11-08T14:51:22Z

test/integration/mecanum_drive_system.cc

+  /// \param[in] _sdfFile SDF file to load.
+  /// \param[in] _cmdVelTopic Command velocity topic.
+  /// \param[in] _odomTopic Odometry topic.
+  protected: void TestPublishCmd(const std::string &_sdfFile,


include <string>

ahcorde · 2024-11-08T14:51:31Z

test/integration/mecanum_drive_system.cc

+      // Create a system that records the vehicle poses
+      test::Relay testSystem;
+
+      std::vector<math::Pose3d> poses;


include <vector>

ahcorde · 2024-11-08T14:51:58Z

test/integration/mecanum_drive_system.cc

+      double period{1.0 / 50.0};
+      double lastMsgTime{1.0};
+      std::vector<math::Pose3d> odomPoses;
+      std::function<void(const msgs::Odometry &)> odomCb =


include <functional>

ahcorde · 2024-11-08T14:59:21Z

test/integration/mecanum_drive_system.cc

+  // Start server
+  ServerConfig serverConfig;
+  serverConfig.SetSdfFile(std::string(PROJECT_SOURCE_PATH) +
+      "/test/worlds/mecanum_drive.sdf");


use gz::common::joinPath()

ahcorde · 2024-11-08T14:59:31Z

test/integration/mecanum_drive_system.cc

+  // Start server
+  ServerConfig serverConfig;
+  serverConfig.SetSdfFile(std::string(PROJECT_SOURCE_PATH) +
+      "/test/worlds/mecanum_drive_custom_frame_id.sdf");


use gz::common::joinPath()

ahcorde · 2024-11-08T14:59:38Z

test/integration/mecanum_drive_system.cc

+  // Start server
+  ServerConfig serverConfig;
+  serverConfig.SetSdfFile(std::string(PROJECT_SOURCE_PATH) +
+      "/test/worlds/mecanum_drive_custom_tf_topic.sdf");


use gz::common::joinPath()

ahcorde · 2024-11-08T15:00:23Z

test/integration/mecanum_drive_system.cc

+
+  msgs::Twist msg;
+  msgs::Set(msg.mutable_linear(), math::Vector3d(0.5, 0, 0));
+  msgs::Set(msg.mutable_angular(), math::Vector3d(0.0, 0, 0.2));


include `gz::math::Vector3.hh

ahcorde · 2024-11-08T15:00:40Z

test/worlds/mecanum_drive.sdf

+      </link>
+
+
+      <joint name='front_left_wheel_joint' type='revolute'>


Suggested change

</link>

<joint name='front_left_wheel_joint' type='revolute'>

</link>

<joint name='front_left_wheel_joint' type='revolute'>

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

muttistefano · 2024-11-08T15:24:03Z

I cannot work anymore on gz-sim8 cause I don't have an Ubuntu 22 PC.
If I move the pull base to gz-sim9 do I mess up something?
Thanks

azeey · 2024-11-11T16:51:40Z

@muttistefano, gz-sim8 is supported on Ubuntu 24.04 too.

muttistefano requested a review from mjcarroll as a code owner January 24, 2024 10:04

github-actions bot added the 🎵 harmonic Gazebo Harmonic label Jan 24, 2024

muttistefano force-pushed the gz-sim8 branch from 2fd86f1 to 19db475 Compare February 3, 2024 08:16

Stefano Mutti and others added 9 commits March 5, 2024 10:54

mecanum src added

3d13216

Signed-off-by: Stefano Mutti <[email protected]>

mecanum src added

63db52b

Signed-off-by: Stefano Mutti <[email protected]>

mecanum src added

fdc24cb

Signed-off-by: Stefano Mutti <[email protected]>

Added MecanumDrive plugin and Tests with relative test sdf

c1a98b2

Signed-off-by: Stefano Mutti <[email protected]>

Added MecanumDrive plugin and Tests with relative test sdf

7a9e1bb

Signed-off-by: Stefano Mutti <[email protected]>

Corrected setWheelParams

6c9cec1

Signed-off-by: Stefano Mutti <[email protected]>

Corrected setWheelParams

740c45e

Signed-off-by: Stefano Mutti <[email protected]>

remove white spaces

fe5d267

Signed-off-by: Stefano Mutti <[email protected]>

remove white spaces

ee0fc38

Signed-off-by: Stefano Mutti <[email protected]>

muttistefano force-pushed the gz-sim8 branch from 62b2c77 to ee0fc38 Compare March 5, 2024 09:54

muttistefano added 3 commits March 5, 2024 10:59

remove white spaces

5623fba

Signed-off-by: Stefano Mutti <[email protected]>

remove unused var in test

e0d5a21

Signed-off-by: Stefano Mutti <[email protected]>

Merge branch 'gazebosim:gz-sim8' into gz-sim8

5051018

Merge branch 'gazebosim:gz-sim8' into gz-sim8

f835ff1

azeey requested changes May 29, 2024

View reviewed changes

azeey self-requested a review June 10, 2024 18:55

removed .vscode and log

24d708e

Signed-off-by: Stefano Mutti <[email protected]>

muttistefano force-pushed the gz-sim8 branch from 8fff4de to 24d708e Compare June 19, 2024 06:31

azeey added the beta Targeting beta release of upcoming collection label Jul 29, 2024

azeey mentioned this pull request Aug 8, 2024

MecanumDrive odometry and tf functions implementation #1665

Closed

9 tasks

azeey reviewed Aug 8, 2024

View reviewed changes

Merge branch 'gz-sim8' into gz-sim8

2f6773a

glpuga reviewed Aug 18, 2024

View reviewed changes

azeey removed the beta Targeting beta release of upcoming collection label Aug 26, 2024

ahcorde requested changes Nov 8, 2024

View reviewed changes

muttistefano and others added 3 commits November 8, 2024 16:06

Update test/integration/mecanum_drive_system.cc

d075b98

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

Update test/integration/mecanum_drive_system.cc

c69c40a

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

Merge branch 'gazebosim:gz-sim8' into gz-sim8

de51e1a

Provide feedback

Saved searches

Use saved searches to filter your results more quickly

Adding MecanumDrive plugin with Odom and Tf, with Tests. #2297

Adding MecanumDrive plugin with Odom and Tf, with Tests. #2297

muttistefano commented Jan 24, 2024 •

edited

Loading

azeey commented Feb 2, 2024

muttistefano commented Feb 9, 2024

azeey commented Feb 12, 2024

will-44 commented May 24, 2024

muttistefano commented May 29, 2024

azeey left a comment

muttistefano commented May 29, 2024

azeey commented Jun 18, 2024

azeey Aug 8, 2024

azeey Aug 8, 2024

azeey Aug 8, 2024

glpuga Aug 18, 2024

glpuga Aug 18, 2024

garyservin commented Nov 8, 2024

ahcorde Nov 8, 2024

ahcorde Nov 8, 2024

ahcorde Nov 8, 2024

ahcorde Nov 8, 2024

ahcorde Nov 8, 2024

ahcorde Nov 8, 2024

ahcorde Nov 8, 2024

ahcorde Nov 8, 2024

ahcorde Nov 8, 2024

ahcorde Nov 8, 2024

muttistefano commented Nov 8, 2024

azeey commented Nov 11, 2024

		// EXPECT_NEAR(poses.back().Pos().X(), finalModelFramePose.Pos().X(), 1e-2);
		// EXPECT_NEAR(poses.back().Pos().Y(), finalModelFramePose.Pos().Y(), 1e-2);

		testCmdVel(true /test forward movement/);
		testCmdVel(false /test backward movement/);

		</link>


		<joint name='front_left_wheel_joint' type='revolute'>

Adding MecanumDrive plugin with Odom and Tf, with Tests. #2297

Are you sure you want to change the base?

Adding MecanumDrive plugin with Odom and Tf, with Tests. #2297

Conversation

muttistefano commented Jan 24, 2024 • edited Loading

🎉 New feature

Summary

Checklist

azeey commented Feb 2, 2024

muttistefano commented Feb 9, 2024

azeey commented Feb 12, 2024

will-44 commented May 24, 2024

muttistefano commented May 29, 2024

azeey left a comment

Choose a reason for hiding this comment

muttistefano commented May 29, 2024

azeey commented Jun 18, 2024

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

garyservin commented Nov 8, 2024

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

muttistefano commented Nov 8, 2024

azeey commented Nov 11, 2024

muttistefano commented Jan 24, 2024 •

edited

Loading