Skip to content

Commit

Permalink
Updated DiffDrive.cc to retain old parameters. They are overwritten i…
Browse files Browse the repository at this point in the history 
…f new parameters are used.

Added new parameters to example diff_drive worlds
Added new parameters to test diff_drive worlds
  • Loading branch information
@roni-kreinin
roni-kreinin committed Dec 13, 2021
1 parent e604d0c commit 20e9461
Show file tree
Hide file tree
Showing 5 changed files with 110 additions and 31 deletions.
16 changes: 16 additions & 0 deletions examples/worlds/diff_drive.sdf
View file
Original file line number Diff line number Diff line change
Expand Up @@ -252,6 +252,14 @@
<wheel_separation>1.25</wheel_separation>
<wheel_radius>0.3</wheel_radius>
<odom_publish_frequency>1</odom_publish_frequency>
<max_linear_acceleration>1</max_linear_acceleration>
<min_linear_acceleration>-1</min_linear_acceleration>
<max_angular_acceleration>2</max_angular_acceleration>
<min_angular_acceleration>-2</min_angular_acceleration>
<max_linear_velocity>0.5</max_linear_velocity>
<min_linear_velocity>-0.5</min_linear_velocity>
<max_angular_velocity>1</max_angular_velocity>
<min_angular_velocity>-1</min_angular_velocity>
</plugin>

</model>
Expand Down Expand Up @@ -431,6 +439,14 @@
<right_joint>right_wheel_joint</right_joint>
<wheel_separation>1.25</wheel_separation>
<wheel_radius>0.3</wheel_radius>
<max_linear_acceleration>1</max_linear_acceleration>
<min_linear_acceleration>-1</min_linear_acceleration>
<max_angular_acceleration>2</max_angular_acceleration>
<min_angular_acceleration>-2</min_angular_acceleration>
<max_linear_velocity>0.5</max_linear_velocity>
<min_linear_velocity>-0.5</min_linear_velocity>
<max_angular_velocity>1</max_angular_velocity>
<min_angular_velocity>-1</min_angular_velocity>
</plugin>

</model>
Expand Down
8 changes: 8 additions & 0 deletions examples/worlds/diff_drive_skid.sdf
View file
Original file line number Diff line number Diff line change
Expand Up @@ -346,6 +346,14 @@
<right_joint>rear_right_wheel_joint</right_joint>
<wheel_separation>1.25</wheel_separation>
<wheel_radius>0.3</wheel_radius>
<max_linear_acceleration>1</max_linear_acceleration>
<min_linear_acceleration>-1</min_linear_acceleration>
<max_angular_acceleration>2</max_angular_acceleration>
<min_angular_acceleration>-2</min_angular_acceleration>
<max_linear_velocity>0.5</max_linear_velocity>
<min_linear_velocity>-0.5</min_linear_velocity>
<max_angular_velocity>1</max_angular_velocity>
<min_angular_velocity>-1</min_angular_velocity>
</plugin>

