From 0cbae95ae689dc66412433c49a6c95151fddc326 Mon Sep 17 00:00:00 2001 From: Stefano Mutti Date: Wed, 8 Jan 2025 10:52:40 +0100 Subject: [PATCH] pull fixes --- src/systems/mecanum_drive/MecanumDrive.cc | 12 +- test/integration/mecanum_drive_system.cc | 165 +++++++++++------- test/worlds/mecanum_drive.sdf | 39 ++--- test/worlds/mecanum_drive_custom_frame_id.sdf | 41 ++--- test/worlds/mecanum_drive_custom_tf_topic.sdf | 41 ++--- test/worlds/mecanum_drive_custom_topics.sdf | 39 ++--- 6 files changed, 177 insertions(+), 160 deletions(-) diff --git a/src/systems/mecanum_drive/MecanumDrive.cc b/src/systems/mecanum_drive/MecanumDrive.cc index b1d798404e..5d81d2cf0a 100644 --- a/src/systems/mecanum_drive/MecanumDrive.cc +++ b/src/systems/mecanum_drive/MecanumDrive.cc @@ -26,6 +26,7 @@ #include #include #include +#include #include #include @@ -521,7 +522,10 @@ void MecanumDrivePrivate::UpdateOdometry(const UpdateInfo &_info, return; } - if (this->frontLeftJoints.empty() || this->frontRightJoints.empty() || this->backLeftJoints.empty() || this->backRightJoints.empty()) + if (this->frontLeftJoints.empty() || + this->frontRightJoints.empty() || + this->backLeftJoints.empty() || + this->backRightJoints.empty()) return; // Get the first joint positions for each wheel joint. @@ -537,7 +541,11 @@ void MecanumDrivePrivate::UpdateOdometry(const UpdateInfo &_info, return; } - this->odom.Update(frontLeftPos->Data()[0],frontRightPos->Data()[0],backLeftPos->Data()[0],backRightPos->Data()[0],std::chrono::steady_clock::time_point(_info.simTime)); + this->odom.Update(frontLeftPos->Data()[0], + frontRightPos->Data()[0], + backLeftPos->Data()[0], + backRightPos->Data()[0], + std::chrono::steady_clock::time_point(_info.simTime)); // Throttle publishing auto diff = _info.simTime - this->lastOdomPubTime; diff --git a/test/integration/mecanum_drive_system.cc b/test/integration/mecanum_drive_system.cc index 2f51e12a18..a735021fbe 100644 --- a/test/integration/mecanum_drive_system.cc +++ b/test/integration/mecanum_drive_system.cc @@ -1,5 +1,5 @@ /* - * Copyright (C) 2018 Open Source Robotics Foundation + * Copyright (C) 2024 Open Source Robotics Foundation * * Licensed under the Apache License, Version 2.0 (the "License"); * you may not use this file except in compliance with the License. @@ -17,6 +17,11 @@ #include +#include +#include +#include +#include + #include #include #include @@ -24,15 +29,17 @@ #include #include #include +#include #include #include + #include "gz/sim/components/Name.hh" +#include "test_config.hh" + #include "gz/sim/components/Model.hh" #include "gz/sim/components/Pose.hh" #include "gz/sim/Server.hh" -#include "gz/sim/SystemLoader.hh" -#include "test_config.hh" #include "../helpers/Relay.hh" #include "../helpers/EnvTestFixture.hh" @@ -53,11 +60,13 @@ class MecanumDriveTest : public InternalFixture<::testing::TestWithParam> const std::string &_cmdVelTopic, const std::string &_odomTopic) { - /// \param[in] forward If forward is true, the 'max_acceleration' - // and 'max_velocity' properties are tested, as the movement - // is forward, otherwise 'min_acceleration' and 'min_velocity' - // properties are tested. - auto testCmdVel = [&](bool forward){ + /// \param[in] fb_component forward/backward motion vector component. + /// \param[in] lr_component left/right motion vector component. + /// \param[in] yaw_component yaw rotation component. + auto testCmdVel = [&](double fb_component, + double lr_component, + double yaw_component) + { // Start server ServerConfig serverConfig; serverConfig.SetSdfFile(_sdfFile); @@ -125,19 +134,22 @@ class MecanumDriveTest : public InternalFixture<::testing::TestWithParam> // Avoid wheel slip by limiting acceleration (1 m/s^2) // and max velocity (0.5 m/s). // See and parameters - // in "/test/worlds/diff_drive.sdf". + // in "/test/worlds/mecanum_drive.sdf". // See , , and - // parameters in "/test/worlds/diff_drive.sdf". + // parameters in "/test/worlds/mecanum_drive.sdf". test::Relay velocityRamp; - const int movementDirection = (forward ? 1 : -1); - double desiredLinVelX = movementDirection * 0.15; - double desiredLinVelY = movementDirection * 0.15; + double desiredLinVelX = fb_component; + double desiredLinVelY = lr_component; + double desiredAngVel = yaw_component; + velocityRamp.OnPreUpdate( [&](const UpdateInfo &/*_info*/, const EntityComponentManager &) { msgs::Set(msg.mutable_linear(), math::Vector3d(desiredLinVelX, desiredLinVelY, 0)); + msgs::Set(msg.mutable_angular(), + math::Vector3d(0.0, 0, desiredAngVel)); pub.Publish(msg); }); @@ -165,7 +177,7 @@ class MecanumDriveTest : public InternalFixture<::testing::TestWithParam> // 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); + math::Pose3d tOdomModel(-0.2,0,0,0,0,0); auto finalModelFramePose = tOdomModel * odomPoses.back() * tOdomModel.Inverse(); @@ -173,76 +185,101 @@ class MecanumDriveTest : public InternalFixture<::testing::TestWithParam> ASSERT_FALSE(odomPoses.empty()); EXPECT_EQ(150u, odomPoses.size()); - auto expectedLowerPosition = - (forward ? poses[0].Pos() : poses[3999].Pos()); - auto expectedGreaterPosition = - (forward ? poses[3999].Pos() : poses[0].Pos()); + auto relativeMotionPos = poses[3999].Pos() - poses[0].Pos(); + auto relativeMotionRot = poses[3999].Rot().Yaw() - poses[0].Rot().Yaw(); + + // if the linear motion is non-zero + // the vehicle should move in the commanded direction + if (std::abs(fb_component) > 0.0 || std::abs(lr_component) > 0.0) + { + auto cmdVelDir = math::Vector2(desiredLinVelX, + desiredLinVelY).Normalized(); + auto motionDir = math::Vector2(relativeMotionPos.X(), + relativeMotionPos.Y()).Normalized(); + auto scalarProjection = cmdVelDir.Dot(motionDir); + EXPECT_GT(motionDir.Length(), 0.5); // is there motion? + EXPECT_GT(scalarProjection, 0.9); // is it in the right direction? + + // Verify velocity and acceleration upper boundaries. + double t = 3.0; + double d = poses[3999].Pos().Distance(poses[0].Pos()); + double v0 = 0; + double v = d / t; + double a = (v - v0) / t; + + // The vehicle should not exceed the max velocity and acceleration + // notice the limits for the x and y direction are separately + // enforced + auto threshold_factor = math::Vector2(fb_component, + lr_component).Length(); + + EXPECT_LT(v, 0.5 * threshold_factor); + EXPECT_LT(a, 1.0 * threshold_factor); + EXPECT_GT(v, -0.5 * threshold_factor); + EXPECT_GT(a, -1.0 * threshold_factor); + } - EXPECT_LT(expectedLowerPosition.X(), expectedGreaterPosition.X()); - EXPECT_LT(expectedLowerPosition.Y(), expectedGreaterPosition.Y()); - EXPECT_NEAR(expectedLowerPosition.Z(), expectedGreaterPosition.Z(), tol); - EXPECT_NEAR(poses[0].Rot().X(), poses[3999].Rot().X(), tol); - EXPECT_NEAR(poses[0].Rot().Y(), poses[3999].Rot().Y(), tol); - EXPECT_NEAR(poses[0].Rot().Z(), poses[3999].Rot().Z(), tol); + // The vehicle should rotate in the commanded direction + if (std::abs(yaw_component) > 0.0) + { + // the relative rotation has the same sign as the commanded rotation + EXPECT_GT(desiredAngVel * relativeMotionRot, 0.0); + } // The value from odometry will be close, but not exactly the ground truth // pose of the robot model. This is partially due to throttling the - // odometry publisher, partially due to a frame difference between the - // odom frame and the model frame, and partially due to wheel slip. - EXPECT_NEAR(poses.back().Pos().X(), odomPoses.back().Pos().X(), 1e-2); - EXPECT_NEAR(poses.back().Pos().Y(), odomPoses.back().Pos().Y(), 1e-2); - // EXPECT_NEAR(poses.back().Pos().X(), finalModelFramePose.Pos().X(), 1e-2); - // EXPECT_NEAR(poses.back().Pos().Y(), finalModelFramePose.Pos().Y(), 1e-2); - - // Verify velocity and acceleration boundaries. - // Moving time. - double t = 3.0; - double d = poses[3999].Pos().Distance(poses[0].Pos()); - double v0 = 0; - double v = d / t; - double a = (v - v0) / t; - - EXPECT_LT(v, 0.5); - EXPECT_LT(a, 1); - EXPECT_GT(v, -0.5); - EXPECT_GT(a, -1); + // odometry publisher, and partially due to wheel slip. + EXPECT_NEAR(poses[1020].Pos().X(), odomPoses[0].Pos().X(), 1e-2); + EXPECT_NEAR(poses[1020].Pos().Y(), odomPoses[0].Pos().Y(), 1e-2); + 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*/); + testCmdVel(0., 0., 0.); /* no motion */ + + testCmdVel(+1., 0., 0.); /* pure forward */ + testCmdVel(-1., 0., 0.); /* pure backward */ + testCmdVel(0., +1., 0.); /* pure left */ + testCmdVel(0., -1., 0.); /* pure right */ + + testCmdVel(+1., +1., 0.); /* forward left */ + testCmdVel(+1., -1., 0.); /* forward right */ + testCmdVel(-1., +1., 0.); /* backward left */ + testCmdVel(-1., -1., 0.); /* backward right */ + + testCmdVel(0., 0., +0.1); /* CW motion */ + testCmdVel(0., 0., -0.1); /* CCW motion */ } }; ///////////////////////////////////////////////// -// 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)) { - TestPublishCmd( - std::string(PROJECT_SOURCE_PATH) + "/test/worlds/mecanum_drive.sdf", + TestPublishCmd(common::joinPaths( + std::string(PROJECT_SOURCE_PATH) + "/test/worlds/mecanum_drive.sdf"), "/model/vehicle_blue/cmd_vel", "/model/vehicle_blue/odometry"); } ///////////////////////////////////////////////// -// See: https://github.com/gazebosim/gz-sim/issues/630 TEST_P(MecanumDriveTest, GZ_UTILS_TEST_ENABLED_ONLY_ON_LINUX(PublishCmdCustomTopics)) { - TestPublishCmd( + TestPublishCmd(common::joinPaths( std::string(PROJECT_SOURCE_PATH) + - "/test/worlds/mecanum_drive_custom_topics.sdf", + "/test/worlds/mecanum_drive_custom_topics.sdf"), "/model/foo/cmdvel", "/model/bar/odom"); } -///////////////////////////////////////////////// +// ///////////////////////////////////////////////// TEST_P(MecanumDriveTest, GZ_UTILS_TEST_DISABLED_ON_WIN32(OdomFrameId)) { // Start server ServerConfig serverConfig; - serverConfig.SetSdfFile(std::string(PROJECT_SOURCE_PATH) + - "/test/worlds/mecanum_drive.sdf"); + serverConfig.SetSdfFile(common::joinPaths(std::string(PROJECT_SOURCE_PATH) + + "/test/worlds/mecanum_drive.sdf")); Server server(serverConfig); EXPECT_FALSE(server.Running()); @@ -294,13 +331,13 @@ TEST_P(MecanumDriveTest, GZ_UTILS_TEST_DISABLED_ON_WIN32(OdomFrameId)) EXPECT_EQ(5u, odomPosesCount); } -///////////////////////////////////////////////// +// ///////////////////////////////////////////////// TEST_P(MecanumDriveTest, GZ_UTILS_TEST_DISABLED_ON_WIN32(OdomCustomFrameId)) { // Start server ServerConfig serverConfig; - serverConfig.SetSdfFile(std::string(PROJECT_SOURCE_PATH) + - "/test/worlds/mecanum_drive_custom_frame_id.sdf"); + serverConfig.SetSdfFile(common::joinPaths(std::string(PROJECT_SOURCE_PATH) + + "/test/worlds/mecanum_drive_custom_frame_id.sdf")); Server server(serverConfig); EXPECT_FALSE(server.Running()); @@ -356,8 +393,8 @@ TEST_P(MecanumDriveTest, GZ_UTILS_TEST_DISABLED_ON_WIN32(Pose_VFrameId)) { // Start server ServerConfig serverConfig; - serverConfig.SetSdfFile(std::string(PROJECT_SOURCE_PATH) + - "/test/worlds/mecanum_drive.sdf"); + serverConfig.SetSdfFile(common::joinPaths(std::string(PROJECT_SOURCE_PATH) + + "/test/worlds/mecanum_drive.sdf")); Server server(serverConfig); EXPECT_FALSE(server.Running()); @@ -416,8 +453,8 @@ TEST_P(MecanumDriveTest, GZ_UTILS_TEST_DISABLED_ON_WIN32(Pose_VCustomFrameId)) { // Start server ServerConfig serverConfig; - serverConfig.SetSdfFile(std::string(PROJECT_SOURCE_PATH) + - "/test/worlds/mecanum_drive_custom_frame_id.sdf"); + serverConfig.SetSdfFile(common::joinPaths(std::string(PROJECT_SOURCE_PATH) + + "/test/worlds/mecanum_drive_custom_frame_id.sdf")); Server server(serverConfig); EXPECT_FALSE(server.Running()); @@ -476,8 +513,8 @@ TEST_P(MecanumDriveTest, GZ_UTILS_TEST_DISABLED_ON_WIN32(Pose_VCustomTfTopic)) { // Start server ServerConfig serverConfig; - serverConfig.SetSdfFile(std::string(PROJECT_SOURCE_PATH) + - "/test/worlds/mecanum_drive_custom_tf_topic.sdf"); + serverConfig.SetSdfFile(common::joinPaths(std::string(PROJECT_SOURCE_PATH) + + "/test/worlds/mecanum_drive_custom_tf_topic.sdf")); Server server(serverConfig); EXPECT_FALSE(server.Running()); diff --git a/test/worlds/mecanum_drive.sdf b/test/worlds/mecanum_drive.sdf index 56a854de08..5af96a7d21 100644 --- a/test/worlds/mecanum_drive.sdf +++ b/test/worlds/mecanum_drive.sdf @@ -120,11 +120,9 @@ 0 0 0 1.5707 0 0 - - - https://fuel.gazebosim.org/1.0/OpenRobotics/models/Mecanum lift/tip/files/meshes/mecanum_wheel_left.STL - 0.006 0.006 0.006 - + + 0.3 + 0.2 0.2 0.2 1 @@ -166,11 +164,9 @@ 0 0 0 1.5707 0 0 - - - https://fuel.gazebosim.org/1.0/OpenRobotics/models/Mecanum lift/tip/files/meshes/mecanum_wheel_right.STL - 0.006 0.006 0.006 - + + 0.3 + 0.2 0.2 0.2 1 @@ -212,11 +208,9 @@ 0 0 0 1.5707 0 0 - - - https://fuel.gazebosim.org/1.0/OpenRobotics/models/Mecanum lift/tip/files/meshes/mecanum_wheel_right.STL - 0.006 0.006 0.006 - + + 0.3 + 0.2 0.2 0.2 1 @@ -258,11 +252,9 @@ 0 0 0 1.5707 0 0 - - - https://fuel.gazebosim.org/1.0/OpenRobotics/models/Mecanum lift/tip/files/meshes/mecanum_wheel_left.STL - 0.006 0.006 0.006 - + + 0.3 + 0.2 0.2 0.2 1 @@ -288,7 +280,6 @@ - chassis front_left_wheel @@ -347,8 +338,10 @@ 1.25 1.511 0.3 - -5 - 5 + -1 + 1 + -0.5 + 0.5 diff --git a/test/worlds/mecanum_drive_custom_frame_id.sdf b/test/worlds/mecanum_drive_custom_frame_id.sdf index 7ac6c92e8f..bc985ace86 100644 --- a/test/worlds/mecanum_drive_custom_frame_id.sdf +++ b/test/worlds/mecanum_drive_custom_frame_id.sdf @@ -67,7 +67,7 @@ - + 0 0 0.325 0 -0 0 @@ -120,11 +120,9 @@ 0 0 0 1.5707 0 0 - - - https://fuel.gazebosim.org/1.0/OpenRobotics/models/Mecanum lift/tip/files/meshes/mecanum_wheel_left.STL - 0.006 0.006 0.006 - + + 0.3 + 0.2 0.2 0.2 1 @@ -166,11 +164,9 @@ 0 0 0 1.5707 0 0 - - - https://fuel.gazebosim.org/1.0/OpenRobotics/models/Mecanum lift/tip/files/meshes/mecanum_wheel_right.STL - 0.006 0.006 0.006 - + + 0.3 + 0.2 0.2 0.2 1 @@ -212,11 +208,9 @@ 0 0 0 1.5707 0 0 - - - https://fuel.gazebosim.org/1.0/OpenRobotics/models/Mecanum lift/tip/files/meshes/mecanum_wheel_right.STL - 0.006 0.006 0.006 - + + 0.3 + 0.2 0.2 0.2 1 @@ -258,11 +252,9 @@ 0 0 0 1.5707 0 0 - - - https://fuel.gazebosim.org/1.0/OpenRobotics/models/Mecanum lift/tip/files/meshes/mecanum_wheel_left.STL - 0.006 0.006 0.006 - + + 0.3 + 0.2 0.2 0.2 1 @@ -288,7 +280,6 @@ - chassis front_left_wheel @@ -347,8 +338,10 @@ 1.25 1.511 0.3 - -5 - 5 + -1 + 1 + -0.5 + 0.5 odom base_footprint diff --git a/test/worlds/mecanum_drive_custom_tf_topic.sdf b/test/worlds/mecanum_drive_custom_tf_topic.sdf index 29591181f3..98b6122b3f 100644 --- a/test/worlds/mecanum_drive_custom_tf_topic.sdf +++ b/test/worlds/mecanum_drive_custom_tf_topic.sdf @@ -67,7 +67,7 @@ - + 0 0 0.325 0 -0 0 @@ -120,11 +120,9 @@ 0 0 0 1.5707 0 0 - - - https://fuel.gazebosim.org/1.0/OpenRobotics/models/Mecanum lift/tip/files/meshes/mecanum_wheel_left.STL - 0.006 0.006 0.006 - + + 0.3 + 0.2 0.2 0.2 1 @@ -166,11 +164,9 @@ 0 0 0 1.5707 0 0 - - - https://fuel.gazebosim.org/1.0/OpenRobotics/models/Mecanum lift/tip/files/meshes/mecanum_wheel_right.STL - 0.006 0.006 0.006 - + + 0.3 + 0.2 0.2 0.2 1 @@ -212,11 +208,9 @@ 0 0 0 1.5707 0 0 - - - https://fuel.gazebosim.org/1.0/OpenRobotics/models/Mecanum lift/tip/files/meshes/mecanum_wheel_right.STL - 0.006 0.006 0.006 - + + 0.3 + 0.2 0.2 0.2 1 @@ -258,11 +252,9 @@ 0 0 0 1.5707 0 0 - - - https://fuel.gazebosim.org/1.0/OpenRobotics/models/Mecanum lift/tip/files/meshes/mecanum_wheel_left.STL - 0.006 0.006 0.006 - + + 0.3 + 0.2 0.2 0.2 1 @@ -288,7 +280,6 @@ - chassis front_left_wheel @@ -347,8 +338,10 @@ 1.25 1.511 0.3 - -5 - 5 + -1 + 1 + -0.5 + 0.5 /model/foo/cmdvel /model/bar/odom tf_foo diff --git a/test/worlds/mecanum_drive_custom_topics.sdf b/test/worlds/mecanum_drive_custom_topics.sdf index 14e5612602..8f923328a6 100644 --- a/test/worlds/mecanum_drive_custom_topics.sdf +++ b/test/worlds/mecanum_drive_custom_topics.sdf @@ -120,11 +120,9 @@ 0 0 0 1.5707 0 0 - - - https://fuel.gazebosim.org/1.0/OpenRobotics/models/Mecanum lift/tip/files/meshes/mecanum_wheel_left.STL - 0.006 0.006 0.006 - + + 0.3 + 0.2 0.2 0.2 1 @@ -166,11 +164,9 @@ 0 0 0 1.5707 0 0 - - - https://fuel.gazebosim.org/1.0/OpenRobotics/models/Mecanum lift/tip/files/meshes/mecanum_wheel_right.STL - 0.006 0.006 0.006 - + + 0.3 + 0.2 0.2 0.2 1 @@ -212,11 +208,9 @@ 0 0 0 1.5707 0 0 - - - https://fuel.gazebosim.org/1.0/OpenRobotics/models/Mecanum lift/tip/files/meshes/mecanum_wheel_right.STL - 0.006 0.006 0.006 - + + 0.3 + 0.2 0.2 0.2 1 @@ -258,11 +252,9 @@ 0 0 0 1.5707 0 0 - - - https://fuel.gazebosim.org/1.0/OpenRobotics/models/Mecanum lift/tip/files/meshes/mecanum_wheel_left.STL - 0.006 0.006 0.006 - + + 0.3 + 0.2 0.2 0.2 1 @@ -288,7 +280,6 @@ - chassis front_left_wheel @@ -347,8 +338,10 @@ 1.25 1.511 0.3 - -5 - 5 + -1 + 1 + -0.5 + 0.5 /model/foo/cmdvel /model/bar/odom