diff --git a/joint_trajectory_controller/src/trajectory.cpp b/joint_trajectory_controller/src/trajectory.cpp index 3ced840c02..094348343f 100644 --- a/joint_trajectory_controller/src/trajectory.cpp +++ b/joint_trajectory_controller/src/trajectory.cpp @@ -185,6 +185,7 @@ bool Trajectory::sample( } } + output_state.time_from_start = next_point.time_from_start; // whole animation has played out start_segment_itr = --end(); end_segment_itr = end(); diff --git a/joint_trajectory_controller/test/test_trajectory_controller.cpp b/joint_trajectory_controller/test/test_trajectory_controller.cpp index ec3391a9e1..e01ef9b9a1 100644 --- a/joint_trajectory_controller/test/test_trajectory_controller.cpp +++ b/joint_trajectory_controller/test/test_trajectory_controller.cpp @@ -461,7 +461,31 @@ TEST_P(TrajectoryControllerTestParameterized, state_topic_consistency_command_jo EXPECT_EQ(n_joints, state->output.effort.size()); } } +/** + * @brief check if time_from_start is populated correctly + */ +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 = 100000000; + publish(tfs, {INITIAL_POS_JOINTS}, rclcpp::Time(0)); + traj_controller_->wait_for_trajectory(executor); + + updateController(); + // give the publish timer one more spin + executor.spin_some(); + + auto state = getState(); + ASSERT_TRUE(state); + EXPECT_EQ(state->feedback.time_from_start.sec, 0u); + EXPECT_EQ(state->feedback.time_from_start.nanosec, 100000000u); +} /** * @brief check if dynamic parameters are updated */