Trajectory blending with new trajectory deferral

doc/release_notes.rst

@@ -48,6 +48,11 @@ joint_trajectory_controller
   up to the first waypoint will use the same interpolation as the rest of the trajectory. (`#2043
   <https://github.com/ros-controls/ros2_controllers/pull/2043>`_)
 * Added decelerate-to-stop functionality when a trajectory is canceled or preempted. Instead of immediately holding position, the controller can now smoothly decelerate each joint to a stop using the per-joint ``max_deceleration_on_cancel`` parameter. (`#2163 <https://github.com/ros-controls/ros2_controllers/pull/2163>`_)
+* Added trajectory deferral support via the ``allow_trajectory_replacement`` parameter. When enabled,
+  a trajectory with a future ``header.stamp`` is deferred: the currently active trajectory
+  continues executing until the start time arrives, then the new trajectory is installed.
+  This is a partial port of the ROS 1 trajectory replacement behavior.
+  (`#2401 <https://github.com/ros-controls/ros2_controllers/pull/2401>`_)
 
 pid_controller
 **************

joint_trajectory_controller/doc/trajectory.rst

@@ -115,10 +115,20 @@ Trajectory Replacement
 
 Joint trajectory messages allow to specify the time at which a new trajectory should start executing by means of the header timestamp, where zero time (the default) means "start now".
 
-.. warning::
-
-  As of now, this functionality is not ported to ROS 2, see `this issue <https://github.com/ros-controls/ros2_controllers/issues/84>`__ for more information.
-  The current implementation just forgets the old trajectory.
+.. note::
+  Partial support for this functionality has been ported to ROS 2 via the ``allow_trajectory_replacement`` parameter (see `#84 <https://github.com/ros-controls/ros2_controllers/issues/84>`__).
+  When enabled, a trajectory arriving with a future ``header.stamp`` is deferred. The controller
+  continues executing the active trajectory until the requested start time is reached, at which
+  point the new trajectory is installed and execution begins.
+
+  The current ROS 2 implementation differs from the legacy behavior described below in the following ways:
+
+  + The controller does not splice or stitch the current and new trajectories together. At the handoff time, the new trajectory fully replaces the old one.
+  + Omitted joints in a partial goal are filled with their current position at the handoff time (hold-in-place), rather than continuing to follow the old trajectory.
+  + Only a single deferred trajectory is stored. A newer deferred trajectory overwrites any previously deferred one.
+  + When a new action goal is accepted and deferred, the currently active goal is immediately aborted instead of continuing to emit feedback until the handoff time.
+  + While a deferred action goal is waiting for its start time, it does not publish action feedback and is not evaluated for path tolerances.
+  + The handoff trigger uses the controller's wall-clock time. If the controller is running with a ``speed_scaling`` other than 1.0, the handoff may occur out of sync with the active trajectory's execution state.
 
 The arrival of a new trajectory command does not necessarily mean that the controller will completely discard the currently running trajectory and substitute it with the new one.
 Rather, the controller will take the useful parts of both and combine them appropriately, yielding a smarter trajectory replacement strategy.

joint_trajectory_controller/include/joint_trajectory_controller/joint_trajectory_controller.hpp

@@ -182,6 +182,14 @@ class JointTrajectoryController : public controller_interface::ControllerInterfa
   realtime_tools::RealtimeBuffer<std::shared_ptr<trajectory_msgs::msg::JointTrajectory>>
     new_trajectory_msg_;
 
+  // Trajectory deferred until its future start time.
+  std::shared_ptr<trajectory_msgs::msg::JointTrajectory> pending_traj_msg_ = nullptr;
+  rclcpp::Time pending_start_;
+  // Written by goal_cancelled_callback (non-RT), read in update() (RT) to drop pending.
+  std::atomic<bool> rt_clear_pending_{false};
+  // Suppresses feedback/tolerance/success for an action goal whose trajectory is still deferred.
+  std::atomic<bool> rt_active_goal_deferred_{false};
+
   std::shared_ptr<trajectory_msgs::msg::JointTrajectory> hold_position_msg_ptr_ = nullptr;
 
   using ControllerStateMsg = control_msgs::msg::JointTrajectoryControllerState;
@@ -233,6 +241,12 @@ class JointTrajectoryController : public controller_interface::ControllerInterfa
   // sorts the joints of the incoming message to our local order
   void sort_to_local_joint_order(
     std::shared_ptr<trajectory_msgs::msg::JointTrajectory> trajectory_msg) const;
+  // true if msg is one of the internally generated hold/success/decelerate trajectories.
+  bool is_internal_hold(const std::shared_ptr<trajectory_msgs::msg::JointTrajectory> & msg) const
+  {
+    return msg == hold_position_msg_ptr_ ||
+           (stop_trajectory_ != nullptr && msg == stop_trajectory_);
+  }
   bool validate_trajectory_msg(const trajectory_msgs::msg::JointTrajectory & trajectory) const;
   void add_new_trajectory_msg(
     const std::shared_ptr<trajectory_msgs::msg::JointTrajectory> & traj_msg);

joint_trajectory_controller/src/joint_trajectory_controller.cpp

