From 0f28f9d966189c2e593ab063fca1357fc68693b4 Mon Sep 17 00:00:00 2001
From: BriceRenaudeau <48433002+BriceRenaudeau@users.noreply.github.com>
Date: Wed, 26 Jul 2023 19:58:58 +0200
Subject: [PATCH] Fix the velocity smoother being stuck when the deadband is
 too high (#3690)

* Move last_cmd update before deadband

* fix lint

(cherry picked from commit cb34d0ce1d24c1c437f548834a31a2ee8c4d9889)
---
 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 04a32081a2a..aa5fbec7830 100644
--- a/nav2_velocity_smoother/src/velocity_smoother.cpp
+++ b/nav2_velocity_smoother/src/velocity_smoother.cpp
@@ -299,13 +299,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));
 }