From ce288bbd2665729cca4ed7faf37f212ba2a25598 Mon Sep 17 00:00:00 2001 From: BriceRenaudeau <48433002+BriceRenaudeau@users.noreply.github.com> Date: Tue, 11 Jul 2023 12:16:11 +0200 Subject: [PATCH 1/2] Move last_cmd update before deadband --- nav2_velocity_smoother/src/velocity_smoother.cpp | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/nav2_velocity_smoother/src/velocity_smoother.cpp b/nav2_velocity_smoother/src/velocity_smoother.cpp index 0f7ee04f142..a68b31d861b 100644 --- a/nav2_velocity_smoother/src/velocity_smoother.cpp +++ b/nav2_velocity_smoother/src/velocity_smoother.cpp @@ -305,13 +305,14 @@ void VelocitySmoother::smootherTimer() current_.linear.y, command_->linear.y, max_accels_[1], max_decels_[1], eta); cmd_vel->angular.z = applyConstraints( current_.angular.z, command_->angular.z, max_accels_[2], max_decels_[2], eta); + last_cmd_ = *cmd_vel; // Apply deadband restrictions & publish cmd_vel->linear.x = fabs(cmd_vel->linear.x) < deadband_velocities_[0] ? 0.0 : cmd_vel->linear.x; cmd_vel->linear.y = fabs(cmd_vel->linear.y) < deadband_velocities_[1] ? 0.0 : cmd_vel->linear.y; cmd_vel->angular.z = fabs(cmd_vel->angular.z) < deadband_velocities_[2] ? 0.0 : cmd_vel->angular.z; - last_cmd_ = *cmd_vel; + smoothed_cmd_pub_->publish(std::move(cmd_vel)); } From e6f7035ab4a728586f8748ce364ec7be1bcaf747 Mon Sep 17 00:00:00 2001 From: BriceRenaudeau <48433002+BriceRenaudeau@users.noreply.github.com> Date: Wed, 26 Jul 2023 15:20:25 +0200 Subject: [PATCH 2/2] fix lint --- nav2_velocity_smoother/src/velocity_smoother.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/nav2_velocity_smoother/src/velocity_smoother.cpp b/nav2_velocity_smoother/src/velocity_smoother.cpp index a68b31d861b..2596c073bae 100644 --- a/nav2_velocity_smoother/src/velocity_smoother.cpp +++ b/nav2_velocity_smoother/src/velocity_smoother.cpp @@ -312,7 +312,7 @@ void VelocitySmoother::smootherTimer() cmd_vel->linear.y = fabs(cmd_vel->linear.y) < deadband_velocities_[1] ? 0.0 : cmd_vel->linear.y; cmd_vel->angular.z = fabs(cmd_vel->angular.z) < deadband_velocities_[2] ? 0.0 : cmd_vel->angular.z; - + smoothed_cmd_pub_->publish(std::move(cmd_vel)); }