Fix JTC state_msg (#1985)

Author: christophfroehlich

joint_trajectory_controller/src/joint_trajectory_controller.cpp

@@ -219,6 +219,8 @@ controller_interface::return_type JointTrajectoryController::update(
     TrajectoryPointConstIter start_segment_itr, end_segment_itr;
     const bool valid_point = traj_external_point_ptr_->sample(
       time, interpolation_method_, state_desired_, start_segment_itr, end_segment_itr);
+    state_desired_.time_from_start = state_current_.time_from_start =
+      time - traj_external_point_ptr_->time_from_start();
 
     if (valid_point)
     {
@@ -1190,15 +1192,9 @@ void JointTrajectoryController::publish_state(
     state_publisher_->msg_.reference.velocities = desired_state.velocities;
     state_publisher_->msg_.reference.accelerations = desired_state.accelerations;
     state_publisher_->msg_.reference.time_from_start = desired_state.time_from_start;
+    state_publisher_->msg_.feedback.time_from_start = current_state.time_from_start;
     state_publisher_->msg_.feedback.positions = current_state.positions;
-    // DESIRED and ACTUAL are deprecated in the message but we are still
-    // reporting on them
-    state_publisher_legacy_->msg_.desired.positions = desired_state.positions;
-    state_publisher_legacy_->msg_.desired.velocities = desired_state.velocities;
-    state_publisher_legacy_->msg_.desired.accelerations = desired_state.accelerations;
-    state_publisher_legacy_->msg_.actual.positions = current_state.positions;
     state_publisher_->msg_.error.positions = state_error.positions;
-    state_publisher_->msg_.feedback.time_from_start = desired_state.time_from_start;
     if (has_velocity_state_interface_)
     {
       state_publisher_->msg_.feedback.velocities = current_state.velocities;
@@ -1209,6 +1205,24 @@ void JointTrajectoryController::publish_state(
       state_publisher_->msg_.feedback.accelerations = current_state.accelerations;
       state_publisher_->msg_.error.accelerations = state_error.accelerations;
     }
+
+    // DESIRED and ACTUAL are deprecated in the message but we are still
+    // reporting on them
+    state_publisher_->msg_.desired.time_from_start = desired_state.time_from_start;
+    state_publisher_->msg_.desired.positions = desired_state.positions;
+    state_publisher_->msg_.desired.velocities = desired_state.velocities;
+    state_publisher_->msg_.desired.accelerations = desired_state.accelerations;
+    state_publisher_->msg_.actual.time_from_start = current_state.time_from_start;
+    state_publisher_->msg_.actual.positions = current_state.positions;
+    if (has_velocity_state_interface_)
+    {
+      state_publisher_->msg_.actual.velocities = current_state.velocities;
+    }
+    if (has_acceleration_state_interface_)
+    {
+      state_publisher_->msg_.actual.accelerations = current_state.accelerations;
+    }
+
     if (read_commands_from_command_interfaces(command_current_))
     {
       state_publisher_->msg_.output = command_current_;

joint_trajectory_controller/test/test_trajectory_controller.cpp

@@ -397,6 +397,7 @@ TEST_P(TrajectoryControllerTestParameterized, state_topic_consistency)
     EXPECT_EQ(n_joints, state->output.effort.size());
   }
 }
+
 TEST_F(TrajectoryControllerTest, time_from_start_populated)
 {
   rclcpp::executors::SingleThreadedExecutor executor;
@@ -410,14 +411,23 @@ TEST_F(TrajectoryControllerTest, time_from_start_populated)
   publish(tfs, {INITIAL_POS_JOINTS}, rclcpp::Time(0));
   traj_controller_->wait_for_trajectory(executor);
 
-  updateController();
+  // 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, but is 0.18s
+  EXPECT_EQ(state->reference.time_from_start.sec, 0u);
+  EXPECT_NEAR(state->reference.time_from_start.nanosec, 200000000u, 20000000u);
   EXPECT_EQ(state->feedback.time_from_start.sec, 0u);
-  EXPECT_EQ(state->feedback.time_from_start.nanosec, 100000000u);
+  EXPECT_NEAR(state->feedback.time_from_start.nanosec, 200000000u, 20000000u);
+  // legacy
+  EXPECT_EQ(state->desired.time_from_start.sec, 0u);
+  EXPECT_NEAR(state->desired.time_from_start.nanosec, 200000000u, 20000000u);
+  EXPECT_EQ(state->actual.time_from_start.sec, 0u);
+  EXPECT_NEAR(state->actual.time_from_start.nanosec, 200000000u, 20000000u);
 }
 
 /**