Apply acceleration until both left and right reach targetspeed (#1009)

Co-authored-by: Louise Poubel <louise@openrobotics.org>
ros-simulation · Jun 19, 2020 · 02e5f96 · 02e5f96
1 parent aa18d71
commit 02e5f96
Showing 1 changed file with 2 additions and 2 deletions.
diff --git a/gazebo_plugins/src/gazebo_ros_diff_drive.cpp b/gazebo_plugins/src/gazebo_ros_diff_drive.cpp
@@ -472,8 +472,8 @@ void GazeboRosDiffDrivePrivate::OnUpdate(const gazebo::common::UpdateInfo & _inf
   // If max_accel == 0, or target speed is reached
   for (unsigned int i = 0; i < num_wheel_pairs_; ++i) {
     if (max_wheel_accel_ == 0 ||
-      (fabs(desired_wheel_speed_[2 * i + LEFT] - current_speed[2 * i + LEFT]) < 0.01) ||
-      (fabs(desired_wheel_speed_[2 * i + RIGHT] - current_speed[2 * i + RIGHT]) < 0.01))
+      ((fabs(desired_wheel_speed_[2 * i + LEFT] - current_speed[2 * i + LEFT]) < 0.01) &&
+      (fabs(desired_wheel_speed_[2 * i + RIGHT] - current_speed[2 * i + RIGHT]) < 0.01)))
     {
       joints_[2 * i + LEFT]->SetParam(
         "vel", 0, desired_wheel_speed_[2 * i + LEFT] / (wheel_diameter_[i] / 2.0));