@@ -267,14 +267,60 @@ controller_interface::return_type JointTrajectoryController::update(
   // Check if a new trajectory message has been received from Non-RT threads
   const auto current_trajectory_msg = current_trajectory_->get_trajectory_msg();
   auto new_external_msg = new_trajectory_msg_.readFromRT();
+
+  // A cancel (goal_cancelled_callback) asked us to drop any deferred trajectory.
+  if (rt_clear_pending_.exchange(false))
+  {
+    pending_traj_msg_ = nullptr;
+    rt_active_goal_deferred_ = false;
+  }
+
+  // The trajectory message to be installed into current_trajectory_ this cycle (if any).
+  std::shared_ptr<trajectory_msgs::msg::JointTrajectory> traj_msg_to_install = nullptr;
+
   // Discard, if a goal is pending but still not active (somewhere stuck in goal_handle_timer_)
   if (
-    current_trajectory_msg != *new_external_msg && (rt_has_pending_goal_ && !active_goal) == false)
+    current_trajectory_msg != *new_external_msg && *new_external_msg != pending_traj_msg_ &&
+    (rt_has_pending_goal_ && !active_goal) == false)
+  {
+    if (is_internal_hold(*new_external_msg))
+    {
+      // Internal hold/success/decelerate: install but never cancel a deferred trajectory.
+      traj_msg_to_install = *new_external_msg;
+    }
+    else if (
+      params_.allow_trajectory_replacement && has_active_trajectory() &&
+      rclcpp::Time((*new_external_msg)->header.stamp, time.get_clock_type()) > time)
+    {
+      // Future-stamped: defer until its start time, keep old trajectory running.
+      pending_traj_msg_ = *new_external_msg;
+      pending_start_ = rclcpp::Time((*new_external_msg)->header.stamp, time.get_clock_type());
+    }
+    else
+    {
+      // Immediate or blending off: install now, drop any previously deferred trajectory.
+      fill_partial_goal(*new_external_msg);
+      sort_to_local_joint_order(*new_external_msg);
+      traj_msg_to_install = *new_external_msg;
+      pending_traj_msg_ = nullptr;
+      rt_active_goal_deferred_ = false;
+    }
+  }
+
+  // FIRE: deferred trajectory's start time reached — install now.
+  if (pending_traj_msg_ && time >= pending_start_)
+  {
+    fill_partial_goal(pending_traj_msg_);
+    sort_to_local_joint_order(pending_traj_msg_);
+    traj_msg_to_install = pending_traj_msg_;
+    pending_traj_msg_ = nullptr;
+    rt_active_goal_deferred_ = false;
+  }
+
+  if (traj_msg_to_install)
   {
-    fill_partial_goal(*new_external_msg);
-    sort_to_local_joint_order(*new_external_msg);
     // TODO(denis): Add here integration of position and velocity
-    current_trajectory_->update(*new_external_msg);
+    current_trajectory_->update(traj_msg_to_install);
   }
 
   // current state update
@@ -460,7 +506,9 @@ controller_interface::return_type JointTrajectoryController::update(
         last_commanded_time_ = time;
       }
 
-      if (active_goal)
+      // Do not report on an action goal whose trajectory is still deferred (blending): its real
+      // trajectory has not started yet, so the old trajectory's progress must not succeed/abort it.
+      if (active_goal && !rt_active_goal_deferred_)
       {
         // send feedback
         auto feedback = std::make_shared<FollowJTrajAction::Feedback>();
@@ -1198,6 +1246,10 @@ controller_interface::CallbackReturn JointTrajectoryController::on_activate(
   current_trajectory_ = std::make_shared<Trajectory>();
   new_trajectory_msg_.writeFromNonRT(std::shared_ptr<trajectory_msgs::msg::JointTrajectory>());
 
+  pending_traj_msg_ = nullptr;
+  rt_active_goal_deferred_ = false;
+  rt_clear_pending_ = false;
+
   subscriber_is_active_ = true;
 
   // Initialize current state storage from hardware state interfaces
@@ -1342,6 +1394,10 @@ bool JointTrajectoryController::reset()
 
   current_trajectory_.reset();
 
+  pending_traj_msg_ = nullptr;
+  rt_active_goal_deferred_ = false;
+  rt_clear_pending_ = false;
+
   return true;
 }
 
@@ -1445,6 +1501,8 @@ rclcpp_action::CancelResponse JointTrajectoryController::goal_cancelled_callback
       // hold current position
       add_new_trajectory_msg(set_hold_position());
     }
+    // Written after add_new_trajectory_msg so the hold is visible before RT clears pending.
+    rt_clear_pending_ = true;
   }
   return rclcpp_action::CancelResponse::ACCEPT;
 }
@@ -1463,6 +1521,12 @@ void JointTrajectoryController::goal_accepted_callback(
 
     add_new_trajectory_msg(traj_msg);
     rt_is_holding_ = false;
+
+    // If blending is on, trajectory will be deferred by update(). Mark it so the result/feedback
+    // block does not judge the goal before it starts.
+    const auto now = get_node()->now();
+    rt_active_goal_deferred_ = params_.allow_trajectory_replacement && has_active_trajectory() &&
+                               rclcpp::Time(traj_msg->header.stamp, now.get_clock_type()) > now;
   }
 
   // Update the active goal

joint_trajectory_controller/src/joint_trajectory_controller_parameters.yaml

@@ -97,6 +97,16 @@ joint_trajectory_controller:
     default_value: false,
     description: "Allow integration in goal trajectories to accept goals without position or velocity specified",
   }
+  allow_trajectory_replacement: {
+    type: bool,
+    default_value: true,
+    description: "Port of the ROS 1 'update existing trajectory' behavior.
+      \n\n
+      If true (default), a trajectory whose ``header.stamp`` lies in the future is deferred: the
+      active trajectory keeps executing until that start time, then the new one is installed.
+      \n\n
+      If false, every incoming trajectory replaces the active one immediately (legacy behavior).",
+  }
   set_last_command_interface_value_as_state_on_activation: {
     type: bool,
     default_value: true,