Skip to content

Filling index field in feedback message of the action interface #1850

New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Merged
merged 4 commits into from
Aug 24, 2025
Merged

Filling index field in feedback message of the action interface #1850

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
4 commits
Select commit Hold shift + click to select a range
e430658
Filling new 'index' field in feedback message of the action interface.
GiuseppeMonetti95 Aug 3, 2025
5a06382
FIX: Avoiding implicit conversion.
GiuseppeMonetti95 Aug 4, 2025
2094eac
FIX: implicit conversion and error in the logic.
GiuseppeMonetti95 Aug 6, 2025
687fb33
Merge branch 'master' into feature/add_index_in_feedback_msg
GiuseppeMonetti95 Aug 21, 2025
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
5 changes: 4 additions & 1 deletion joint_trajectory_controller/src/joint_trajectory_controller.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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
Expand Down Expand Up @@ -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,
Expand Down Expand Up @@ -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<int32_t>(next_point_index);
active_goal->setFeedback(feedback);

// check abort
Expand Down
75 changes: 59 additions & 16 deletions joint_trajectory_controller/test/test_trajectory_actions.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -204,6 +204,14 @@ class TestTrajectoryActions : public TrajectoryControllerTest
return;
}
}

size_t get_index_from_time(const rclcpp::Time & time, const std::vector<double> & times_vector)
{
const double time_in_seconds = time.seconds();
return static_cast<size_t>(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
Expand Down Expand Up @@ -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<double> points_times{0.2, 0.3};
std::vector<std::vector<double>> 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<FollowJointTrajectoryMsg>::SharedPtr,
const std::shared_ptr<const FollowJointTrajectoryMsg::Feedback>) { feedback_recv = true; };
const std::shared_ptr<const FollowJointTrajectoryMsg::Feedback> 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<int32_t>(expected_index), feedback_msg->index);
};

std::shared_future<typename GoalHandle::SharedPtr> gh_future;
// send goal with multiple points
std::vector<std::vector<double>> points_positions{{{4.0, 5.0, 6.0}}, {{7.0, 8.0, 9.0}}};
{
std::vector<JointTrajectoryPoint> 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);

Expand Down Expand Up @@ -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<double> points_times{0.2, 0.3};
std::vector<std::vector<double>> points_positions{{{4.0, 5.0, 6.0}}, {{7.0, 8.0, 9.0}}};
std::vector<std::vector<double>> 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<FollowJointTrajectoryMsg>::SharedPtr,
const std::shared_ptr<const FollowJointTrajectoryMsg::Feedback>) { feedback_recv = true; };
const std::shared_ptr<const FollowJointTrajectoryMsg::Feedback> 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<int32_t>(expected_index), feedback_msg->index);
feedback_recv = true;
};

std::shared_future<typename GoalHandle::SharedPtr> gh_future;
// send goal with multiple points
std::vector<std::vector<double>> points_positions{{{4.0, 5.0, 6.0}}, {{7.0, 8.0, 9.0}}};
std::vector<std::vector<double>> points_velocities{{{1.0, 1.0, 1.0}}, {{2.0, 2.0, 2.0}}};
{
std::vector<JointTrajectoryPoint> 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);
Expand Down Expand Up @@ -459,27 +486,36 @@ TEST_F(TestTrajectoryActions, test_goal_tolerances_multi_point_success)
SetUpExecutor(params);
SetUpControllerHardware();

// defining points and times
std::vector<double> points_times{0.2, 0.3};
std::vector<std::vector<double>> 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<FollowJointTrajectoryMsg>::SharedPtr,
const std::shared_ptr<const FollowJointTrajectoryMsg::Feedback>) { feedback_recv = true; };
const std::shared_ptr<const FollowJointTrajectoryMsg::Feedback> 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<int32_t>(expected_index), feedback_msg->index);
feedback_recv = true;
};

std::shared_future<typename GoalHandle::SharedPtr> gh_future;
// send goal with multiple points
std::vector<std::vector<double>> points_positions{{{4.0, 5.0, 6.0}}, {{7.0, 8.0, 9.0}}};
{
std::vector<JointTrajectoryPoint> 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);
Expand Down Expand Up @@ -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<double> points_times{0.1, 0.2};
std::vector<std::vector<double>> points_positions{{{4.0, 5.0, 6.0}}, {{7.0, 8.0, 9.0}}};

// add feedback
goal_options_.feedback_callback =
[&](
rclcpp_action::ClientGoalHandle<FollowJointTrajectoryMsg>::SharedPtr,
const std::shared_ptr<const FollowJointTrajectoryMsg::Feedback> 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<int32_t>(expected_index), feedback_msg->index);

auto time_diff_sec = [](const builtin_interfaces::msg::Duration & msg)
{ return static_cast<double>(msg.sec) + static_cast<double>(msg.nanosec) * 1e-9; };

Expand All @@ -1147,17 +1191,16 @@ TEST_P(TestTrajectoryActionsTestScalingFactor, test_scaling_execution_time_succe

std::shared_future<typename GoalHandle::SharedPtr> gh_future;
// send goal
std::vector<std::vector<double>> points_positions{{{4.0, 5.0, 6.0}}, {{7.0, 8.0, 9.0}}};
std::vector<JointTrajectoryPoint> 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);
Expand Down