From 567584e5e1500cc3624d3cb49385eb012e6a1865 Mon Sep 17 00:00:00 2001 From: scgroot Date: Thu, 17 Oct 2019 15:44:29 +0200 Subject: [PATCH] Apply acceleration until both left and right reach targetspeed --- gazebo_plugins/src/gazebo_ros_diff_drive.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/gazebo_plugins/src/gazebo_ros_diff_drive.cpp b/gazebo_plugins/src/gazebo_ros_diff_drive.cpp index 1c70c5502..fcd72c5cf 100644 --- a/gazebo_plugins/src/gazebo_ros_diff_drive.cpp +++ b/gazebo_plugins/src/gazebo_ros_diff_drive.cpp @@ -459,8 +459,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));