Add new parameter to nav2_velocity_smoother to also smooth the timestamp of TwistStamped messages

nav2_bringup/params/nav2_multirobot_params_all.yaml

@@ -289,6 +289,7 @@ waypoint_follower:
 velocity_smoother:
   ros__parameters:
     smoothing_frequency: 20.0
+    stamp_smoothed_velocity_with_smoothing_time: False
     scale_velocities: False
     feedback: "OPEN_LOOP"
     max_velocity: [0.26, 0.0, 1.0]

nav2_bringup/params/nav2_params.yaml

@@ -359,6 +359,7 @@ route_server:
 velocity_smoother:
   ros__parameters:
     smoothing_frequency: 20.0
+    stamp_smoothed_velocity_with_smoothing_time: False
     scale_velocities: False
     feedback: "OPEN_LOOP"
     max_velocity: [0.5, 0.0, 2.0]

nav2_velocity_smoother/README.md

@@ -60,6 +60,7 @@ velocity_smoother:
    	odom_topic: "odom"  # Topic of odometry to use for estimating current velocities
    	odom_duration: 0.1  # Period of time (s) to sample odometry information in for velocity estimation
 	enable_stamped_cmd_vel: false # Whether to stamp the velocity. True uses TwistStamped. False uses Twist
+    stamp_smoothed_velocity_with_smoothing_time: false # Whether to smooth the timestamp of the header message of TwistStamped output instead of keeping the timestamp of the last received command
 ```
 
 ## Topics
@@ -87,4 +88,4 @@ The minimum and maximum velocities for rotation (e.g. ``Vw``) represent left and
 
 Deadband velocities are minimum thresholds, below which we set its value to `0`. This can be useful when your robot's breaking torque from stand still is non-trivial so sending very small values will pull high amounts of current.
 
-The `VelocitySmoother` node makes use of a [nav2_util::TwistSubscriber](../nav2_util/README.md#twist-publisher-and-twist-subscriber-for-commanded-velocities).
+The `VelocitySmoother` node makes use of a [nav2_util::TwistSubscriber](../nav2_util/README.md#twist-publisher-and-twist-subscriber-for-commanded-velocities).

nav2_velocity_smoother/include/nav2_velocity_smoother/velocity_smoother.hpp

@@ -149,6 +149,8 @@ class VelocitySmoother : public nav2_util::LifecycleNode
   bool open_loop_;
   bool stopped_{true};
   bool scale_velocities_;
+  // whether to overwite the timestamp of the smoothed message or to keep last command's timestamp
+  bool stamp_smoothed_velocity_with_smoothing_time_;
   std::vector<double> max_velocities_;
   std::vector<double> min_velocities_;
   std::vector<double> max_accels_;

nav2_velocity_smoother/src/velocity_smoother.cpp

@@ -145,6 +145,12 @@ VelocitySmoother::on_configure(const rclcpp_lifecycle::State & state)
     return nav2_util::CallbackReturn::FAILURE;
   }
 
+  // Define option to overwrite the timestamp of the message containing the smoothed velocity
+  declare_parameter_if_not_declared(
+    node, "stamp_smoothed_velocity_with_smoothing_time", rclcpp::ParameterValue(false));
+  node->get_parameter(
+    "stamp_smoothed_velocity_with_smoothing_time", stamp_smoothed_velocity_with_smoothing_time_);
+
   // Setup inputs / outputs
   smoothed_cmd_pub_ = std::make_unique<nav2_util::TwistPublisher>(node, "cmd_vel_smoothed", 1);
   cmd_sub_ = std::make_unique<nav2_util::TwistSubscriber>(
@@ -309,11 +315,21 @@ void VelocitySmoother::smootherTimer()
     return;
   }
 
+  auto const delta_time_since_last_command = now() - last_command_time_;
+
   auto cmd_vel = std::make_unique<geometry_msgs::msg::TwistStamped>();
-  cmd_vel->header = command_->header;
+  cmd_vel->header.frame_id = command_->header.frame_id;
+  if (stamp_smoothed_velocity_with_smoothing_time_) {
+    // Smooth the timestamp of the smoothed message
+    // Do not keep the same timestamp of the last command; this causes jerky behavior
+    // See https://github.com/ros-navigation/navigation2/issues/5857
+    cmd_vel->header.stamp = command_->header.stamp + delta_time_since_last_command;
+  } else {
+    cmd_vel->header.stamp = command_->header.stamp;
+  }
 
   // Check for velocity timeout. If nothing received, publish zeros to apply deceleration
-  if (now() - last_command_time_ > velocity_timeout_) {
+  if (delta_time_since_last_command > velocity_timeout_) {
     if (last_cmd_.twist == geometry_msgs::msg::Twist() || stopped_) {
       stopped_ = true;
       return;