diff --git a/src/systems/ackermann_steering/AckermannSteering.cc b/src/systems/ackermann_steering/AckermannSteering.cc index 74b19a3ee3..95fb2f9366 100644 --- a/src/systems/ackermann_steering/AckermannSteering.cc +++ b/src/systems/ackermann_steering/AckermannSteering.cc @@ -28,6 +28,7 @@ #include #include #include +#include #include #include @@ -38,8 +39,6 @@ #include "ignition/gazebo/Model.hh" #include "ignition/gazebo/Util.hh" -#include "SpeedLimiter.hh" - using namespace ignition; using namespace gazebo; using namespace systems; @@ -164,10 +163,10 @@ class ignition::gazebo::systems::AckermannSteeringPrivate public: std::chrono::steady_clock::duration lastOdomTime{0}; /// \brief Linear velocity limiter. - public: std::unique_ptr limiterLin; + public: std::unique_ptr limiterLin; /// \brief Angular velocity limiter. - public: std::unique_ptr limiterAng; + public: std::unique_ptr limiterAng; /// \brief Previous control command. public: Commands last0Cmd; @@ -258,56 +257,48 @@ void AckermannSteering::Configure(const Entity &_entity, this->dataPtr->wheelRadius = _sdf->Get("wheel_radius", this->dataPtr->wheelRadius).first; - // Parse speed limiter parameters. - bool hasVelocityLimits = false; - bool hasAccelerationLimits = false; - bool hasJerkLimits = false; - double minVel = std::numeric_limits::lowest(); - double maxVel = std::numeric_limits::max(); - double minAccel = std::numeric_limits::lowest(); - double maxAccel = std::numeric_limits::max(); - double minJerk = std::numeric_limits::lowest(); - double maxJerk = std::numeric_limits::max(); + // Instantiate the speed limiters. + this->dataPtr->limiterLin = std::make_unique(); + this->dataPtr->limiterAng = std::make_unique(); + // Parse speed limiter parameters. if (_sdf->HasElement("min_velocity")) { - minVel = _sdf->Get("min_velocity"); - hasVelocityLimits = true; + const double minVel = _sdf->Get("min_velocity"); + this->dataPtr->limiterLin->SetMinVelocity(minVel); + this->dataPtr->limiterAng->SetMinVelocity(minVel); } if (_sdf->HasElement("max_velocity")) { - maxVel = _sdf->Get("max_velocity"); - hasVelocityLimits = true; + const double maxVel = _sdf->Get("max_velocity"); + this->dataPtr->limiterLin->SetMaxVelocity(maxVel); + this->dataPtr->limiterAng->SetMaxVelocity(maxVel); } if (_sdf->HasElement("min_acceleration")) { - minAccel = _sdf->Get("min_acceleration"); - hasAccelerationLimits = true; + const double minAccel = _sdf->Get("min_acceleration"); + this->dataPtr->limiterLin->SetMinAcceleration(minAccel); + this->dataPtr->limiterAng->SetMinAcceleration(minAccel); } if (_sdf->HasElement("max_acceleration")) { - maxAccel = _sdf->Get("max_acceleration"); - hasAccelerationLimits = true; + const double maxAccel = _sdf->Get("max_acceleration"); + this->dataPtr->limiterLin->SetMaxAcceleration(maxAccel); + this->dataPtr->limiterAng->SetMaxAcceleration(maxAccel); } if (_sdf->HasElement("min_jerk")) { - minJerk = _sdf->Get("min_jerk"); - hasJerkLimits = true; + const double minJerk = _sdf->Get("min_jerk"); + this->dataPtr->limiterLin->SetMinJerk(minJerk); + this->dataPtr->limiterAng->SetMinJerk(minJerk); } if (_sdf->HasElement("max_jerk")) { - maxJerk = _sdf->Get("max_jerk"); - hasJerkLimits = true; + const double maxJerk = _sdf->Get("max_jerk"); + this->dataPtr->limiterLin->SetMaxJerk(maxJerk); + this->dataPtr->limiterAng->SetMaxJerk(maxJerk); } - // Instantiate the speed limiters. - this->dataPtr->limiterLin = std::make_unique( - hasVelocityLimits, hasAccelerationLimits, hasJerkLimits, - minVel, maxVel, minAccel, maxAccel, minJerk, maxJerk); - - this->dataPtr->limiterAng = std::make_unique( - hasVelocityLimits, hasAccelerationLimits, hasJerkLimits, - minVel, maxVel, minAccel, maxAccel, minJerk, maxJerk); double odomFreq = _sdf->Get("odom_publish_frequency", 50).first; if (odomFreq > 0) @@ -696,11 +687,11 @@ void AckermannSteeringPrivate::UpdateVelocity( angVel = this->targetVel.angular().z(); } - const double dt = std::chrono::duration(_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/ackermann_steering/CMakeLists.txt b/src/systems/ackermann_steering/CMakeLists.txt index 69af11afea..d797c6fe20 100644 --- a/src/systems/ackermann_steering/CMakeLists.txt +++ b/src/systems/ackermann_steering/CMakeLists.txt @@ -1,7 +1,6 @@ gz_add_system(ackermann-steering SOURCES AckermannSteering.cc - SpeedLimiter.cc PUBLIC_LINK_LIBS ignition-common${IGN_COMMON_VER}::ignition-common${IGN_COMMON_VER} ) diff --git a/src/systems/ackermann_steering/SpeedLimiter.cc b/src/systems/ackermann_steering/SpeedLimiter.cc deleted file mode 100644 index ebae2b5b81..0000000000 --- a/src/systems/ackermann_steering/SpeedLimiter.cc +++ /dev/null @@ -1,209 +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 - -#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. - /// \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: 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(_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/ackermann_steering/SpeedLimiter.hh b/src/systems/ackermann_steering/SpeedLimiter.hh deleted file mode 100644 index 5a86ac2b10..0000000000 --- a/src/systems/ackermann_steering/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 - -#include - -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 IGNITION_GAZEBO_VISIBLE 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 dataPtr; - }; - } -} -} -} - -#endif