Skip to content

Commit

Permalink
pull fixes
Browse files Browse the repository at this point in the history
  • Loading branch information
Stefano Mutti committed Jan 8, 2025
1 parent 24d708e commit 0cbae95
Show file tree
Hide file tree
Showing 6 changed files with 177 additions and 160 deletions.
12 changes: 10 additions & 2 deletions src/systems/mecanum_drive/MecanumDrive.cc
View file
Original file line number Diff line number Diff line change
Expand Up @@ -26,6 +26,7 @@
#include <set>
#include <string>
#include <vector>
#include <chrono>

#include <gz/common/Profiler.hh>
#include <gz/math/MecanumDriveOdometry.hh>
Expand Down Expand Up @@ -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.
Expand All @@ -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;
Expand Down
165 changes: 101 additions & 64 deletions test/integration/mecanum_drive_system.cc
View file
Original file line number Diff line number Diff line change
@@ -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.
Expand All @@ -17,22 +17,29 @@

#include <gtest/gtest.h>

#include <string>
#include <vector>
#include <functional>
#include <thread>

#include <gz/msgs/odometry.pb.h>
#include <gz/msgs/pose_v.pb.h>
#include <gz/msgs/twist.pb.h>

#include <gz/common/Console.hh>
#include <gz/common/Util.hh>
#include <gz/math/Pose3.hh>
#include <gz/math/Vector3.hh>
#include <gz/transport/Node.hh>
#include <gz/utils/ExtraTestMacros.hh>


#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"
Expand All @@ -53,11 +60,13 @@ class MecanumDriveTest : public InternalFixture<::testing::TestWithParam<int>>
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);
Expand Down Expand Up @@ -125,19 +134,22 @@ class MecanumDriveTest : public InternalFixture<::testing::TestWithParam<int>>
// Avoid wheel slip by limiting acceleration (1 m/s^2)
// and max velocity (0.5 m/s).
// See <max_velocity> and <max_aceleration> parameters
// in "/test/worlds/diff_drive.sdf".
// in "/test/worlds/mecanum_drive.sdf".
// See <min_velocity>, <min_aceleration>, <min_jerk> and
// <max_jerk> parameters in "/test/worlds/diff_drive.sdf".
// <max_jerk> 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);
});

Expand Down Expand Up @@ -165,84 +177,109 @@ class MecanumDriveTest : public InternalFixture<::testing::TestWithParam<int>>
// 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();

// Odom for 3s
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());
Expand Down Expand Up @@ -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());
Expand Down Expand Up @@ -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());
Expand Down Expand Up @@ -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());
Expand Down Expand Up @@ -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());
Expand Down
Loading

0 comments on commit 0cbae95

Please sign in to comment.