From 854810c2e9a453ba536db005b590551cd556a11c Mon Sep 17 00:00:00 2001 From: Christoph Froehlich Date: Wed, 29 Nov 2023 19:04:12 +0000 Subject: [PATCH] [JTC] Improve update methods for tests (backport #858) --- .../test/test_trajectory_actions.cpp | 16 +- .../test/test_trajectory_controller.cpp | 967 ++++++++---------- .../test/test_trajectory_controller_utils.hpp | 143 ++- 3 files changed, 565 insertions(+), 561 deletions(-) diff --git a/joint_trajectory_controller/test/test_trajectory_actions.cpp b/joint_trajectory_controller/test/test_trajectory_actions.cpp index 3b80abcb9b..ed13a24485 100644 --- a/joint_trajectory_controller/test/test_trajectory_actions.cpp +++ b/joint_trajectory_controller/test/test_trajectory_actions.cpp @@ -242,7 +242,7 @@ TEST_P(TestTrajectoryActionsTestParameterized, test_success_single_point_sendgoa EXPECT_EQ(rclcpp_action::ResultCode::SUCCEEDED, common_resultcode_); // run an update - updateController(rclcpp::Duration::from_seconds(0.01)); + updateControllerAsync(rclcpp::Duration::from_seconds(0.01)); // it should be holding the last position goal // i.e., active but trivial trajectory (one point only) @@ -298,7 +298,7 @@ TEST_P(TestTrajectoryActionsTestParameterized, test_success_multi_point_sendgoal EXPECT_EQ(rclcpp_action::ResultCode::SUCCEEDED, common_resultcode_); // run an update - updateController(rclcpp::Duration::from_seconds(0.01)); + updateControllerAsync(rclcpp::Duration::from_seconds(0.01)); // it should be holding the last position goal // i.e., active but trivial trajectory (one point only) @@ -350,7 +350,7 @@ TEST_F(TestTrajectoryActions, test_goal_tolerances_single_point_success) control_msgs::action::FollowJointTrajectory_Result::SUCCESSFUL, common_action_result_code_); // run an update - updateController(rclcpp::Duration::from_seconds(0.01)); + updateControllerAsync(rclcpp::Duration::from_seconds(0.01)); // it should be holding the last position goal // i.e., active but trivial trajectory (one point only) @@ -409,7 +409,7 @@ TEST_F(TestTrajectoryActions, test_goal_tolerances_multi_point_success) control_msgs::action::FollowJointTrajectory_Result::SUCCESSFUL, common_action_result_code_); // run an update - updateController(rclcpp::Duration::from_seconds(0.01)); + updateControllerAsync(rclcpp::Duration::from_seconds(0.01)); // it should be holding the last position goal // i.e., active but trivial trajectory (one point only) @@ -460,7 +460,7 @@ TEST_P(TestTrajectoryActionsTestParameterized, test_state_tolerances_fail) common_action_result_code_); // run an update - updateController(rclcpp::Duration::from_seconds(0.01)); + updateControllerAsync(rclcpp::Duration::from_seconds(0.01)); // it should be holding the position (being the initial one) // i.e., active but trivial trajectory (one point only) @@ -509,7 +509,7 @@ TEST_P(TestTrajectoryActionsTestParameterized, test_goal_tolerances_fail) common_action_result_code_); // run an update - updateController(rclcpp::Duration::from_seconds(0.01)); + updateControllerAsync(rclcpp::Duration::from_seconds(0.01)); // it should be holding the position (being the initial one) // i.e., active but trivial trajectory (one point only) @@ -555,7 +555,7 @@ TEST_P(TestTrajectoryActionsTestParameterized, test_no_time_from_start_state_tol common_action_result_code_); // run an update - updateController(rclcpp::Duration::from_seconds(0.01)); + updateControllerAsync(rclcpp::Duration::from_seconds(0.01)); // it should be holding the position (being the initial one) // i.e., active but trivial trajectory (one point only) @@ -603,7 +603,7 @@ TEST_P(TestTrajectoryActionsTestParameterized, test_cancel_hold_position) std::vector cancelled_position{joint_pos_[0], joint_pos_[1], joint_pos_[2]}; // run an update - updateController(rclcpp::Duration::from_seconds(0.01)); + updateControllerAsync(rclcpp::Duration::from_seconds(0.01)); // it should be holding the last position, // i.e., active but trivial trajectory (one point only) diff --git a/joint_trajectory_controller/test/test_trajectory_controller.cpp b/joint_trajectory_controller/test/test_trajectory_controller.cpp index 8b29c2cb16..7c026d5e7a 100644 --- a/joint_trajectory_controller/test/test_trajectory_controller.cpp +++ b/joint_trajectory_controller/test/test_trajectory_controller.cpp @@ -26,8 +26,6 @@ #include #include -#include "gmock/gmock.h" - #include "builtin_interfaces/msg/duration.hpp" #include "builtin_interfaces/msg/time.hpp" #include "controller_interface/controller_interface.hpp" @@ -103,77 +101,20 @@ TEST_P(TrajectoryControllerTestParameterized, check_interface_names) const auto state = traj_controller_->get_node()->configure(); ASSERT_EQ(state.id(), State::PRIMARY_STATE_INACTIVE); - std::vector state_interface_names; - state_interface_names.reserve(joint_names_.size() * state_interface_types_.size()); - for (const auto & joint : joint_names_) - { - for (const auto & interface : state_interface_types_) - { - state_interface_names.push_back(joint + "/" + interface); - } - } - auto state_interfaces = traj_controller_->state_interface_configuration(); - EXPECT_EQ(state_interfaces.type, controller_interface::interface_configuration_type::INDIVIDUAL); - EXPECT_EQ(state_interfaces.names.size(), joint_names_.size() * state_interface_types_.size()); - ASSERT_THAT(state_interfaces.names, testing::UnorderedElementsAreArray(state_interface_names)); - - std::vector command_interface_names; - command_interface_names.reserve(joint_names_.size() * command_interface_types_.size()); - for (const auto & joint : joint_names_) - { - for (const auto & interface : command_interface_types_) - { - command_interface_names.push_back(joint + "/" + interface); - } - } - auto command_interfaces = traj_controller_->command_interface_configuration(); - EXPECT_EQ( - command_interfaces.type, controller_interface::interface_configuration_type::INDIVIDUAL); - EXPECT_EQ(command_interfaces.names.size(), joint_names_.size() * command_interface_types_.size()); - ASSERT_THAT( - command_interfaces.names, testing::UnorderedElementsAreArray(command_interface_names)); + compare_joints(joint_names_, joint_names_); } TEST_P(TrajectoryControllerTestParameterized, check_interface_names_with_command_joints) { rclcpp::executors::MultiThreadedExecutor executor; - // set command_joints parameter + // set command_joints parameter different than joint_names_ 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(); ASSERT_EQ(state.id(), State::PRIMARY_STATE_INACTIVE); - std::vector state_interface_names; - state_interface_names.reserve(joint_names_.size() * state_interface_types_.size()); - for (const auto & joint : joint_names_) - { - for (const auto & interface : state_interface_types_) - { - state_interface_names.push_back(joint + "/" + interface); - } - } - auto state_interfaces = traj_controller_->state_interface_configuration(); - EXPECT_EQ(state_interfaces.type, controller_interface::interface_configuration_type::INDIVIDUAL); - EXPECT_EQ(state_interfaces.names.size(), joint_names_.size() * state_interface_types_.size()); - ASSERT_THAT(state_interfaces.names, testing::UnorderedElementsAreArray(state_interface_names)); - - std::vector command_interface_names; - command_interface_names.reserve(command_joint_names_.size() * command_interface_types_.size()); - for (const auto & joint : command_joint_names_) - { - for (const auto & interface : command_interface_types_) - { - command_interface_names.push_back(joint + "/" + interface); - } - } - auto command_interfaces = traj_controller_->command_interface_configuration(); - EXPECT_EQ( - command_interfaces.type, controller_interface::interface_configuration_type::INDIVIDUAL); - EXPECT_EQ( - command_interfaces.names.size(), command_joint_names_.size() * command_interface_types_.size()); - ASSERT_THAT( - command_interfaces.names, testing::UnorderedElementsAreArray(command_interface_names)); + compare_joints(joint_names_, command_joint_names_); } TEST_P(TrajectoryControllerTestParameterized, activate) @@ -316,6 +257,11 @@ TEST_P(TrajectoryControllerTestParameterized, correct_initialization_using_param executor.cancel(); } +/** + * @brief test if correct topic is received + * + * this test doesn't use class variables but subscribes to the state topic + */ TEST_P(TrajectoryControllerTestParameterized, state_topic_consistency) { rclcpp::executors::SingleThreadedExecutor executor; @@ -422,7 +368,7 @@ TEST_P(TrajectoryControllerTestParameterized, update_dynamic_parameters) SetUpAndActivateTrajectoryController(executor); - updateController(); + updateControllerAsync(); auto pids = traj_controller_->get_pids(); if (traj_controller_->use_closed_loop_pid_adapter()) @@ -433,7 +379,7 @@ TEST_P(TrajectoryControllerTestParameterized, update_dynamic_parameters) double kp = 1.0; SetPidParameters(kp); - updateController(); + updateControllerAsync(); pids = traj_controller_->get_pids(); EXPECT_EQ(pids.size(), 3); @@ -458,7 +404,7 @@ TEST_P(TrajectoryControllerTestParameterized, update_dynamic_tolerances) SetUpAndActivateTrajectoryController(executor); - updateController(); + updateControllerAsync(); // test default parameters { @@ -486,7 +432,7 @@ TEST_P(TrajectoryControllerTestParameterized, update_dynamic_tolerances) { traj_controller_->get_node()->set_parameter(param); } - updateController(); + updateControllerAsync(); { auto tols = traj_controller_->get_tolerances(); @@ -513,7 +459,7 @@ TEST_P(TrajectoryControllerTestParameterized, no_hold_on_startup) SetUpAndActivateTrajectoryController(executor, {start_with_holding_parameter}); constexpr auto FIRST_POINT_TIME = std::chrono::milliseconds(250); - updateController(rclcpp::Duration(FIRST_POINT_TIME)); + updateControllerAsync(rclcpp::Duration(FIRST_POINT_TIME)); // after startup without start_with_holding being set, we expect no active trajectory ASSERT_FALSE(traj_controller_->has_active_traj()); @@ -531,7 +477,7 @@ TEST_P(TrajectoryControllerTestParameterized, hold_on_startup) SetUpAndActivateTrajectoryController(executor, {start_with_holding_parameter}); constexpr auto FIRST_POINT_TIME = std::chrono::milliseconds(250); - updateController(rclcpp::Duration(FIRST_POINT_TIME)); + updateControllerAsync(rclcpp::Duration(FIRST_POINT_TIME)); // after startup with start_with_holding being set, we expect an active trajectory: ASSERT_TRUE(traj_controller_->has_active_traj()); // one point, being the position at startup @@ -552,7 +498,6 @@ TEST_P(TrajectoryControllerTestParameterized, position_error_not_angle_wraparoun constexpr double k_p = 10.0; std::vector params = {}; SetUpAndActivateTrajectoryController(executor, params, true, k_p, 0.0, false); - subscribeToState(); size_t n_joints = joint_names_.size(); @@ -568,84 +513,206 @@ TEST_P(TrajectoryControllerTestParameterized, position_error_not_angle_wraparoun publish(time_from_start, points, rclcpp::Time(), {}, points_velocities); traj_controller_->wait_for_trajectory(executor); - // first update - updateController(rclcpp::Duration(FIRST_POINT_TIME)); - - // Spin to receive latest state - executor.spin_some(); - auto state_msg = getState(); - ASSERT_TRUE(state_msg); + updateControllerAsync(rclcpp::Duration(FIRST_POINT_TIME)); - const auto allowed_delta = 0.1; + // get states from class variables + auto state_feedback = traj_controller_->get_state_feedback(); + auto state_reference = traj_controller_->get_state_reference(); + auto state_error = traj_controller_->get_state_error(); // no update of state_interface - EXPECT_EQ(state_msg->feedback.positions, INITIAL_POS_JOINTS); + EXPECT_EQ(state_feedback.positions, INITIAL_POS_JOINTS); // has the msg the correct vector sizes? - EXPECT_EQ(n_joints, state_msg->reference.positions.size()); - EXPECT_EQ(n_joints, state_msg->feedback.positions.size()); - EXPECT_EQ(n_joints, state_msg->error.positions.size()); + EXPECT_EQ(n_joints, state_reference.positions.size()); + EXPECT_EQ(n_joints, state_feedback.positions.size()); + EXPECT_EQ(n_joints, state_error.positions.size()); // are the correct reference positions used? - EXPECT_NEAR(points[0][0], state_msg->reference.positions[0], allowed_delta); - EXPECT_NEAR(points[0][1], state_msg->reference.positions[1], allowed_delta); - EXPECT_NEAR(points[0][2], state_msg->reference.positions[2], 3 * allowed_delta); + EXPECT_NEAR(points[0][0], state_reference.positions[0], COMMON_THRESHOLD); + EXPECT_NEAR(points[0][1], state_reference.positions[1], COMMON_THRESHOLD); + EXPECT_NEAR(points[0][2], state_reference.positions[2], COMMON_THRESHOLD); // no normalization of position error - EXPECT_NEAR( - state_msg->error.positions[0], state_msg->reference.positions[0] - INITIAL_POS_JOINTS[0], EPS); - EXPECT_NEAR( - state_msg->error.positions[1], state_msg->reference.positions[1] - INITIAL_POS_JOINTS[1], EPS); - EXPECT_NEAR( - state_msg->error.positions[2], state_msg->reference.positions[2] - INITIAL_POS_JOINTS[2], EPS); + EXPECT_NEAR(state_error.positions[0], state_reference.positions[0] - INITIAL_POS_JOINTS[0], EPS); + 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], allowed_delta); - EXPECT_NEAR(points[0][1], joint_pos_[1], allowed_delta); - EXPECT_NEAR(points[0][2], joint_pos_[2], allowed_delta); - EXPECT_NEAR(points[0][0], state_msg->output.positions[0], allowed_delta); - EXPECT_NEAR(points[0][1], state_msg->output.positions[1], allowed_delta); - EXPECT_NEAR(points[0][2], state_msg->output.positions[2], allowed_delta); + 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_ + if (traj_controller_->use_closed_loop_pid_adapter()) + { + // we expect u = k_p * (s_d-s) for positions + EXPECT_NEAR( + k_p * (state_reference.positions[0] - INITIAL_POS_JOINTS[0]), joint_vel_[0], + k_p * COMMON_THRESHOLD); + EXPECT_NEAR( + k_p * (state_reference.positions[1] - INITIAL_POS_JOINTS[1]), joint_vel_[1], + k_p * COMMON_THRESHOLD); + EXPECT_NEAR( + k_p * (state_reference.positions[2] - INITIAL_POS_JOINTS[2]), joint_vel_[2], + k_p * COMMON_THRESHOLD); + } + else + { + // interpolated points_velocities only + // check command interface + EXPECT_LT(0.0, joint_vel_[0]); + EXPECT_LT(0.0, joint_vel_[1]); + EXPECT_LT(0.0, joint_vel_[2]); + } + } + + if (traj_controller_->has_effort_command_interface()) + { + // use_closed_loop_pid_adapter_ + if (traj_controller_->use_closed_loop_pid_adapter()) + { + // we expect u = k_p * (s_d-s) for positions + EXPECT_NEAR( + k_p * (state_reference.positions[0] - INITIAL_POS_JOINTS[0]), joint_eff_[0], + k_p * COMMON_THRESHOLD); + EXPECT_NEAR( + k_p * (state_reference.positions[1] - INITIAL_POS_JOINTS[1]), joint_eff_[1], + k_p * COMMON_THRESHOLD); + EXPECT_NEAR( + k_p * (state_reference.positions[2] - INITIAL_POS_JOINTS[2]), joint_eff_[2], + k_p * COMMON_THRESHOLD); + } + else + { + // interpolated points_velocities only + // check command interface + EXPECT_LT(0.0, joint_eff_[0]); + EXPECT_LT(0.0, joint_eff_[1]); + EXPECT_LT(0.0, joint_eff_[2]); + } + } + + executor.cancel(); +} + +/** + * @brief check if position error of revolute joints are angle_wraparound if configured so + */ +TEST_P(TrajectoryControllerTestParameterized, position_error_angle_wraparound) +{ + rclcpp::executors::MultiThreadedExecutor executor; + constexpr double k_p = 10.0; + std::vector params = {}; + SetUpAndActivateTrajectoryController(executor, params, true, k_p, 0.0, true); + + size_t n_joints = joint_names_.size(); + + // send msg + constexpr auto FIRST_POINT_TIME = std::chrono::milliseconds(250); + builtin_interfaces::msg::Duration time_from_start{rclcpp::Duration(FIRST_POINT_TIME)}; + // *INDENT-OFF* + std::vector> points{ + {{3.3, 4.4, 6.6}}, {{7.7, 8.8, 9.9}}, {{10.10, 11.11, 12.12}}}; + std::vector> points_velocities{ + {{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); + + updateControllerAsync(rclcpp::Duration(FIRST_POINT_TIME)); + + // get states from class variables + auto state_feedback = traj_controller_->get_state_feedback(); + auto state_reference = traj_controller_->get_state_reference(); + auto state_error = traj_controller_->get_state_error(); + + // no update of state_interface + EXPECT_EQ(state_feedback.positions, INITIAL_POS_JOINTS); + + // has the msg the correct vector sizes? + EXPECT_EQ(n_joints, state_reference.positions.size()); + EXPECT_EQ(n_joints, state_feedback.positions.size()); + EXPECT_EQ(n_joints, state_error.positions.size()); + + // are the correct reference positions used? + EXPECT_NEAR(points[0][0], state_reference.positions[0], COMMON_THRESHOLD); + EXPECT_NEAR(points[0][1], state_reference.positions[1], COMMON_THRESHOLD); + EXPECT_NEAR(points[0][2], state_reference.positions[2], COMMON_THRESHOLD); + + // is error.positions[2] angle_wraparound? + EXPECT_NEAR(state_error.positions[0], state_reference.positions[0] - INITIAL_POS_JOINTS[0], EPS); + 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] - 2 * M_PI, EPS); + + if (traj_controller_->has_position_command_interface()) { // check command interface - EXPECT_LT(0.0, joint_vel_[0]); - EXPECT_LT(0.0, joint_vel_[1]); - EXPECT_LT(0.0, joint_vel_[2]); - EXPECT_LT(0.0, state_msg->output.velocities[0]); - EXPECT_LT(0.0, state_msg->output.velocities[1]); - EXPECT_LT(0.0, state_msg->output.velocities[2]); + 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_ if (traj_controller_->use_closed_loop_pid_adapter()) { - // we expect u = k_p * (s_d-s) + // we expect u = k_p * (s_d-s) for positions[0] and positions[1] EXPECT_NEAR( - k_p * (state_msg->reference.positions[0] - INITIAL_POS_JOINTS[0]), joint_vel_[0], - k_p * allowed_delta); + k_p * (state_reference.positions[0] - INITIAL_POS_JOINTS[0]), joint_vel_[0], + k_p * COMMON_THRESHOLD); EXPECT_NEAR( - k_p * (state_msg->reference.positions[1] - INITIAL_POS_JOINTS[1]), joint_vel_[1], - k_p * allowed_delta); - // no normalization of position error + k_p * (state_reference.positions[1] - INITIAL_POS_JOINTS[1]), joint_vel_[1], + k_p * COMMON_THRESHOLD); + // is error of positions[2] angle_wraparound? + EXPECT_GT(0.0, joint_vel_[2]); EXPECT_NEAR( - k_p * (state_msg->reference.positions[2] - INITIAL_POS_JOINTS[2]), joint_vel_[2], - k_p * allowed_delta); + k_p * (state_reference.positions[2] - INITIAL_POS_JOINTS[2] - 2 * M_PI), joint_vel_[2], + k_p * COMMON_THRESHOLD); + } + else + { + // interpolated points_velocities only + // check command interface + EXPECT_LT(0.0, joint_vel_[0]); + EXPECT_LT(0.0, joint_vel_[1]); + EXPECT_LT(0.0, joint_vel_[2]); } } if (traj_controller_->has_effort_command_interface()) { - // check command interface - EXPECT_LT(0.0, joint_eff_[0]); - EXPECT_LT(0.0, joint_eff_[1]); - EXPECT_LT(0.0, joint_eff_[2]); - EXPECT_LT(0.0, state_msg->output.effort[0]); - EXPECT_LT(0.0, state_msg->output.effort[1]); - EXPECT_LT(0.0, state_msg->output.effort[2]); + // use_closed_loop_pid_adapter_ + if (traj_controller_->use_closed_loop_pid_adapter()) + { + // we expect u = k_p * (s_d-s) for positions[0] and positions[1] + EXPECT_NEAR( + k_p * (state_reference.positions[0] - INITIAL_POS_JOINTS[0]), joint_eff_[0], + k_p * COMMON_THRESHOLD); + EXPECT_NEAR( + k_p * (state_reference.positions[1] - INITIAL_POS_JOINTS[1]), joint_eff_[1], + k_p * COMMON_THRESHOLD); + // is error of positions[2] angle_wraparound? + EXPECT_GT(0.0, joint_eff_[2]); + EXPECT_NEAR( + k_p * (state_reference.positions[2] - INITIAL_POS_JOINTS[2] - 2 * M_PI), joint_eff_[2], + k_p * COMMON_THRESHOLD); + } + else + { + // interpolated points_velocities only + // check command interface + EXPECT_LT(0.0, joint_eff_[0]); + EXPECT_LT(0.0, joint_eff_[1]); + EXPECT_LT(0.0, joint_eff_[2]); + } } executor.cancel(); @@ -685,13 +752,13 @@ TEST_P(TrajectoryControllerTestParameterized, decline_false_cmd_timeout) /** * @brief check if no timeout is triggered */ +// TODO(anyone) make test independent of clock source to use updateControllerAsync TEST_P(TrajectoryControllerTestParameterized, no_timeout) { rclcpp::executors::MultiThreadedExecutor executor; // zero is default value, just for demonstration rclcpp::Parameter cmd_timeout_parameter("cmd_timeout", 0.0); SetUpAndActivateTrajectoryController(executor, {cmd_timeout_parameter}, false); - subscribeToState(); size_t n_joints = joint_names_.size(); @@ -707,28 +774,27 @@ TEST_P(TrajectoryControllerTestParameterized, no_timeout) publish(time_from_start, points, rclcpp::Time(0, 0, RCL_STEADY_TIME), {}, points_velocities); traj_controller_->wait_for_trajectory(executor); - // first update updateController(rclcpp::Duration(FIRST_POINT_TIME) * 4); - // Spin to receive latest state - executor.spin_some(); - auto state_msg = getState(); - ASSERT_TRUE(state_msg); + // get states from class variables + auto state_feedback = traj_controller_->get_state_feedback(); + auto state_reference = traj_controller_->get_state_reference(); + auto state_error = traj_controller_->get_state_error(); // has the msg the correct vector sizes? - EXPECT_EQ(n_joints, state_msg->reference.positions.size()); + EXPECT_EQ(n_joints, state_reference.positions.size()); // is the trajectory still active? EXPECT_TRUE(traj_controller_->has_active_traj()); // should still hold the points from above EXPECT_TRUE(traj_controller_->has_nontrivial_traj()); - EXPECT_NEAR(state_msg->reference.positions[0], points.at(2).at(0), 1e-2); - EXPECT_NEAR(state_msg->reference.positions[1], points.at(2).at(1), 1e-2); - EXPECT_NEAR(state_msg->reference.positions[2], points.at(2).at(2), 1e-2); + EXPECT_NEAR(state_reference.positions[0], points.at(2).at(0), 1e-2); + EXPECT_NEAR(state_reference.positions[1], points.at(2).at(1), 1e-2); + EXPECT_NEAR(state_reference.positions[2], points.at(2).at(2), 1e-2); // value of velocities is different from above due to spline interpolation - EXPECT_GT(state_msg->reference.velocities[0], 0.0); - EXPECT_GT(state_msg->reference.velocities[1], 0.0); - EXPECT_GT(state_msg->reference.velocities[2], 0.0); + EXPECT_GT(state_reference.velocities[0], 0.0); + EXPECT_GT(state_reference.velocities[1], 0.0); + EXPECT_GT(state_reference.velocities[2], 0.0); executor.cancel(); } @@ -736,6 +802,7 @@ TEST_P(TrajectoryControllerTestParameterized, no_timeout) /** * @brief check if timeout is triggered */ +// TODO(anyone) make test independent of clock source to use updateControllerAsync TEST_P(TrajectoryControllerTestParameterized, timeout) { rclcpp::executors::MultiThreadedExecutor executor; @@ -743,7 +810,6 @@ TEST_P(TrajectoryControllerTestParameterized, timeout) rclcpp::Parameter cmd_timeout_parameter("cmd_timeout", cmd_timeout); double kp = 1.0; // activate feedback control for testing velocity/effort PID SetUpAndActivateTrajectoryController(executor, {cmd_timeout_parameter}, false, kp); - subscribeToState(); // send msg constexpr auto FIRST_POINT_TIME = std::chrono::milliseconds(250); @@ -768,11 +834,6 @@ TEST_P(TrajectoryControllerTestParameterized, timeout) // update until timeout should have happened updateController(rclcpp::Duration(FIRST_POINT_TIME)); - // Spin to receive latest state - executor.spin_some(); - auto state_msg = getState(); - ASSERT_TRUE(state_msg); - // after timeout, set_hold_position adds a new trajectory // is a trajectory active? EXPECT_TRUE(traj_controller_->has_active_traj()); @@ -792,130 +853,6 @@ TEST_P(TrajectoryControllerTestParameterized, timeout) executor.cancel(); } -/** - * @brief check if position error of revolute joints are angle_wraparound if configured so - */ -TEST_P(TrajectoryControllerTestParameterized, position_error_angle_wraparound) -{ - rclcpp::executors::MultiThreadedExecutor executor; - constexpr double k_p = 10.0; - std::vector params = {}; - SetUpAndActivateTrajectoryController(executor, params, true, k_p, 0.0, true); - subscribeToState(); - - size_t n_joints = joint_names_.size(); - - // send msg - constexpr auto FIRST_POINT_TIME = std::chrono::milliseconds(250); - builtin_interfaces::msg::Duration time_from_start{rclcpp::Duration(FIRST_POINT_TIME)}; - // *INDENT-OFF* - std::vector> points{ - {{3.3, 4.4, 6.6}}, {{7.7, 8.8, 9.9}}, {{10.10, 11.11, 12.12}}}; - std::vector> points_velocities{ - {{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); - - // first update - updateController(rclcpp::Duration(FIRST_POINT_TIME)); - - // Spin to receive latest state - executor.spin_some(); - auto state_msg = getState(); - ASSERT_TRUE(state_msg); - - const auto allowed_delta = 0.1; - - // no update of state_interface - EXPECT_EQ(state_msg->feedback.positions, INITIAL_POS_JOINTS); - - // has the msg the correct vector sizes? - EXPECT_EQ(n_joints, state_msg->reference.positions.size()); - EXPECT_EQ(n_joints, state_msg->feedback.positions.size()); - EXPECT_EQ(n_joints, state_msg->error.positions.size()); - - // are the correct reference positions used? - EXPECT_NEAR(points[0][0], state_msg->reference.positions[0], allowed_delta); - EXPECT_NEAR(points[0][1], state_msg->reference.positions[1], allowed_delta); - EXPECT_NEAR(points[0][2], state_msg->reference.positions[2], 3 * allowed_delta); - - // is error.positions[2] angle_wraparound? - EXPECT_NEAR( - state_msg->error.positions[0], state_msg->reference.positions[0] - INITIAL_POS_JOINTS[0], EPS); - EXPECT_NEAR( - state_msg->error.positions[1], state_msg->reference.positions[1] - INITIAL_POS_JOINTS[1], EPS); - EXPECT_NEAR( - state_msg->error.positions[2], - state_msg->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], allowed_delta); - EXPECT_NEAR(points[0][1], joint_pos_[1], allowed_delta); - EXPECT_NEAR(points[0][2], joint_pos_[2], allowed_delta); - } - - if (traj_controller_->has_velocity_command_interface()) - { - // use_closed_loop_pid_adapter_ - if (traj_controller_->use_closed_loop_pid_adapter()) - { - // we expect u = k_p * (s_d-s) for positions[0] and positions[1] - EXPECT_NEAR( - k_p * (state_msg->reference.positions[0] - INITIAL_POS_JOINTS[0]), joint_vel_[0], - k_p * allowed_delta); - EXPECT_NEAR( - k_p * (state_msg->reference.positions[1] - INITIAL_POS_JOINTS[1]), joint_vel_[1], - k_p * allowed_delta); - // is error of positions[2] angle_wraparound? - EXPECT_GT(0.0, joint_vel_[2]); - EXPECT_NEAR( - k_p * (state_msg->reference.positions[2] - INITIAL_POS_JOINTS[2] - 2 * M_PI), joint_vel_[2], - k_p * allowed_delta); - } - else - { - // interpolated points_velocities only - // check command interface - EXPECT_LT(0.0, joint_vel_[0]); - EXPECT_LT(0.0, joint_vel_[1]); - EXPECT_LT(0.0, joint_vel_[2]); - } - } - - if (traj_controller_->has_effort_command_interface()) - { - // use_closed_loop_pid_adapter_ - if (traj_controller_->use_closed_loop_pid_adapter()) - { - // we expect u = k_p * (s_d-s) for positions[0] and positions[1] - EXPECT_NEAR( - k_p * (state_msg->reference.positions[0] - INITIAL_POS_JOINTS[0]), joint_eff_[0], - k_p * allowed_delta); - EXPECT_NEAR( - k_p * (state_msg->reference.positions[1] - INITIAL_POS_JOINTS[1]), joint_eff_[1], - k_p * allowed_delta); - // is error of positions[2] angle_wraparound? - EXPECT_GT(0.0, joint_eff_[2]); - EXPECT_NEAR( - k_p * (state_msg->reference.positions[2] - INITIAL_POS_JOINTS[2] - 2 * M_PI), joint_eff_[2], - k_p * allowed_delta); - } - else - { - // interpolated points_velocities only - // check command interface - EXPECT_LT(0.0, joint_eff_[0]); - EXPECT_LT(0.0, joint_eff_[1]); - EXPECT_LT(0.0, joint_eff_[2]); - } - } - - executor.cancel(); -} - /** * @brief check if use_closed_loop_pid is active */ @@ -944,7 +881,6 @@ TEST_P(TrajectoryControllerTestParameterized, velocity_error) { rclcpp::executors::MultiThreadedExecutor executor; SetUpAndActivateTrajectoryController(executor, {}, true); - subscribeToState(); size_t n_joints = joint_names_.size(); @@ -960,35 +896,34 @@ TEST_P(TrajectoryControllerTestParameterized, velocity_error) publish(time_from_start, points_positions, rclcpp::Time(), {}, points_velocities); traj_controller_->wait_for_trajectory(executor); - // first update - updateController(rclcpp::Duration(FIRST_POINT_TIME)); + updateControllerAsync(rclcpp::Duration(FIRST_POINT_TIME)); - // Spin to receive latest state - executor.spin_some(); - auto state_msg = getState(); - ASSERT_TRUE(state_msg); + // get states from class variables + auto state_feedback = traj_controller_->get_state_feedback(); + auto state_reference = traj_controller_->get_state_reference(); + auto state_error = traj_controller_->get_state_error(); // has the msg the correct vector sizes? - EXPECT_EQ(n_joints, state_msg->reference.positions.size()); - EXPECT_EQ(n_joints, state_msg->feedback.positions.size()); - EXPECT_EQ(n_joints, state_msg->error.positions.size()); + EXPECT_EQ(n_joints, state_reference.positions.size()); + EXPECT_EQ(n_joints, state_feedback.positions.size()); + EXPECT_EQ(n_joints, state_error.positions.size()); if (traj_controller_->has_velocity_state_interface()) { - EXPECT_EQ(n_joints, state_msg->reference.velocities.size()); - EXPECT_EQ(n_joints, state_msg->feedback.velocities.size()); - EXPECT_EQ(n_joints, state_msg->error.velocities.size()); + EXPECT_EQ(n_joints, state_reference.velocities.size()); + EXPECT_EQ(n_joints, state_feedback.velocities.size()); + EXPECT_EQ(n_joints, state_error.velocities.size()); } if (traj_controller_->has_acceleration_state_interface()) { - EXPECT_EQ(n_joints, state_msg->reference.accelerations.size()); - EXPECT_EQ(n_joints, state_msg->feedback.accelerations.size()); - EXPECT_EQ(n_joints, state_msg->error.accelerations.size()); + EXPECT_EQ(n_joints, state_reference.accelerations.size()); + EXPECT_EQ(n_joints, state_feedback.accelerations.size()); + EXPECT_EQ(n_joints, state_error.accelerations.size()); } // no change in state interface should happen if (traj_controller_->has_velocity_state_interface()) { - EXPECT_EQ(state_msg->feedback.velocities, INITIAL_VEL_JOINTS); + EXPECT_EQ(state_feedback.velocities, INITIAL_VEL_JOINTS); } // is the velocity error correct? if ( @@ -998,9 +933,9 @@ TEST_P(TrajectoryControllerTestParameterized, velocity_error) { // don't check against a value, because spline interpolation might overshoot depending on // interface combinations - EXPECT_GE(state_msg->error.velocities[0], points_velocities[0][0]); - EXPECT_GE(state_msg->error.velocities[1], points_velocities[0][1]); - EXPECT_GE(state_msg->error.velocities[2], points_velocities[0][2]); + EXPECT_GE(state_error.velocities[0], points_velocities[0][0]); + EXPECT_GE(state_error.velocities[1], points_velocities[0][1]); + EXPECT_GE(state_error.velocities[2], points_velocities[0][2]); } executor.cancel(); @@ -1016,6 +951,7 @@ TEST_P(TrajectoryControllerTestParameterized, test_jumbled_joint_order) SetUpAndActivateTrajectoryController(executor); std::vector points_positions = {1.0, 2.0, 3.0}; std::vector jumble_map = {1, 2, 0}; + double dt = 0.25; { trajectory_msgs::msg::JointTrajectory traj_msg; const std::vector jumbled_joint_names{ @@ -1025,29 +961,27 @@ TEST_P(TrajectoryControllerTestParameterized, test_jumbled_joint_order) traj_msg.header.stamp = rclcpp::Time(0); traj_msg.points.resize(1); - traj_msg.points[0].time_from_start = rclcpp::Duration::from_seconds(0.25); + traj_msg.points[0].time_from_start = rclcpp::Duration::from_seconds(dt); traj_msg.points[0].positions.resize(3); traj_msg.points[0].positions[0] = points_positions.at(jumble_map[0]); traj_msg.points[0].positions[1] = points_positions.at(jumble_map[1]); traj_msg.points[0].positions[2] = points_positions.at(jumble_map[2]); traj_msg.points[0].velocities.resize(3); - for (size_t i = 0; i < 3; i++) + traj_msg.points[0].accelerations.resize(3); + + for (size_t dof = 0; dof < 3; dof++) { - // factor 2 comes from the linear interpolation in the spline trajectory for constant - // acceleration - traj_msg.points[0].velocities[i] = - 2 * (traj_msg.points[0].positions[i] - joint_pos_[jumble_map[i]]) / 0.25; + traj_msg.points[0].velocities[dof] = + (traj_msg.points[0].positions[dof] - joint_pos_[jumble_map[dof]]) / dt; + traj_msg.points[0].accelerations[dof] = + (traj_msg.points[0].velocities[dof] - joint_vel_[jumble_map[dof]]) / dt; } trajectory_publisher_->publish(traj_msg); } traj_controller_->wait_for_trajectory(executor); - // update just before 0.25 seconds (shorter than the trajectory duration of 0.25 seconds, - // otherwise acceleration is zero from the spline trajectory) - // TODO(destogl): Make this time a bit shorter to increase stability on the CI? - // Currently COMMON_THRESHOLD is adjusted. - updateController(rclcpp::Duration::from_seconds(0.25 - 1e-3)); + updateControllerAsync(rclcpp::Duration::from_seconds(dt)); if (traj_controller_->has_position_command_interface()) { @@ -1068,19 +1002,9 @@ TEST_P(TrajectoryControllerTestParameterized, test_jumbled_joint_order) if (traj_controller_->has_acceleration_command_interface()) { - if (traj_controller_->has_velocity_state_interface()) - { - EXPECT_GT(0.0, joint_acc_[0]); - EXPECT_GT(0.0, joint_acc_[1]); - EXPECT_GT(0.0, joint_acc_[2]); - } - else - { - // no velocity state interface: no acceleration from trajectory sampling - EXPECT_EQ(0.0, joint_acc_[0]); - EXPECT_EQ(0.0, joint_acc_[1]); - EXPECT_EQ(0.0, joint_acc_[2]); - } + EXPECT_GT(0.0, joint_acc_[0]); + EXPECT_GT(0.0, joint_acc_[1]); + EXPECT_GT(0.0, joint_acc_[2]); } if (traj_controller_->has_effort_command_interface()) @@ -1106,37 +1030,42 @@ TEST_P(TrajectoryControllerTestParameterized, test_partial_joint_list) const double initial_joint1_cmd = joint_pos_[0]; const double initial_joint2_cmd = joint_pos_[1]; const double initial_joint3_cmd = joint_pos_[2]; + const double dt = 0.25; trajectory_msgs::msg::JointTrajectory traj_msg; { - std::vector partial_joint_names{joint_names_[1], joint_names_[0]}; + std::vector jumble_map = {1, 0}; + std::vector partial_joint_names{ + joint_names_[jumble_map[0]], joint_names_[jumble_map[1]]}; traj_msg.joint_names = partial_joint_names; traj_msg.header.stamp = rclcpp::Time(0); traj_msg.points.resize(1); - traj_msg.points[0].time_from_start = rclcpp::Duration::from_seconds(0.25); + traj_msg.points[0].time_from_start = rclcpp::Duration::from_seconds(dt); traj_msg.points[0].positions.resize(2); traj_msg.points[0].positions[0] = 2.0; traj_msg.points[0].positions[1] = 1.0; traj_msg.points[0].velocities.resize(2); - traj_msg.points[0].velocities[0] = - std::copysign(2.0, traj_msg.points[0].positions[0] - initial_joint2_cmd); - traj_msg.points[0].velocities[1] = - std::copysign(1.0, traj_msg.points[0].positions[1] - initial_joint1_cmd); + traj_msg.points[0].accelerations.resize(2); + for (size_t dof = 0; dof < 2; dof++) + { + traj_msg.points[0].velocities[dof] = + (traj_msg.points[0].positions[dof] - joint_pos_[jumble_map[dof]]) / dt; + traj_msg.points[0].accelerations[dof] = + (traj_msg.points[0].velocities[dof] - joint_vel_[jumble_map[dof]]) / dt; + } trajectory_publisher_->publish(traj_msg); } traj_controller_->wait_for_trajectory(executor); - updateController(rclcpp::Duration::from_seconds(0.25)); - - double threshold = 0.001; + updateControllerAsync(rclcpp::Duration::from_seconds(dt)); if (traj_controller_->has_position_command_interface()) { - EXPECT_NEAR(traj_msg.points[0].positions[1], joint_pos_[0], threshold); - EXPECT_NEAR(traj_msg.points[0].positions[0], joint_pos_[1], threshold); - EXPECT_NEAR(initial_joint3_cmd, joint_pos_[2], threshold) + EXPECT_NEAR(traj_msg.points[0].positions[1], joint_pos_[0], COMMON_THRESHOLD); + EXPECT_NEAR(traj_msg.points[0].positions[0], joint_pos_[1], COMMON_THRESHOLD); + EXPECT_NEAR(initial_joint3_cmd, joint_pos_[2], COMMON_THRESHOLD) << "Joint 3 command should be current position"; } @@ -1148,7 +1077,7 @@ TEST_P(TrajectoryControllerTestParameterized, test_partial_joint_list) is_same_sign_or_zero(traj_msg.points[0].positions[0] - initial_joint2_cmd, joint_vel_[0])); EXPECT_TRUE( is_same_sign_or_zero(traj_msg.points[0].positions[1] - initial_joint1_cmd, joint_vel_[1])); - EXPECT_NEAR(0.0, joint_vel_[2], threshold) + EXPECT_NEAR(0.0, joint_vel_[2], COMMON_THRESHOLD) << "Joint 3 velocity should be 0.0 since it's not in the goal"; } @@ -1156,24 +1085,15 @@ TEST_P(TrajectoryControllerTestParameterized, test_partial_joint_list) { // estimate the sign of the acceleration // joint rotates forward - if (traj_controller_->has_velocity_state_interface()) - { - EXPECT_TRUE( - is_same_sign_or_zero(traj_msg.points[0].positions[0] - initial_joint2_cmd, joint_acc_[0])) - << "Joint1: " << traj_msg.points[0].positions[0] - initial_joint2_cmd << " vs. " - << joint_acc_[0]; - EXPECT_TRUE( - is_same_sign_or_zero(traj_msg.points[0].positions[1] - initial_joint1_cmd, joint_acc_[1])) - << "Joint2: " << traj_msg.points[0].positions[1] - initial_joint1_cmd << " vs. " - << joint_acc_[1]; - } - else - { - // no velocity state interface: no acceleration from trajectory sampling - EXPECT_EQ(0.0, joint_acc_[0]); - EXPECT_EQ(0.0, joint_acc_[1]); - } - EXPECT_NEAR(0.0, joint_acc_[2], threshold) + EXPECT_TRUE( + is_same_sign_or_zero(traj_msg.points[0].positions[0] - initial_joint2_cmd, joint_acc_[0])) + << "Joint1: " << traj_msg.points[0].positions[0] - initial_joint2_cmd << " vs. " + << joint_acc_[0]; + EXPECT_TRUE( + is_same_sign_or_zero(traj_msg.points[0].positions[1] - initial_joint1_cmd, joint_acc_[1])) + << "Joint2: " << traj_msg.points[0].positions[1] - initial_joint1_cmd << " vs. " + << joint_acc_[1]; + EXPECT_NEAR(0.0, joint_acc_[2], COMMON_THRESHOLD) << "Joint 3 acc should be 0.0 since it's not in the goal"; } @@ -1185,7 +1105,7 @@ TEST_P(TrajectoryControllerTestParameterized, test_partial_joint_list) is_same_sign_or_zero(traj_msg.points[0].positions[0] - initial_joint2_cmd, joint_eff_[0])); EXPECT_TRUE( is_same_sign_or_zero(traj_msg.points[0].positions[1] - initial_joint1_cmd, joint_eff_[1])); - EXPECT_NEAR(0.0, joint_eff_[2], threshold) + EXPECT_NEAR(0.0, joint_eff_[2], COMMON_THRESHOLD) << "Joint 3 effort should be 0.0 since it's not in the goal"; } @@ -1227,47 +1147,45 @@ TEST_P(TrajectoryControllerTestParameterized, test_partial_joint_list_not_allowe traj_controller_->wait_for_trajectory(executor); // update for 0.5 seconds - updateController(rclcpp::Duration::from_seconds(0.25)); - - double threshold = 0.001; + updateControllerAsync(rclcpp::Duration::from_seconds(0.25)); if (traj_controller_->has_position_command_interface()) { - EXPECT_NEAR(initial_joint1_cmd, joint_pos_[0], threshold) + EXPECT_NEAR(initial_joint1_cmd, joint_pos_[0], COMMON_THRESHOLD) << "All joints command should be current position because goal was rejected"; - EXPECT_NEAR(initial_joint2_cmd, joint_pos_[1], threshold) + EXPECT_NEAR(initial_joint2_cmd, joint_pos_[1], COMMON_THRESHOLD) << "All joints command should be current position because goal was rejected"; - EXPECT_NEAR(initial_joint3_cmd, joint_pos_[2], threshold) + EXPECT_NEAR(initial_joint3_cmd, joint_pos_[2], COMMON_THRESHOLD) << "All joints command should be current position because goal was rejected"; } if (traj_controller_->has_velocity_command_interface()) { - EXPECT_NEAR(INITIAL_VEL_JOINTS[0], joint_vel_[0], threshold) + EXPECT_NEAR(INITIAL_VEL_JOINTS[0], joint_vel_[0], COMMON_THRESHOLD) << "All joints velocities should be 0.0 because goal was rejected"; - EXPECT_NEAR(INITIAL_VEL_JOINTS[1], joint_vel_[1], threshold) + EXPECT_NEAR(INITIAL_VEL_JOINTS[1], joint_vel_[1], COMMON_THRESHOLD) << "All joints velocities should be 0.0 because goal was rejected"; - EXPECT_NEAR(INITIAL_VEL_JOINTS[2], joint_vel_[2], threshold) + EXPECT_NEAR(INITIAL_VEL_JOINTS[2], joint_vel_[2], COMMON_THRESHOLD) << "All joints velocities should be 0.0 because goal was rejected"; } if (traj_controller_->has_acceleration_command_interface()) { - EXPECT_NEAR(INITIAL_ACC_JOINTS[0], joint_acc_[0], threshold) + EXPECT_NEAR(INITIAL_ACC_JOINTS[0], joint_acc_[0], COMMON_THRESHOLD) << "All joints accelerations should be 0.0 because goal was rejected"; - EXPECT_NEAR(INITIAL_ACC_JOINTS[1], joint_acc_[1], threshold) + EXPECT_NEAR(INITIAL_ACC_JOINTS[1], joint_acc_[1], COMMON_THRESHOLD) << "All joints accelerations should be 0.0 because goal was rejected"; - EXPECT_NEAR(INITIAL_ACC_JOINTS[2], joint_acc_[2], threshold) + EXPECT_NEAR(INITIAL_ACC_JOINTS[2], joint_acc_[2], COMMON_THRESHOLD) << "All joints accelerations should be 0.0 because goal was rejected"; } if (traj_controller_->has_effort_command_interface()) { - EXPECT_NEAR(INITIAL_EFF_JOINTS[0], joint_eff_[0], threshold) + EXPECT_NEAR(INITIAL_EFF_JOINTS[0], joint_eff_[0], COMMON_THRESHOLD) << "All joints efforts should be 0.0 because goal was rejected"; - EXPECT_NEAR(INITIAL_EFF_JOINTS[1], joint_eff_[1], threshold) + EXPECT_NEAR(INITIAL_EFF_JOINTS[1], joint_eff_[1], COMMON_THRESHOLD) << "All joints efforts should be 0.0 because goal was rejected"; - EXPECT_NEAR(INITIAL_EFF_JOINTS[2], joint_eff_[2], threshold) + EXPECT_NEAR(INITIAL_EFF_JOINTS[2], joint_eff_[2], COMMON_THRESHOLD) << "All joints efforts should be 0.0 because goal was rejected"; } @@ -1408,8 +1326,6 @@ TEST_P(TrajectoryControllerTestParameterized, test_trajectory_replace) rclcpp::Parameter partial_joints_parameters("allow_partial_joints_goal", true); SetUpAndActivateTrajectoryController(executor, {partial_joints_parameters}); - subscribeToState(); - std::vector> points_old{{{2., 3., 4.}}}; std::vector> points_old_velocities{{{0.2, 0.3, 0.4}}}; std::vector> points_partial_new{{1.5}}; @@ -1424,8 +1340,8 @@ TEST_P(TrajectoryControllerTestParameterized, test_trajectory_replace) expected_actual.velocities = {points_old_velocities[0].begin(), points_old_velocities[0].end()}; expected_desired.velocities = {points_old_velocities[0].begin(), points_old_velocities[0].end()}; // Check that we reached end of points_old trajectory - // Denis: delta was 0.1 with 0.2 works for me - waitAndCompareState(expected_actual, expected_desired, executor, rclcpp::Duration(delay), 0.2); + auto end_time = + waitAndCompareState(expected_actual, expected_desired, executor, rclcpp::Duration(delay), 0.1); RCLCPP_INFO(traj_controller_->get_node()->get_logger(), "Sending new trajectory"); points_partial_new_velocities[0][0] = @@ -1440,7 +1356,8 @@ 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); + waitAndCompareState( + expected_actual, expected_desired, executor, rclcpp::Duration(delay), 0.1, end_time); } /** @@ -1450,7 +1367,6 @@ TEST_P(TrajectoryControllerTestParameterized, test_ignore_old_trajectory) { rclcpp::executors::SingleThreadedExecutor executor; SetUpAndActivateTrajectoryController(executor, {}); - subscribeToState(); // TODO(anyone): add expectations for velocities and accelerations std::vector> points_old{{{2., 3., 4.}, {4., 5., 6.}}}; @@ -1464,7 +1380,8 @@ TEST_P(TrajectoryControllerTestParameterized, test_ignore_old_trajectory) 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 - waitAndCompareState(expected_actual, expected_desired, executor, rclcpp::Duration(delay), 0.1); + auto end_time = + waitAndCompareState(expected_actual, expected_desired, executor, 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 @@ -1473,14 +1390,14 @@ 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); + waitAndCompareState( + expected_actual, expected_desired, executor, rclcpp::Duration(delay), 0.1, end_time); } TEST_P(TrajectoryControllerTestParameterized, test_ignore_partial_old_trajectory) { rclcpp::executors::SingleThreadedExecutor executor; SetUpAndActivateTrajectoryController(executor, {}); - subscribeToState(); std::vector> points_old{{{2., 3., 4.}, {4., 5., 6.}}}; std::vector> points_new{{{-1., -2., -3.}, {-2., -4., -6.}}}; @@ -1493,19 +1410,20 @@ TEST_P(TrajectoryControllerTestParameterized, test_ignore_partial_old_trajectory 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 - waitAndCompareState(expected_actual, expected_desired, executor, rclcpp::Duration(delay), 0.1); + auto end_time = + waitAndCompareState(expected_actual, expected_desired, executor, rclcpp::Duration(delay), 0.1); // send points_new before the old trajectory is finished RCLCPP_INFO( traj_controller_->get_node()->get_logger(), "Sending new trajectory partially in the past"); // New trajectory first point is in the past, second is in the future - rclcpp::Time new_traj_start = - rclcpp::Clock(RCL_STEADY_TIME).now() - delay - std::chrono::milliseconds(100); + rclcpp::Time new_traj_start = end_time - delay - std::chrono::milliseconds(100); publish(time_from_start, points_new, new_traj_start); // 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); + waitAndCompareState( + expected_actual, expected_desired, executor, rclcpp::Duration(delay), 0.1, end_time); } TEST_P(TrajectoryControllerTestParameterized, test_execute_partial_traj_in_future) @@ -1513,7 +1431,6 @@ TEST_P(TrajectoryControllerTestParameterized, test_execute_partial_traj_in_futur rclcpp::Parameter partial_joints_parameters("allow_partial_joints_goal", true); rclcpp::executors::SingleThreadedExecutor executor; SetUpAndActivateTrajectoryController(executor, {partial_joints_parameters}); - subscribeToState(); RCLCPP_WARN( traj_controller_->get_node()->get_logger(), @@ -1538,7 +1455,8 @@ TEST_P(TrajectoryControllerTestParameterized, test_execute_partial_traj_in_futur expected_actual.positions = {full_traj[0].begin(), full_traj[0].end()}; expected_desired = expected_actual; // Check that we reached end of points_old[0]trajectory and are starting points_old[1] - waitAndCompareState(expected_actual, expected_desired, executor, rclcpp::Duration(delay), 0.1); + auto end_time = + waitAndCompareState(expected_actual, expected_desired, executor, 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"); @@ -1551,28 +1469,21 @@ 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); + expected_actual, expected_desired, executor, rclcpp::Duration(delay * (2 + 2)), 0.1, end_time); } -// TODO(destogl) this test fails with errors -// second publish() gives an error, because end time is before current time -// as well as -// 2: The difference between joint_state_pos_[0] and joint_pos_[0] is 0.02999799000000003, -// which exceeds COMMON_THRESHOLD, where -// 2: joint_state_pos_[0] evaluates to 6.2999999999999998, -// 2: joint_pos_[0] evaluates to 6.2700020099999998, and -// 2: COMMON_THRESHOLD evaluates to 0.0011000000000000001. -// 2: [ FAILED ] PositionTrajectoryControllers/TrajectoryControllerTestParameterized. -// test_jump_when_state_tracking_error_updated/0, where GetParam() = -// ({ "position" }, { "position" }) (3372 ms) - -#if 0 TEST_P(TrajectoryControllerTestParameterized, test_jump_when_state_tracking_error_updated) { rclcpp::executors::SingleThreadedExecutor executor; // default if false so it will not be actually set parameter rclcpp::Parameter is_open_loop_parameters("open_loop_control", false); - SetUpAndActivateTrajectoryController(executor, {is_open_loop_parameters}, true); + SetUpAndActivateTrajectoryController(executor, {is_open_loop_parameters}, true); + + if (traj_controller_->has_position_command_interface() == false) + { + // only makes sense with position command interface + return; + } // goal setup std::vector first_goal = {3.3, 4.4, 5.5}; @@ -1585,95 +1496,95 @@ 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); std::vector> points{{first_goal}}; - publish(time_from_start, points, - rclcpp::Time(0.0, 0.0, RCL_STEADY_TIME), {}, first_goal_velocities); + publish( + time_from_start, points, rclcpp::Time(0.0, 0.0, RCL_STEADY_TIME), {}, first_goal_velocities); traj_controller_->wait_for_trajectory(executor); - updateController(rclcpp::Duration::from_seconds(1.1)); + updateControllerAsync(rclcpp::Duration::from_seconds(1.1)); - if (traj_controller_->has_position_command_interface()) - { - // JTC is executing trajectory in open-loop therefore: - // - internal state does not have to be updated (in this test-case it shouldn't) - // - internal command is updated - EXPECT_NEAR(INITIAL_POS_JOINT1, joint_state_pos_[0], COMMON_THRESHOLD); - EXPECT_NEAR(first_goal[0], joint_pos_[0], COMMON_THRESHOLD); - - // State interface should have offset from the command before starting a new trajectory - joint_state_pos_[0] = first_goal[0] - state_from_command_offset; - - // Move joint further in the same direction as before (to the second goal) - points = {{second_goal}}; - publish(time_from_start, points, - rclcpp::Time(1.0, 0.0, RCL_STEADY_TIME), {}, second_goal_velocities); - traj_controller_->wait_for_trajectory(executor); - - // 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); - updateController(rclcpp::Duration::from_seconds(0.01)); - // Expect backward commands at first - EXPECT_NEAR(joint_state_pos_[0], joint_pos_[0], state_from_command_offset + COMMON_THRESHOLD); - EXPECT_GT(joint_pos_[0], joint_state_pos_[0]); - EXPECT_LT(joint_pos_[0], first_goal[0]); - updateController(rclcpp::Duration::from_seconds(0.01)); - EXPECT_GT(joint_pos_[0], joint_state_pos_[0]); - EXPECT_LT(joint_pos_[0], first_goal[0]); - updateController(rclcpp::Duration::from_seconds(0.01)); - EXPECT_GT(joint_pos_[0], joint_state_pos_[0]); - EXPECT_LT(joint_pos_[0], first_goal[0]); - - // Finally the second goal will be commanded/reached - updateController(rclcpp::Duration::from_seconds(1.1)); - EXPECT_NEAR(second_goal[0], joint_pos_[0], COMMON_THRESHOLD); - - // State interface should have offset from the command before starting a new trajectory - joint_state_pos_[0] = second_goal[0] - state_from_command_offset; - - // 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); - - // 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); - updateController(rclcpp::Duration::from_seconds(0.01)); - // Expect backward commands at first - EXPECT_NEAR(joint_state_pos_[0], joint_pos_[0], COMMON_THRESHOLD); - EXPECT_LT(joint_pos_[0], joint_state_pos_[0]); - EXPECT_GT(joint_pos_[0], first_goal[0]); - updateController(rclcpp::Duration::from_seconds(0.01)); - EXPECT_LT(joint_pos_[0], joint_state_pos_[0]); - EXPECT_GT(joint_pos_[0], first_goal[0]); - updateController(rclcpp::Duration::from_seconds(0.01)); - EXPECT_LT(joint_pos_[0], joint_state_pos_[0]); - EXPECT_GT(joint_pos_[0], first_goal[0]); - - // Finally the first goal will be commanded/reached - updateController(rclcpp::Duration::from_seconds(1.1)); - EXPECT_NEAR(first_goal[0], joint_pos_[0], COMMON_THRESHOLD); - } + // JTC is executing trajectory in open-loop therefore: + // - internal state does not have to be updated (in this test-case it shouldn't) + // - internal command is updated + EXPECT_NEAR(INITIAL_POS_JOINT1, joint_state_pos_[0], COMMON_THRESHOLD); + EXPECT_NEAR(first_goal[0], joint_pos_[0], COMMON_THRESHOLD); + + // State interface should have offset from the command before starting a new trajectory + joint_state_pos_[0] = first_goal[0] - state_from_command_offset; + + // 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); + + // 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)); + // 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] + (second_goal[0] - joint_state_pos_[0]) * trajectory_frac, joint_pos_[0], + 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); + 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); + EXPECT_GT(joint_pos_[0], joint_state_pos_[0]); + EXPECT_LT(joint_pos_[0], first_goal[0]); + + // Finally the second goal will be commanded/reached + updateControllerAsync(rclcpp::Duration::from_seconds(1.1), end_time); + EXPECT_NEAR(second_goal[0], joint_pos_[0], COMMON_THRESHOLD); + + // State interface should have offset from the command before starting a new trajectory + joint_state_pos_[0] = second_goal[0] - state_from_command_offset; + + // 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); + + // 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)); + // Expect backward commands at first, consider advancement of the trajectory + EXPECT_NEAR( + joint_state_pos_[0] + (first_goal[0] - joint_state_pos_[0]) * trajectory_frac, joint_pos_[0], + COMMON_THRESHOLD); + 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); + 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); + EXPECT_LT(joint_pos_[0], joint_state_pos_[0]); + EXPECT_GT(joint_pos_[0], first_goal[0]); + + // Finally the first goal will be commanded/reached + updateControllerAsync(rclcpp::Duration::from_seconds(1.1), end_time); + EXPECT_NEAR(first_goal[0], joint_pos_[0], COMMON_THRESHOLD); executor.cancel(); } -#endif - -// TODO(destogl) this test fails -// 2: The difference between second_goal[0] and joint_pos_[0] is 0.032986635000000319, -// which exceeds COMMON_THRESHOLD, where -// 2: second_goal[0] evaluates to 6.5999999999999996, -// 2: joint_pos_[0] evaluates to 6.5670133649999993, and -// 2: COMMON_THRESHOLD evaluates to 0.0011000000000000001. -// 2: [ FAILED ] PositionTrajectoryControllers/TrajectoryControllerTestParameterized. -// test_no_jump_when_state_tracking_error_not_updated/1, where GetParam() = -// ({ "position" }, { "position", "velocity" }) (3374 ms) -#if 0 + TEST_P(TrajectoryControllerTestParameterized, test_no_jump_when_state_tracking_error_not_updated) { 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); + SetUpAndActivateTrajectoryController(executor, {is_open_loop_parameters}, true); + + if (traj_controller_->has_position_command_interface() == false) + { + // only makes sense with position command interface + return; + } // goal setup std::vector first_goal = {3.3, 4.4, 5.5}; @@ -1684,77 +1595,79 @@ 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); 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); - updateController(rclcpp::Duration::from_seconds(1.1)); + updateControllerAsync(rclcpp::Duration::from_seconds(1.1)); - if (traj_controller_->has_position_command_interface()) - { - // JTC is executing trajectory in open-loop therefore: - // - internal state does not have to be updated (in this test-case it shouldn't) - // - internal command is updated - EXPECT_NEAR(INITIAL_POS_JOINT1, joint_state_pos_[0], COMMON_THRESHOLD); - EXPECT_NEAR(first_goal[0], joint_pos_[0], COMMON_THRESHOLD); - - // State interface should have offset from the command before starting a new trajectory - joint_state_pos_[0] = first_goal[0] - state_from_command_offset; - - // 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); - - // 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); - updateController(rclcpp::Duration::from_seconds(0.01)); - // There should not be backward commands - EXPECT_NEAR(first_goal[0], joint_pos_[0], COMMON_THRESHOLD); - EXPECT_GT(joint_pos_[0], first_goal[0]); - EXPECT_LT(joint_pos_[0], second_goal[0]); - updateController(rclcpp::Duration::from_seconds(0.01)); - EXPECT_GT(joint_pos_[0], first_goal[0]); - EXPECT_LT(joint_pos_[0], second_goal[0]); - updateController(rclcpp::Duration::from_seconds(0.01)); - EXPECT_GT(joint_pos_[0], first_goal[0]); - EXPECT_LT(joint_pos_[0], second_goal[0]); - - // Finally the second goal will be commanded/reached - updateController(rclcpp::Duration::from_seconds(1.1)); - EXPECT_NEAR(second_goal[0], joint_pos_[0], COMMON_THRESHOLD); - - // State interface should have offset from the command before starting a new trajectory - joint_state_pos_[0] = second_goal[0] - state_from_command_offset; - - // 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); - - // 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); - updateController(rclcpp::Duration::from_seconds(0.01)); - // There should not be a jump toward commands - EXPECT_NEAR(second_goal[0], joint_pos_[0], COMMON_THRESHOLD); - EXPECT_LT(joint_pos_[0], second_goal[0]); - EXPECT_GT(joint_pos_[0], first_goal[0]); - updateController(rclcpp::Duration::from_seconds(0.01)); - EXPECT_GT(joint_pos_[0], first_goal[0]); - EXPECT_LT(joint_pos_[0], second_goal[0]); - updateController(rclcpp::Duration::from_seconds(0.01)); - EXPECT_GT(joint_pos_[0], first_goal[0]); - EXPECT_LT(joint_pos_[0], second_goal[0]); - - // Finally the first goal will be commanded/reached - updateController(rclcpp::Duration::from_seconds(1.1)); - EXPECT_NEAR(first_goal[0], joint_pos_[0], COMMON_THRESHOLD); - } + // JTC is executing trajectory in open-loop therefore: + // - internal state does not have to be updated (in this test-case it shouldn't) + // - internal command is updated + EXPECT_NEAR(INITIAL_POS_JOINT1, joint_state_pos_[0], COMMON_THRESHOLD); + EXPECT_NEAR(first_goal[0], joint_pos_[0], COMMON_THRESHOLD); + + // State interface should have offset from the command before starting a new trajectory + joint_state_pos_[0] = first_goal[0] - state_from_command_offset; + + // 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); + + // 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)); + // There should not be backward commands + EXPECT_NEAR( + first_goal[0] + (second_goal[0] - first_goal[0]) * trajectory_frac, joint_pos_[0], + COMMON_THRESHOLD); + 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); + 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); + EXPECT_GT(joint_pos_[0], first_goal[0]); + EXPECT_LT(joint_pos_[0], second_goal[0]); + + // Finally the second goal will be commanded/reached + updateControllerAsync(rclcpp::Duration::from_seconds(1.1), end_time); + EXPECT_NEAR(second_goal[0], joint_pos_[0], COMMON_THRESHOLD); + + // State interface should have offset from the command before starting a new trajectory + joint_state_pos_[0] = second_goal[0] - state_from_command_offset; + + // 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); + + // 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); + // There should not be a jump toward commands + EXPECT_NEAR( + second_goal[0] + (first_goal[0] - second_goal[0]) * trajectory_frac, joint_pos_[0], + COMMON_THRESHOLD); + 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); + 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); + EXPECT_GT(joint_pos_[0], first_goal[0]); + EXPECT_LT(joint_pos_[0], second_goal[0]); + + // Finally the first goal will be commanded/reached + updateControllerAsync(rclcpp::Duration::from_seconds(1.1), end_time); + EXPECT_NEAR(first_goal[0], joint_pos_[0], COMMON_THRESHOLD); executor.cancel(); } -#endif // Testing that values are read from state interfaces when hardware is started for the first // time and hardware state has offset --> this is indicated by NaN values in command interfaces @@ -1902,7 +1815,7 @@ TEST_P(TrajectoryControllerTestParameterized, test_state_tolerances_fail) // *INDENT-ON* publish(time_from_start, points, rclcpp::Time(0, 0, RCL_STEADY_TIME), {}, points_velocities); traj_controller_->wait_for_trajectory(executor); - updateController(rclcpp::Duration(FIRST_POINT_TIME)); + updateControllerAsync(rclcpp::Duration(FIRST_POINT_TIME)); // it should have aborted and be holding now expectHoldingPoint(joint_state_pos_); @@ -1934,7 +1847,7 @@ TEST_P(TrajectoryControllerTestParameterized, test_goal_tolerances_fail) // *INDENT-ON* publish(time_from_start, points, rclcpp::Time(0, 0, RCL_STEADY_TIME), {}, points_velocities); traj_controller_->wait_for_trajectory(executor); - updateController(rclcpp::Duration(4 * FIRST_POINT_TIME)); + auto end_time = updateControllerAsync(rclcpp::Duration(4 * FIRST_POINT_TIME)); // it should have aborted and be holding now expectHoldingPoint(joint_state_pos_); @@ -1942,7 +1855,7 @@ TEST_P(TrajectoryControllerTestParameterized, test_goal_tolerances_fail) // what happens if we wait longer but it harms the tolerance again? auto hold_position = joint_state_pos_; joint_state_pos_.at(0) = -3.3; - updateController(rclcpp::Duration(FIRST_POINT_TIME)); + updateControllerAsync(rclcpp::Duration(FIRST_POINT_TIME), end_time); // it should be still holding the old point expectHoldingPoint(hold_position); } diff --git a/joint_trajectory_controller/test/test_trajectory_controller_utils.hpp b/joint_trajectory_controller/test/test_trajectory_controller_utils.hpp index 46e3d45e29..62941d0775 100644 --- a/joint_trajectory_controller/test/test_trajectory_controller_utils.hpp +++ b/joint_trajectory_controller/test/test_trajectory_controller_utils.hpp @@ -21,7 +21,7 @@ #include #include -#include "gtest/gtest.h" +#include "gmock/gmock.h" #include "hardware_interface/types/hardware_interface_type_values.hpp" #include "joint_trajectory_controller/joint_trajectory_controller.hpp" @@ -29,7 +29,7 @@ namespace { -const double COMMON_THRESHOLD = 0.0011; // destogl: increased for 0.0001 for stable CI builds? +const double COMMON_THRESHOLD = 0.001; const double INITIAL_POS_JOINT1 = 1.1; const double INITIAL_POS_JOINT2 = 2.1; const double INITIAL_POS_JOINT3 = 3.1; @@ -151,6 +151,10 @@ class TestableJointTrajectoryController double get_cmd_timeout() { return cmd_timeout_; } + trajectory_msgs::msg::JointTrajectoryPoint get_state_feedback() { return state_current_; } + trajectory_msgs::msg::JointTrajectoryPoint get_state_reference() { return state_desired_; } + trajectory_msgs::msg::JointTrajectoryPoint get_state_error() { return state_error_; } + rclcpp::WaitSet joint_cmd_sub_wait_set_; }; @@ -233,21 +237,24 @@ class TrajectoryControllerTest : public ::testing::Test const std::vector initial_acc_joints = INITIAL_ACC_JOINTS, const std::vector initial_eff_joints = INITIAL_EFF_JOINTS) { - SetUpTrajectoryController(executor); - - // add this to simplify tests, can be overwritten by parameters - rclcpp::Parameter nonzero_vel_parameter("allow_nonzero_velocity_at_trajectory_end", true); - traj_controller_->get_node()->set_parameter(nonzero_vel_parameter); - - // set pid parameters before configure - SetPidParameters(k_p, ff, angle_wraparound); - - // set optional parameters - for (const auto & param : parameters) + auto has_nonzero_vel_param = + std::find_if( + parameters.begin(), parameters.end(), + [](const rclcpp::Parameter & param) { + return param.get_name() == "allow_nonzero_velocity_at_trajectory_end"; + }) != parameters.end(); + + std::vector parameters_local = parameters; + if (!has_nonzero_vel_param) { - traj_controller_->get_node()->set_parameter(param); + // add this to simplify tests, if not set already + parameters_local.emplace_back("allow_nonzero_velocity_at_trajectory_end", true); } + // read-only parameters have to be set before init -> won't be read otherwise + SetUpTrajectoryController(executor, parameters_local); + // set pid parameters before configure + SetPidParameters(k_p, ff, angle_wraparound); traj_controller_->get_node()->configure(); ActivateTrajectoryController( @@ -407,43 +414,85 @@ class TrajectoryControllerTest : public ::testing::Test trajectory_publisher_->publish(traj_msg); } - void updateController(rclcpp::Duration wait_time = rclcpp::Duration::from_seconds(0.2)) + /** + * @brief a wrapper for update() method of JTC, running synchronously with the clock + * @param wait_time - the time span for updating the controller + * @param update_rate - the rate at which the controller is updated + * + * @note use the faster updateControllerAsync() if no subscriptions etc. + * have to be used from the waitSet/executor + */ + void updateController( + rclcpp::Duration wait_time = rclcpp::Duration::from_seconds(0.2), + const rclcpp::Duration update_rate = rclcpp::Duration::from_seconds(0.01)) { auto clock = rclcpp::Clock(RCL_STEADY_TIME); const auto start_time = clock.now(); const auto end_time = start_time + wait_time; auto previous_time = start_time; - while (clock.now() < end_time) + while (clock.now() <= end_time) { auto now = clock.now(); traj_controller_->update(now, now - previous_time); previous_time = now; + std::this_thread::sleep_for(update_rate.to_chrono()); + } + } + + /** + * @brief a wrapper for update() method of JTC, running asynchronously from the clock + * @return the time at which the update finished + * @param wait_time - the time span for updating the controller + * @param start_time - the time at which the update should start + * @param update_rate - the rate at which the controller is updated + * + * @note this is faster than updateController() and can be used if no subscriptions etc. + * have to be used from the waitSet/executor + */ + rclcpp::Time updateControllerAsync( + rclcpp::Duration wait_time = rclcpp::Duration::from_seconds(0.2), + rclcpp::Time start_time = rclcpp::Time(0, 0, RCL_STEADY_TIME), + const rclcpp::Duration update_rate = rclcpp::Duration::from_seconds(0.01)) + { + if (start_time == rclcpp::Time(0, 0, RCL_STEADY_TIME)) + { + start_time = rclcpp::Clock(RCL_STEADY_TIME).now(); } + const auto end_time = start_time + wait_time; + auto time_counter = start_time; + while (time_counter <= end_time) + { + traj_controller_->update(time_counter, update_rate); + time_counter += update_rate; + } + return end_time; } - void waitAndCompareState( + rclcpp::Time waitAndCompareState( trajectory_msgs::msg::JointTrajectoryPoint expected_actual, trajectory_msgs::msg::JointTrajectoryPoint expected_desired, rclcpp::Executor & executor, - rclcpp::Duration controller_wait_time, double allowed_delta) + rclcpp::Duration controller_wait_time, double allowed_delta, + rclcpp::Time start_time = rclcpp::Time(0, 0, RCL_STEADY_TIME)) { { std::lock_guard guard(state_mutex_); state_msg_.reset(); } traj_controller_->wait_for_trajectory(executor); - updateController(controller_wait_time); - // Spin to receive latest state - executor.spin_some(); - auto state_msg = getState(); - ASSERT_TRUE(state_msg); + auto end_time = updateControllerAsync(controller_wait_time, start_time); + + // get states from class variables + auto state_feedback = traj_controller_->get_state_feedback(); + auto state_reference = traj_controller_->get_state_reference(); + for (size_t i = 0; i < expected_actual.positions.size(); ++i) { SCOPED_TRACE("Joint " + std::to_string(i)); // TODO(anyone): add checking for velocities and accelerations if (traj_controller_->has_position_command_interface()) { - EXPECT_NEAR(expected_actual.positions[i], state_msg->feedback.positions[i], allowed_delta); + EXPECT_NEAR(expected_actual.positions[i], state_feedback.positions[i], allowed_delta); } } @@ -453,10 +502,11 @@ class TrajectoryControllerTest : public ::testing::Test // TODO(anyone): add checking for velocities and accelerations if (traj_controller_->has_position_command_interface()) { - EXPECT_NEAR( - expected_desired.positions[i], state_msg->reference.positions[i], allowed_delta); + EXPECT_NEAR(expected_desired.positions[i], state_reference.positions[i], allowed_delta); } } + + return end_time; } std::shared_ptr getState() const @@ -573,6 +623,47 @@ class TrajectoryControllerTest : public ::testing::Test } } + /** + * @brief compares the joint names and interface types of the controller with the given ones + */ + void compare_joints( + std::vector state_joint_names, std::vector command_joint_names) + { + std::vector state_interface_names; + state_interface_names.reserve(state_joint_names.size() * state_interface_types_.size()); + for (const auto & joint : state_joint_names) + { + for (const auto & interface : state_interface_types_) + { + state_interface_names.push_back(joint + "/" + interface); + } + } + auto state_interfaces = traj_controller_->state_interface_configuration(); + EXPECT_EQ( + state_interfaces.type, controller_interface::interface_configuration_type::INDIVIDUAL); + EXPECT_EQ( + state_interfaces.names.size(), state_joint_names.size() * state_interface_types_.size()); + ASSERT_THAT(state_interfaces.names, testing::UnorderedElementsAreArray(state_interface_names)); + + std::vector command_interface_names; + command_interface_names.reserve(command_joint_names.size() * command_interface_types_.size()); + for (const auto & joint : command_joint_names) + { + for (const auto & interface : command_interface_types_) + { + command_interface_names.push_back(joint + "/" + interface); + } + } + auto command_interfaces = traj_controller_->command_interface_configuration(); + EXPECT_EQ( + command_interfaces.type, controller_interface::interface_configuration_type::INDIVIDUAL); + EXPECT_EQ( + command_interfaces.names.size(), + command_joint_names.size() * command_interface_types_.size()); + ASSERT_THAT( + command_interfaces.names, testing::UnorderedElementsAreArray(command_interface_names)); + } + std::string controller_name_; std::vector joint_names_;