From d50b6ba7eaf760436194e7263ceb4399afbec653 Mon Sep 17 00:00:00 2001 From: Michael Wrock Date: Thu, 17 Jul 2025 03:41:19 -0700 Subject: [PATCH 1/2] [JTC] added time_from_start to action feedback (#1755) --- .../src/joint_trajectory_controller.cpp | 2 ++ .../src/trajectory.cpp | 1 + .../test/test_trajectory_controller.cpp | 21 +++++++++++++++++++ 3 files changed, 24 insertions(+) diff --git a/joint_trajectory_controller/src/joint_trajectory_controller.cpp b/joint_trajectory_controller/src/joint_trajectory_controller.cpp index 65c4709b5f..e65a087272 100644 --- a/joint_trajectory_controller/src/joint_trajectory_controller.cpp +++ b/joint_trajectory_controller/src/joint_trajectory_controller.cpp @@ -1240,8 +1240,10 @@ void JointTrajectoryController::publish_state( state_publisher_->msg_.reference.positions = desired_state.positions; 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.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; diff --git a/joint_trajectory_controller/src/trajectory.cpp b/joint_trajectory_controller/src/trajectory.cpp index 3ced840c02..86282e875e 100644 --- a/joint_trajectory_controller/src/trajectory.cpp +++ b/joint_trajectory_controller/src/trajectory.cpp @@ -177,6 +177,7 @@ bool Trajectory::sample( } start_segment_itr = begin() + static_cast(i); end_segment_itr = begin() + static_cast(i + 1); + output_state.time_from_start = next_point.time_from_start; if (search_monotonically_increasing) { last_sample_idx_ = i; diff --git a/joint_trajectory_controller/test/test_trajectory_controller.cpp b/joint_trajectory_controller/test/test_trajectory_controller.cpp index a33962af3f..954350f4f0 100644 --- a/joint_trajectory_controller/test/test_trajectory_controller.cpp +++ b/joint_trajectory_controller/test/test_trajectory_controller.cpp @@ -461,6 +461,27 @@ TEST_P(TrajectoryControllerTestParameterized, state_topic_consistency_command_jo EXPECT_EQ(n_joints, state->output.effort.size()); } } +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 From 3785515150d0945244bc6dde708a3b9b01c2c19c Mon Sep 17 00:00:00 2001 From: Bence Magyar Date: Thu, 17 Jul 2025 12:54:36 +0100 Subject: [PATCH 2/2] fix format --- .../test/test_trajectory_controller.cpp | 7 ++++--- 1 file changed, 4 insertions(+), 3 deletions(-) diff --git a/joint_trajectory_controller/test/test_trajectory_controller.cpp b/joint_trajectory_controller/test/test_trajectory_controller.cpp index 954350f4f0..fe304f6458 100644 --- a/joint_trajectory_controller/test/test_trajectory_controller.cpp +++ b/joint_trajectory_controller/test/test_trajectory_controller.cpp @@ -468,7 +468,9 @@ TEST_F(TrajectoryControllerTest, time_from_start_populated) subscribeToState(executor); // schedule a single waypoint at 100ms: - builtin_interfaces::msg::Duration tfs; tfs.sec = 0; tfs.nanosec = 100000000; + 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); @@ -478,11 +480,10 @@ TEST_F(TrajectoryControllerTest, time_from_start_populated) auto state = getState(); ASSERT_TRUE(state); - EXPECT_EQ(state->feedback.time_from_start.sec, 0u); + 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 */