diff --git a/joint_trajectory_controller/src/joint_trajectory_controller.cpp b/joint_trajectory_controller/src/joint_trajectory_controller.cpp index a55f6e2a9d..1d3e666192 100644 --- a/joint_trajectory_controller/src/joint_trajectory_controller.cpp +++ b/joint_trajectory_controller/src/joint_trajectory_controller.cpp @@ -175,7 +175,7 @@ controller_interface::return_type JointTrajectoryController::update( } } - // don't update goal after we sampled the trajectory to avoid any racecondition + // don't update goal after we sampled the trajectory to avoid any race condition const auto active_goal = *rt_active_goal_.readFromRT(); // Check if a new trajectory message has been received from Non-RT threads @@ -231,6 +231,8 @@ controller_interface::return_type JointTrajectoryController::update( traj_time_, interpolation_method_, state_desired_, start_segment_itr, end_segment_itr); state_desired_.time_from_start = traj_time_ - current_trajectory_->time_from_start(); + const auto next_point_index = std::distance(current_trajectory_->begin(), end_segment_itr); + // Sample setpoint for next control cycle const bool valid_point = current_trajectory_->sample( traj_time_ + update_period_, interpolation_method_, command_next_, start_segment_itr, @@ -373,6 +375,7 @@ controller_interface::return_type JointTrajectoryController::update( feedback->actual = state_current_; feedback->desired = state_desired_; feedback->error = state_error_; + feedback->index = static_cast(next_point_index); active_goal->setFeedback(feedback); // check abort diff --git a/joint_trajectory_controller/test/test_trajectory_actions.cpp b/joint_trajectory_controller/test/test_trajectory_actions.cpp index e6dca43eba..9d66bec2e6 100644 --- a/joint_trajectory_controller/test/test_trajectory_actions.cpp +++ b/joint_trajectory_controller/test/test_trajectory_actions.cpp @@ -204,6 +204,14 @@ class TestTrajectoryActions : public TrajectoryControllerTest return; } } + + size_t get_index_from_time(const rclcpp::Time & time, const std::vector & times_vector) + { + const double time_in_seconds = time.seconds(); + return static_cast(std::distance( + times_vector.begin(), + std::lower_bound(times_vector.begin(), times_vector.end(), time_in_seconds))); + } }; // From the tutorial: https://www.sandordargo.com/blog/2019/04/24/parameterized-testing-with-gtest @@ -313,25 +321,35 @@ TEST_P(TestTrajectoryActionsTestParameterized, test_success_multi_point_sendgoal SetUpExecutor({params}, false, 1.0, 0.0); SetUpControllerHardware(); + // defining points and times + std::vector points_times{0.2, 0.3}; + std::vector> points_positions{{{4.0, 5.0, 6.0}}, {{7.0, 8.0, 9.0}}}; + // add feedback bool feedback_recv = false; goal_options_.feedback_callback = [&]( rclcpp_action::ClientGoalHandle::SharedPtr, - const std::shared_ptr) { feedback_recv = true; }; + const std::shared_ptr feedback_msg) + { + feedback_recv = true; + + size_t expected_index = + get_index_from_time(rclcpp::Time(0, 0) + feedback_msg->desired.time_from_start, points_times); + EXPECT_EQ(static_cast(expected_index), feedback_msg->index); + }; std::shared_future gh_future; // send goal with multiple points - std::vector> points_positions{{{4.0, 5.0, 6.0}}, {{7.0, 8.0, 9.0}}}; { std::vector points; JointTrajectoryPoint point1; - point1.time_from_start = rclcpp::Duration::from_seconds(0.2); + point1.time_from_start = rclcpp::Duration::from_seconds(points_times.at(0)); point1.positions = points_positions.at(0); points.push_back(point1); JointTrajectoryPoint point2; - point2.time_from_start = rclcpp::Duration::from_seconds(0.3); + point2.time_from_start = rclcpp::Duration::from_seconds(points_times.at(1)); point2.positions = points_positions.at(1); points.push_back(point2); @@ -360,27 +378,36 @@ TEST_P(TestTrajectoryActionsTestParameterized, test_success_multi_point_with_vel SetUpExecutor(params, false, 1.0, 0.0); SetUpControllerHardware(); + // defining points and times + std::vector points_times{0.2, 0.3}; + std::vector> points_positions{{{4.0, 5.0, 6.0}}, {{7.0, 8.0, 9.0}}}; + std::vector> points_velocities{{{1.0, 1.0, 1.0}}, {{2.0, 2.0, 2.0}}}; + // add feedback bool feedback_recv = false; goal_options_.feedback_callback = [&]( rclcpp_action::ClientGoalHandle::SharedPtr, - const std::shared_ptr) { feedback_recv = true; }; + const std::shared_ptr feedback_msg) + { + size_t expected_index = + get_index_from_time(rclcpp::Time(0, 0) + feedback_msg->desired.time_from_start, points_times); + EXPECT_EQ(static_cast(expected_index), feedback_msg->index); + feedback_recv = true; + }; std::shared_future gh_future; // send goal with multiple points - std::vector> points_positions{{{4.0, 5.0, 6.0}}, {{7.0, 8.0, 9.0}}}; - std::vector> points_velocities{{{1.0, 1.0, 1.0}}, {{2.0, 2.0, 2.0}}}; { std::vector points; JointTrajectoryPoint point1; - point1.time_from_start = rclcpp::Duration::from_seconds(0.2); + point1.time_from_start = rclcpp::Duration::from_seconds(points_times.at(0)); point1.positions = points_positions.at(0); point1.velocities = points_velocities.at(0); points.push_back(point1); JointTrajectoryPoint point2; - point2.time_from_start = rclcpp::Duration::from_seconds(0.3); + point2.time_from_start = rclcpp::Duration::from_seconds(points_times.at(1)); point2.positions = points_positions.at(1); point2.velocities = points_velocities.at(1); points.push_back(point2); @@ -459,27 +486,36 @@ TEST_F(TestTrajectoryActions, test_goal_tolerances_multi_point_success) SetUpExecutor(params); SetUpControllerHardware(); + // defining points and times + std::vector points_times{0.2, 0.3}; + std::vector> points_positions{{{4.0, 5.0, 6.0}}, {{7.0, 8.0, 9.0}}}; + // add feedback bool feedback_recv = false; goal_options_.feedback_callback = [&]( rclcpp_action::ClientGoalHandle::SharedPtr, - const std::shared_ptr) { feedback_recv = true; }; + const std::shared_ptr feedback_msg) + { + size_t expected_index = + get_index_from_time(rclcpp::Time(0, 0) + feedback_msg->desired.time_from_start, points_times); + EXPECT_EQ(static_cast(expected_index), feedback_msg->index); + feedback_recv = true; + }; std::shared_future gh_future; // send goal with multiple points - std::vector> points_positions{{{4.0, 5.0, 6.0}}, {{7.0, 8.0, 9.0}}}; { std::vector points; JointTrajectoryPoint point1; - point1.time_from_start = rclcpp::Duration::from_seconds(0.2); + point1.time_from_start = rclcpp::Duration::from_seconds(points_times.at(0)); point1.positions.resize(joint_names_.size()); point1.positions = points_positions.at(0); points.push_back(point1); JointTrajectoryPoint point2; - point2.time_from_start = rclcpp::Duration::from_seconds(0.3); + point2.time_from_start = rclcpp::Duration::from_seconds(points_times.at(1)); point2.positions.resize(joint_names_.size()); point2.positions = points_positions.at(1); @@ -1128,12 +1164,20 @@ TEST_P(TestTrajectoryActionsTestScalingFactor, test_scaling_execution_time_succe SetUpExecutor({params}, false, 1.0, 0.0); SetUpControllerHardware(); + // defining points and times + std::vector points_times{0.1, 0.2}; + std::vector> points_positions{{{4.0, 5.0, 6.0}}, {{7.0, 8.0, 9.0}}}; + // add feedback goal_options_.feedback_callback = [&]( rclcpp_action::ClientGoalHandle::SharedPtr, const std::shared_ptr feedback_msg) { + size_t expected_index = + get_index_from_time(rclcpp::Time(0, 0) + feedback_msg->desired.time_from_start, points_times); + EXPECT_EQ(static_cast(expected_index), feedback_msg->index); + auto time_diff_sec = [](const builtin_interfaces::msg::Duration & msg) { return static_cast(msg.sec) + static_cast(msg.nanosec) * 1e-9; }; @@ -1147,17 +1191,16 @@ TEST_P(TestTrajectoryActionsTestScalingFactor, test_scaling_execution_time_succe std::shared_future gh_future; // send goal - std::vector> points_positions{{{4.0, 5.0, 6.0}}, {{7.0, 8.0, 9.0}}}; std::vector points; JointTrajectoryPoint point1; - point1.time_from_start = rclcpp::Duration::from_seconds(0.1); + point1.time_from_start = rclcpp::Duration::from_seconds(points_times.at(0)); point1.positions.resize(joint_names_.size()); point1.positions = points_positions.at(0); points.push_back(point1); JointTrajectoryPoint point2; - point2.time_from_start = rclcpp::Duration::from_seconds(0.2); + point2.time_from_start = rclcpp::Duration::from_seconds(points_times.at(1)); point2.positions.resize(joint_names_.size()); point2.positions = points_positions.at(1);