[JTC] Trajectory replacement does not interpolate from active trajectory

[christophfroehlich]

Describe the bug
While copying documentation from ROS 1 Wiki we realized that the trajectory replacement does not work as expected.

To Reproduce
Steps to reproduce the behavior:

1. Checkout the jtc_tests branch from my gazebo_ros2_control fork
2. Run this launch file
   ros2 launch gazebo_ros2_control_demos cart_example_velocity_jtc.launch.py
3. Run this action client
   ros2 run gazebo_ros2_control_demos example_position_with_velocity_nonzero
4. Run it again before the old goal was reached.

Used JTC configuration:

joint_trajectory_controller:
  ros__parameters:
    joints:
      - slider_to_cart
    command_interfaces:
      - velocity
    state_interfaces:
      - position
      - velocity
    allow_nonzero_velocity_stop: true
    gains.slider_to_cart.ff_velocity_scale: 1.0

Action goal:

• position + velocity points are sent -> cubic spline.
• traj start is left zero, which means "start now" acc to ROS 1 Wiki and as far as I understand the code here.

Expected behavior
The trajectory should be interpolated from the current state (or current desired values) to the new one, as described in the ROS 1 Wiki

Screenshots
At 9s a new action goal was sent and immediately overridden by the new one:

Environment (please complete the following information):

• OS: Ubuntu 22.04
• Version: master branches of ros-controls repos

[christophfroehlich]

I seems that this was never ported to ROS 2

ros2_controllers/joint_trajectory_controller/test/test_trajectory_controller.cpp

Lines 1229 to 1234 in 83042fb

	// RCLCPP_WARN(
	// traj_node->get_logger(),
	// "Test disabled until current_trajectory is taken into account when adding a new trajectory.");
	// // https://github.com/ros-controls/ros_controllers/blob/melodic-devel/
	// // joint_trajectory_controller/include/joint_trajectory_controller/init_joint_trajectory.h#L149
	// return;

references to

https://github.com/ros-controls/ros_controllers/blob/b752394cae97ca09c772240abdf5e3ff06a7ae02/joint_trajectory_controller/include/joint_trajectory_controller/init_joint_trajectory.h#L148-L153

[christophfroehlich]

With the work of #714 I started understanding the trajectory generator. The issue described here is no issue at all, this is what happens here:

• Header time stamp traj start=0 means start now.
• point[0].time_from_start was set to zero
• The spline tries to reach point[0] immediately -> This means that there is no chance to interpolate from the current state.

With point[0].time_from_start=1s, we get the desired behavior: