From 1bd6870a1115a0551506bbd45afd88c6ce16c2a8 Mon Sep 17 00:00:00 2001 From: "Felix Exner (fexner)" Date: Fri, 22 Nov 2024 11:22:20 +0100 Subject: [PATCH] [JTC] Sample at t + dT instead of the beginning of the control cycle (#1306) --- .../joint_trajectory_controller.hpp | 2 + .../trajectory.hpp | 11 +- .../src/joint_trajectory_controller.cpp | 29 ++- .../src/trajectory.cpp | 8 +- .../test/test_trajectory_controller.cpp | 166 ++++++++++++------ .../test/test_trajectory_controller_utils.hpp | 4 +- 6 files changed, 147 insertions(+), 73 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 2f0a1f4df0..67954b8378 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 @@ -112,6 +112,7 @@ class JointTrajectoryController : public controller_interface::ControllerInterfa // Preallocate variables used in the realtime update() function trajectory_msgs::msg::JointTrajectoryPoint state_current_; trajectory_msgs::msg::JointTrajectoryPoint command_current_; + trajectory_msgs::msg::JointTrajectoryPoint command_next_; trajectory_msgs::msg::JointTrajectoryPoint state_desired_; trajectory_msgs::msg::JointTrajectoryPoint state_error_; @@ -124,6 +125,7 @@ class JointTrajectoryController : public controller_interface::ControllerInterfa // Parameters from ROS for joint_trajectory_controller std::shared_ptr param_listener_; Params params_; + rclcpp::Duration update_period_{0, 0}; trajectory_msgs::msg::JointTrajectoryPoint last_commanded_state_; /// Specify interpolation method. Default to splines. diff --git a/joint_trajectory_controller/include/joint_trajectory_controller/trajectory.hpp b/joint_trajectory_controller/include/joint_trajectory_controller/trajectory.hpp index 14373b006e..d650c369d0 100644 --- a/joint_trajectory_controller/include/joint_trajectory_controller/trajectory.hpp +++ b/joint_trajectory_controller/include/joint_trajectory_controller/trajectory.hpp @@ -69,8 +69,10 @@ class Trajectory * acceleration respectively. Deduction assumes that the provided velocity or acceleration have to * be reached at the time defined in the segment. * - * This function assumes that sampling is only done at monotonically increasing \p sample_time - * for any trajectory. + * This function by default assumes that sampling is only done at monotonically increasing \p + * sample_time for any trajectory. That means, it will only search for a point matching the sample + * time after the point it has been called before. If this isn't desired, set \p + * search_monotonically_increasing to false. * * Specific case returns for start_segment_itr and end_segment_itr: * - Sampling before the trajectory start: @@ -94,13 +96,16 @@ class Trajectory * description above. * \param[out] end_segment_itr Iterator to the end segment for given \p sample_time. See * description above. + * \param[in] search_monotonically_increasing If set to true, the next sample call will start + * searching in the trajectory at the index of this call's result. */ JOINT_TRAJECTORY_CONTROLLER_PUBLIC bool sample( const rclcpp::Time & sample_time, const interpolation_methods::InterpolationMethod interpolation_method, trajectory_msgs::msg::JointTrajectoryPoint & output_state, - TrajectoryPointConstIter & start_segment_itr, TrajectoryPointConstIter & end_segment_itr); + TrajectoryPointConstIter & start_segment_itr, TrajectoryPointConstIter & end_segment_itr, + const bool search_monotonically_increasing = true); /** * Do interpolation between 2 states given a time in between their respective timestamps diff --git a/joint_trajectory_controller/src/joint_trajectory_controller.cpp b/joint_trajectory_controller/src/joint_trajectory_controller.cpp index bea37e02be..0fc39ecaaf 100644 --- a/joint_trajectory_controller/src/joint_trajectory_controller.cpp +++ b/joint_trajectory_controller/src/joint_trajectory_controller.cpp @@ -18,6 +18,7 @@ #include #include +#include #include #include @@ -180,6 +181,7 @@ controller_interface::return_type JointTrajectoryController::update( if (has_active_trajectory()) { bool first_sample = false; + TrajectoryPointConstIter start_segment_itr, end_segment_itr; // if sampling the first time, set the point before you sample if (!traj_external_point_ptr_->is_sampled_already()) { @@ -196,11 +198,15 @@ controller_interface::return_type JointTrajectoryController::update( } } - // find segment for current timestamp - TrajectoryPointConstIter start_segment_itr, end_segment_itr; - const bool valid_point = traj_external_point_ptr_->sample( + // Sample expected state from the trajectory + traj_external_point_ptr_->sample( time, interpolation_method_, state_desired_, start_segment_itr, end_segment_itr); + // Sample setpoint for next control cycle + const bool valid_point = traj_external_point_ptr_->sample( + time + update_period_, interpolation_method_, command_next_, start_segment_itr, + end_segment_itr, false); + if (valid_point) { const rclcpp::Time traj_start = traj_external_point_ptr_->time_from_start(); @@ -276,7 +282,7 @@ controller_interface::return_type JointTrajectoryController::update( // Update PIDs for (auto i = 0ul; i < dof_; ++i) { - tmp_command_[i] = (state_desired_.velocities[i] * ff_velocity_scale_[i]) + + tmp_command_[i] = (command_next_.velocities[i] * ff_velocity_scale_[i]) + pids_[i]->computeCommand( state_error_.positions[i], state_error_.velocities[i], (uint64_t)period.nanoseconds()); @@ -286,7 +292,7 @@ controller_interface::return_type JointTrajectoryController::update( // set values for next hardware write() if (has_position_command_interface_) { - assign_interface_from_point(joint_command_interface_[0], state_desired_.positions); + assign_interface_from_point(joint_command_interface_[0], command_next_.positions); } if (has_velocity_command_interface_) { @@ -296,12 +302,12 @@ controller_interface::return_type JointTrajectoryController::update( } else { - assign_interface_from_point(joint_command_interface_[1], state_desired_.velocities); + assign_interface_from_point(joint_command_interface_[1], command_next_.velocities); } } if (has_acceleration_command_interface_) { - assign_interface_from_point(joint_command_interface_[2], state_desired_.accelerations); + assign_interface_from_point(joint_command_interface_[2], command_next_.accelerations); } if (has_effort_command_interface_) { @@ -309,7 +315,7 @@ controller_interface::return_type JointTrajectoryController::update( } // store the previous command. Used in open-loop control mode - last_commanded_state_ = state_desired_; + last_commanded_state_ = command_next_; } if (active_goal) @@ -867,6 +873,13 @@ controller_interface::CallbackReturn JointTrajectoryController::on_configure( std::string(get_node()->get_name()) + "/query_state", std::bind(&JointTrajectoryController::query_state_service, this, _1, _2)); + if (get_update_rate() == 0) + { + throw std::runtime_error("Controller's update rate is set to 0. This should not happen!"); + } + update_period_ = + rclcpp::Duration(0.0, static_cast(1.0e9 / static_cast(get_update_rate()))); + return CallbackReturn::SUCCESS; } diff --git a/joint_trajectory_controller/src/trajectory.cpp b/joint_trajectory_controller/src/trajectory.cpp index 512b16f3c5..1c50d26c62 100644 --- a/joint_trajectory_controller/src/trajectory.cpp +++ b/joint_trajectory_controller/src/trajectory.cpp @@ -91,7 +91,8 @@ bool Trajectory::sample( const rclcpp::Time & sample_time, const interpolation_methods::InterpolationMethod interpolation_method, trajectory_msgs::msg::JointTrajectoryPoint & output_state, - TrajectoryPointConstIter & start_segment_itr, TrajectoryPointConstIter & end_segment_itr) + TrajectoryPointConstIter & start_segment_itr, TrajectoryPointConstIter & end_segment_itr, + const bool search_monotonically_increasing) { THROW_ON_NULLPTR(trajectory_msg_) @@ -176,7 +177,10 @@ bool Trajectory::sample( } start_segment_itr = begin() + i; end_segment_itr = begin() + (i + 1); - last_sample_idx_ = i; + if (search_monotonically_increasing) + { + last_sample_idx_ = i; + } return true; } } diff --git a/joint_trajectory_controller/test/test_trajectory_controller.cpp b/joint_trajectory_controller/test/test_trajectory_controller.cpp index f188c0f04b..21d0277541 100644 --- a/joint_trajectory_controller/test/test_trajectory_controller.cpp +++ b/joint_trajectory_controller/test/test_trajectory_controller.cpp @@ -53,7 +53,7 @@ TEST_P(TrajectoryControllerTestParameterized, configure_state_ignores_commands) traj_controller_->get_node()->set_parameter( rclcpp::Parameter("allow_nonzero_velocity_at_trajectory_end", true)); - const auto state = traj_controller_->get_node()->configure(); + const auto state = traj_controller_->configure(); ASSERT_EQ(state.id(), State::PRIMARY_STATE_INACTIVE); // send msg @@ -84,7 +84,7 @@ TEST_P(TrajectoryControllerTestParameterized, check_interface_names) rclcpp::executors::MultiThreadedExecutor executor; SetUpTrajectoryController(executor); - const auto state = traj_controller_->get_node()->configure(); + const auto state = traj_controller_->configure(); ASSERT_EQ(state.id(), State::PRIMARY_STATE_INACTIVE); compare_joints(joint_names_, joint_names_); @@ -97,7 +97,7 @@ TEST_P(TrajectoryControllerTestParameterized, check_interface_names_with_command const rclcpp::Parameter command_joint_names_param("command_joints", command_joint_names_); SetUpTrajectoryController(executor, {command_joint_names_param}); - const auto state = traj_controller_->get_node()->configure(); + const auto state = traj_controller_->configure(); ASSERT_EQ(state.id(), State::PRIMARY_STATE_INACTIVE); compare_joints(joint_names_, command_joint_names_); @@ -108,7 +108,7 @@ TEST_P(TrajectoryControllerTestParameterized, activate) rclcpp::executors::MultiThreadedExecutor executor; SetUpTrajectoryController(executor); - auto state = traj_controller_->get_node()->configure(); + auto state = traj_controller_->configure(); ASSERT_EQ(state.id(), State::PRIMARY_STATE_INACTIVE); auto cmd_if_conf = traj_controller_->command_interface_configuration(); @@ -162,7 +162,7 @@ TEST_P(TrajectoryControllerTestParameterized, cleanup_after_configure) SetUpTrajectoryController(executor); // configure controller - auto state = traj_controller_->get_node()->configure(); + auto state = traj_controller_->configure(); ASSERT_EQ(State::PRIMARY_STATE_INACTIVE, state.id()); // cleanup controller @@ -178,6 +178,7 @@ TEST_P(TrajectoryControllerTestParameterized, correct_initialization_using_param SetUpTrajectoryController(executor); traj_controller_->get_node()->set_parameter( rclcpp::Parameter("allow_nonzero_velocity_at_trajectory_end", true)); + traj_controller_->get_node()->set_parameter(rclcpp::Parameter("update_rate", 10)); // This call is replacing the way parameters are set via launch auto state = traj_controller_->configure(); @@ -206,8 +207,10 @@ TEST_P(TrajectoryControllerTestParameterized, correct_initialization_using_param // wait for reaching the first point // controller would process the second point when deactivated below + // Since the trajectory will be sampled at time + update_period, we need to pass the time 0.15 + // seconds to sample at t=0.25 sec traj_controller_->update( - rclcpp::Time(static_cast(0.25 * 1e9)), rclcpp::Duration::from_seconds(0.15)); + rclcpp::Time(static_cast(0.15 * 1e9)), rclcpp::Duration::from_seconds(0.15)); EXPECT_TRUE(traj_controller_->has_active_traj()); if (traj_controller_->has_position_command_interface()) { @@ -644,7 +647,7 @@ TEST_P(TrajectoryControllerTestParameterized, position_error_not_angle_wraparoun size_t n_joints = joint_names_.size(); // send msg - constexpr auto FIRST_POINT_TIME = std::chrono::milliseconds(250); + constexpr auto FIRST_POINT_TIME = std::chrono::milliseconds(300); builtin_interfaces::msg::Duration time_from_start{rclcpp::Duration(FIRST_POINT_TIME)}; // *INDENT-OFF* std::vector> points{ @@ -653,7 +656,23 @@ TEST_P(TrajectoryControllerTestParameterized, position_error_not_angle_wraparoun publish(time_from_start, points, rclcpp::Time()); traj_controller_->wait_for_trajectory(executor); - updateControllerAsync(rclcpp::Duration(FIRST_POINT_TIME)); + const rclcpp::Duration controller_period = + rclcpp::Duration::from_seconds(1.0 / traj_controller_->get_update_rate()); + + auto end_time = updateControllerAsync( + rclcpp::Duration(FIRST_POINT_TIME) - controller_period, rclcpp::Time(0, 0, RCL_STEADY_TIME), + controller_period); + if (traj_controller_->has_position_command_interface()) + { + // check command interface + // One step before the first point, the target should hit the setpoint + EXPECT_NEAR(points[0][0], joint_pos_[0], COMMON_THRESHOLD); + EXPECT_NEAR(points[0][1], joint_pos_[1], COMMON_THRESHOLD); + EXPECT_NEAR(points[0][2], joint_pos_[2], COMMON_THRESHOLD); + } + + // Propagate to actual setpoint time + traj_controller_->update(end_time + controller_period, controller_period); // get states from class variables auto state_feedback = traj_controller_->get_state_feedback(); @@ -678,14 +697,6 @@ TEST_P(TrajectoryControllerTestParameterized, position_error_not_angle_wraparoun EXPECT_NEAR(state_error.positions[1], state_reference.positions[1] - INITIAL_POS_JOINTS[1], EPS); EXPECT_NEAR(state_error.positions[2], state_reference.positions[2] - INITIAL_POS_JOINTS[2], EPS); - if (traj_controller_->has_position_command_interface()) - { - // check command interface - EXPECT_NEAR(points[0][0], joint_pos_[0], COMMON_THRESHOLD); - EXPECT_NEAR(points[0][1], joint_pos_[1], COMMON_THRESHOLD); - EXPECT_NEAR(points[0][2], joint_pos_[2], COMMON_THRESHOLD); - } - if (traj_controller_->has_velocity_command_interface()) { // use_closed_loop_pid_adapter_ @@ -745,7 +756,7 @@ TEST_P(TrajectoryControllerTestParameterized, position_error_angle_wraparound) size_t n_joints = joint_names_.size(); // send msg - constexpr auto FIRST_POINT_TIME = std::chrono::milliseconds(250); + constexpr auto FIRST_POINT_TIME = std::chrono::milliseconds(300); builtin_interfaces::msg::Duration time_from_start{rclcpp::Duration(FIRST_POINT_TIME)}; // *INDENT-OFF* std::vector> points{ @@ -756,7 +767,23 @@ TEST_P(TrajectoryControllerTestParameterized, position_error_angle_wraparound) publish(time_from_start, points, rclcpp::Time(), {}, points_velocities); traj_controller_->wait_for_trajectory(executor); - updateControllerAsync(rclcpp::Duration(FIRST_POINT_TIME)); + const rclcpp::Duration controller_period = + rclcpp::Duration::from_seconds(1.0 / traj_controller_->get_update_rate()); + auto end_time = updateControllerAsync( + rclcpp::Duration(FIRST_POINT_TIME) - controller_period, rclcpp::Time(0, 0, RCL_STEADY_TIME), + controller_period); + + if (traj_controller_->has_position_command_interface()) + { + // check command interface + // One step before the first point, the target should hit the setpoint + EXPECT_NEAR(points[0][0], joint_pos_[0], COMMON_THRESHOLD); + EXPECT_NEAR(points[0][1], joint_pos_[1], COMMON_THRESHOLD); + EXPECT_NEAR(points[0][2], joint_pos_[2], COMMON_THRESHOLD); + } + + // Propagate to actual setpoint time + traj_controller_->update(end_time + controller_period, controller_period); // get states from class variables auto state_feedback = traj_controller_->get_state_feedback(); @@ -782,14 +809,6 @@ TEST_P(TrajectoryControllerTestParameterized, position_error_angle_wraparound) EXPECT_NEAR( state_error.positions[2], state_reference.positions[2] - INITIAL_POS_JOINTS[2] - 2 * M_PI, EPS); - if (traj_controller_->has_position_command_interface()) - { - // check command interface - EXPECT_NEAR(points[0][0], joint_pos_[0], COMMON_THRESHOLD); - EXPECT_NEAR(points[0][1], joint_pos_[1], COMMON_THRESHOLD); - EXPECT_NEAR(points[0][2], joint_pos_[2], COMMON_THRESHOLD); - } - if (traj_controller_->has_velocity_command_interface()) { // use_closed_loop_pid_adapter_ @@ -1548,16 +1567,27 @@ TEST_P(TrajectoryControllerTestParameterized, test_ignore_old_trajectory) trajectory_msgs::msg::JointTrajectoryPoint expected_actual, expected_desired; expected_actual.positions = {points_old[0].begin(), points_old[0].end()}; expected_desired = expected_actual; + traj_controller_->wait_for_trajectory(executor); + auto end_time = updateControllerAsync( + rclcpp::Duration(delay), rclcpp::Time(0, 0, RCL_STEADY_TIME), + rclcpp::Duration::from_seconds(1.0 / traj_controller_->get_update_rate())); + // Check that we reached end of points_old[0] trajectory - auto end_time = - waitAndCompareState(expected_actual, expected_desired, executor, rclcpp::Duration(delay), 0.1); + if (traj_controller_->has_position_command_interface()) + { + auto state_feedback = traj_controller_->get_state_feedback(); + for (size_t i = 0; i < expected_actual.positions.size(); ++i) + { + EXPECT_NEAR(expected_actual.positions[i], state_feedback.positions[i], 0.01); + } + } RCLCPP_INFO(traj_controller_->get_node()->get_logger(), "Sending new trajectory in the past"); // New trajectory will end before current time rclcpp::Time new_traj_start = rclcpp::Clock(RCL_STEADY_TIME).now() - delay - std::chrono::milliseconds(100); expected_actual.positions = {points_old[1].begin(), points_old[1].end()}; - expected_desired = expected_actual; + expected_desired = expected_actual; // robot should be standing still by now publish(time_from_start, points_new, new_traj_start); waitAndCompareState( expected_actual, expected_desired, executor, rclcpp::Duration(delay), 0.1, end_time); @@ -1577,10 +1607,20 @@ TEST_P(TrajectoryControllerTestParameterized, test_ignore_partial_old_trajectory // send points_old and wait to reach first point publish(time_from_start, points_old, rclcpp::Time()); expected_actual.positions = {points_old[0].begin(), points_old[0].end()}; - 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); + traj_controller_->wait_for_trajectory(executor); + auto end_time = updateControllerAsync( + rclcpp::Duration(delay), rclcpp::Time(0, 0, RCL_STEADY_TIME), + rclcpp::Duration::from_seconds(1.0 / traj_controller_->get_update_rate())); + + if (traj_controller_->has_position_command_interface()) + { + // Check that we reached end of points_old[0] trajectory + auto state_feedback = traj_controller_->get_state_feedback(); + for (size_t i = 0; i < expected_actual.positions.size(); ++i) + { + EXPECT_NEAR(expected_actual.positions[i], state_feedback.positions[i], 0.01); + } + } // send points_new before the old trajectory is finished RCLCPP_INFO( @@ -1653,6 +1693,8 @@ TEST_P(TrajectoryControllerTestParameterized, test_jump_when_state_tracking_erro // only makes sense with position command interface return; } + auto controller_period = + rclcpp::Duration::from_seconds(1.0 / traj_controller_->get_update_rate()); // goal setup std::vector first_goal = {3.3, 4.4, 5.5}; @@ -1665,8 +1707,8 @@ TEST_P(TrajectoryControllerTestParameterized, test_jump_when_state_tracking_erro builtin_interfaces::msg::Duration time_from_start; time_from_start.sec = 1; time_from_start.nanosec = 0; - double trajectory_frac = rclcpp::Duration::from_seconds(0.01).seconds() / - (time_from_start.sec + time_from_start.nanosec * 1e-9); + double trajectory_frac = + controller_period.seconds() / (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), {}, first_goal_velocities); @@ -1690,7 +1732,7 @@ TEST_P(TrajectoryControllerTestParameterized, test_jump_when_state_tracking_erro // One the first update(s) there should be a "jump" in opposite direction from command // (towards the state value) EXPECT_NEAR(first_goal[0], joint_pos_[0], COMMON_THRESHOLD); - auto end_time = updateControllerAsync(rclcpp::Duration::from_seconds(0.01)); + auto end_time = updateControllerAsync(controller_period); // Expect backward commands at first, consider advancement of the trajectory // exact value is not directly predictable, because of the spline interpolation -> increase // tolerance @@ -1699,10 +1741,10 @@ TEST_P(TrajectoryControllerTestParameterized, test_jump_when_state_tracking_erro 0.1); EXPECT_GT(joint_pos_[0], joint_state_pos_[0]); EXPECT_LT(joint_pos_[0], first_goal[0]); - end_time = updateControllerAsync(rclcpp::Duration::from_seconds(0.01), end_time); + end_time = updateControllerAsync(controller_period, end_time); EXPECT_GT(joint_pos_[0], joint_state_pos_[0]); EXPECT_LT(joint_pos_[0], first_goal[0]); - end_time = updateControllerAsync(rclcpp::Duration::from_seconds(0.01), end_time); + end_time = updateControllerAsync(controller_period, end_time); EXPECT_GT(joint_pos_[0], joint_state_pos_[0]); EXPECT_LT(joint_pos_[0], first_goal[0]); @@ -1721,17 +1763,19 @@ TEST_P(TrajectoryControllerTestParameterized, test_jump_when_state_tracking_erro // One the first update(s) there should be a "jump" in the goal direction from command // (towards the state value) EXPECT_NEAR(second_goal[0], joint_pos_[0], COMMON_THRESHOLD); - end_time = updateControllerAsync(rclcpp::Duration::from_seconds(0.01)); + end_time = updateControllerAsync(controller_period); // Expect backward commands at first, consider advancement of the trajectory + // exact value is not directly predictable, because of the spline interpolation -> increase + // tolerance EXPECT_NEAR( joint_state_pos_[0] + (first_goal[0] - joint_state_pos_[0]) * trajectory_frac, joint_pos_[0], - COMMON_THRESHOLD); + 0.1); EXPECT_LT(joint_pos_[0], joint_state_pos_[0]); EXPECT_GT(joint_pos_[0], first_goal[0]); - end_time = updateControllerAsync(rclcpp::Duration::from_seconds(0.01), end_time); + end_time = updateControllerAsync(controller_period, end_time); EXPECT_LT(joint_pos_[0], joint_state_pos_[0]); EXPECT_GT(joint_pos_[0], first_goal[0]); - end_time = updateControllerAsync(rclcpp::Duration::from_seconds(0.01), end_time); + end_time = updateControllerAsync(controller_period, end_time); EXPECT_LT(joint_pos_[0], joint_state_pos_[0]); EXPECT_GT(joint_pos_[0], first_goal[0]); @@ -1747,13 +1791,17 @@ TEST_P(TrajectoryControllerTestParameterized, test_no_jump_when_state_tracking_e rclcpp::executors::SingleThreadedExecutor executor; // set open loop to true, this should change behavior from above rclcpp::Parameter is_open_loop_parameters("open_loop_control", true); - SetUpAndActivateTrajectoryController(executor, {is_open_loop_parameters}, true); + rclcpp::Parameter update_rate_param("update_rate", 100); + SetUpAndActivateTrajectoryController( + executor, {is_open_loop_parameters, update_rate_param}, true); if (traj_controller_->has_position_command_interface() == false) { // only makes sense with position command interface return; } + auto controller_period = + rclcpp::Duration::from_seconds(1.0 / traj_controller_->get_update_rate()); // goal setup std::vector first_goal = {3.3, 4.4, 5.5}; @@ -1764,8 +1812,8 @@ TEST_P(TrajectoryControllerTestParameterized, test_no_jump_when_state_tracking_e builtin_interfaces::msg::Duration time_from_start; time_from_start.sec = 1; time_from_start.nanosec = 0; - double trajectory_frac = rclcpp::Duration::from_seconds(0.01).seconds() / - (time_from_start.sec + time_from_start.nanosec * 1e-9); + double trajectory_frac = + controller_period.seconds() / (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); @@ -1788,17 +1836,18 @@ TEST_P(TrajectoryControllerTestParameterized, test_no_jump_when_state_tracking_e // One the first update(s) there **should not** be a "jump" in opposite direction from // command (towards the state value) EXPECT_NEAR(first_goal[0], joint_pos_[0], COMMON_THRESHOLD); - auto end_time = updateControllerAsync(rclcpp::Duration::from_seconds(0.01)); + auto end_time = updateControllerAsync(controller_period); // There should not be backward commands + // exact value is not directly predictable, because of the spline interpolation -> increase + // tolerance EXPECT_NEAR( - first_goal[0] + (second_goal[0] - first_goal[0]) * trajectory_frac, joint_pos_[0], - COMMON_THRESHOLD); + first_goal[0] + (second_goal[0] - first_goal[0]) * trajectory_frac, joint_pos_[0], 0.1); EXPECT_GT(joint_pos_[0], first_goal[0]); EXPECT_LT(joint_pos_[0], second_goal[0]); - end_time = updateControllerAsync(rclcpp::Duration::from_seconds(0.01), end_time); + end_time = updateControllerAsync(controller_period, end_time); EXPECT_GT(joint_pos_[0], first_goal[0]); EXPECT_LT(joint_pos_[0], second_goal[0]); - end_time = updateControllerAsync(rclcpp::Duration::from_seconds(0.01), end_time); + end_time = updateControllerAsync(controller_period, end_time); EXPECT_GT(joint_pos_[0], first_goal[0]); EXPECT_LT(joint_pos_[0], second_goal[0]); @@ -1817,17 +1866,18 @@ TEST_P(TrajectoryControllerTestParameterized, test_no_jump_when_state_tracking_e // One the first update(s) there **should not** be a "jump" in the goal direction from // command (towards the state value) EXPECT_NEAR(second_goal[0], joint_pos_[0], COMMON_THRESHOLD); - end_time = updateControllerAsync(rclcpp::Duration::from_seconds(0.01), end_time); + end_time = updateControllerAsync(controller_period, end_time); // There should not be a jump toward commands + // exact value is not directly predictable, because of the spline interpolation -> increase + // tolerance EXPECT_NEAR( - second_goal[0] + (first_goal[0] - second_goal[0]) * trajectory_frac, joint_pos_[0], - COMMON_THRESHOLD); + second_goal[0] + (first_goal[0] - second_goal[0]) * trajectory_frac, joint_pos_[0], 0.1); EXPECT_LT(joint_pos_[0], second_goal[0]); EXPECT_GT(joint_pos_[0], first_goal[0]); - end_time = updateControllerAsync(rclcpp::Duration::from_seconds(0.01), end_time); + end_time = updateControllerAsync(controller_period, end_time); EXPECT_GT(joint_pos_[0], first_goal[0]); EXPECT_LT(joint_pos_[0], second_goal[0]); - end_time = updateControllerAsync(rclcpp::Duration::from_seconds(0.01), end_time); + end_time = updateControllerAsync(controller_period, end_time); EXPECT_GT(joint_pos_[0], first_goal[0]); EXPECT_LT(joint_pos_[0], second_goal[0]); @@ -2098,7 +2148,7 @@ TEST_F(TrajectoryControllerTest, incorrect_initialization_using_interface_parame // command interfaces: empty command_interface_types_ = {}; EXPECT_EQ(SetUpTrajectoryControllerLocal(), controller_interface::return_type::OK); - auto state = traj_controller_->get_node()->configure(); + auto state = traj_controller_->configure(); EXPECT_EQ(state.id(), State::PRIMARY_STATE_UNCONFIGURED); // command interfaces: bad_name @@ -2143,7 +2193,7 @@ TEST_F(TrajectoryControllerTest, incorrect_initialization_using_interface_parame command_interface_types_ = {"velocity"}; state_interface_types_ = {"position"}; EXPECT_EQ(SetUpTrajectoryControllerLocal(), controller_interface::return_type::OK); - state = traj_controller_->get_node()->configure(); + state = traj_controller_->configure(); EXPECT_EQ(state.id(), State::PRIMARY_STATE_UNCONFIGURED); state_interface_types_ = {"velocity"}; EXPECT_EQ(SetUpTrajectoryControllerLocal(), controller_interface::return_type::ERROR); @@ -2152,7 +2202,7 @@ TEST_F(TrajectoryControllerTest, incorrect_initialization_using_interface_parame command_interface_types_ = {"effort"}; state_interface_types_ = {"position"}; EXPECT_EQ(SetUpTrajectoryControllerLocal(), controller_interface::return_type::OK); - state = traj_controller_->get_node()->configure(); + state = traj_controller_->configure(); EXPECT_EQ(state.id(), State::PRIMARY_STATE_UNCONFIGURED); state_interface_types_ = {"velocity"}; EXPECT_EQ(SetUpTrajectoryControllerLocal(), controller_interface::return_type::ERROR); diff --git a/joint_trajectory_controller/test/test_trajectory_controller_utils.hpp b/joint_trajectory_controller/test/test_trajectory_controller_utils.hpp index a850891331..7dff81f08f 100644 --- a/joint_trajectory_controller/test/test_trajectory_controller_utils.hpp +++ b/joint_trajectory_controller/test/test_trajectory_controller_utils.hpp @@ -278,7 +278,7 @@ class TrajectoryControllerTest : public ::testing::Test traj_controller_->set_node_options(node_options); return traj_controller_->init( - controller_name_, urdf, 0, "", traj_controller_->define_custom_node_options()); + controller_name_, urdf, 100, "", traj_controller_->define_custom_node_options()); } void SetPidParameters(double p_value = 0.0, double ff_value = 1.0) @@ -324,7 +324,7 @@ class TrajectoryControllerTest : public ::testing::Test // set pid parameters before configure SetPidParameters(k_p, ff); - traj_controller_->get_node()->configure(); + traj_controller_->configure(); ActivateTrajectoryController( separate_cmd_and_state_values, initial_pos_joints, initial_vel_joints, initial_acc_joints,