Skip to content

Commit a13e650

Browse files
committed
separate angular limiter parameters from linear
Signed-off-by: Sikiru Salau <[email protected]>
1 parent 4782ee9 commit a13e650

File tree

1 file changed

+30
-6
lines changed

1 file changed

+30
-6
lines changed

src/systems/ackermann_steering/AckermannSteering.cc

Lines changed: 30 additions & 6 deletions
Original file line numberDiff line numberDiff line change
@@ -346,37 +346,61 @@ void AckermannSteering::Configure(const Entity &_entity,
346346
{
347347
const double minVel = _sdf->Get<double>("min_velocity");
348348
this->dataPtr->limiterLin->SetMinVelocity(minVel);
349-
this->dataPtr->limiterAng->SetMinVelocity(minVel);
349+
}
350+
if (_sdf->HasElement("min_angular_velocity"))
351+
{
352+
const double minAngVel = _sdf->Get<double>("min_angular_velocity");
353+
this->dataPtr->limiterAng->SetMinVelocity(minAngVel);
350354
}
351355
if (_sdf->HasElement("max_velocity"))
352356
{
353357
const double maxVel = _sdf->Get<double>("max_velocity");
354358
this->dataPtr->limiterLin->SetMaxVelocity(maxVel);
355-
this->dataPtr->limiterAng->SetMaxVelocity(maxVel);
359+
}
360+
if (_sdf->HasElement("max_angular_velocity"))
361+
{
362+
const double maxAngVel = _sdf->Get<double>("max_angular_velocity");
363+
this->dataPtr->limiterAng->SetMaxVelocity(maxAngVel);
356364
}
357365
if (_sdf->HasElement("min_acceleration"))
358366
{
359367
const double minAccel = _sdf->Get<double>("min_acceleration");
360368
this->dataPtr->limiterLin->SetMinAcceleration(minAccel);
361-
this->dataPtr->limiterAng->SetMinAcceleration(minAccel);
369+
}
370+
if (_sdf->HasElement("min_angular_acceleration"))
371+
{
372+
const double minAngAccel = _sdf->Get<double>("min_angular_acceleration");
373+
this->dataPtr->limiterAng->SetMinAcceleration(minAngAccel);
362374
}
363375
if (_sdf->HasElement("max_acceleration"))
364376
{
365377
const double maxAccel = _sdf->Get<double>("max_acceleration");
366378
this->dataPtr->limiterLin->SetMaxAcceleration(maxAccel);
367-
this->dataPtr->limiterAng->SetMaxAcceleration(maxAccel);
379+
}
380+
if (_sdf->HasElement("max_angular_acceleration"))
381+
{
382+
const double maxAngAccel = _sdf->Get<double>("max_angular_acceleration");
383+
this->dataPtr->limiterAng->SetMaxAcceleration(maxAngAccel);
368384
}
369385
if (_sdf->HasElement("min_jerk"))
370386
{
371387
const double minJerk = _sdf->Get<double>("min_jerk");
372388
this->dataPtr->limiterLin->SetMinJerk(minJerk);
373-
this->dataPtr->limiterAng->SetMinJerk(minJerk);
389+
}
390+
if (_sdf->HasElement("min_angular_jerk"))
391+
{
392+
const double minAngJerk = _sdf->Get<double>("min_angular_jerk");
393+
this->dataPtr->limiterAng->SetMinJerk(minAngJerk);
374394
}
375395
if (_sdf->HasElement("max_jerk"))
376396
{
377397
const double maxJerk = _sdf->Get<double>("max_jerk");
378398
this->dataPtr->limiterLin->SetMaxJerk(maxJerk);
379-
this->dataPtr->limiterAng->SetMaxJerk(maxJerk);
399+
}
400+
if (_sdf->HasElement("max_angular_jerk"))
401+
{
402+
const double maxAngJerk = _sdf->Get<double>("max_angular_jerk");
403+
this->dataPtr->limiterAng->SetMaxJerk(maxAngJerk);
380404
}
381405

382406
if (!this->dataPtr->steeringOnly)

0 commit comments

Comments
 (0)