Smooth the timestamp of TwistStamped messages in velocity_smoother

nav2_velocity_smoother/README.md

@@ -20,6 +20,7 @@ This package was created to do the following:
 - Provide open loop and closed loop options
 - Component nodes for use in single-process systems and stand-alone node format
 - Dynamically reconfigurable parameters
+- Smooth command timestamps upon different planning and smoothing frequencies
 
 ## Design
 
@@ -30,7 +31,7 @@ Thusly, it takes in a command via the `cmd_vel` topic and produces a smoothed ou
 The node is designed on a regular timer running at a configurable rate.
 This is in contrast to simply computing a smoothed velocity command in the callback of each `cmd_vel` input from Nav2.
 This allows us to interpolate commands at a higher frequency than Nav2's local trajectory planners can provide.
-For example, if a local trajectory planner is running at 20hz, the velocity smoother can run at 100hz to provide approximately 5 messages to a robot controller which will be smoothed by kinematic limits at each timestep.
+For example, if a local trajectory planner is running at 20hz, the velocity smoother can run at 100hz to provide approximately 5 messages to a robot controller which will be smoothed by kinematic limits at each timestep. Furthermore, the timestamps of the command messages will also be smoothed according to the rule: `cmd_vel_timestamp = cmd_vel_timestamp_of_last_input_command + (now() - last_receive_time_of_input_command)`
 This provides a more regular stream of commands to a robot base and interpolates commands between the current velocity and the desired velocity more finely for smoother acceleration / motion profiles.
 While this is not required, it is a nice design feature.
 It is possible to also simply run the smoother at `cmd_vel` rate to smooth velocities alone without interpolation.

nav2_velocity_smoother/src/velocity_smoother.cpp

@@ -316,11 +316,17 @@ 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;
+  // 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;
 
   // 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;