From 71f8066037b7b7dbf93cc6bfccda66d6d4662fe8 Mon Sep 17 00:00:00 2001 From: Sai Kishor Kothakota Date: Mon, 15 Jul 2024 12:11:30 +0200 Subject: [PATCH] Add initial fix of the command subscription to the JTC --- .../joint_trajectory_controller.hpp | 5 ++ .../src/joint_trajectory_controller.cpp | 4 +- .../test/test_trajectory_controller.cpp | 57 +++++++++---------- .../test/test_trajectory_controller_utils.hpp | 28 +++++++-- 4 files changed, 58 insertions(+), 36 deletions(-) diff --git a/joint_trajectory_controller/include/joint_trajectory_controller/joint_trajectory_controller.hpp b/joint_trajectory_controller/include/joint_trajectory_controller/joint_trajectory_controller.hpp index c3fbb58456..77b749ad0c 100644 --- a/joint_trajectory_controller/include/joint_trajectory_controller/joint_trajectory_controller.hpp +++ b/joint_trajectory_controller/include/joint_trajectory_controller/joint_trajectory_controller.hpp @@ -103,6 +103,11 @@ class JointTrajectoryController : public controller_interface::ControllerInterfa const rclcpp_lifecycle::State & previous_state) override; protected: + virtual rclcpp::CallbackGroup::SharedPtr get_callback_group() + { + return get_node()->get_node_base_interface()->get_default_callback_group(); + } + // To reduce number of variables and to make the code shorter the interfaces are ordered in types // as the following constants const std::vector allowed_interface_types_ = { diff --git a/joint_trajectory_controller/src/joint_trajectory_controller.cpp b/joint_trajectory_controller/src/joint_trajectory_controller.cpp index 31598bb813..750552d634 100644 --- a/joint_trajectory_controller/src/joint_trajectory_controller.cpp +++ b/joint_trajectory_controller/src/joint_trajectory_controller.cpp @@ -796,10 +796,12 @@ controller_interface::CallbackReturn JointTrajectoryController::on_configure( init_hold_position_msg(); // create subscriber and publishers + rclcpp::SubscriptionOptions options; + options.callback_group = get_callback_group(); joint_command_subscriber_ = get_node()->create_subscription( "~/joint_trajectory", rclcpp::SystemDefaultsQoS(), - std::bind(&JointTrajectoryController::topic_callback, this, std::placeholders::_1)); + std::bind(&JointTrajectoryController::topic_callback, this, std::placeholders::_1), options); publisher_ = get_node()->create_publisher( "~/controller_state", rclcpp::SystemDefaultsQoS()); diff --git a/joint_trajectory_controller/test/test_trajectory_controller.cpp b/joint_trajectory_controller/test/test_trajectory_controller.cpp index 611c1d4c1b..81ea189d42 100644 --- a/joint_trajectory_controller/test/test_trajectory_controller.cpp +++ b/joint_trajectory_controller/test/test_trajectory_controller.cpp @@ -71,7 +71,7 @@ TEST_P(TrajectoryControllerTestParameterized, configure_state_ignores_commands) {{0.01, 0.01, 0.01}}, {{0.05, 0.05, 0.05}}, {{0.06, 0.06, 0.06}}}; // *INDENT-ON* publish(time_from_start, points, rclcpp::Time(), {}, points_velocities); - traj_controller_->wait_for_trajectory(executor); + traj_controller_->wait_for_trajectory(); traj_controller_->update( rclcpp::Time(static_cast(0.5 * 1e9)), rclcpp::Duration::from_seconds(0.5)); @@ -146,7 +146,7 @@ TEST_P(TrajectoryControllerTestParameterized, cleanup) {{0.01, 0.01, 0.01}}, {{0.05, 0.05, 0.05}}, {{0.06, 0.06, 0.06}}}; // *INDENT-ON* publish(time_from_start, points, rclcpp::Time(), {}, points_velocities); - traj_controller_->wait_for_trajectory(executor); + traj_controller_->wait_for_trajectory(); traj_controller_->update( rclcpp::Time(static_cast(0.5 * 1e9)), rclcpp::Duration::from_seconds(0.5)); @@ -204,7 +204,7 @@ TEST_P(TrajectoryControllerTestParameterized, correct_initialization_using_param {{0.01, 0.01, 0.01}}, {{0.05, 0.05, 0.05}}, {{0.06, 0.06, 0.06}}}; // *INDENT-ON* publish(time_from_start, points, rclcpp::Time(), {}, points_velocities); - traj_controller_->wait_for_trajectory(executor); + traj_controller_->wait_for_trajectory(); // first update traj_controller_->update(rclcpp::Time(0), rclcpp::Duration::from_seconds(0.1)); @@ -656,7 +656,7 @@ TEST_P(TrajectoryControllerTestParameterized, position_error_not_angle_wraparoun {{3.3, 4.4, 6.6}}, {{7.7, 8.8, 9.9}}, {{10.10, 11.11, 12.12}}}; // *INDENT-ON* publish(time_from_start, points, rclcpp::Time()); - traj_controller_->wait_for_trajectory(executor); + traj_controller_->wait_for_trajectory(); updateControllerAsync(rclcpp::Duration(FIRST_POINT_TIME)); @@ -759,7 +759,7 @@ TEST_P(TrajectoryControllerTestParameterized, position_error_angle_wraparound) {{0.01, 0.01, 0.01}}, {{0.05, 0.05, 0.05}}, {{0.06, 0.06, 0.06}}}; // *INDENT-ON* publish(time_from_start, points, rclcpp::Time(), {}, points_velocities); - traj_controller_->wait_for_trajectory(executor); + traj_controller_->wait_for_trajectory(); updateControllerAsync(rclcpp::Duration(FIRST_POINT_TIME)); @@ -897,7 +897,7 @@ TEST_P(TrajectoryControllerTestParameterized, no_timeout) {{0.01, 0.01, 0.01}}, {{0.05, 0.05, 0.05}}, {{0.06, 0.06, 0.06}}}; // *INDENT-ON* publish(time_from_start, points, rclcpp::Time(0, 0, RCL_STEADY_TIME), {}, points_velocities); - traj_controller_->wait_for_trajectory(executor); + traj_controller_->wait_for_trajectory(); updateController(rclcpp::Duration(FIRST_POINT_TIME) * 4); @@ -947,7 +947,7 @@ TEST_P(TrajectoryControllerTestParameterized, timeout) // *INDENT-ON* publish(time_from_start, points, rclcpp::Time(0, 0, RCL_STEADY_TIME), {}, points_velocities); - traj_controller_->wait_for_trajectory(executor); + traj_controller_->wait_for_trajectory(); // update until end of trajectory -> no timeout should have occurred updateController(rclcpp::Duration(FIRST_POINT_TIME) * 3); @@ -1019,7 +1019,7 @@ TEST_P(TrajectoryControllerTestParameterized, velocity_error) {{0.1, 0.1, 0.1}}, {{0.2, 0.2, 0.2}}, {{0.3, 0.3, 0.3}}}; // *INDENT-ON* publish(time_from_start, points_positions, rclcpp::Time(), {}, points_velocities); - traj_controller_->wait_for_trajectory(executor); + traj_controller_->wait_for_trajectory(); updateControllerAsync(rclcpp::Duration(FIRST_POINT_TIME)); @@ -1105,7 +1105,7 @@ TEST_P(TrajectoryControllerTestParameterized, test_jumbled_joint_order) trajectory_publisher_->publish(traj_msg); } - traj_controller_->wait_for_trajectory(executor); + traj_controller_->wait_for_trajectory(); updateControllerAsync(rclcpp::Duration::from_seconds(dt)); if (traj_controller_->has_position_command_interface()) @@ -1183,7 +1183,7 @@ TEST_P(TrajectoryControllerTestParameterized, test_partial_joint_list) trajectory_publisher_->publish(traj_msg); } - traj_controller_->wait_for_trajectory(executor); + traj_controller_->wait_for_trajectory(); updateControllerAsync(rclcpp::Duration::from_seconds(dt)); if (traj_controller_->has_position_command_interface()) @@ -1270,7 +1270,7 @@ TEST_P(TrajectoryControllerTestParameterized, test_partial_joint_list_not_allowe trajectory_publisher_->publish(traj_msg); } - traj_controller_->wait_for_trajectory(executor); + traj_controller_->wait_for_trajectory(); // update for 0.5 seconds updateControllerAsync(rclcpp::Duration::from_seconds(0.25)); @@ -1515,7 +1515,7 @@ TEST_P(TrajectoryControllerTestParameterized, test_trajectory_replace) expected_desired.velocities = {points_old_velocities[0].begin(), points_old_velocities[0].end()}; // Check that we reached end of points_old trajectory auto end_time = - waitAndCompareState(expected_actual, expected_desired, executor, rclcpp::Duration(delay), 0.1); + waitAndCompareState(expected_actual, expected_desired, rclcpp::Duration(delay), 0.1); RCLCPP_INFO(traj_controller_->get_node()->get_logger(), "Sending new trajectory"); points_partial_new_velocities[0][0] = @@ -1530,8 +1530,7 @@ TEST_P(TrajectoryControllerTestParameterized, test_trajectory_replace) expected_desired.velocities[1] = 0.0; expected_desired.velocities[2] = 0.0; expected_actual = expected_desired; - waitAndCompareState( - expected_actual, expected_desired, executor, rclcpp::Duration(delay), 0.1, end_time); + waitAndCompareState(expected_actual, expected_desired, rclcpp::Duration(delay), 0.1, end_time); } /** @@ -1555,7 +1554,7 @@ TEST_P(TrajectoryControllerTestParameterized, test_ignore_old_trajectory) expected_desired = expected_actual; // Check that we reached end of points_old[0] trajectory auto end_time = - waitAndCompareState(expected_actual, expected_desired, executor, rclcpp::Duration(delay), 0.1); + waitAndCompareState(expected_actual, expected_desired, rclcpp::Duration(delay), 0.1); RCLCPP_INFO(traj_controller_->get_node()->get_logger(), "Sending new trajectory in the past"); // New trajectory will end before current time @@ -1564,8 +1563,7 @@ TEST_P(TrajectoryControllerTestParameterized, test_ignore_old_trajectory) expected_actual.positions = {points_old[1].begin(), points_old[1].end()}; expected_desired = expected_actual; publish(time_from_start, points_new, new_traj_start); - waitAndCompareState( - expected_actual, expected_desired, executor, rclcpp::Duration(delay), 0.1, end_time); + waitAndCompareState(expected_actual, expected_desired, rclcpp::Duration(delay), 0.1, end_time); } TEST_P(TrajectoryControllerTestParameterized, test_ignore_partial_old_trajectory) @@ -1585,7 +1583,7 @@ TEST_P(TrajectoryControllerTestParameterized, test_ignore_partial_old_trajectory expected_desired = expected_actual; // Check that we reached end of points_old[0]trajectory auto end_time = - waitAndCompareState(expected_actual, expected_desired, executor, rclcpp::Duration(delay), 0.1); + waitAndCompareState(expected_actual, expected_desired, rclcpp::Duration(delay), 0.1); // send points_new before the old trajectory is finished RCLCPP_INFO( @@ -1596,8 +1594,7 @@ TEST_P(TrajectoryControllerTestParameterized, test_ignore_partial_old_trajectory // it should not have accepted the new goal but finish the old one expected_actual.positions = {points_old[1].begin(), points_old[1].end()}; expected_desired.positions = {points_old[1].begin(), points_old[1].end()}; - waitAndCompareState( - expected_actual, expected_desired, executor, rclcpp::Duration(delay), 0.1, end_time); + waitAndCompareState(expected_actual, expected_desired, rclcpp::Duration(delay), 0.1, end_time); } TEST_P(TrajectoryControllerTestParameterized, test_execute_partial_traj_in_future) @@ -1630,7 +1627,7 @@ TEST_P(TrajectoryControllerTestParameterized, test_execute_partial_traj_in_futur expected_desired = expected_actual; // Check that we reached end of points_old[0]trajectory and are starting points_old[1] auto end_time = - waitAndCompareState(expected_actual, expected_desired, executor, rclcpp::Duration(delay), 0.1); + waitAndCompareState(expected_actual, expected_desired, rclcpp::Duration(delay), 0.1); // Send partial trajectory starting after full trajecotry is complete RCLCPP_INFO(traj_controller_->get_node()->get_logger(), "Sending new trajectory in the future"); @@ -1643,7 +1640,7 @@ TEST_P(TrajectoryControllerTestParameterized, test_execute_partial_traj_in_futur expected_desired = expected_actual; waitAndCompareState( - expected_actual, expected_desired, executor, rclcpp::Duration(delay * (2 + 2)), 0.1, end_time); + expected_actual, expected_desired, rclcpp::Duration(delay * (2 + 2)), 0.1, end_time); } TEST_P(TrajectoryControllerTestParameterized, test_jump_when_state_tracking_error_updated) @@ -1675,7 +1672,7 @@ TEST_P(TrajectoryControllerTestParameterized, test_jump_when_state_tracking_erro std::vector> points{{first_goal}}; publish( time_from_start, points, rclcpp::Time(0.0, 0.0, RCL_STEADY_TIME), {}, first_goal_velocities); - traj_controller_->wait_for_trajectory(executor); + traj_controller_->wait_for_trajectory(); updateControllerAsync(rclcpp::Duration::from_seconds(1.1)); // JTC is executing trajectory in open-loop therefore: @@ -1690,7 +1687,7 @@ TEST_P(TrajectoryControllerTestParameterized, test_jump_when_state_tracking_erro // Move joint further in the same direction as before (to the second goal) points = {{second_goal}}; publish(time_from_start, points, rclcpp::Time(0, 0, RCL_STEADY_TIME), {}, second_goal_velocities); - traj_controller_->wait_for_trajectory(executor); + traj_controller_->wait_for_trajectory(); // One the first update(s) there should be a "jump" in opposite direction from command // (towards the state value) @@ -1721,7 +1718,7 @@ TEST_P(TrajectoryControllerTestParameterized, test_jump_when_state_tracking_erro // Move joint back to the first goal points = {{first_goal}}; publish(time_from_start, points, rclcpp::Time(0.0, 0.0, RCL_STEADY_TIME)); - traj_controller_->wait_for_trajectory(executor); + traj_controller_->wait_for_trajectory(); // One the first update(s) there should be a "jump" in the goal direction from command // (towards the state value) @@ -1773,7 +1770,7 @@ TEST_P(TrajectoryControllerTestParameterized, test_no_jump_when_state_tracking_e (time_from_start.sec + time_from_start.nanosec * 1e-9); std::vector> points{{first_goal}}; publish(time_from_start, points, rclcpp::Time(0.0, 0.0, RCL_STEADY_TIME)); - traj_controller_->wait_for_trajectory(executor); + traj_controller_->wait_for_trajectory(); updateControllerAsync(rclcpp::Duration::from_seconds(1.1)); // JTC is executing trajectory in open-loop therefore: @@ -1788,7 +1785,7 @@ TEST_P(TrajectoryControllerTestParameterized, test_no_jump_when_state_tracking_e // Move joint further in the same direction as before (to the second goal) points = {{second_goal}}; publish(time_from_start, points, rclcpp::Time(0.0, 0.0, RCL_STEADY_TIME)); - traj_controller_->wait_for_trajectory(executor); + traj_controller_->wait_for_trajectory(); // One the first update(s) there **should not** be a "jump" in opposite direction from // command (towards the state value) @@ -1817,7 +1814,7 @@ TEST_P(TrajectoryControllerTestParameterized, test_no_jump_when_state_tracking_e // Move joint back to the first goal points = {{first_goal}}; publish(time_from_start, points, rclcpp::Time(0.0, 0.0, RCL_STEADY_TIME)); - traj_controller_->wait_for_trajectory(executor); + traj_controller_->wait_for_trajectory(); // One the first update(s) there **should not** be a "jump" in the goal direction from // command (towards the state value) @@ -1988,7 +1985,7 @@ TEST_P(TrajectoryControllerTestParameterized, test_state_tolerances_fail) {{0.01, 0.01, 0.01}}, {{0.05, 0.05, 0.05}}, {{0.06, 0.06, 0.06}}}; // *INDENT-ON* publish(time_from_start, points, rclcpp::Time(0, 0, RCL_STEADY_TIME), {}, points_velocities); - traj_controller_->wait_for_trajectory(executor); + traj_controller_->wait_for_trajectory(); updateControllerAsync(rclcpp::Duration(FIRST_POINT_TIME)); // it should have aborted and be holding now @@ -2020,7 +2017,7 @@ TEST_P(TrajectoryControllerTestParameterized, test_goal_tolerances_fail) {{0.01, 0.01, 0.01}}, {{0.05, 0.05, 0.05}}, {{0.06, 0.06, 0.06}}}; // *INDENT-ON* publish(time_from_start, points, rclcpp::Time(0, 0, RCL_STEADY_TIME), {}, points_velocities); - traj_controller_->wait_for_trajectory(executor); + traj_controller_->wait_for_trajectory(); auto end_time = updateControllerAsync(rclcpp::Duration(4 * FIRST_POINT_TIME)); // it should have aborted and be holding now diff --git a/joint_trajectory_controller/test/test_trajectory_controller_utils.hpp b/joint_trajectory_controller/test/test_trajectory_controller_utils.hpp index bbe12251a1..32eb6cf3d5 100644 --- a/joint_trajectory_controller/test/test_trajectory_controller_utils.hpp +++ b/joint_trajectory_controller/test/test_trajectory_controller_utils.hpp @@ -89,6 +89,11 @@ class TestableJointTrajectoryController using joint_trajectory_controller::JointTrajectoryController::JointTrajectoryController; using joint_trajectory_controller::JointTrajectoryController::validate_trajectory_msg; + rclcpp::CallbackGroup::SharedPtr get_callback_group() override + { + return get_node()->create_callback_group(rclcpp::CallbackGroupType::MutuallyExclusive, false); + } + controller_interface::CallbackReturn on_configure( const rclcpp_lifecycle::State & previous_state) override { @@ -111,13 +116,26 @@ class TestableJointTrajectoryController * @return true if new JointTrajectory msg was received, false if timeout */ bool wait_for_trajectory( - rclcpp::Executor & executor, const std::chrono::milliseconds & timeout = std::chrono::milliseconds{500}) { - bool success = joint_cmd_sub_wait_set_.wait(timeout).kind() == rclcpp::WaitResultKind::Ready; + const auto wait_result = joint_cmd_sub_wait_set_.wait(timeout); + bool success = wait_result.kind() == rclcpp::WaitResultKind::Ready; if (success) { - executor.spin_some(); + if (wait_result.kind() == rclcpp::WaitResultKind::Ready) + { + if (wait_result.get_wait_set().get_rcl_wait_set().subscriptions[0U]) + { + trajectory_msgs::msg::JointTrajectory msg; + rclcpp::MessageInfo msg_info; + if (joint_command_subscriber_->take(msg, msg_info)) + { + std::shared_ptr type_erased_msg = + std::make_shared(msg); + joint_command_subscriber_->handle_message(type_erased_msg, msg_info); + } + } + } } return success; } @@ -544,7 +562,7 @@ class TrajectoryControllerTest : public ::testing::Test rclcpp::Time waitAndCompareState( trajectory_msgs::msg::JointTrajectoryPoint expected_actual, - trajectory_msgs::msg::JointTrajectoryPoint expected_desired, rclcpp::Executor & executor, + trajectory_msgs::msg::JointTrajectoryPoint expected_desired, rclcpp::Duration controller_wait_time, double allowed_delta, rclcpp::Time start_time = rclcpp::Time(0, 0, RCL_STEADY_TIME)) { @@ -552,7 +570,7 @@ class TrajectoryControllerTest : public ::testing::Test std::lock_guard guard(state_mutex_); state_msg_.reset(); } - traj_controller_->wait_for_trajectory(executor); + traj_controller_->wait_for_trajectory(); auto end_time = updateControllerAsync(controller_wait_time, start_time); // get states from class variables