Trajectory replacement with full blending at message arrival time

[vedh1234]

Description

Port of the legacy ROS 1 "update existing trajectory" behavior to the ROS 2 joint_trajectory_controller.

Related to #84

What this PR does:

• Enables the allow_trajectory_replacement parameter (default: true).
• Merge-at-Arrival Architecture: When a new trajectory arrives, it is immediately spliced with the currently active trajectory rather than deferring/parking the goal.
• Constructs a continuous, single trajectory for the controller to execute:
  • Prefix: The arm continues exactly along its old path until the new trajectory's start time.
  • Bridge: A smooth, velocity-continuous transition anchor is generated exactly at the handoff point.
  • Suffix: Joints omitted from the new trajectory continue and complete their original path.
• Eliminates the legacy ROS 2 behavior where a future-stamped trajectory would immediately kill the active trajectory and create a slow spline ramp to the first point.
• Immediate trajectories (stamp = 0 or stamp <= now) behave exactly as before.

ROS 1 Feature Parity Achieved in this PR:

• Smooth Future-Stamped Blending: Commanded joints faithfully follow their old path until the new trajectory's exact start time.
• Partial Goal Continuity: Omitted joints do not freeze; they outlast the new trajectory and complete their original motion (Suffix).
• Speed Scaling Safety: The blending anchor uses the trajectory's internal scaled cursor (traj_time_) rather than wall-clock time, guaranteeing zero position jumps even under speed scaling (an improvement over some legacy implementations).

What this PR does NOT target (ROS 1 Parity Gaps):

• Omitted Joint Spline Fidelity: In ROS 1, omitted joints perfectly retained their original independent trajectory segments. Due to ROS 2's monolithic trajectory architecture, omitted joints must be re-sampled onto the new trajectory's time grid. If the new trajectory has sparse waypoints, the omitted joint will re-interpolate between them, which may result in minor deviations from its original intermediate path.

Is this user-facing behaviour change?

Yes. When allow_trajectory_replacement is true (default), publishing a future-stamped trajectory will now splice into the current motion.

Did you use Generative AI?

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

Additional Information

• The 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.
• 3 new parameterized integration tests (blend_no_position_jump_under_speed_scaling, blend_omitted_joint_outlasts_shorter_new_trajectory, blend_commanded_joint_follows_old_path) were added to validate the prefix/suffix math.

TODOs

• Fork the repository.
• Modify the source; please focus on the specific change you are contributing.
• Ensure local tests pass. (colcon test and pre-commit run)
• 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]

Codecov Report

❌ Patch coverage is 88.72180% with 15 lines in your changes missing coverage. Please review.
✅ Project coverage is 85.30%. Comparing base (68630ae) to head (cc5ac5c).
⚠️ Report is 10 commits behind head on master.

Files with missing lines	Patch %	Lines
...ory_controller/src/joint_trajectory_controller.cpp	84.61%	4 Missing and 10 partials ⚠️
...ntroller/test/test_trajectory_controller_utils.hpp	0.00%	0 Missing and 1 partial ⚠️

Additional details and impacted files

@@            Coverage Diff             @@
##           master    #2419      +/-   ##
==========================================
- Coverage   85.61%   85.30%   -0.32%     
==========================================
  Files         148      147       -1     
  Lines       15837    14985     -852     
  Branches     1339     1277      -62     
==========================================
- Hits        13559    12783     -776     
+ Misses       1792     1726      -66     
+ Partials      486      476      -10     

Flag	Coverage Δ
unittests	85.30% <88.72%> (-0.32%)	⬇️

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	40.00% <ø> (ø)
...ory_controller/test/test_trajectory_controller.cpp	99.18% <100.00%> (-0.17%)	⬇️
...ntroller/test/test_trajectory_controller_utils.hpp	83.95% <0.00%> (-0.51%)	⬇️
...ory_controller/src/joint_trajectory_controller.cpp	83.99% <84.61%> (+<0.01%)	⬆️

... and 18 files with indirect coverage changes

🚀 New features to boost your workflow:

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