</model>
Expand Down
93 changes: 70 additions & 23 deletions src/systems/diff_drive/DiffDrive.cc
View file
Original file line number Diff line number Diff line change
Expand Up @@ -210,62 +210,109 @@ void DiffDrive::Configure(const Entity &_entity,
this->dataPtr->limiterAng = std::make_unique<ignition::math::SpeedLimiter>();

// Parse speed limiter parameters.

// Min Velocity
if (_sdf->HasElement("min_velocity"))
{
const double minVel = _sdf->Get<double>("min_velocity");
this->dataPtr->limiterLin->SetMinVelocity(minVel);
this->dataPtr->limiterAng->SetMinVelocity(minVel);
}
if (_sdf->HasElement("min_linear_velocity"))
{
const double minLinVel = _sdf->Get<double>("min_linear_velocity");
this->dataPtr->limiterLin->SetMinVelocity(minLinVel);
}
if (_sdf->HasElement("max_linear_velocity"))
{
const double maxLinVel = _sdf->Get<double>("max_linear_velocity");
this->dataPtr->limiterLin->SetMaxVelocity(maxLinVel);
}
if (_sdf->HasElement("min_linear_acceleration"))
if (_sdf->HasElement("min_angular_velocity"))
{
const double minLinAccel = _sdf->Get<double>("min_linear_acceleration");
this->dataPtr->limiterLin->SetMinAcceleration(minLinAccel);
const double minAngVel = _sdf->Get<double>("min_angular_velocity");
this->dataPtr->limiterAng->SetMinVelocity(minAngVel);
}
if (_sdf->HasElement("max_linear_acceleration"))

// Max Velocity
if (_sdf->HasElement("max_velocity"))
{
const double maxLinAccel = _sdf->Get<double>("max_linear_acceleration");
this->dataPtr->limiterLin->SetMaxAcceleration(maxLinAccel);
const double maxVel = _sdf->Get<double>("max_velocity");
this->dataPtr->limiterLin->SetMaxVelocity(maxVel);
this->dataPtr->limiterAng->SetMaxVelocity(maxVel);
}
if (_sdf->HasElement("min_linear_jerk"))
if (_sdf->HasElement("max_linear_velocity"))
{
const double minLinJerk = _sdf->Get<double>("min_linear_jerk");
this->dataPtr->limiterLin->SetMinJerk(minLinJerk);
const double maxLinVel = _sdf->Get<double>("max_linear_velocity");
this->dataPtr->limiterLin->SetMaxVelocity(maxLinVel);
}
if (_sdf->HasElement("max_linear_jerk"))
if (_sdf->HasElement("max_angular_velocity"))
{
const double maxLinJerk = _sdf->Get<double>("max_linear_jerk");
this->dataPtr->limiterLin->SetMaxJerk(maxLinJerk);
const double maxAngVel = _sdf->Get<double>("max_angular_velocity");
this->dataPtr->limiterAng->SetMaxVelocity(maxAngVel);
}

if (_sdf->HasElement("min_angular_velocity"))
// Min Acceleration
if (_sdf->HasElement("min_acceleration"))
{
const double minAngVel = _sdf->Get<double>("min_angular_velocity");
this->dataPtr->limiterAng->SetMinVelocity(minAngVel);
const double minAccel = _sdf->Get<double>("min_acceleration");
this->dataPtr->limiterLin->SetMinAcceleration(minAccel);
this->dataPtr->limiterAng->SetMinAcceleration(minAccel);
}
if (_sdf->HasElement("max_angular_velocity"))
if (_sdf->HasElement("min_linear_acceleration"))
{
const double maxAngVel = _sdf->Get<double>("max_angular_velocity");
this->dataPtr->limiterAng->SetMaxVelocity(maxAngVel);
const double minLinAccel = _sdf->Get<double>("min_linear_acceleration");
this->dataPtr->limiterLin->SetMinAcceleration(minLinAccel);
}
if (_sdf->HasElement("min_angular_acceleration"))
{
const double minAngAccel = _sdf->Get<double>("min_angular_acceleration");
this->dataPtr->limiterAng->SetMinAcceleration(minAngAccel);
}

// Max Acceleration
if (_sdf->HasElement("max_acceleration"))
{
const double maxAccel = _sdf->Get<double>("max_acceleration");
this->dataPtr->limiterLin->SetMaxAcceleration(maxAccel);
this->dataPtr->limiterAng->SetMaxAcceleration(maxAccel);
}
if (_sdf->HasElement("max_linear_acceleration"))
{
const double maxLinAccel = _sdf->Get<double>("max_linear_acceleration");
this->dataPtr->limiterLin->SetMaxAcceleration(maxLinAccel);
}
if (_sdf->HasElement("max_angular_acceleration"))
{
const double maxAngAccel = _sdf->Get<double>("max_angular_acceleration");
this->dataPtr->limiterAng->SetMaxAcceleration(maxAngAccel);
}

// Min Jerk
if (_sdf->HasElement("min_jerk"))
{
const double minJerk = _sdf->Get<double>("min_jerk");
this->dataPtr->limiterLin->SetMinJerk(minJerk);
this->dataPtr->limiterAng->SetMinJerk(minJerk);
}
if (_sdf->HasElement("min_linear_jerk"))
{
const double minLinJerk = _sdf->Get<double>("min_linear_jerk");
this->dataPtr->limiterLin->SetMinJerk(minLinJerk);
}
if (_sdf->HasElement("min_angular_jerk"))
{
const double minAngJerk = _sdf->Get<double>("min_angular_jerk");
this->dataPtr->limiterAng->SetMinJerk(minAngJerk);
}

// Max Jerk
if (_sdf->HasElement("max_jerk"))
{
const double maxJerk = _sdf->Get<double>("max_jerk");
this->dataPtr->limiterLin->SetMaxJerk(maxJerk);
this->dataPtr->limiterAng->SetMaxJerk(maxJerk);
}
if (_sdf->HasElement("max_linear_jerk"))
{
const double maxLinJerk = _sdf->Get<double>("max_linear_jerk");
this->dataPtr->limiterLin->SetMaxJerk(maxLinJerk);
}
if (_sdf->HasElement("max_angular_jerk"))
{
const double maxAngJerk = _sdf->Get<double>("max_angular_jerk");
Expand Down
12 changes: 8 additions & 4 deletions test/worlds/diff_drive.sdf
View file
Original file line number Diff line number Diff line change
Expand Up @@ -227,10 +227,14 @@
<right_joint>right_wheel_joint</right_joint>
<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>
<max_linear_acceleration>1</max_linear_acceleration>
<min_linear_acceleration>-1</min_linear_acceleration>
<max_angular_acceleration>2</max_angular_acceleration>
<min_angular_acceleration>-2</min_angular_acceleration>
<max_linear_velocity>0.5</max_linear_velocity>
<min_linear_velocity>-0.5</min_linear_velocity>
<max_angular_velocity>1</max_angular_velocity>
<min_angular_velocity>-1</min_angular_velocity>
<!-- no odom_publisher_frequency defaults to 50 Hz -->
</plugin>

Expand Down
12 changes: 8 additions & 4 deletions test/worlds/diff_drive_custom_topics.sdf
View file
Original file line number Diff line number Diff line change
Expand Up @@ -229,10 +229,14 @@
<wheel_radius>0.3</wheel_radius>
<topic>/model/foo/cmdvel</topic>
<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>
<max_linear_acceleration>1</max_linear_acceleration>
<min_linear_acceleration>-1</min_linear_acceleration>
<max_angular_acceleration>2</max_angular_acceleration>
<min_angular_acceleration>-2</min_angular_acceleration>
<max_linear_velocity>0.5</max_linear_velocity>
<min_linear_velocity>-0.5</min_linear_velocity>
<max_angular_velocity>1</max_angular_velocity>
<min_angular_velocity>-1</min_angular_velocity>
<!-- no odom_publisher_frequency defaults to 50 Hz -->
</plugin>

Expand Down

0 comments on commit 20e9461

Please sign in to comment.