From a13e650efdb9626791c6b4f058c3f7c01028d10b Mon Sep 17 00:00:00 2001 From: Sikiru Salau Date: Thu, 30 Jan 2025 16:20:33 +0100 Subject: [PATCH 1/2] separate angular limiter parameters from linear Signed-off-by: Sikiru Salau --- .../ackermann_steering/AckermannSteering.cc | 36 +++++++++++++++---- 1 file changed, 30 insertions(+), 6 deletions(-) diff --git a/src/systems/ackermann_steering/AckermannSteering.cc b/src/systems/ackermann_steering/AckermannSteering.cc index f1398c6c95..ec4eada508 100644 --- a/src/systems/ackermann_steering/AckermannSteering.cc +++ b/src/systems/ackermann_steering/AckermannSteering.cc @@ -346,37 +346,61 @@ void AckermannSteering::Configure(const Entity &_entity, { const double minVel = _sdf->Get("min_velocity"); this->dataPtr->limiterLin->SetMinVelocity(minVel); - this->dataPtr->limiterAng->SetMinVelocity(minVel); + } + if (_sdf->HasElement("min_angular_velocity")) + { + const double minAngVel = _sdf->Get("min_angular_velocity"); + this->dataPtr->limiterAng->SetMinVelocity(minAngVel); } if (_sdf->HasElement("max_velocity")) { const double maxVel = _sdf->Get("max_velocity"); this->dataPtr->limiterLin->SetMaxVelocity(maxVel); - this->dataPtr->limiterAng->SetMaxVelocity(maxVel); + } + if (_sdf->HasElement("max_angular_velocity")) + { + const double maxAngVel = _sdf->Get("max_angular_velocity"); + this->dataPtr->limiterAng->SetMaxVelocity(maxAngVel); } if (_sdf->HasElement("min_acceleration")) { const double minAccel = _sdf->Get("min_acceleration"); this->dataPtr->limiterLin->SetMinAcceleration(minAccel); - this->dataPtr->limiterAng->SetMinAcceleration(minAccel); + } + if (_sdf->HasElement("min_angular_acceleration")) + { + const double minAngAccel = _sdf->Get("min_angular_acceleration"); + this->dataPtr->limiterAng->SetMinAcceleration(minAngAccel); } if (_sdf->HasElement("max_acceleration")) { const double maxAccel = _sdf->Get("max_acceleration"); this->dataPtr->limiterLin->SetMaxAcceleration(maxAccel); - this->dataPtr->limiterAng->SetMaxAcceleration(maxAccel); + } + if (_sdf->HasElement("max_angular_acceleration")) + { + const double maxAngAccel = _sdf->Get("max_angular_acceleration"); + this->dataPtr->limiterAng->SetMaxAcceleration(maxAngAccel); } if (_sdf->HasElement("min_jerk")) { const double minJerk = _sdf->Get("min_jerk"); this->dataPtr->limiterLin->SetMinJerk(minJerk); - this->dataPtr->limiterAng->SetMinJerk(minJerk); + } + if (_sdf->HasElement("min_angular_jerk")) + { + const double minAngJerk = _sdf->Get("min_angular_jerk"); + this->dataPtr->limiterAng->SetMinJerk(minAngJerk); } if (_sdf->HasElement("max_jerk")) { const double maxJerk = _sdf->Get("max_jerk"); this->dataPtr->limiterLin->SetMaxJerk(maxJerk); - this->dataPtr->limiterAng->SetMaxJerk(maxJerk); + } + if (_sdf->HasElement("max_angular_jerk")) + { + const double maxAngJerk = _sdf->Get("max_angular_jerk"); + this->dataPtr->limiterAng->SetMaxJerk(maxAngJerk); } if (!this->dataPtr->steeringOnly) From f7395db20f0d2913f83c27122928b0e19af6d456 Mon Sep 17 00:00:00 2001 From: Sikiru Salau Date: Thu, 30 Jan 2025 18:22:38 +0100 Subject: [PATCH 2/2] document new angular limit parameters in AckermannSteering.hh Signed-off-by: Sikiru Salau --- src/systems/ackermann_steering/AckermannSteering.hh | 10 ++++++++-- 1 file changed, 8 insertions(+), 2 deletions(-) diff --git a/src/systems/ackermann_steering/AckermannSteering.hh b/src/systems/ackermann_steering/AckermannSteering.hh index 84583379c4..f5a3d9d658 100644 --- a/src/systems/ackermann_steering/AckermannSteering.hh +++ b/src/systems/ackermann_steering/AckermannSteering.hh @@ -92,11 +92,17 @@ namespace systems /// element is optional, and the default value is 50Hz. /// /// - ``: Minimum velocity [m/s], usually <= 0. + /// - ``: Minimum angular velocity [m/s], usually <= 0. /// - ``: Maximum velocity [m/s], usually >= 0. + /// - ``: Maximum angular velocity [m/s], usually >= 0. /// - ``: Minimum acceleration [m/s^2], usually <= 0. + /// - ``: Minimum angular acceleration [m/s^2], usually <= 0. /// - ``: Maximum acceleration [m/s^2], usually >= 0. - /// - ``: jerk [m/s^3], usually <= 0. - /// - ``: jerk [m/s^3], usually >= 0. + /// - ``: Maximum angular acceleration [m/s^2], usually >= 0. + /// - ``: Minimum jerk [m/s^3], usually <= 0. + /// - ``: Minimum jerk [m/s^3], usually <= 0. + /// - ``: Maximum jerk [m/s^3], usually >= 0. + /// - ``: Maximum jerk [m/s^3], usually >= 0. /// /// - ``: Custom topic that this system will subscribe to in order to /// receive command messages. This element is optional, and the