Backport of PR:2594 for humble

MIGRATION.md

@@ -17,6 +17,20 @@ API changes in MoveIt releases
 - Static member variable interface of the CollisionDetectorAllocatorTemplate for the string NAME was replaced with a virtual method `getName`.
 - Enhance `RDFLoader` to load from string parameter OR string topic (and add the ability to publish a string topic).
 
+## ROS Humble
+
+### MoveIt Servo — rolling command window (`moveit_servo`)
+A new parameter `max_expected_latency` (default `0.1` s) has been added to `ServoParameters`.
+When `command_out_type` is `trajectory_msgs/JointTrajectory`, Servo now publishes **multi-point rolling-window trajectories** instead of single-point messages. This improves motion smoothness and reduces latency-induced overshoot.
+
+**Required config change:** Add the following to your servo YAML configuration:
+```yaml
+max_expected_latency: 0.1  # seconds; tune to your network/hardware latency
+```
+The recommended `publish_period` has also changed from `0.034` to `0.01` s so the rolling window contains enough points. A validation warning is emitted if `publish_period > max_expected_latency / 3`.
+
+`std_msgs/Float64MultiArray` output is unaffected by this change.
+
 ## ROS Noetic
 - RobotModel no longer overrides empty URDF collision geometry by matching the visual geometry of the link.
 - Planned trajectories are *slow* by default.

moveit_ros/moveit_servo/MIGRATION.md

@@ -2,5 +2,20 @@
 
 API changes since last Noetic Release
 
+## Rolling command window (Humble backport of PR #2594)
+
+A new parameter `max_expected_latency` has been added to the servo configuration.
+When `command_out_type` is `trajectory_msgs/JointTrajectory`, Servo now publishes multi-point rolling-window trajectories for smoother motion.
+
+Add to your servo YAML:
+```yaml
+max_expected_latency: 0.1  # seconds
+publish_period: 0.01        # reduced from 0.034 for a denser rolling window
+```
+
+`std_msgs/Float64MultiArray` output is unchanged.
+
+## Noetic → ROS 2
+
 - Servo::getLatestJointState was removed.  To get the latest joint positions of the group servo is working with use the CSM in the PSM.  Here is an example of how to get the latest joint positions:
         planning_scene_monitor_->getStateMonitor()->getCurrentState()->copyJointGroupPositions(move_group_name, positions);

moveit_ros/moveit_servo/config/panda_simulated_config.yaml

@@ -16,7 +16,8 @@ scale:
 # override_velocity_scaling_factor = 0.0 # valid range [0.0:1.0]
 
 ## Properties of outgoing commands
-publish_period: 0.034  # 1/Nominal publish rate [seconds]
+publish_period: 0.01  # 1/Nominal publish rate [seconds]
+max_expected_latency: 0.1  # delay between sending a command and the robot executing it [seconds]
 low_latency_mode: false  # Set this to true to publish as soon as an incoming Twist command is received (publish_period is ignored)
 
 # What type of topic does your robot driver expect?

moveit_ros/moveit_servo/config/panda_simulated_config_pose_tracking.yaml

@@ -16,7 +16,8 @@ scale:
 # override_velocity_scaling_factor = 0.0 # valid range [0.0:1.0]
 
 ## Properties of outgoing commands
-publish_period: 0.034  # 1/Nominal publish rate [seconds]
+publish_period: 0.01  # 1/Nominal publish rate [seconds]
+max_expected_latency: 0.1  # delay between sending a command and the robot executing it [seconds]
 
 # What type of topic does your robot driver expect?
 # Currently supported are std_msgs/Float64MultiArray or trajectory_msgs/JointTrajectory

moveit_ros/moveit_servo/include/moveit_servo/servo_calcs.h

@@ -40,7 +40,9 @@
 
 // C++
 #include <condition_variable>
+#include <deque>
 #include <mutex>
+#include <optional>
 #include <thread>
 #include <atomic>
 
@@ -176,6 +178,19 @@ class ServoCalcs
   void composeJointTrajMessage(const sensor_msgs::msg::JointState& joint_state,
                                trajectory_msgs::msg::JointTrajectory& joint_trajectory);
 
