Trajectory blending with new trajectory deferral by vedh1234 · Pull Request #2401 · ros-controls/ros2_controllers

vedh1234 · 2026-06-09T20:30:08Z

Description

Partial port of the ROS 1 "update existing trajectory" behavior to ROS 2 joint_trajectory_controller.

Related to #84

What this PR does:

Adds a new parameter allow_trajectory_blending (default: true)
When enabled, a trajectory with a future header.stamp is deferred: the currently active trajectory continues executing until that start time arrives, then the new trajectory is installed
This eliminates the legacy behavior where a future-stamped trajectory would immediately kill the active trajectory and create a slow spline ramp to the first point
Deferred trajectories are correctly dropped on goal cancellation, controller deactivation, and controller reset
Action Server feedback/tolerance checking is suppressed during the deferral period to prevent spurious aborts
Immediate trajectories (stamp = 0 or stamp <= now) behave exactly as before (no behavioral change)

What this PR does NOT target: ( Compared to ROS1 blending feature )

Does not allow omitted joints to continue their old trajectory after the new one starts (ROS 1 full blend). Omitted joints freeze at their current position via fill_partial_goal(), same as legacy ROS 2 behavior
Does not handle speed scaling during the deferral period. The trigger uses wall-clock time, so if speed_scaling != 1.0, there will be a positional mismatch at handoff
Does not track multiple action goals simultaneously. The new goal immediately preempts the old one at the Action Server level; only the physical trajectory execution is deferred

Is this user-facing behavior change?

Yes. When allow_trajectory_blending is true (default), the active trajectory will continue running until the new trajectory's start time.

Did you use Generative AI?

Yes, Google Gemini and Claude were used to make improvements in manually written code, architectural analysis, edge-case review.

Additional Information

The existing disabled test test_execute_partial_traj_in_future referenced in Port over "update existing trajectory" to joint_trajectory_controller #84 has been enabled and passes

TODOs

To send us a pull request, please:

