Adding MecanumDrive plugin with Odom and Tf, with Tests. #2297
Conversation
|
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). |
|
Hi @azeey , thanks for the answer. |
|
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]>
Signed-off-by: Stefano Mutti <[email protected]>
Signed-off-by: Stefano Mutti <[email protected]>
Signed-off-by: Stefano Mutti <[email protected]>
Signed-off-by: Stefano Mutti <[email protected]>
Signed-off-by: Stefano Mutti <[email protected]>
Signed-off-by: Stefano Mutti <[email protected]>
Signed-off-by: Stefano Mutti <[email protected]>
Signed-off-by: Stefano Mutti <[email protected]>
Signed-off-by: Stefano Mutti <[email protected]>
Signed-off-by: Stefano Mutti <[email protected]>
|
Hello ! |
|
If I'm not mistaken, the checks are fine, the only thing missing is the review from @mjcarroll . |
There was a problem hiding this comment.
@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?
|
My bad; it should be fixed now. |
|
Can you fix DCO? |
Signed-off-by: Stefano Mutti <[email protected]>
| @@ -0,0 +1,537 @@ | |||
| /* | |||
| * Copyright (C) 2018 Open Source Robotics Foundation | |||
There was a problem hiding this comment.
| * Copyright (C) 2018 Open Source Robotics Foundation | |
| * Copyright (C) 2024 Open Source Robotics Foundation |
| using namespace std::chrono_literals; | ||
|
|
||
| /// \brief Test MecanumDrive system | ||
| class MecanumDriveTest : public InternalFixture<::testing::TestWithParam<int>> |
There was a problem hiding this comment.
I don't think we need to run this multiple times. Can you change this to
| class MecanumDriveTest : public InternalFixture<::testing::TestWithParam<int>> | |
| class MecanumDriveTest : public InternalFixture<::testing::Test> |
and change all the TEST_P in this file to TEST_F
| #include "gz/sim/components/Model.hh" | ||
| #include "gz/sim/components/Pose.hh" | ||
| #include "gz/sim/Server.hh" | ||
| #include "gz/sim/SystemLoader.hh" |
There was a problem hiding this comment.
This header seems to be unused. Can you remove?
| // EXPECT_NEAR(poses.back().Pos().X(), finalModelFramePose.Pos().X(), 1e-2); | ||
| // EXPECT_NEAR(poses.back().Pos().Y(), finalModelFramePose.Pos().Y(), 1e-2); |
There was a problem hiding this comment.
Is there a reason for commenting these out? finalModelFramePose is now an unused variable.
| ///////////////////////////////////////////////// | ||
| // 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)) |
There was a problem hiding this comment.
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?
| testCmdVel(true /*test forward movement*/); | ||
| testCmdVel(false /*test backward movement*/); |
There was a problem hiding this comment.
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.
| // 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); |
There was a problem hiding this comment.
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.
🎉 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
codecheckpassed (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-bymessages.🔸🔸🔸🔸🔸🔸🔸🔸🔸🔸🔸🔸🔸🔸🔸🔸🔸🔸🔸🔸🔸🔸🔸🔸🔸🔸🔸🔸🔸🔸🔸🔸🔸🔸🔸🔸🔸