+  /**
+   * Add a joint state to the rolling command window, evicting stale entries and applying
+   * velocity smoothing to prevent trajectory overshoot.
+   * @param joint_state The new joint state to append (positions and velocities must be set)
+   */
+  void updateSlidingWindow(const sensor_msgs::msg::JointState& joint_state);
+
+  /**
+   * Build a multi-point JointTrajectory from the rolling window for smooth command delivery.
+   * Returns empty optional if fewer than MIN_POINTS_FOR_TRAJ_MSG entries are available.
+   */
+  std::optional<trajectory_msgs::msg::JointTrajectory> composeRollingWindowTrajectory() const;
+
   /** \brief Set the filters to the specified values */
   void resetLowPassFilters(const sensor_msgs::msg::JointState& joint_state);
 
@@ -298,6 +313,20 @@ class ServoCalcs
 
   trajectory_msgs::msg::JointTrajectory::SharedPtr last_sent_command_;
 
+  // Minimum number of trajectory points required to publish a rolling-window trajectory message.
+  static constexpr int MIN_POINTS_FOR_TRAJ_MSG = 3;
+
+  // A timestamped joint state entry for the rolling command window.
+  struct TimestampedJointState
+  {
+    sensor_msgs::msg::JointState state;
+    rclcpp::Time time_stamp;
+  };
+
+  // Rolling window of future joint commands used to compose multi-point trajectory messages.
+  // Spans [now - max_expected_latency, now + max_expected_latency].
+  std::deque<TimestampedJointState> joint_cmd_rolling_window_;
+
   // ROS
   rclcpp::Subscription<sensor_msgs::msg::JointState>::SharedPtr joint_state_sub_;
   rclcpp::Subscription<geometry_msgs::msg::TwistStamped>::SharedPtr twist_stamped_sub_;

moveit_ros/moveit_servo/include/moveit_servo/servo_parameters.h

@@ -74,6 +74,8 @@ struct ServoParameters
   // Properties of outgoing commands
   std::string command_out_topic{ "/panda_arm_controller/joint_trajectory" };
   double publish_period{ 0.034 };
+  // Maximum expected latency between generating a servo command and the controller receiving it [seconds]
+  double max_expected_latency{ 0.1 };
   std::string command_out_type{ "trajectory_msgs/JointTrajectory" };
   bool publish_joint_positions{ true };
   bool publish_joint_velocities{ true };

moveit_ros/moveit_servo/src/servo_calcs.cpp

