diff --git a/src/systems/diff_drive/CMakeLists.txt b/src/systems/diff_drive/CMakeLists.txt
index 0a415da6d4..7c7da3ec05 100644
--- a/src/systems/diff_drive/CMakeLists.txt
+++ b/src/systems/diff_drive/CMakeLists.txt
@@ -1,7 +1,6 @@
gz_add_system(diff-drive
SOURCES
DiffDrive.cc
- SpeedLimiter.cc
PUBLIC_LINK_LIBS
ignition-common${IGN_COMMON_VER}::ignition-common${IGN_COMMON_VER}
)
diff --git a/src/systems/diff_drive/DiffDrive.cc b/src/systems/diff_drive/DiffDrive.cc
index e8a49f4356..eb356bd491 100644
--- a/src/systems/diff_drive/DiffDrive.cc
+++ b/src/systems/diff_drive/DiffDrive.cc
@@ -27,6 +27,7 @@
#include <ignition/common/Profiler.hh>
#include <ignition/math/DiffDriveOdometry.hh>
#include <ignition/math/Quaternion.hh>
+#include <ignition/math/SpeedLimiter.hh>
#include <ignition/plugin/Register.hh>
#include <ignition/transport/Node.hh>
@@ -36,8 +37,6 @@
#include "ignition/gazebo/Link.hh"
#include "ignition/gazebo/Model.hh"
-#include "SpeedLimiter.hh"
-
using namespace ignition;
using namespace gazebo;
using namespace systems;
@@ -123,10 +122,10 @@ class ignition::gazebo::systems::DiffDrivePrivate
public: transport::Node::Publisher tfPub;
/// \brief Linear velocity limiter.
- public: std::unique_ptr<SpeedLimiter> limiterLin;
+ public: std::unique_ptr<ignition::math::SpeedLimiter> limiterLin;
/// \brief Angular velocity limiter.
- public: std::unique_ptr<SpeedLimiter> limiterAng;
+ public: std::unique_ptr<ignition::math::SpeedLimiter> limiterAng;
/// \brief Previous control command.
public: Commands last0Cmd;
@@ -197,56 +196,48 @@ void DiffDrive::Configure(const Entity &_entity,
this->dataPtr->wheelRadius = _sdf->Get<double>("wheel_radius",
this->dataPtr->wheelRadius).first;
- // Parse speed limiter parameters.
- bool hasVelocityLimits = false;
- bool hasAccelerationLimits = false;
- bool hasJerkLimits = false;
- double minVel = std::numeric_limits<double>::lowest();
- double maxVel = std::numeric_limits<double>::max();
- double minAccel = std::numeric_limits<double>::lowest();
- double maxAccel = std::numeric_limits<double>::max();
- double minJerk = std::numeric_limits<double>::lowest();
- double maxJerk = std::numeric_limits<double>::max();
+ // Instantiate the speed limiters.
+ this->dataPtr->limiterLin = std::make_unique<ignition::math::SpeedLimiter>();
+ this->dataPtr->limiterAng = std::make_unique<ignition::math::SpeedLimiter>();
+ // Parse speed limiter parameters.
if (_sdf->HasElement("min_velocity"))
{
- minVel = _sdf->Get<double>("min_velocity");
- hasVelocityLimits = true;
+ const double minVel = _sdf->Get<double>("min_velocity");
+ this->dataPtr->limiterLin->SetMinVelocity(minVel);
+ this->dataPtr->limiterAng->SetMinVelocity(minVel);
}
if (_sdf->HasElement("max_velocity"))
{
- maxVel = _sdf->Get<double>("max_velocity");
- hasVelocityLimits = true;
+ const double maxVel = _sdf->Get<double>("max_velocity");
+ this->dataPtr->limiterLin->SetMaxVelocity(maxVel);
+ this->dataPtr->limiterAng->SetMaxVelocity(maxVel);
}
if (_sdf->HasElement("min_acceleration"))
{
- minAccel = _sdf->Get<double>("min_acceleration");
- hasAccelerationLimits = true;
+ const double minAccel = _sdf->Get<double>("min_acceleration");
+ this->dataPtr->limiterLin->SetMinAcceleration(minAccel);
+ this->dataPtr->limiterAng->SetMinAcceleration(minAccel);
}
if (_sdf->HasElement("max_acceleration"))
{
- maxAccel = _sdf->Get<double>("max_acceleration");
- hasAccelerationLimits = true;
+ const double maxAccel = _sdf->Get<double>("max_acceleration");
+ this->dataPtr->limiterLin->SetMaxAcceleration(maxAccel);
+ this->dataPtr->limiterAng->SetMaxAcceleration(maxAccel);
}
if (_sdf->HasElement("min_jerk"))
{
- minJerk = _sdf->Get<double>("min_jerk");
- hasJerkLimits = true;
+ const double minJerk = _sdf->Get<double>("min_jerk");
+ this->dataPtr->limiterLin->SetMinJerk(minJerk);
+ this->dataPtr->limiterAng->SetMinJerk(minJerk);
}
if (_sdf->HasElement("max_jerk"))
{
- maxJerk = _sdf->Get<double>("max_jerk");
- hasJerkLimits = true;
+ const double maxJerk = _sdf->Get<double>("max_jerk");
+ this->dataPtr->limiterLin->SetMaxJerk(maxJerk);
+ this->dataPtr->limiterAng->SetMaxJerk(maxJerk);
}
- // Instantiate the speed limiters.
- this->dataPtr->limiterLin = std::make_unique<SpeedLimiter>(
- hasVelocityLimits, hasAccelerationLimits, hasJerkLimits,
- minVel, maxVel, minAccel, maxAccel, minJerk, maxJerk);
-
- this->dataPtr->limiterAng = std::make_unique<SpeedLimiter>(
- hasVelocityLimits, hasAccelerationLimits, hasJerkLimits,
- minVel, maxVel, minAccel, maxAccel, minJerk, maxJerk);
double odomFreq = _sdf->Get<double>("odom_publish_frequency", 50).first;
if (odomFreq > 0)
@@ -502,11 +493,11 @@ void DiffDrivePrivate::UpdateVelocity(const ignition::gazebo::UpdateInfo &_info,
angVel = this->targetVel.angular().z();
}
- const double dt = std::chrono::duration<double>(_info.dt).count();
-
// Limit the target velocity if needed.
- this->limiterLin->Limit(linVel, this->last0Cmd.lin, this->last1Cmd.lin, dt);
- this->limiterAng->Limit(angVel, this->last0Cmd.ang, this->last1Cmd.ang, dt);
+ this->limiterLin->Limit(
+ linVel, this->last0Cmd.lin, this->last1Cmd.lin, _info.dt);
+ this->limiterAng->Limit(
+ angVel, this->last0Cmd.ang, this->last1Cmd.ang, _info.dt);
// Update history of commands.
this->last1Cmd = last0Cmd;
diff --git a/src/systems/diff_drive/SpeedLimiter.cc b/src/systems/diff_drive/SpeedLimiter.cc
deleted file mode 100644
index 59b50f54b9..0000000000
--- a/src/systems/diff_drive/SpeedLimiter.cc
+++ /dev/null
@@ -1,200 +0,0 @@
-/*********************************************************************
- * Software License Agreement (BSD License)
- *
- * Copyright (c) 2013, PAL Robotics, S.L.
- * All rights reserved.
- *
- * Redistribution and use in source and binary forms, with or without
- * modification, are permitted provided that the following conditions
- * are met:
- *
- * * Redistributions of source code must retain the above copyright
- * notice, this list of conditions and the following disclaimer.
- * * Redistributions in binary form must reproduce the above
- * copyright notice, this list of conditions and the following
- * disclaimer in the documentation and/or other materials provided
- * with the distribution.
- * * Neither the name of the PAL Robotics nor the names of its
- * contributors may be used to endorse or promote products derived
- * from this software without specific prior written permission.
- *
- * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
- * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
- * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
- * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
- * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
- * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
- * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
- * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
- * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
- * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
- * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
- * POSSIBILITY OF SUCH DAMAGE.
- *********************************************************************/
-
-/*
- * Author: Enrique Fernández
- * Modified: Carlos Agüero
- */
-
-#include <ignition/math/Helpers.hh>
-
-#include "SpeedLimiter.hh"
-
-using namespace ignition;
-using namespace gazebo;
-using namespace systems;
-
-/// \brief Private SpeedLimiter data class.
-class ignition::gazebo::systems::SpeedLimiterPrivate
-{
- /// \brief Class constructor.
- public: SpeedLimiterPrivate(bool _hasVelocityLimits,
- bool _hasAccelerationLimits,
- bool _hasJerkLimits,
- double _minVelocity,
- double _maxVelocity,
- double _minAcceleration,
- double _maxAcceleration,
- double _minJerk,
- double _maxJerk)
- : hasVelocityLimits(_hasVelocityLimits),
- hasAccelerationLimits(_hasAccelerationLimits),
- hasJerkLimits(_hasJerkLimits),
- minVelocity(_minVelocity),
- maxVelocity(_maxVelocity),
- minAcceleration(_minAcceleration),
- maxAcceleration(_maxAcceleration),
- minJerk(_minJerk),
- maxJerk(_maxJerk)
- {
- }
-
- /// \brief Enable/Disable velocity.
- public: bool hasVelocityLimits;
-
- /// \brief Enable/Disable acceleration.
- public: bool hasAccelerationLimits;
-
- /// \brief Enable/Disable jerk.
- public: bool hasJerkLimits;
-
- /// \brief Minimum velocity limit.
- public: double minVelocity;
-
- /// \brief Maximum velocity limit.
- public: double maxVelocity;
-
- /// \brief Minimum acceleration limit.
- public: double minAcceleration;
-
- /// \brief Maximum acceleration limit.
- public: double maxAcceleration;
-
- /// \brief Minimum jerk limit.
- public: double minJerk;
-
- /// \brief Maximum jerk limit.
- public: double maxJerk;
-};
-
-//////////////////////////////////////////////////
-SpeedLimiter::SpeedLimiter(bool _hasVelocityLimits,
- bool _hasAccelerationLimits,
- bool _hasJerkLimits,
- double _minVelocity,
- double _maxVelocity,
- double _minAcceleration,
- double _maxAcceleration,
- double _minJerk,
- double _maxJerk)
- : dataPtr(std::make_unique<SpeedLimiterPrivate>(_hasVelocityLimits,
- _hasAccelerationLimits, _hasJerkLimits, _minVelocity, _maxVelocity,
- _minAcceleration, _maxAcceleration, _minJerk, _maxJerk))
-{
-}
-
-//////////////////////////////////////////////////
-SpeedLimiter::~SpeedLimiter()
-{
-}
-
-//////////////////////////////////////////////////
-double SpeedLimiter::Limit(double &_v, double _v0, double _v1, double _dt) const
-{
- const double tmp = _v;
-
- this->LimitJerk(_v, _v0, _v1, _dt);
- this->LimitAcceleration(_v, _v0, _dt);
- this->LimitVelocity(_v);
-
- if (ignition::math::equal(tmp, 0.0))
- return 1.0;
- else
- return _v / tmp;
-}
-
-//////////////////////////////////////////////////
-double SpeedLimiter::LimitVelocity(double &_v) const
-{
- const double tmp = _v;
-
- if (this->dataPtr->hasVelocityLimits)
- {
- _v = ignition::math::clamp(
- _v, this->dataPtr->minVelocity, this->dataPtr->maxVelocity);
- }
-
- if (ignition::math::equal(tmp, 0.0))
- return 1.0;
- else
- return _v / tmp;
-}
-
-//////////////////////////////////////////////////
-double SpeedLimiter::LimitAcceleration(double &_v, double _v0, double _dt) const
-{
- const double tmp = _v;
-
- if (this->dataPtr->hasAccelerationLimits)
- {
- const double dvMin = this->dataPtr->minAcceleration * _dt;
- const double dvMax = this->dataPtr->maxAcceleration * _dt;
-
- const double dv = ignition::math::clamp(_v - _v0, dvMin, dvMax);
-
- _v = _v0 + dv;
- }
-
- if (ignition::math::equal(tmp, 0.0))
- return 1.0;
- else
- return _v / tmp;
-}
-
-//////////////////////////////////////////////////
-double SpeedLimiter::LimitJerk(double &_v, double _v0, double _v1, double _dt)
- const
-{
- const double tmp = _v;
-
- if (this->dataPtr->hasJerkLimits)
- {
- const double dv = _v - _v0;
- const double dv0 = _v0 - _v1;
-
- const double dt2 = 2. * _dt * _dt;
-
- const double daMin = this->dataPtr->minJerk * dt2;
- const double daMax = this->dataPtr->maxJerk * dt2;
-
- const double da = ignition::math::clamp(dv - dv0, daMin, daMax);
-
- _v = _v0 + dv0 + da;
- }
-
- if (ignition::math::equal(tmp, 0.0))
- return 1.0;
- else
- return _v / tmp;
-}
diff --git a/src/systems/diff_drive/SpeedLimiter.hh b/src/systems/diff_drive/SpeedLimiter.hh
deleted file mode 100644
index 34286bc3ae..0000000000
--- a/src/systems/diff_drive/SpeedLimiter.hh
+++ /dev/null
@@ -1,130 +0,0 @@
-/*********************************************************************
- * Software License Agreement (BSD License)
- *
- * Copyright (c) 2013, PAL Robotics, S.L.
- * All rights reserved.
- *
- * Redistribution and use in source and binary forms, with or without
- * modification, are permitted provided that the following conditions
- * are met:
- *
- * * Redistributions of source code must retain the above copyright
- * notice, this list of conditions and the following disclaimer.
- * * Redistributions in binary form must reproduce the above
- * copyright notice, this list of conditions and the following
- * disclaimer in the documentation and/or other materials provided
- * with the distribution.
- * * Neither the name of the PAL Robotics nor the names of its
- * contributors may be used to endorse or promote products derived
- * from this software without specific prior written permission.
- *
- * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
- * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
- * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
- * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
- * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
- * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
- * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
- * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
- * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
- * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
- * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
- * POSSIBILITY OF SUCH DAMAGE.
- *********************************************************************/
-
-/*
- * Author: Enrique Fernández
- * Modified: Carlos Agüero
- */
-
-#ifndef IGNITION_GAZEBO_SYSTEMS_SPEEDLIMITER_HH_
-#define IGNITION_GAZEBO_SYSTEMS_SPEEDLIMITER_HH_
-
-#include <memory>
-
-#include <ignition/gazebo/System.hh>
-
-namespace ignition
-{
-namespace gazebo
-{
-// Inline bracket to help doxygen filtering.
-inline namespace IGNITION_GAZEBO_VERSION_NAMESPACE {
-namespace systems
-{
- // Forward declaration.
- class SpeedLimiterPrivate;
-
- /// \brief Class to limit velocity, acceleration and jerk.
- /// \ref https://github.com/ros-controls/ros_controllers/tree/melodic-devel/diff_drive_controller
- class SpeedLimiter
- {
- /// \brief Constructor.
- /// \param [in] _hasVelocityLimits if true, applies velocity limits.
- /// \param [in] _hasAccelerationLimits if true, applies acceleration limits.
- /// \param [in] _hasJerkLimits if true, applies jerk limits.
- /// \param [in] _minVelocity Minimum velocity [m/s], usually <= 0.
- /// \param [in] _maxVelocity Maximum velocity [m/s], usually >= 0.
- /// \param [in] _minAcceleration Minimum acceleration [m/s^2], usually <= 0.
- /// \param [in] _maxAcceleration Maximum acceleration [m/s^2], usually >= 0.
- /// \param [in] _minJerk Minimum jerk [m/s^3], usually <= 0.
- /// \param [in] _maxJerk Maximum jerk [m/s^3], usually >= 0.
- public: SpeedLimiter(bool _hasVelocityLimits = false,
- bool _hasAccelerationLimits = false,
- bool _hasJerkLimits = false,
- double _minVelocity = 0.0,
- double _maxVelocity = 0.0,
- double _minAcceleration = 0.0,
- double _maxAcceleration = 0.0,
- double _minJerk = 0.0,
- double _maxJerk = 0.0);
-
- /// \brief Destructor.
- public: ~SpeedLimiter();
-
- /// \brief Limit the velocity and acceleration.
- /// \param [in, out] _v Velocity [m/s].
- /// \param [in] _v0 Previous velocity to v [m/s].
- /// \param [in] _v1 Previous velocity to v0 [m/s].
- /// \param [in] _dt Time step [s].
- /// \return Limiting factor (1.0 if none).
- public: double Limit(double &_v,
- double _v0,
- double _v1,
- double _dt) const;
-
- /// \brief Limit the velocity.
- /// \param [in, out] _v Velocity [m/s].
- /// \return Limiting factor (1.0 if none).
- public: double LimitVelocity(double &_v) const;
-
- /// \brief Limit the acceleration.
- /// \param [in, out] _v Velocity [m/s].
- /// \param [in] _v0 Previous velocity [m/s].
- /// \param [in] _dt Time step [s].
- /// \return Limiting factor (1.0 if none).
- public: double LimitAcceleration(double &_v,
- double _v0,
- double _dt) const;
-
- /// \brief Limit the jerk.
- /// \param [in, out] _v Velocity [m/s].
- /// \param [in] _v0 Previous velocity to v [m/s].
- /// \param [in] _v1 Previous velocity to v0 [m/s].
- /// \param [in] _dt Time step [s].
- /// \return Limiting factor (1.0 if none).
- /// \see http://en.wikipedia.org/wiki/Jerk_%28physics%29#Motion_control.
- public: double LimitJerk(double &_v,
- double _v0,
- double _v1,
- double _dt) const;
-
- /// \brief Private data pointer.
- private: std::unique_ptr<SpeedLimiterPrivate> dataPtr;
- };
- }
-}
-}
-}
-
-#endif
diff --git a/test/integration/diff_drive_system.cc b/test/integration/diff_drive_system.cc
index df756fcd52..067476bfb0 100644
--- a/test/integration/diff_drive_system.cc
+++ b/test/integration/diff_drive_system.cc
@@ -54,145 +54,165 @@ class DiffDriveTest : public ::testing::TestWithParam<int>
const std::string &_cmdVelTopic,
const std::string &_odomTopic)
{
- // Start server
- ServerConfig serverConfig;
- serverConfig.SetSdfFile(_sdfFile);
-
- Server server(serverConfig);
- EXPECT_FALSE(server.Running());
- EXPECT_FALSE(*server.Running(0));
-
- // Create a system that records the vehicle poses
- test::Relay testSystem;
-
- std::vector<math::Pose3d> poses;
- testSystem.OnPostUpdate([&poses](const gazebo::UpdateInfo &,
- const gazebo::EntityComponentManager &_ecm)
- {
- auto id = _ecm.EntityByComponents(
- components::Model(),
- components::Name("vehicle"));
- EXPECT_NE(kNullEntity, id);
+ /// \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){
+ // Start server
+ ServerConfig serverConfig;
+ serverConfig.SetSdfFile(_sdfFile);
+
+ Server server(serverConfig);
+ EXPECT_FALSE(server.Running());
+ EXPECT_FALSE(*server.Running(0));
+
+ // Create a system that records the vehicle poses
+ test::Relay testSystem;
+
+ std::vector<math::Pose3d> poses;
+ testSystem.OnPostUpdate([&poses](const gazebo::UpdateInfo &,
+ const gazebo::EntityComponentManager &_ecm)
+ {
+ auto id = _ecm.EntityByComponents(
+ components::Model(),
+ components::Name("vehicle"));
+ EXPECT_NE(kNullEntity, id);
- auto poseComp = _ecm.Component<components::Pose>(id);
- ASSERT_NE(nullptr, poseComp);
+ auto poseComp = _ecm.Component<components::Pose>(id);
+ ASSERT_NE(nullptr, poseComp);
- poses.push_back(poseComp->Data());
- });
- server.AddSystem(testSystem.systemPtr);
+ poses.push_back(poseComp->Data());
+ });
+ server.AddSystem(testSystem.systemPtr);
- // Run server and check that vehicle didn't move
- server.Run(true, 1000, false);
+ // Run server and check that vehicle didn't move
+ server.Run(true, 1000, false);
- EXPECT_EQ(1000u, poses.size());
+ EXPECT_EQ(1000u, poses.size());
- for (const auto &pose : poses)
- {
- EXPECT_EQ(poses[0], pose);
- }
-
- // Get odometry messages
- double period{1.0 / 50.0};
- double lastMsgTime{1.0};
- std::vector<math::Pose3d> odomPoses;
- std::function<void(const msgs::Odometry &)> odomCb =
- [&](const msgs::Odometry &_msg)
+ for (const auto &pose : poses)
{
- ASSERT_TRUE(_msg.has_header());
- ASSERT_TRUE(_msg.header().has_stamp());
-
- double msgTime =
- static_cast<double>(_msg.header().stamp().sec()) +
- static_cast<double>(_msg.header().stamp().nsec()) * 1e-9;
-
- EXPECT_DOUBLE_EQ(msgTime, lastMsgTime + period);
- lastMsgTime = msgTime;
-
- odomPoses.push_back(msgs::Convert(_msg.pose()));
- };
-
- // Publish command and check that vehicle moved
- transport::Node node;
- auto pub = node.Advertise<msgs::Twist>(_cmdVelTopic);
- node.Subscribe(_odomTopic, odomCb);
-
- msgs::Twist msg;
-
- // Avoid wheel slip by limiting acceleration
- // 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".
- test::Relay velocityRamp;
- const double desiredLinVel = 10.5;
- const double desiredAngVel = 0.2;
- velocityRamp.OnPreUpdate(
- [&](const gazebo::UpdateInfo &/*_info*/,
- const gazebo::EntityComponentManager &)
+ EXPECT_EQ(poses[0], pose);
+ }
+
+ // Get odometry messages
+ double period{1.0 / 50.0};
+ double lastMsgTime{1.0};
+ std::vector<math::Pose3d> odomPoses;
+ std::function<void(const msgs::Odometry &)> odomCb =
+ [&](const msgs::Odometry &_msg)
{
- msgs::Set(msg.mutable_linear(),
- math::Vector3d(desiredLinVel, 0, 0));
- msgs::Set(msg.mutable_angular(),
- math::Vector3d(0.0, 0, desiredAngVel));
- pub.Publish(msg);
- });
-
- server.AddSystem(velocityRamp.systemPtr);
-
- server.Run(true, 3000, false);
+ ASSERT_TRUE(_msg.has_header());
+ ASSERT_TRUE(_msg.header().has_stamp());
+
+ double msgTime =
+ static_cast<double>(_msg.header().stamp().sec()) +
+ static_cast<double>(_msg.header().stamp().nsec()) * 1e-9;
+
+ EXPECT_DOUBLE_EQ(msgTime, lastMsgTime + period);
+ lastMsgTime = msgTime;
+
+ odomPoses.push_back(msgs::Convert(_msg.pose()));
+ };
+
+ // Publish command and check that vehicle moved
+ transport::Node node;
+ auto pub = node.Advertise<msgs::Twist>(_cmdVelTopic);
+ node.Subscribe(_odomTopic, odomCb);
+
+ msgs::Twist msg;
+
+ // 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".
+ // See <min_velocity>, <min_aceleration>, <min_jerk> and
+ // <max_jerk> parameters in "/test/worlds/diff_drive.sdf".
+ test::Relay velocityRamp;
+ const int movementDirection = (forward ? 1 : -1);
+ double desiredLinVel = movementDirection * 10.5;
+ double desiredAngVel = 0.2;
+ velocityRamp.OnPreUpdate(
+ [&](const gazebo::UpdateInfo &/*_info*/,
+ const gazebo::EntityComponentManager &)
+ {
+ msgs::Set(msg.mutable_linear(),
+ math::Vector3d(desiredLinVel, 0, 0));
+ msgs::Set(msg.mutable_angular(),
+ math::Vector3d(0.0, 0, desiredAngVel));
+ pub.Publish(msg);
+ });
+
+ server.AddSystem(velocityRamp.systemPtr);
+
+ server.Run(true, 3000, false);
+
+ // Poses for 4s
+ ASSERT_EQ(4000u, poses.size());
+
+ int sleep = 0;
+ int maxSleep = 30;
+ for (; odomPoses.size() < 150 && sleep < maxSleep; ++sleep)
+ {
+ std::this_thread::sleep_for(std::chrono::milliseconds(100));
+ }
+ ASSERT_NE(maxSleep, sleep);
+
+ // 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);
+ 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());
+
+ 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_LT(poses[0].Rot().Z(), poses[3999].Rot().Z());
+
+ // 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[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);
+
+ // 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);
- // Poses for 4s
- ASSERT_EQ(4000u, poses.size());
+ };
- int sleep = 0;
- int maxSleep = 30;
- for (; odomPoses.size() < 150 && sleep < maxSleep; ++sleep)
- {
- std::this_thread::sleep_for(std::chrono::milliseconds(100));
- }
- ASSERT_NE(maxSleep, sleep);
-
- // 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);
- auto finalModelFramePose =
- tOdomModel * odomPoses.back() * tOdomModel.Inverse();
-
- // Odom for 3s
- ASSERT_FALSE(odomPoses.empty());
- EXPECT_EQ(150u, odomPoses.size());
-
- EXPECT_LT(poses[0].Pos().X(), poses[3999].Pos().X());
- EXPECT_LT(poses[0].Pos().Y(), poses[3999].Pos().Y());
- EXPECT_NEAR(poses[0].Pos().Z(), poses[3999].Pos().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_LT(poses[0].Rot().Z(), poses[3999].Rot().Z());
-
- // 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[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);
-
- // Max velocities/accelerations expectations.
- // Moving time.
- double t = 3.0;
- double d = poses[3999].Pos().Distance(poses[1000].Pos());
- const double v0 = 0;
- double v = d / t;
- double a = (v - v0) / t;
- EXPECT_LT(v, 0.5);
- EXPECT_LT(a, 1);
+ testCmdVel(true /*test forward movement*/);
+ testCmdVel(false /*test backward movement*/);
}
};
diff --git a/test/worlds/diff_drive.sdf b/test/worlds/diff_drive.sdf
index b2b00bb0c4..dcb6a1fc2e 100644
--- a/test/worlds/diff_drive.sdf
+++ b/test/worlds/diff_drive.sdf
@@ -228,7 +228,9 @@
<wheel_separation>1.25</wheel_separation>
<wheel_radius>0.3</wheel_radius>
<max_acceleration>1</max_acceleration>
+ <min_acceleration>-1</min_acceleration>
<max_velocity>0.5</max_velocity>
+ <min_velocity>-0.5</min_velocity>
<!-- no odom_publisher_frequency defaults to 50 Hz -->
</plugin>
diff --git a/test/worlds/diff_drive_custom_topics.sdf b/test/worlds/diff_drive_custom_topics.sdf
index 075c1c6744..15a414d1e3 100644
--- a/test/worlds/diff_drive_custom_topics.sdf
+++ b/test/worlds/diff_drive_custom_topics.sdf
@@ -231,6 +231,8 @@
<odom_topic>/model/bar/odom</odom_topic>
<max_acceleration>1</max_acceleration>
<max_velocity>0.5</max_velocity>
+ <min_acceleration>-1</min_acceleration>
+ <min_velocity>-0.5</min_velocity>
<!-- no odom_publisher_frequency defaults to 50 Hz -->
</plugin>