Backport nav2 vel smoother timeout fix

nav2_velocity_smoother/package.xml

@@ -2,7 +2,7 @@
 <?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
 <package format="3">
   <name>nav2_velocity_smoother</name>
-  <version>1.1.5</version>
+  <version>1.0.0</version>
   <description>Nav2's Output velocity smoother</description>
   <maintainer email="stevenmacenski@gmail.com">Steve Macenski</maintainer>
   <license>Apache-2.0</license>

nav2_velocity_smoother/src/velocity_smoother.cpp

@@ -202,23 +202,45 @@ double VelocitySmoother::applyConstraints(
   const double accel, const double decel, const double eta)
 {
   double dv = v_cmd - v_curr;
-  const double v_component_max = accel / smoothing_frequency_;
-  const double v_component_min = decel / smoothing_frequency_;
+
+  double v_component_max;
+  double v_component_min;
+
+  // Accelerating if magnitude of v_cmd is above magnitude of v_curr
+  // and if v_cmd and v_curr have the same sign (i.e. speed is NOT passing through 0.0)
+  // Deccelerating otherwise
+  if (v_curr * v_cmd >= 0.0) {
+    if (abs(v_cmd) >= abs(v_curr)) {
+      v_component_max = accel / smoothing_frequency_;
+      v_component_min = -accel / smoothing_frequency_;
+    } else {
+      v_component_max = -decel / smoothing_frequency_;
+      v_component_min = decel / smoothing_frequency_;
+    }
+  } else {
+    v_component_max = -decel / smoothing_frequency_;
+    v_component_min = decel / smoothing_frequency_;
+  }
+
   return v_curr + std::clamp(eta * dv, v_component_min, v_component_max);
 }
 
 void VelocitySmoother::smootherTimer()
 {
+  // Wait until the first command is received
+  if (!command_) {
+    return;
+  }
+
   auto cmd_vel = std::make_unique<geometry_msgs::msg::Twist>();
 
-  // Check for velocity timeout. If nothing received, publish zeros to stop robot
+  // Check for velocity timeout. If nothing received, publish zeros to apply deceleration
   if (now() - last_command_time_ > velocity_timeout_) {
-    last_cmd_ = geometry_msgs::msg::Twist();
-    if (!stopped_) {
-      smoothed_cmd_pub_->publish(std::move(cmd_vel));
+    if (last_cmd_ == geometry_msgs::msg::Twist() || stopped_) {
+      stopped_ = true;
+      return;
     }
-    stopped_ = true;
-    return;
+    *command_ = geometry_msgs::msg::Twist();
   }
 
   stopped_ = false;
@@ -271,16 +293,12 @@ void VelocitySmoother::smootherTimer()
   cmd_vel->angular.z = applyConstraints(
     current_.angular.z, command_->angular.z, max_accels_[2], max_decels_[2], eta);
 
-  // If open loop, assume we achieved it
-  if (open_loop_) {
-    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));
 }