Fork the repository.
Modify the source; please focus on the specific change you are contributing. If you also reformat all the code, it will be hard for us to focus on your change.
Ensure local tests pass. (colcon test and pre-commit run (requires you to install pre-commit by pip3 install pre-commit)
Commit to your fork using clear commit messages.
Send a pull request, answering any default questions in the pull request interface.
Pay attention to any automated CI failures reported in the pull request, and stay involved in the conversation.

codecov · 2026-06-09T21:11:32Z

Codecov Report

❌ Patch coverage is 96.46018% with 8 lines in your changes missing coverage. Please review.
✅ Project coverage is 86.83%. Comparing base (a466e1a) to head (c81e361).

Files with missing lines	Patch %	Lines
...ectory_controller/test/test_trajectory_actions.cpp	96.55%	0 Missing and 5 partials ⚠️
...ory_controller/src/joint_trajectory_controller.cpp	97.29%	0 Missing and 1 partial ⚠️
...ory_controller/test/test_trajectory_controller.cpp	97.50%	0 Missing and 1 partial ⚠️
...ntroller/test/test_trajectory_controller_utils.hpp	0.00%	0 Missing and 1 partial ⚠️

Additional details and impacted files

@@            Coverage Diff             @@
##           master    #2401      +/-   ##
==========================================
+ Coverage   86.70%   86.83%   +0.13%     
==========================================
  Files         148      148              
  Lines       15858    16091     +233     
  Branches     1351     1361      +10     
==========================================
+ Hits        13749    13973     +224     
+ Misses       1613     1612       -1     
- Partials      496      506      +10

Flag	Coverage Δ
unittests	`86.83% <96.46%> (+0.13%)`	⬆️

Flags with carried forward coverage won't be shown. Click here to find out more.

Files with missing lines	Coverage Δ
...jectory_controller/joint_trajectory_controller.hpp	`62.50% <100.00%> (+22.50%)`	⬆️
...ory_controller/src/joint_trajectory_controller.cpp	`85.23% <97.29%> (+0.54%)`	⬆️
...ory_controller/test/test_trajectory_controller.cpp	`98.81% <97.50%> (-0.04%)`	⬇️
...ntroller/test/test_trajectory_controller_utils.hpp	`83.72% <0.00%> (-0.60%)`	⬇️
...ectory_controller/test/test_trajectory_actions.cpp	`97.50% <96.55%> (-0.20%)`	⬇️

... and 1 file with indirect coverage changes

🚀 New features to boost your workflow:

❄️ Test Analytics: Detect flaky tests, report on failures, and find test suite problems.

christophfroehlich

Partial port of the ROS 1 "update existing trajectory" behavior to ROS 2 joint_trajectory_controller.

Fixes #84

For documentation here, what is different from the ROS 1 behavior/what is maybe missing to port?

Please also update the release_notes and update the documentation.

saikishor · 2026-06-15T21:45:51Z

I'll review the logic and tests soon. I need time

saikishor

The main logic looks good.

I still need to verify the tests though

…he-new-traj

saikishor

Some comments on tests

saikishor · 2026-06-24T07:13:48Z

+  std::this_thread::sleep_for(std::chrono::milliseconds(150));  // well before 500 ms fire time
+  auto goal_handle_b = gh_b.get();
+  ASSERT_TRUE(goal_handle_b);
+  action_client_->async_cancel_goal(goal_handle_b);
+
+  controller_hw_thread_.join();
+
+  EXPECT_EQ(rclcpp_action::ResultCode::CANCELED, resultcode_b);


once the goal_b is sent you are not verifying the status of goal_a handle

That had a separate test testing it, but I'll add it here as well.

saikishor · 2026-06-24T07:20:31Z

+  EXPECT_EQ(rclcpp_action::ResultCode::ABORTED, resultcode_a);
+  EXPECT_EQ(rclcpp_action::ResultCode::SUCCEEDED, resultcode_b);


I see that this is being triggered, but in this test shouldn't the A start immediately instead of future timestamp and then when B enter it is aborted. This is a nice test

Maybe it is interesting to get the time_now and verify it continuously during the cycle?

Goal A already starts immediately (no header.stamp set). Goal B is the future-stamped one that gets deferred. So the behaviour is exactly as you described: A starts immediately, then when B is accepted, A gets preempted and ABORTED while B's trajectory is deferred until its stamp

The final ABORTED/SUCCEEDED check already proves the essential behaviour: A was preempted when B was accepted, and B successfully executed after its deferred start time. Happy to add intermediate checks in a follow-up if you feel it's important for the coverage.

saikishor · 2026-06-24T07:22:02Z

+/**
+ * @brief A stamp=0 topic trajectory preempts a deferred action goal at the trajectory level:
+ * the topic trajectory installs immediately and the deferred goal never fires.
+ * NOTE: the action goal handle is not formally canceled on topic preemption — that


Maybe it should cancel?

We shall discuss about that behaviour today

This is a pre-existing issue in JTC, topic_callback has never explicitly aborted the active action goal on topic preemption, even before this PR. The goal handle survives and eventually gets ABORTED maybe via tolerance violation when the robot follows the topic trajectory instead.

should we abort it when a topic traj comes on top of action goal ?
If Yes we can open a separate PR resolving this.

saikishor · 2026-06-24T07:23:00Z

+  {
+    std::vector<JointTrajectoryPoint> points_init(1);
+    points_init[0].time_from_start = rclcpp::Duration::from_seconds(2.0);
+    points_init[0].positions = {4.0, 5.0, 6.0};
+    control_msgs::action::FollowJointTrajectory_Goal goal_init;
+    goal_init.goal_time_tolerance = rclcpp::Duration::from_seconds(1.0);
+    goal_init.trajectory.joint_names = joint_names_;
+    goal_init.trajectory.points = points_init;
+    action_client_->async_send_goal(goal_init, goal_options_);
+  }
+  std::this_thread::sleep_for(std::chrono::milliseconds(200));
+
+  {
+    std::vector<JointTrajectoryPoint> points_a(1);
+    points_a[0].time_from_start = rclcpp::Duration::from_seconds(0.5);
+    points_a[0].positions = {7.0, 8.0, 9.0};
+    control_msgs::action::FollowJointTrajectory_Goal goal_a;
+    goal_a.goal_time_tolerance = rclcpp::Duration::from_seconds(1.0);
+    goal_a.trajectory.joint_names = joint_names_;
+    goal_a.trajectory.points = points_a;
+    goal_a.trajectory.header.stamp =
+      traj_controller_->get_node()->now() + rclcpp::Duration::from_seconds(1.0);
+    action_client_->async_send_goal(goal_a, goal_options_);


Any reasons for sending 2 goals for this test?

goal_a creates the active trajectory that causes goal_b to be deferred. Without goal_a running, goal_b (future-stamped) would install immediately since has_active_trajectory() would be false.

saikishor · 2026-06-24T07:25:03Z

+    {
+      auto clock = rclcpp::Clock(RCL_ROS_TIME);
+      auto now_time = clock.now();
+      auto last_time = now_time;
+      const auto end_time = now_time + rclcpp::Duration::from_seconds(0.3);
+      while (clock.now() < end_time)
+      {
+        now_time = clock.now();
+        traj_controller_->update(now_time, now_time - last_time);
+        last_time = now_time;
+      }
+      traj_controller_->get_node()->deactivate();
+      std::this_thread::sleep_for(std::chrono::milliseconds(200));
+    });
+  std::this_thread::sleep_for(std::chrono::milliseconds(100));


This may end up flaky test. Increase the time and reduce the sleep?

Okay will do that.

saikishor · 2026-06-24T07:25:35Z

+  {
+    std::vector<JointTrajectoryPoint> points_a(1);
+    points_a[0].time_from_start = rclcpp::Duration::from_seconds(1.0);
+    points_a[0].positions = {4.0, 5.0, 6.0};
+    control_msgs::action::FollowJointTrajectory_Goal goal_a;
+    goal_a.goal_time_tolerance = rclcpp::Duration::from_seconds(1.0);
+    goal_a.trajectory.joint_names = joint_names_;
+    goal_a.trajectory.points = points_a;
+    action_client_->async_send_goal(goal_a, goal_options_);
+  }
+  std::this_thread::sleep_for(std::chrono::milliseconds(50));
+
+  {
+    std::vector<JointTrajectoryPoint> points_b(1);
+    points_b[0].time_from_start = rclcpp::Duration::from_seconds(0.4);
+    points_b[0].positions = {7.0, 8.0, 9.0};
+    control_msgs::action::FollowJointTrajectory_Goal goal_b;
+    goal_b.goal_time_tolerance = rclcpp::Duration::from_seconds(1.0);
+    goal_b.trajectory.joint_names = joint_names_;
+    goal_b.trajectory.points = points_b;
+    goal_b.trajectory.header.stamp =
+      traj_controller_->get_node()->now() + rclcpp::Duration::from_seconds(0.5);
+    action_client_->async_send_goal(goal_b, goal_options_);
+  }


Again why 2 goals here? I don't understand the reasoning behind it

Same reason: goal_a creates the active trajectory that causes goal_b to be deferred. Without goal_a running, goal_b (future-stamped) would install immediately since has_active_trajectory() would be false.

saikishor · 2026-06-24T07:28:00Z

+    EXPECT_NEAR(4.55, state.positions[2], 0.25);
+    EXPECT_GT(state.positions[2], 4.1);  // clearly above receive-time value ≈3.73


Isn't this condition redundant?

Okay right, it was just to check if the joint moved beyond that point. But agreed, it's redundant, I'll remove the EXPECT_GT.

vedh1234 · 2026-06-24T17:21:31Z

@saikishor I have addressed your comments and also made requested changes. While going through I also realised that there is a helper API for sending goals. I've committed that as well.

…blending-with-deferring-the-new-traj

github-actions Bot requested review from Maverobot, arne48, bmagyar, christophfroehlich and mhubii June 9, 2026 20:30

christophfroehlich requested changes Jun 10, 2026

View reviewed changes

saikishor reviewed Jun 15, 2026

View reviewed changes

Comment thread doc/release_notes.rst Outdated

Comment thread joint_trajectory_controller/test/test_trajectory_controller_utils.hpp

vedh1234 added 6 commits June 17, 2026 20:06

feat: trajectory blending with new traj deferral

39acd72

chore: add the todo back

071a873

chore: fix formatting

b81bafa

feat: trajectory deferral test coverage expansion

9b1f568

fix: change parameter name to allow_trajectory_replacement

a7b1784

feat: release note and documentation update

e7db781

vedh1234 force-pushed the feat--trajectory-blending-with-deferring-the-new-traj branch from 498c4ed to e7db781 Compare June 17, 2026 20:09

saikishor requested changes Jun 17, 2026

View reviewed changes

vedh1234 and others added 2 commits June 18, 2026 13:10

fix: wording fix in docs

f136046

Merge branch 'master' into feat--trajectory-blending-with-deferring-t…

8672347

…he-new-traj

saikishor requested changes Jun 24, 2026

View reviewed changes

fix: test related fixes

c42a18c

Merge remote-tracking branch 'upstream/master' into feat--trajectory-…

c81e361

…blending-with-deferring-the-new-traj

		EXPECT_EQ(rclcpp_action::ResultCode::ABORTED, resultcode_a);
		EXPECT_EQ(rclcpp_action::ResultCode::SUCCEEDED, resultcode_b);

		EXPECT_NEAR(4.55, state.positions[2], 0.25);
		EXPECT_GT(state.positions[2], 4.1); // clearly above receive-time value ≈3.73

Uh oh!

Conversation

vedh1234 commented Jun 9, 2026 • edited Loading Uh oh! There was an error while loading. Please reload this page.

Uh oh!

Description

Is this user-facing behavior change?

Did you use Generative AI?

Additional Information

TODOs

Uh oh!

codecov Bot commented Jun 9, 2026 • edited Loading Uh oh! There was an error while loading. Please reload this page.

Uh oh!

Codecov Report

Uh oh!

christophfroehlich left a comment

Choose a reason for hiding this comment

Uh oh!

Uh oh!

Uh oh!

saikishor commented Jun 15, 2026

Uh oh!

saikishor left a comment

Choose a reason for hiding this comment

Uh oh!

Uh oh!

Uh oh!

Uh oh!

Uh oh!

Uh oh!

saikishor left a comment

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

vedh1234 commented Jun 24, 2026

Uh oh!

Reviewers

Assignees

Labels

Projects

Milestone

Development

Uh oh!

4 participants

vedh1234 commented Jun 9, 2026 •

edited

Loading

codecov Bot commented Jun 9, 2026 •

edited

Loading