JTC: sum periods (ros-controls#1395)

Author: fmauch

joint_trajectory_controller/include/joint_trajectory_controller/joint_trajectory_controller.hpp

@@ -127,6 +127,8 @@ class JointTrajectoryController : public controller_interface::ControllerInterfa
   Params params_;
   rclcpp::Duration update_period_{0, 0};
 
+  rclcpp::Time traj_time_;
+
   trajectory_msgs::msg::JointTrajectoryPoint last_commanded_state_;
   /// Specify interpolation method. Default to splines.
   interpolation_methods::InterpolationMethod interpolation_method_{

joint_trajectory_controller/src/joint_trajectory_controller.cpp

@@ -196,15 +196,20 @@ controller_interface::return_type JointTrajectoryController::update(
         traj_external_point_ptr_->set_point_before_trajectory_msg(
           time, state_current_, joints_angle_wraparound_);
       }
+      traj_time_ = time;
+    }
+    else
+    {
+      traj_time_ += period;
     }
 
     // Sample expected state from the trajectory
     traj_external_point_ptr_->sample(
-      time, interpolation_method_, state_desired_, start_segment_itr, end_segment_itr);
+      traj_time_, interpolation_method_, state_desired_, start_segment_itr, end_segment_itr);
 
     // Sample setpoint for next control cycle
     const bool valid_point = traj_external_point_ptr_->sample(
-      time + update_period_, interpolation_method_, command_next_, start_segment_itr,
+      traj_time_ + update_period_, interpolation_method_, command_next_, start_segment_itr,
       end_segment_itr, false);
 
     if (valid_point)
@@ -217,7 +222,7 @@ controller_interface::return_type JointTrajectoryController::update(
       // time_difference is
       // - negative until first point is reached
       // - counting from zero to time_from_start of next point
-      double time_difference = time.seconds() - segment_time_from_start.seconds();
+      double time_difference = traj_time_.seconds() - segment_time_from_start.seconds();
       bool tolerance_violated_while_moving = false;
       bool outside_goal_tolerance = false;
       bool within_goal_time = true;

joint_trajectory_controller/test/test_trajectory_actions.cpp

@@ -83,12 +83,15 @@ class TestTrajectoryActions : public TrajectoryControllerTest
       {
         // controller hardware cycle update loop
         auto clock = rclcpp::Clock(RCL_STEADY_TIME);
-        auto start_time = clock.now();
+        auto now_time = clock.now();
+        auto last_time = now_time;
         rclcpp::Duration wait = rclcpp::Duration::from_seconds(2.0);
-        auto end_time = start_time + wait;
+        auto end_time = last_time + wait;
         while (clock.now() < end_time)
         {
-          traj_controller_->update(clock.now(), clock.now() - start_time);
+          now_time = clock.now();
+          traj_controller_->update(now_time, now_time - last_time);
+          last_time = now_time;
         }
       });