@@ -464,33 +464,40 @@ void ServoCalcs::calculateSingleIteration()
 
   if (ok_to_publish_ && !paused_)
   {
-    // Clear out position commands if user did not request them (can cause interpolation issues)
-    if (!parameters_->publish_joint_positions)
-    {
-      joint_trajectory->points[0].positions.clear();
-    }
-    // Likewise for velocity and acceleration
-    if (!parameters_->publish_joint_velocities)
-    {
-      joint_trajectory->points[0].velocities.clear();
-    }
-    if (!parameters_->publish_joint_accelerations)
-    {
-      joint_trajectory->points[0].accelerations.clear();
-    }
-
-    // Put the outgoing msg in the right format
-    // (trajectory_msgs/JointTrajectory or std_msgs/Float64MultiArray).
     if (parameters_->command_out_type == "trajectory_msgs/JointTrajectory")
     {
-      // When a joint_trajectory_controller receives a new command, a stamp of 0 indicates "begin immediately"
-      // See http://wiki.ros.org/joint_trajectory_controller#Trajectory_replacement
+      // Update the rolling window with the current servo command.
+      // internal_joint_state_ always holds full positions and velocities.
+      updateSlidingWindow(internal_joint_state_);
+
+      // Try to compose and publish a multi-point rolling-window trajectory.
+      const auto rolling_traj = composeRollingWindowTrajectory();
+      if (rolling_traj)
+      {
+        trajectory_outgoing_cmd_pub_->publish(rolling_traj.value());
+      }
+
+      // Keep last_sent_command_ as a single-point message (stamp=0 means "begin immediately").
+      // This is used by the halt path to know the last commanded positions.
+      if (!parameters_->publish_joint_positions)
+        joint_trajectory->points[0].positions.clear();
+      if (!parameters_->publish_joint_velocities)
+        joint_trajectory->points[0].velocities.clear();
+      if (!parameters_->publish_joint_accelerations)
+        joint_trajectory->points[0].accelerations.clear();
       joint_trajectory->header.stamp = rclcpp::Time(0);
       *last_sent_command_ = *joint_trajectory;
-      trajectory_outgoing_cmd_pub_->publish(std::move(joint_trajectory));
     }
     else if (parameters_->command_out_type == "std_msgs/Float64MultiArray")
     {
+      // Float64MultiArray output is unaffected by the rolling window — keep original behavior.
+      if (!parameters_->publish_joint_positions)
+        joint_trajectory->points[0].positions.clear();
+      if (!parameters_->publish_joint_velocities)
+        joint_trajectory->points[0].velocities.clear();
+      if (!parameters_->publish_joint_accelerations)
+        joint_trajectory->points[0].accelerations.clear();
+
       auto joints = std::make_unique<std_msgs::msg::Float64MultiArray>();
       if (parameters_->publish_joint_positions && !joint_trajectory->points.empty())
         joints->data = joint_trajectory->points[0].positions;
@@ -500,6 +507,11 @@ void ServoCalcs::calculateSingleIteration()
       multiarray_outgoing_cmd_pub_->publish(std::move(joints));
     }
   }
+  else
+  {
+    // Not publishing — clear the rolling window so stale future commands don't linger.
+    joint_cmd_rolling_window_.clear();
+  }
 
   // Update the filters if we haven't yet
   if (!updated_filters_)
@@ -814,6 +826,95 @@ void ServoCalcs::composeJointTrajMessage(const sensor_msgs::msg::JointState& joi
   joint_trajectory.points.push_back(point);
 }
 
