Add time_from_start to action feedback and state message (cherry-pick #1755) (backport #1820) (#1988)

Author: mergify[bot]

joint_trajectory_controller/src/joint_trajectory_controller.cpp

@@ -203,12 +203,15 @@ controller_interface::return_type JointTrajectoryController::update(
     // Sample expected state from the trajectory
     current_trajectory_->sample(
       traj_time_, interpolation_method_, state_desired_, start_segment_itr, end_segment_itr);
+    state_desired_.time_from_start = traj_time_ - current_trajectory_->time_from_start();
 
     // Sample setpoint for next control cycle
     const bool valid_point = current_trajectory_->sample(
       traj_time_ + update_period_, interpolation_method_, command_next_, start_segment_itr,
       end_segment_itr, false);
 
+    state_current_.time_from_start = time - current_trajectory_->time_from_start();
+
     if (valid_point)
     {
       const rclcpp::Time traj_start = current_trajectory_->time_from_start();
@@ -1218,9 +1221,11 @@ void JointTrajectoryController::publish_state(
   if (state_publisher_)
   {
     state_msg_.header.stamp = time;
+    state_msg_.reference.time_from_start = desired_state.time_from_start;
     state_msg_.reference.positions = desired_state.positions;
     state_msg_.reference.velocities = desired_state.velocities;
     state_msg_.reference.accelerations = desired_state.accelerations;
+    state_msg_.feedback.time_from_start = current_state.time_from_start;
     state_msg_.feedback.positions = current_state.positions;
     state_msg_.error.positions = state_error.positions;
     if (has_velocity_state_interface_)

joint_trajectory_controller/src/trajectory.cpp

@@ -177,6 +177,7 @@ bool Trajectory::sample(
       }
       start_segment_itr = begin() + static_cast<TrajectoryPointConstIter::difference_type>(i);
       end_segment_itr = begin() + static_cast<TrajectoryPointConstIter::difference_type>(i + 1);
+      output_state.time_from_start = next_point.time_from_start;
       if (search_monotonically_increasing)
       {
         last_sample_idx_ = i;

joint_trajectory_controller/test/test_trajectory_controller.cpp

@@ -461,6 +461,33 @@ TEST_P(TrajectoryControllerTestParameterized, state_topic_consistency_command_jo
   }
 }
 
+TEST_F(TrajectoryControllerTest, time_from_start_populated)
+{
+  rclcpp::executors::SingleThreadedExecutor executor;
+  SetUpAndActivateTrajectoryController(executor, {});
+  subscribeToState(executor);
+
+  // schedule a single waypoint at 100ms:
+  builtin_interfaces::msg::Duration tfs;
+  tfs.sec = 0;
+  tfs.nanosec = 100000000u;
+  publish(tfs, {INITIAL_POS_JOINTS}, rclcpp::Time(0));
+  traj_controller_->wait_for_trajectory(executor);
+
+  // update for 0.2s
+  updateController(rclcpp::Duration::from_seconds(0.2));
+  // give the publish timer one more spin
+  executor.spin_some();
+
+  auto state = getState();
+  ASSERT_TRUE(state);
+  // should be around 0.2s
+  EXPECT_EQ(state->feedback.time_from_start.sec, 0u);
+  EXPECT_NEAR(state->feedback.time_from_start.nanosec, 200000000u, 10000000u);
+  EXPECT_EQ(state->reference.time_from_start.sec, 0u);
+  EXPECT_NEAR(state->reference.time_from_start.nanosec, 200000000u, 10000000u);
+}
+
 /**
  * @brief check if dynamic parameters are updated
  */