+void ServoCalcs::updateSlidingWindow(const sensor_msgs::msg::JointState& joint_state)
+{
+  const rclcpp::Time cur_time = node_->now();
+
+  TimestampedJointState next_stamped;
+  next_stamped.state = joint_state;
+  next_stamped.time_stamp = cur_time + rclcpp::Duration::from_seconds(parameters_->max_expected_latency);
+
+  const rclcpp::Duration active_window = rclcpp::Duration::from_seconds(parameters_->max_expected_latency);
+
+  // Evict entries older than (current_time - max_expected_latency).
+  while (!joint_cmd_rolling_window_.empty() && joint_cmd_rolling_window_.front().time_stamp < (cur_time - active_window))
+  {
+    joint_cmd_rolling_window_.pop_front();
+  }
+
+  // Drop any trailing entries that share the same timestamp as the new command.
+  while (!joint_cmd_rolling_window_.empty() && next_stamped.time_stamp == joint_cmd_rolling_window_.back().time_stamp)
+  {
+    joint_cmd_rolling_window_.pop_back();
+  }
+
+  // Smooth velocity of the last stored entry to prevent trajectory overshoot.
+  // If the motion direction reverses, zero the velocity. Otherwise use the
+  // minimum of the forward and backward finite-difference velocities.
+  if (joint_cmd_rolling_window_.size() >= 2)
+  {
+    const size_t n = joint_cmd_rolling_window_.size();
+    auto& last = joint_cmd_rolling_window_[n - 1];
+    const auto& second_last = joint_cmd_rolling_window_[n - 2];
+
+    for (size_t i = 0; i < joint_state.position.size(); ++i)
+    {
+      const double dir1 = second_last.state.position[i] - last.state.position[i];
+      const double dir2 = next_stamped.state.position[i] - last.state.position[i];
+
+      // A sign product <= 0 means the direction changed (v-shape in position).
+      if (dir1 * dir2 <= 0.0)
+      {
+        last.state.velocity[i] = 0.0;
+      }
+      else
+      {
+        const double dt1 = (next_stamped.time_stamp - last.time_stamp).seconds();
+        const double dt2 = (last.time_stamp - second_last.time_stamp).seconds();
+        const double vel_fwd = (next_stamped.state.position[i] - last.state.position[i]) / dt1;
+        const double vel_bwd = (last.state.position[i] - second_last.state.position[i]) / dt2;
+        last.state.velocity[i] = (std::abs(vel_fwd) < std::abs(vel_bwd)) ? vel_fwd : vel_bwd;
+      }
+      next_stamped.state.velocity[i] = last.state.velocity[i];
+    }
+  }
+
+  joint_cmd_rolling_window_.push_back(next_stamped);
+}
+
+std::optional<trajectory_msgs::msg::JointTrajectory> ServoCalcs::composeRollingWindowTrajectory() const
+{
+  if (static_cast<int>(joint_cmd_rolling_window_.size()) < MIN_POINTS_FOR_TRAJ_MSG)
+  {
+    return std::nullopt;
+  }
+
+  trajectory_msgs::msg::JointTrajectory traj;
+  traj.header.frame_id = parameters_->planning_frame;
+  traj.header.stamp = joint_cmd_rolling_window_.front().time_stamp;
+  traj.joint_names = joint_cmd_rolling_window_.front().state.name;
+
+  // Publish all points except the last — its velocity is still being updated
+  // by the next call to updateSlidingWindow.
+  for (size_t i = 0; i < joint_cmd_rolling_window_.size() - 1; ++i)
+  {
+    const auto& stamped = joint_cmd_rolling_window_[i];
+    trajectory_msgs::msg::JointTrajectoryPoint point;
+    point.time_from_start = stamped.time_stamp - traj.header.stamp;
+
+    if (parameters_->publish_joint_positions)
+      point.positions = stamped.state.position;
+    if (parameters_->publish_joint_velocities)
+      point.velocities = stamped.state.velocity;
+    if (parameters_->publish_joint_accelerations)
+      point.accelerations = std::vector<double>(stamped.state.name.size(), 0.0);
+
+    traj.points.push_back(point);
+  }
+
+  return traj;
+}
+
 std::vector<const moveit::core::JointModel*>
 ServoCalcs::enforcePositionLimits(sensor_msgs::msg::JointState& joint_state) const
 {

moveit_ros/moveit_servo/src/servo_parameters.cpp

@@ -153,6 +153,12 @@ void ServoParameters::declare(const std::string& ns,
   node_parameters->declare_parameter(
       ns + ".publish_period", ParameterValue{ parameters.publish_period },
       ParameterDescriptorBuilder{}.type(PARAMETER_DOUBLE).description("1/Nominal publish rate [seconds]"));
+  node_parameters->declare_parameter(
+      ns + ".max_expected_latency", ParameterValue{ parameters.max_expected_latency },
+      ParameterDescriptorBuilder{}
+          .type(PARAMETER_DOUBLE)
+          .description("Maximum expected latency between generating a servo command and the controller receiving it "
+                       "[seconds]. Used to build a rolling trajectory window for smooth command delivery."));
   node_parameters->declare_parameter(
       ns + ".command_out_type", ParameterValue{ parameters.command_out_type },
       ParameterDescriptorBuilder{}
@@ -290,6 +296,7 @@ ServoParameters ServoParameters::get(const std::string& ns,
   // Properties of outgoing commands
   parameters.command_out_topic = node_parameters->get_parameter(ns + ".command_out_topic").as_string();
   parameters.publish_period = node_parameters->get_parameter(ns + ".publish_period").as_double();
+  parameters.max_expected_latency = node_parameters->get_parameter(ns + ".max_expected_latency").as_double();
   parameters.command_out_type = node_parameters->get_parameter(ns + ".command_out_type").as_string();
   parameters.publish_joint_positions = node_parameters->get_parameter(ns + ".publish_joint_positions").as_bool();
   parameters.publish_joint_velocities = node_parameters->get_parameter(ns + ".publish_joint_velocities").as_bool();
@@ -360,6 +367,21 @@ std::optional<ServoParameters> ServoParameters::validate(ServoParameters paramet
                         "greater than zero. Check yaml file.");
     return std::nullopt;
   }
+  if (parameters.max_expected_latency <= 0.)
+  {
+    RCLCPP_WARN(LOGGER, "Parameter 'max_expected_latency' should be greater than zero. Check yaml file.");
+    return std::nullopt;
+  }
+  // For smooth rolling-window delivery we need at least 3 points, so warn if the ratio is too small.
+  static constexpr int MIN_POINTS_FOR_TRAJ_MSG = 3;
+  if (parameters.max_expected_latency / MIN_POINTS_FOR_TRAJ_MSG < parameters.publish_period)
+  {
+    RCLCPP_WARN(LOGGER,
+                "For smooth servo delivery, 'publish_period' (%.4f s) should be less than 1/%d of "
+                "'max_expected_latency' (%.4f s). Consider setting 'publish_period: 0.01' and "
+                "'max_expected_latency: 0.1' in your servo config.",
+                parameters.publish_period, MIN_POINTS_FOR_TRAJ_MSG, parameters.max_expected_latency);
+  }
   if (parameters.num_outgoing_halt_msgs_to_publish < 0)
   {
     RCLCPP_WARN(LOGGER, "Parameter 'num_outgoing_halt_msgs_to_publish' should be greater than zero. Check yaml file.");

moveit_ros/moveit_servo/test/config/servo_settings.yaml

@@ -15,6 +15,7 @@ scale:
 ## Properties of outgoing commands
 low_latency_mode: false  # Set this to true to tie the output rate to the input rate
 publish_period: 0.01  # 1/Nominal publish rate [seconds]
+max_expected_latency: 0.1  # delay between sending a command and the robot executing it [seconds]
 
 # What type of topic does your robot driver expect?
 # Currently supported are std_msgs/Float64MultiArray (for ros_control JointGroupVelocityController or JointGroupPositionController)

moveit_ros/moveit_servo/test/servo_calcs_unit_tests.cpp

@@ -168,6 +168,45 @@ TEST_F(ServoCalcsUnitTests, SingularityScaling)
   EXPECT_EQ(scaling_factor, 0);
 }
 
+// ---------------------------------------------------------------------------
+// ServoParameters validation tests for the rolling-window (PR #2594) fields
+// ---------------------------------------------------------------------------
+
+TEST(ServoParameterValidation, MaxExpectedLatencyZeroFails)
+{
+  moveit_servo::ServoParameters params;
+  params.max_expected_latency = 0.0;
+  EXPECT_FALSE(moveit_servo::ServoParameters::validate(params).has_value())
+      << "max_expected_latency = 0 should fail validation";
+}
+
+TEST(ServoParameterValidation, MaxExpectedLatencyNegativeFails)
+{
+  moveit_servo::ServoParameters params;
+  params.max_expected_latency = -0.05;
+  EXPECT_FALSE(moveit_servo::ServoParameters::validate(params).has_value())
+      << "Negative max_expected_latency should fail validation";
+}
+
+TEST(ServoParameterValidation, MaxExpectedLatencyPositivePasses)
+{
+  moveit_servo::ServoParameters params;
+  params.max_expected_latency = 0.1;
+  params.publish_period = 0.01;  // satisfies the window-density check
+  EXPECT_TRUE(moveit_servo::ServoParameters::validate(params).has_value())
+      << "Valid max_expected_latency and publish_period should pass validation";
+}
+
+TEST(ServoParameterValidation, WindowDensityWarningDoesNotFail)
+{
+  // publish_period > max_expected_latency / 3 should only warn, not fail.
+  moveit_servo::ServoParameters params;
+  params.max_expected_latency = 0.1;
+  params.publish_period = 0.05;  // 0.05 > 0.1/3 ≈ 0.033 → triggers the density warning
+  EXPECT_TRUE(moveit_servo::ServoParameters::validate(params).has_value())
+      << "Density warning condition should not cause validation failure";
+}
+
 int main(int argc, char** argv)
 {
   rclcpp::init(argc, argv);