From 5919e1a6c207a6fd4ccbc4f2c3556c969d824dfa Mon Sep 17 00:00:00 2001 From: Christoph Froehlich Date: Sun, 26 Nov 2023 11:21:39 +0000 Subject: [PATCH 01/14] Get msg from class variable and async update --- .../test/test_trajectory_controller.cpp | 433 +++++++++--------- .../test/test_trajectory_controller_utils.hpp | 27 ++ 2 files changed, 245 insertions(+), 215 deletions(-) diff --git a/joint_trajectory_controller/test/test_trajectory_controller.cpp b/joint_trajectory_controller/test/test_trajectory_controller.cpp index 8b29c2cb16..49f883de2d 100644 --- a/joint_trajectory_controller/test/test_trajectory_controller.cpp +++ b/joint_trajectory_controller/test/test_trajectory_controller.cpp @@ -323,51 +323,51 @@ TEST_P(TrajectoryControllerTestParameterized, state_topic_consistency) subscribeToState(); updateController(); - // Spin to receive latest state - executor.spin_some(); - auto state = getState(); + // get state message from class variable + auto state_msg = traj_controller_->get_state_msg(); size_t n_joints = joint_names_.size(); for (unsigned int i = 0; i < n_joints; ++i) { - EXPECT_EQ(joint_names_[i], state->joint_names[i]); + EXPECT_EQ(joint_names_[i], state_msg.joint_names[i]); } // No trajectory by default, no reference state or error EXPECT_TRUE( - state->reference.positions.empty() || state->reference.positions == INITIAL_POS_JOINTS); + state_msg.reference.positions.empty() || state_msg.reference.positions == INITIAL_POS_JOINTS); EXPECT_TRUE( - state->reference.velocities.empty() || state->reference.velocities == INITIAL_VEL_JOINTS); + state_msg.reference.velocities.empty() || state_msg.reference.velocities == INITIAL_VEL_JOINTS); EXPECT_TRUE( - state->reference.accelerations.empty() || state->reference.accelerations == INITIAL_EFF_JOINTS); + state_msg.reference.accelerations.empty() || + state_msg.reference.accelerations == INITIAL_EFF_JOINTS); std::vector zeros(3, 0); - EXPECT_EQ(state->error.positions, zeros); - EXPECT_TRUE(state->error.velocities.empty() || state->error.velocities == zeros); - EXPECT_TRUE(state->error.accelerations.empty() || state->error.accelerations == zeros); + EXPECT_EQ(state_msg.error.positions, zeros); + EXPECT_TRUE(state_msg.error.velocities.empty() || state_msg.error.velocities == zeros); + EXPECT_TRUE(state_msg.error.accelerations.empty() || state_msg.error.accelerations == zeros); // expect feedback including all state_interfaces - EXPECT_EQ(n_joints, state->feedback.positions.size()); + EXPECT_EQ(n_joints, state_msg.feedback.positions.size()); if ( std::find(state_interface_types_.begin(), state_interface_types_.end(), "velocity") == state_interface_types_.end()) { - EXPECT_TRUE(state->feedback.velocities.empty()); + EXPECT_TRUE(state_msg.feedback.velocities.empty()); } else { - EXPECT_EQ(n_joints, state->feedback.velocities.size()); + EXPECT_EQ(n_joints, state_msg.feedback.velocities.size()); } if ( std::find(state_interface_types_.begin(), state_interface_types_.end(), "acceleration") == state_interface_types_.end()) { - EXPECT_TRUE(state->feedback.accelerations.empty()); + EXPECT_TRUE(state_msg.feedback.accelerations.empty()); } else { - EXPECT_EQ(n_joints, state->feedback.accelerations.size()); + EXPECT_EQ(n_joints, state_msg.feedback.accelerations.size()); } // expect output including all command_interfaces @@ -375,41 +375,41 @@ TEST_P(TrajectoryControllerTestParameterized, state_topic_consistency) std::find(command_interface_types_.begin(), command_interface_types_.end(), "position") == command_interface_types_.end()) { - EXPECT_TRUE(state->output.positions.empty()); + EXPECT_TRUE(state_msg.output.positions.empty()); } else { - EXPECT_EQ(n_joints, state->output.positions.size()); + EXPECT_EQ(n_joints, state_msg.output.positions.size()); } if ( std::find(command_interface_types_.begin(), command_interface_types_.end(), "velocity") == command_interface_types_.end()) { - EXPECT_TRUE(state->output.velocities.empty()); + EXPECT_TRUE(state_msg.output.velocities.empty()); } else { - EXPECT_EQ(n_joints, state->output.velocities.size()); + EXPECT_EQ(n_joints, state_msg.output.velocities.size()); } if ( std::find(command_interface_types_.begin(), command_interface_types_.end(), "acceleration") == command_interface_types_.end()) { - EXPECT_TRUE(state->output.accelerations.empty()); + EXPECT_TRUE(state_msg.output.accelerations.empty()); } else { - EXPECT_EQ(n_joints, state->output.accelerations.size()); + EXPECT_EQ(n_joints, state_msg.output.accelerations.size()); } if ( std::find(command_interface_types_.begin(), command_interface_types_.end(), "effort") == command_interface_types_.end()) { - EXPECT_TRUE(state->output.effort.empty()); + EXPECT_TRUE(state_msg.output.effort.empty()); } else { - EXPECT_EQ(n_joints, state->output.effort.size()); + EXPECT_EQ(n_joints, state_msg.output.effort.size()); } } @@ -569,35 +569,33 @@ TEST_P(TrajectoryControllerTestParameterized, position_error_not_angle_wraparoun 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 state message from class variable + auto state_msg = traj_controller_->get_state_msg(); const auto allowed_delta = 0.1; // no update of state_interface - EXPECT_EQ(state_msg->feedback.positions, INITIAL_POS_JOINTS); + 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()); + 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); + 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); // no normalization of position error EXPECT_NEAR( - state_msg->error.positions[0], state_msg->reference.positions[0] - INITIAL_POS_JOINTS[0], EPS); + 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); + 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); + state_msg.error.positions[2], state_msg.reference.positions[2] - INITIAL_POS_JOINTS[2], EPS); if (traj_controller_->has_position_command_interface()) { @@ -605,47 +603,182 @@ TEST_P(TrajectoryControllerTestParameterized, position_error_not_angle_wraparoun 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); } 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_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); + 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); + } + 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_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); + EXPECT_GT(0.0, joint_eff_[2]); + EXPECT_NEAR( + k_p * (state_msg.reference.positions[2] - INITIAL_POS_JOINTS[2]), 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 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 + updateControllerAsync(rclcpp::Duration(FIRST_POINT_TIME)); + + // get state message from class variable + auto state_msg = traj_controller_->get_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_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], 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) + // 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 * (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 * (state_msg.reference.positions[1] - INITIAL_POS_JOINTS[1]), joint_vel_[1], k_p * allowed_delta); - // no normalization of position error + // 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 * (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()) { - // 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_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(); @@ -710,25 +843,23 @@ TEST_P(TrajectoryControllerTestParameterized, no_timeout) // 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 state message from class variable + auto state_msg = traj_controller_->get_state_msg(); // has the msg the correct vector sizes? - EXPECT_EQ(n_joints, state_msg->reference.positions.size()); + EXPECT_EQ(n_joints, state_msg.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_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); // 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_msg.reference.velocities[0], 0.0); + EXPECT_GT(state_msg.reference.velocities[1], 0.0); + EXPECT_GT(state_msg.reference.velocities[2], 0.0); executor.cancel(); } @@ -768,10 +899,8 @@ 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); + // get state message from class variable + auto state_msg = traj_controller_->get_state_msg(); // after timeout, set_hold_position adds a new trajectory // is a trajectory active? @@ -792,130 +921,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 */ @@ -963,32 +968,30 @@ TEST_P(TrajectoryControllerTestParameterized, velocity_error) // first update updateController(rclcpp::Duration(FIRST_POINT_TIME)); - // Spin to receive latest state - executor.spin_some(); - auto state_msg = getState(); - ASSERT_TRUE(state_msg); + // get state message from class variable + auto state_msg = traj_controller_->get_state_msg(); // 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_msg.reference.positions.size()); + EXPECT_EQ(n_joints, state_msg.feedback.positions.size()); + EXPECT_EQ(n_joints, state_msg.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_msg.reference.velocities.size()); + EXPECT_EQ(n_joints, state_msg.feedback.velocities.size()); + EXPECT_EQ(n_joints, state_msg.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_msg.reference.accelerations.size()); + EXPECT_EQ(n_joints, state_msg.feedback.accelerations.size()); + EXPECT_EQ(n_joints, state_msg.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_msg.feedback.velocities, INITIAL_VEL_JOINTS); } // is the velocity error correct? if ( @@ -998,9 +1001,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_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]); } executor.cancel(); diff --git a/joint_trajectory_controller/test/test_trajectory_controller_utils.hpp b/joint_trajectory_controller/test/test_trajectory_controller_utils.hpp index 3a7e7e5cf1..0cd009b80f 100644 --- a/joint_trajectory_controller/test/test_trajectory_controller_utils.hpp +++ b/joint_trajectory_controller/test/test_trajectory_controller_utils.hpp @@ -151,6 +151,13 @@ class TestableJointTrajectoryController double get_cmd_timeout() { return cmd_timeout_; } + joint_trajectory_controller::JointTrajectoryController::ControllerStateMsg get_state_msg() + { + return state_publisher_->msg_; + } + + void unlock_publisher() { state_publisher_->unlock(); } + rclcpp::WaitSet joint_cmd_sub_wait_set_; }; @@ -422,6 +429,26 @@ class TrajectoryControllerTest : public ::testing::Test } } + void updateControllerAsync(rclcpp::Duration wait_time = rclcpp::Duration::from_seconds(0.2)) + { + auto clock = rclcpp::Clock(RCL_STEADY_TIME); + const auto start_time = clock.now(); + const auto end_time = start_time + wait_time; + auto time_counter = start_time; + // set 10ms as update rate + auto update_rate = rclcpp::Duration::from_seconds(0.01); + while (time_counter < end_time) + { + // ensure that try_lock has success in the update method + traj_controller_->unlock_publisher(); + // needed for the realtime publisher's thread for publishingLoop + std::this_thread::sleep_for(std::chrono::microseconds(500)); + + traj_controller_->update(time_counter, update_rate); + time_counter += update_rate; + } + } + void waitAndCompareState( trajectory_msgs::msg::JointTrajectoryPoint expected_actual, trajectory_msgs::msg::JointTrajectoryPoint expected_desired, rclcpp::Executor & executor, From b8ed1b348fd17409f514d6353ea4d09cf0d339fe Mon Sep 17 00:00:00 2001 From: Christoph Froehlich Date: Sun, 26 Nov 2023 13:23:28 +0000 Subject: [PATCH 02/14] Use states from class variables --- .../test/test_trajectory_controller.cpp | 179 +++++++++--------- .../test/test_trajectory_controller_utils.hpp | 28 ++- 2 files changed, 111 insertions(+), 96 deletions(-) diff --git a/joint_trajectory_controller/test/test_trajectory_controller.cpp b/joint_trajectory_controller/test/test_trajectory_controller.cpp index 49f883de2d..d808441866 100644 --- a/joint_trajectory_controller/test/test_trajectory_controller.cpp +++ b/joint_trajectory_controller/test/test_trajectory_controller.cpp @@ -571,38 +571,35 @@ TEST_P(TrajectoryControllerTestParameterized, position_error_not_angle_wraparoun // first update updateControllerAsync(rclcpp::Duration(FIRST_POINT_TIME)); - // get state message from class variable - auto state_msg = traj_controller_->get_state_msg(); - - 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], 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()) @@ -612,15 +609,14 @@ TEST_P(TrajectoryControllerTestParameterized, position_error_not_angle_wraparoun { // we expect u = k_p * (s_d-s) for positions 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); - EXPECT_GT(0.0, joint_vel_[2]); + k_p * (state_reference.positions[1] - INITIAL_POS_JOINTS[1]), joint_vel_[1], + k_p * COMMON_THRESHOLD); 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]), joint_vel_[2], + k_p * COMMON_THRESHOLD); } else { @@ -639,15 +635,14 @@ TEST_P(TrajectoryControllerTestParameterized, position_error_not_angle_wraparoun { // we expect u = k_p * (s_d-s) for positions EXPECT_NEAR( - k_p * (state_msg.reference.positions[0] - INITIAL_POS_JOINTS[0]), joint_eff_[0], - k_p * allowed_delta); + k_p * (state_reference.positions[0] - INITIAL_POS_JOINTS[0]), joint_eff_[0], + k_p * COMMON_THRESHOLD); EXPECT_NEAR( - k_p * (state_msg.reference.positions[1] - INITIAL_POS_JOINTS[1]), joint_eff_[1], - k_p * allowed_delta); - EXPECT_GT(0.0, joint_eff_[2]); + k_p * (state_reference.positions[1] - INITIAL_POS_JOINTS[1]), joint_eff_[1], + k_p * COMMON_THRESHOLD); EXPECT_NEAR( - k_p * (state_msg.reference.positions[2] - INITIAL_POS_JOINTS[2]), joint_eff_[2], - k_p * allowed_delta); + k_p * (state_reference.positions[2] - INITIAL_POS_JOINTS[2]), joint_eff_[2], + k_p * COMMON_THRESHOLD); } else { @@ -690,39 +685,36 @@ TEST_P(TrajectoryControllerTestParameterized, position_error_angle_wraparound) // first update updateControllerAsync(rclcpp::Duration(FIRST_POINT_TIME)); - // get state message from class variable - auto state_msg = traj_controller_->get_state_msg(); - - 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); // 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_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); + 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], 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], 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()) @@ -732,16 +724,16 @@ TEST_P(TrajectoryControllerTestParameterized, position_error_angle_wraparound) { // 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); + 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] - 2 * M_PI), 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 { @@ -760,16 +752,16 @@ TEST_P(TrajectoryControllerTestParameterized, position_error_angle_wraparound) { // 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); + k_p * (state_reference.positions[0] - INITIAL_POS_JOINTS[0]), joint_eff_[0], + k_p * COMMON_THRESHOLD); EXPECT_NEAR( - k_p * (state_msg.reference.positions[1] - INITIAL_POS_JOINTS[1]), joint_eff_[1], - k_p * allowed_delta); + 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_msg.reference.positions[2] - INITIAL_POS_JOINTS[2] - 2 * M_PI), joint_eff_[2], - k_p * allowed_delta); + k_p * (state_reference.positions[2] - INITIAL_POS_JOINTS[2] - 2 * M_PI), joint_eff_[2], + k_p * COMMON_THRESHOLD); } else { @@ -843,23 +835,25 @@ TEST_P(TrajectoryControllerTestParameterized, no_timeout) // first update updateController(rclcpp::Duration(FIRST_POINT_TIME) * 4); - // get state message from class variable - auto state_msg = traj_controller_->get_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(); } @@ -899,9 +893,6 @@ TEST_P(TrajectoryControllerTestParameterized, timeout) // update until timeout should have happened updateController(rclcpp::Duration(FIRST_POINT_TIME)); - // get state message from class variable - auto state_msg = traj_controller_->get_state_msg(); - // after timeout, set_hold_position adds a new trajectory // is a trajectory active? EXPECT_TRUE(traj_controller_->has_active_traj()); @@ -968,30 +959,32 @@ TEST_P(TrajectoryControllerTestParameterized, velocity_error) // first update updateController(rclcpp::Duration(FIRST_POINT_TIME)); - // get state message from class variable - auto state_msg = traj_controller_->get_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 ( @@ -1001,9 +994,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(); diff --git a/joint_trajectory_controller/test/test_trajectory_controller_utils.hpp b/joint_trajectory_controller/test/test_trajectory_controller_utils.hpp index 0cd009b80f..7bf4eebde4 100644 --- a/joint_trajectory_controller/test/test_trajectory_controller_utils.hpp +++ b/joint_trajectory_controller/test/test_trajectory_controller_utils.hpp @@ -158,6 +158,10 @@ class TestableJointTrajectoryController void unlock_publisher() { state_publisher_->unlock(); } + 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_; }; @@ -421,7 +425,7 @@ class TrajectoryControllerTest : public ::testing::Test 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); @@ -429,7 +433,8 @@ class TrajectoryControllerTest : public ::testing::Test } } - void updateControllerAsync(rclcpp::Duration wait_time = rclcpp::Duration::from_seconds(0.2)) + void updateControllerAsyncPublisher( + rclcpp::Duration wait_time = rclcpp::Duration::from_seconds(0.2)) { auto clock = rclcpp::Clock(RCL_STEADY_TIME); const auto start_time = clock.now(); @@ -437,8 +442,10 @@ class TrajectoryControllerTest : public ::testing::Test auto time_counter = start_time; // set 10ms as update rate auto update_rate = rclcpp::Duration::from_seconds(0.01); - while (time_counter < end_time) + while (time_counter <= end_time) { + std::cerr << "update " << time_counter.seconds() << std::endl; + // ensure that try_lock has success in the update method traj_controller_->unlock_publisher(); // needed for the realtime publisher's thread for publishingLoop @@ -449,6 +456,21 @@ class TrajectoryControllerTest : public ::testing::Test } } + void updateControllerAsync(rclcpp::Duration wait_time = rclcpp::Duration::from_seconds(0.2)) + { + auto clock = rclcpp::Clock(RCL_STEADY_TIME); + const auto start_time = clock.now(); + const auto end_time = start_time + wait_time; + auto time_counter = start_time; + // set 10ms as update rate + auto update_rate = rclcpp::Duration::from_seconds(0.01); + while (time_counter <= end_time) + { + traj_controller_->update(time_counter, update_rate); + time_counter += update_rate; + } + } + void waitAndCompareState( trajectory_msgs::msg::JointTrajectoryPoint expected_actual, trajectory_msgs::msg::JointTrajectoryPoint expected_desired, rclcpp::Executor & executor, From 2d7ef0805076055a382832121b23a73bdd708267 Mon Sep 17 00:00:00 2001 From: Christoph Froehlich Date: Sun, 26 Nov 2023 13:33:03 +0000 Subject: [PATCH 03/14] Revert changes to state_topic_consistency test --- .../test/test_trajectory_controller.cpp | 51 ++++++++++--------- 1 file changed, 28 insertions(+), 23 deletions(-) diff --git a/joint_trajectory_controller/test/test_trajectory_controller.cpp b/joint_trajectory_controller/test/test_trajectory_controller.cpp index d808441866..2e122caa0a 100644 --- a/joint_trajectory_controller/test/test_trajectory_controller.cpp +++ b/joint_trajectory_controller/test/test_trajectory_controller.cpp @@ -316,6 +316,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; @@ -323,51 +328,51 @@ TEST_P(TrajectoryControllerTestParameterized, state_topic_consistency) subscribeToState(); updateController(); - // get state message from class variable - auto state_msg = traj_controller_->get_state_msg(); + // Spin to receive latest state + executor.spin_some(); + auto state = getState(); size_t n_joints = joint_names_.size(); for (unsigned int i = 0; i < n_joints; ++i) { - EXPECT_EQ(joint_names_[i], state_msg.joint_names[i]); + EXPECT_EQ(joint_names_[i], state->joint_names[i]); } // No trajectory by default, no reference state or error EXPECT_TRUE( - state_msg.reference.positions.empty() || state_msg.reference.positions == INITIAL_POS_JOINTS); + state->reference.positions.empty() || state->reference.positions == INITIAL_POS_JOINTS); EXPECT_TRUE( - state_msg.reference.velocities.empty() || state_msg.reference.velocities == INITIAL_VEL_JOINTS); + state->reference.velocities.empty() || state->reference.velocities == INITIAL_VEL_JOINTS); EXPECT_TRUE( - state_msg.reference.accelerations.empty() || - state_msg.reference.accelerations == INITIAL_EFF_JOINTS); + state->reference.accelerations.empty() || state->reference.accelerations == INITIAL_EFF_JOINTS); std::vector zeros(3, 0); - EXPECT_EQ(state_msg.error.positions, zeros); - EXPECT_TRUE(state_msg.error.velocities.empty() || state_msg.error.velocities == zeros); - EXPECT_TRUE(state_msg.error.accelerations.empty() || state_msg.error.accelerations == zeros); + EXPECT_EQ(state->error.positions, zeros); + EXPECT_TRUE(state->error.velocities.empty() || state->error.velocities == zeros); + EXPECT_TRUE(state->error.accelerations.empty() || state->error.accelerations == zeros); // expect feedback including all state_interfaces - EXPECT_EQ(n_joints, state_msg.feedback.positions.size()); + EXPECT_EQ(n_joints, state->feedback.positions.size()); if ( std::find(state_interface_types_.begin(), state_interface_types_.end(), "velocity") == state_interface_types_.end()) { - EXPECT_TRUE(state_msg.feedback.velocities.empty()); + EXPECT_TRUE(state->feedback.velocities.empty()); } else { - EXPECT_EQ(n_joints, state_msg.feedback.velocities.size()); + EXPECT_EQ(n_joints, state->feedback.velocities.size()); } if ( std::find(state_interface_types_.begin(), state_interface_types_.end(), "acceleration") == state_interface_types_.end()) { - EXPECT_TRUE(state_msg.feedback.accelerations.empty()); + EXPECT_TRUE(state->feedback.accelerations.empty()); } else { - EXPECT_EQ(n_joints, state_msg.feedback.accelerations.size()); + EXPECT_EQ(n_joints, state->feedback.accelerations.size()); } // expect output including all command_interfaces @@ -375,41 +380,41 @@ TEST_P(TrajectoryControllerTestParameterized, state_topic_consistency) std::find(command_interface_types_.begin(), command_interface_types_.end(), "position") == command_interface_types_.end()) { - EXPECT_TRUE(state_msg.output.positions.empty()); + EXPECT_TRUE(state->output.positions.empty()); } else { - EXPECT_EQ(n_joints, state_msg.output.positions.size()); + EXPECT_EQ(n_joints, state->output.positions.size()); } if ( std::find(command_interface_types_.begin(), command_interface_types_.end(), "velocity") == command_interface_types_.end()) { - EXPECT_TRUE(state_msg.output.velocities.empty()); + EXPECT_TRUE(state->output.velocities.empty()); } else { - EXPECT_EQ(n_joints, state_msg.output.velocities.size()); + EXPECT_EQ(n_joints, state->output.velocities.size()); } if ( std::find(command_interface_types_.begin(), command_interface_types_.end(), "acceleration") == command_interface_types_.end()) { - EXPECT_TRUE(state_msg.output.accelerations.empty()); + EXPECT_TRUE(state->output.accelerations.empty()); } else { - EXPECT_EQ(n_joints, state_msg.output.accelerations.size()); + EXPECT_EQ(n_joints, state->output.accelerations.size()); } if ( std::find(command_interface_types_.begin(), command_interface_types_.end(), "effort") == command_interface_types_.end()) { - EXPECT_TRUE(state_msg.output.effort.empty()); + EXPECT_TRUE(state->output.effort.empty()); } else { - EXPECT_EQ(n_joints, state_msg.output.effort.size()); + EXPECT_EQ(n_joints, state->output.effort.size()); } } From 9f22f85cbca735ca130b763066ef68d559ad18e8 Mon Sep 17 00:00:00 2001 From: Christoph Froehlich Date: Sun, 26 Nov 2023 14:13:59 +0000 Subject: [PATCH 04/14] Use async update where possible --- .../test/test_trajectory_controller.cpp | 65 ++++++++++--------- .../test/test_trajectory_controller_utils.hpp | 3 +- 2 files changed, 35 insertions(+), 33 deletions(-) diff --git a/joint_trajectory_controller/test/test_trajectory_controller.cpp b/joint_trajectory_controller/test/test_trajectory_controller.cpp index 2e122caa0a..f97bc96463 100644 --- a/joint_trajectory_controller/test/test_trajectory_controller.cpp +++ b/joint_trajectory_controller/test/test_trajectory_controller.cpp @@ -427,7 +427,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()) @@ -438,7 +438,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); @@ -463,7 +463,7 @@ TEST_P(TrajectoryControllerTestParameterized, update_dynamic_tolerances) SetUpAndActivateTrajectoryController(executor); - updateController(); + updateControllerAsync(); // test default parameters { @@ -491,7 +491,7 @@ TEST_P(TrajectoryControllerTestParameterized, update_dynamic_tolerances) { traj_controller_->get_node()->set_parameter(param); } - updateController(); + updateControllerAsync(); { auto tols = traj_controller_->get_tolerances(); @@ -518,7 +518,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()); @@ -536,7 +536,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 @@ -557,7 +557,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(); @@ -671,7 +670,6 @@ TEST_P(TrajectoryControllerTestParameterized, position_error_angle_wraparound) 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(); @@ -815,13 +813,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(); @@ -866,6 +864,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; @@ -873,7 +872,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); @@ -945,7 +943,6 @@ TEST_P(TrajectoryControllerTestParameterized, velocity_error) { rclcpp::executors::MultiThreadedExecutor executor; SetUpAndActivateTrajectoryController(executor, {}, true); - subscribeToState(); size_t n_joints = joint_names_.size(); @@ -962,7 +959,7 @@ TEST_P(TrajectoryControllerTestParameterized, velocity_error) traj_controller_->wait_for_trajectory(executor); // first update - updateController(rclcpp::Duration(FIRST_POINT_TIME)); + updateControllerAsync(rclcpp::Duration(FIRST_POINT_TIME)); // get states from class variables auto state_feedback = traj_controller_->get_state_feedback(); @@ -1048,6 +1045,7 @@ TEST_P(TrajectoryControllerTestParameterized, test_jumbled_joint_order) // 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. + // TODO(christophfroehlich): make the async update work here, e.g., add acceleration points? updateController(rclcpp::Duration::from_seconds(0.25 - 1e-3)); if (traj_controller_->has_position_command_interface()) @@ -1129,6 +1127,7 @@ TEST_P(TrajectoryControllerTestParameterized, test_partial_joint_list) } traj_controller_->wait_for_trajectory(executor); + // TODO(christophfroehlich): make the async update work here, e.g., add acceleration points? updateController(rclcpp::Duration::from_seconds(0.25)); double threshold = 0.001; @@ -1228,7 +1227,7 @@ 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)); + updateControllerAsync(rclcpp::Duration::from_seconds(0.25)); double threshold = 0.001; @@ -1590,7 +1589,7 @@ TEST_P(TrajectoryControllerTestParameterized, test_jump_when_state_tracking_erro 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()) { @@ -1612,20 +1611,20 @@ 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); - updateController(rclcpp::Duration::from_seconds(0.01)); + updateControllerAsync(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)); + updateControllerAsync(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)); + updateControllerAsync(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)); + updateControllerAsync(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 @@ -1639,20 +1638,20 @@ 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); - updateController(rclcpp::Duration::from_seconds(0.01)); + updateControllerAsync(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)); + updateControllerAsync(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)); + updateControllerAsync(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)); + updateControllerAsync(rclcpp::Duration::from_seconds(1.1)); EXPECT_NEAR(first_goal[0], joint_pos_[0], COMMON_THRESHOLD); } @@ -1688,7 +1687,7 @@ TEST_P(TrajectoryControllerTestParameterized, test_no_jump_when_state_tracking_e 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()) { @@ -1709,20 +1708,20 @@ 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); - updateController(rclcpp::Duration::from_seconds(0.01)); + updateControllerAsync(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)); + updateControllerAsync(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)); + updateControllerAsync(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)); + updateControllerAsync(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 @@ -1736,20 +1735,20 @@ 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); - updateController(rclcpp::Duration::from_seconds(0.01)); + updateControllerAsync(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)); + updateControllerAsync(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)); + updateControllerAsync(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)); + updateControllerAsync(rclcpp::Duration::from_seconds(1.1)); EXPECT_NEAR(first_goal[0], joint_pos_[0], COMMON_THRESHOLD); } @@ -1903,7 +1902,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_); @@ -1935,6 +1934,8 @@ 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); + // TODO(christophfroehlich) make this work with updateControllerAsync once + // https://github.com/ros-controls/ros2_controllers/pull/842 is merged updateController(rclcpp::Duration(4 * FIRST_POINT_TIME)); // it should have aborted and be holding now diff --git a/joint_trajectory_controller/test/test_trajectory_controller_utils.hpp b/joint_trajectory_controller/test/test_trajectory_controller_utils.hpp index 7bf4eebde4..85dadef3e5 100644 --- a/joint_trajectory_controller/test/test_trajectory_controller_utils.hpp +++ b/joint_trajectory_controller/test/test_trajectory_controller_utils.hpp @@ -425,7 +425,8 @@ class TrajectoryControllerTest : public ::testing::Test const auto end_time = start_time + wait_time; auto previous_time = start_time; - while (clock.now() <= end_time) + // it will end a time instant before end_time! + while (clock.now() < end_time) { auto now = clock.now(); traj_controller_->update(now, now - previous_time); From 9d4fcf0003ece1532b55d57d5ed00da3501cca64 Mon Sep 17 00:00:00 2001 From: Christoph Froehlich Date: Sun, 26 Nov 2023 14:29:04 +0000 Subject: [PATCH 05/14] Use async update for action tests --- .../test/test_trajectory_actions.cpp | 16 ++++++++-------- 1 file changed, 8 insertions(+), 8 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) From b9bf3ab3c29068ff9566561619b416c9baa826e3 Mon Sep 17 00:00:00 2001 From: Christoph Froehlich Date: Sun, 26 Nov 2023 14:29:41 +0000 Subject: [PATCH 06/14] Use class variables instead of state_msg subscriber --- .../test/test_trajectory_controller.cpp | 15 ++++++--------- .../test/test_trajectory_controller_utils.hpp | 14 +++++++------- 2 files changed, 13 insertions(+), 16 deletions(-) diff --git a/joint_trajectory_controller/test/test_trajectory_controller.cpp b/joint_trajectory_controller/test/test_trajectory_controller.cpp index f97bc96463..9d2c401a1a 100644 --- a/joint_trajectory_controller/test/test_trajectory_controller.cpp +++ b/joint_trajectory_controller/test/test_trajectory_controller.cpp @@ -1014,6 +1014,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{ @@ -1023,7 +1024,7 @@ 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]); @@ -1034,19 +1035,19 @@ TEST_P(TrajectoryControllerTestParameterized, test_jumbled_joint_order) // 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; + 2 * (traj_msg.points[0].positions[i] - joint_pos_[jumble_map[i]]) / 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, + // update just before dt seconds (shorter than the trajectory duration of dt 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. // TODO(christophfroehlich): make the async update work here, e.g., add acceleration points? - updateController(rclcpp::Duration::from_seconds(0.25 - 1e-3)); + updateController(rclcpp::Duration::from_seconds(dt - 1e-3)); if (traj_controller_->has_position_command_interface()) { @@ -1408,8 +1409,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}}; @@ -1425,6 +1424,7 @@ TEST_P(TrajectoryControllerTestParameterized, test_trajectory_replace) expected_desired.velocities = {points_old_velocities[0].begin(), points_old_velocities[0].end()}; // Check that we reached end of points_old trajectory // Denis: delta was 0.1 with 0.2 works for me + // TODO(christophfroehlich): change to 0.1 again waitAndCompareState(expected_actual, expected_desired, executor, rclcpp::Duration(delay), 0.2); RCLCPP_INFO(traj_controller_->get_node()->get_logger(), "Sending new trajectory"); @@ -1450,7 +1450,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.}}}; @@ -1480,7 +1479,6 @@ 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.}}}; @@ -1513,7 +1511,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(), diff --git a/joint_trajectory_controller/test/test_trajectory_controller_utils.hpp b/joint_trajectory_controller/test/test_trajectory_controller_utils.hpp index 85dadef3e5..b16cb4fc24 100644 --- a/joint_trajectory_controller/test/test_trajectory_controller_utils.hpp +++ b/joint_trajectory_controller/test/test_trajectory_controller_utils.hpp @@ -483,17 +483,18 @@ class TrajectoryControllerTest : public ::testing::Test } 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); + + // 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); } } @@ -503,8 +504,7 @@ 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); } } } From d97a21f41bcecea586755d6979ace59e178cdd1b Mon Sep 17 00:00:00 2001 From: Christoph Froehlich Date: Sun, 26 Nov 2023 16:18:09 +0000 Subject: [PATCH 07/14] Fix test_goal_tolerances_fail --- .../test/test_trajectory_controller.cpp | 9 ++++----- .../test/test_trajectory_controller_utils.hpp | 11 ++++++++--- 2 files changed, 12 insertions(+), 8 deletions(-) diff --git a/joint_trajectory_controller/test/test_trajectory_controller.cpp b/joint_trajectory_controller/test/test_trajectory_controller.cpp index 9d2c401a1a..97a454a61a 100644 --- a/joint_trajectory_controller/test/test_trajectory_controller.cpp +++ b/joint_trajectory_controller/test/test_trajectory_controller.cpp @@ -59,7 +59,7 @@ using test_trajectory_controllers::TrajectoryControllerTest; using test_trajectory_controllers::TrajectoryControllerTestParameterized; void spin(rclcpp::executors::MultiThreadedExecutor * exe) { exe->spin(); } - +#if 0 TEST_P(TrajectoryControllerTestParameterized, configure_state_ignores_commands) { rclcpp::executors::MultiThreadedExecutor executor; @@ -1905,6 +1905,7 @@ TEST_P(TrajectoryControllerTestParameterized, test_state_tolerances_fail) expectHoldingPoint(joint_state_pos_); } +#endif TEST_P(TrajectoryControllerTestParameterized, test_goal_tolerances_fail) { // set joint tolerance parameters @@ -1931,9 +1932,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); - // TODO(christophfroehlich) make this work with updateControllerAsync once - // https://github.com/ros-controls/ros2_controllers/pull/842 is merged - 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_); @@ -1941,7 +1940,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 b16cb4fc24..f05bcbedd0 100644 --- a/joint_trajectory_controller/test/test_trajectory_controller_utils.hpp +++ b/joint_trajectory_controller/test/test_trajectory_controller_utils.hpp @@ -457,10 +457,14 @@ class TrajectoryControllerTest : public ::testing::Test } } - void updateControllerAsync(rclcpp::Duration wait_time = rclcpp::Duration::from_seconds(0.2)) + rclcpp::Time updateControllerAsync( + rclcpp::Duration wait_time = rclcpp::Duration::from_seconds(0.2), + rclcpp::Time start_time = rclcpp::Time(0, 0, RCL_STEADY_TIME)) { - auto clock = rclcpp::Clock(RCL_STEADY_TIME); - const auto start_time = clock.now(); + 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; // set 10ms as update rate @@ -470,6 +474,7 @@ class TrajectoryControllerTest : public ::testing::Test traj_controller_->update(time_counter, update_rate); time_counter += update_rate; } + return end_time; } void waitAndCompareState( From 1846b64e8fc5e7d9a4d8cfb9c9c054c71764fd7d Mon Sep 17 00:00:00 2001 From: Christoph Froehlich Date: Sun, 26 Nov 2023 16:34:10 +0000 Subject: [PATCH 08/14] Use async update with waitAndCompareState --- .../test/test_trajectory_controller.cpp | 31 ++++++++++--------- .../test/test_trajectory_controller_utils.hpp | 9 ++++-- 2 files changed, 23 insertions(+), 17 deletions(-) diff --git a/joint_trajectory_controller/test/test_trajectory_controller.cpp b/joint_trajectory_controller/test/test_trajectory_controller.cpp index 97a454a61a..2fda0afa9d 100644 --- a/joint_trajectory_controller/test/test_trajectory_controller.cpp +++ b/joint_trajectory_controller/test/test_trajectory_controller.cpp @@ -59,7 +59,7 @@ using test_trajectory_controllers::TrajectoryControllerTest; using test_trajectory_controllers::TrajectoryControllerTestParameterized; void spin(rclcpp::executors::MultiThreadedExecutor * exe) { exe->spin(); } -#if 0 + TEST_P(TrajectoryControllerTestParameterized, configure_state_ignores_commands) { rclcpp::executors::MultiThreadedExecutor executor; @@ -1423,9 +1423,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 - // TODO(christophfroehlich): change to 0.1 again - 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 +1439,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); } /** @@ -1463,7 +1463,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 @@ -1472,7 +1473,8 @@ 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) @@ -1491,19 +1493,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) @@ -1535,7 +1538,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"); @@ -1548,7 +1552,7 @@ TEST_P(TrajectoryControllerTestParameterized, test_execute_partial_traj_in_futur expected_desired = expected_actual; waitAndCompareState( - expected_actual, expected_desired, executor, rclcpp::Duration(delay * (2 + 2)), 0.1); + expected_actual, expected_desired, executor, rclcpp::Duration(delay * (2 + 2)), 0.1, end_time); } // TODO(destogl) this test fails with errors @@ -1905,7 +1909,6 @@ TEST_P(TrajectoryControllerTestParameterized, test_state_tolerances_fail) expectHoldingPoint(joint_state_pos_); } -#endif TEST_P(TrajectoryControllerTestParameterized, test_goal_tolerances_fail) { // set joint tolerance parameters diff --git a/joint_trajectory_controller/test/test_trajectory_controller_utils.hpp b/joint_trajectory_controller/test/test_trajectory_controller_utils.hpp index f05bcbedd0..6b91a40278 100644 --- a/joint_trajectory_controller/test/test_trajectory_controller_utils.hpp +++ b/joint_trajectory_controller/test/test_trajectory_controller_utils.hpp @@ -477,17 +477,18 @@ class TrajectoryControllerTest : public ::testing::Test 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); + auto end_time = updateControllerAsync(controller_wait_time, start_time); // get states from class variables auto state_feedback = traj_controller_->get_state_feedback(); @@ -512,6 +513,8 @@ class TrajectoryControllerTest : public ::testing::Test EXPECT_NEAR(expected_desired.positions[i], state_reference.positions[i], allowed_delta); } } + + return end_time; } std::shared_ptr getState() const From f0cf8fc54b883f4060480ed73e16672e23422561 Mon Sep 17 00:00:00 2001 From: Christoph Froehlich Date: Sun, 26 Nov 2023 16:42:58 +0000 Subject: [PATCH 09/14] Cleanup utils file --- .../test/test_trajectory_controller_utils.hpp | 25 ------------------- 1 file changed, 25 deletions(-) diff --git a/joint_trajectory_controller/test/test_trajectory_controller_utils.hpp b/joint_trajectory_controller/test/test_trajectory_controller_utils.hpp index 6b91a40278..217488a561 100644 --- a/joint_trajectory_controller/test/test_trajectory_controller_utils.hpp +++ b/joint_trajectory_controller/test/test_trajectory_controller_utils.hpp @@ -156,8 +156,6 @@ class TestableJointTrajectoryController return state_publisher_->msg_; } - void unlock_publisher() { state_publisher_->unlock(); } - 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_; } @@ -434,29 +432,6 @@ class TrajectoryControllerTest : public ::testing::Test } } - void updateControllerAsyncPublisher( - rclcpp::Duration wait_time = rclcpp::Duration::from_seconds(0.2)) - { - auto clock = rclcpp::Clock(RCL_STEADY_TIME); - const auto start_time = clock.now(); - const auto end_time = start_time + wait_time; - auto time_counter = start_time; - // set 10ms as update rate - auto update_rate = rclcpp::Duration::from_seconds(0.01); - while (time_counter <= end_time) - { - std::cerr << "update " << time_counter.seconds() << std::endl; - - // ensure that try_lock has success in the update method - traj_controller_->unlock_publisher(); - // needed for the realtime publisher's thread for publishingLoop - std::this_thread::sleep_for(std::chrono::microseconds(500)); - - traj_controller_->update(time_counter, update_rate); - time_counter += update_rate; - } - } - rclcpp::Time updateControllerAsync( rclcpp::Duration wait_time = rclcpp::Duration::from_seconds(0.2), rclcpp::Time start_time = rclcpp::Time(0, 0, RCL_STEADY_TIME)) From 49235c0263d40dd5d40b57eeaf64842bdaf99292 Mon Sep 17 00:00:00 2001 From: Christoph Froehlich Date: Sun, 26 Nov 2023 17:10:48 +0000 Subject: [PATCH 10/14] Use async update for remaining tests --- .../test/test_trajectory_controller.cpp | 126 +++++++----------- .../test/test_trajectory_controller_utils.hpp | 2 +- 2 files changed, 52 insertions(+), 76 deletions(-) diff --git a/joint_trajectory_controller/test/test_trajectory_controller.cpp b/joint_trajectory_controller/test/test_trajectory_controller.cpp index 2fda0afa9d..ffc4b7b4c1 100644 --- a/joint_trajectory_controller/test/test_trajectory_controller.cpp +++ b/joint_trajectory_controller/test/test_trajectory_controller.cpp @@ -572,7 +572,6 @@ 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 updateControllerAsync(rclcpp::Duration(FIRST_POINT_TIME)); // get states from class variables @@ -685,7 +684,6 @@ TEST_P(TrajectoryControllerTestParameterized, position_error_angle_wraparound) publish(time_from_start, points, rclcpp::Time(), {}, points_velocities); traj_controller_->wait_for_trajectory(executor); - // first update updateControllerAsync(rclcpp::Duration(FIRST_POINT_TIME)); // get states from class variables @@ -835,7 +833,6 @@ 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); // get states from class variables @@ -958,7 +955,6 @@ TEST_P(TrajectoryControllerTestParameterized, velocity_error) publish(time_from_start, points_positions, rclcpp::Time(), {}, points_velocities); traj_controller_->wait_for_trajectory(executor); - // first update updateControllerAsync(rclcpp::Duration(FIRST_POINT_TIME)); // get states from class variables @@ -1030,24 +1026,21 @@ TEST_P(TrajectoryControllerTestParameterized, test_jumbled_joint_order) 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]]) / dt; + 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 dt seconds (shorter than the trajectory duration of dt 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. - // TODO(christophfroehlich): make the async update work here, e.g., add acceleration points? - updateController(rclcpp::Duration::from_seconds(dt - 1e-3)); + updateControllerAsync(rclcpp::Duration::from_seconds(dt)); if (traj_controller_->has_position_command_interface()) { @@ -1068,19 +1061,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,38 +1089,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); - // TODO(christophfroehlich): make the async update work here, e.g., add acceleration points? - 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"; } @@ -1149,7 +1136,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"; } @@ -1157,24 +1144,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"; } @@ -1186,7 +1164,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"; } @@ -1230,45 +1208,43 @@ TEST_P(TrajectoryControllerTestParameterized, test_partial_joint_list_not_allowe // update for 0.5 seconds updateControllerAsync(rclcpp::Duration::from_seconds(0.25)); - double threshold = 0.001; - 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"; } diff --git a/joint_trajectory_controller/test/test_trajectory_controller_utils.hpp b/joint_trajectory_controller/test/test_trajectory_controller_utils.hpp index 217488a561..12597ab735 100644 --- a/joint_trajectory_controller/test/test_trajectory_controller_utils.hpp +++ b/joint_trajectory_controller/test/test_trajectory_controller_utils.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; From c1a48dfd441fe65706f8a2b6fc4f448ab3fc5e46 Mon Sep 17 00:00:00 2001 From: Christoph Froehlich Date: Sun, 26 Nov 2023 17:50:32 +0000 Subject: [PATCH 11/14] Use sleep_for in the updateController method --- .../test/test_trajectory_controller_utils.hpp | 13 ++++--------- 1 file changed, 4 insertions(+), 9 deletions(-) diff --git a/joint_trajectory_controller/test/test_trajectory_controller_utils.hpp b/joint_trajectory_controller/test/test_trajectory_controller_utils.hpp index 12597ab735..86570adae6 100644 --- a/joint_trajectory_controller/test/test_trajectory_controller_utils.hpp +++ b/joint_trajectory_controller/test/test_trajectory_controller_utils.hpp @@ -39,6 +39,8 @@ const std::vector INITIAL_VEL_JOINTS = {0.0, 0.0, 0.0}; const std::vector INITIAL_ACC_JOINTS = {0.0, 0.0, 0.0}; const std::vector INITIAL_EFF_JOINTS = {0.0, 0.0, 0.0}; +const auto update_rate = rclcpp::Duration::from_seconds(0.01); + bool is_same_sign_or_zero(double val1, double val2) { return val1 * val2 > 0.0 || (val1 == 0.0 && val2 == 0.0); @@ -151,11 +153,6 @@ class TestableJointTrajectoryController double get_cmd_timeout() { return cmd_timeout_; } - joint_trajectory_controller::JointTrajectoryController::ControllerStateMsg get_state_msg() - { - return state_publisher_->msg_; - } - 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_; } @@ -423,12 +420,12 @@ class TrajectoryControllerTest : public ::testing::Test const auto end_time = start_time + wait_time; auto previous_time = start_time; - // it will end a time instant before end_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()); } } @@ -442,8 +439,6 @@ class TrajectoryControllerTest : public ::testing::Test } const auto end_time = start_time + wait_time; auto time_counter = start_time; - // set 10ms as update rate - auto update_rate = rclcpp::Duration::from_seconds(0.01); while (time_counter <= end_time) { traj_controller_->update(time_counter, update_rate); From 7dbe31449e75c4f9234fcfa9dab8feddf989290f Mon Sep 17 00:00:00 2001 From: Christoph Froehlich Date: Sun, 26 Nov 2023 18:00:09 +0000 Subject: [PATCH 12/14] Add description for update functions --- .../test/test_trajectory_controller_utils.hpp | 27 ++++++++++++++++--- 1 file changed, 23 insertions(+), 4 deletions(-) diff --git a/joint_trajectory_controller/test/test_trajectory_controller_utils.hpp b/joint_trajectory_controller/test/test_trajectory_controller_utils.hpp index 86570adae6..c3d7b75f7e 100644 --- a/joint_trajectory_controller/test/test_trajectory_controller_utils.hpp +++ b/joint_trajectory_controller/test/test_trajectory_controller_utils.hpp @@ -39,8 +39,6 @@ const std::vector INITIAL_VEL_JOINTS = {0.0, 0.0, 0.0}; const std::vector INITIAL_ACC_JOINTS = {0.0, 0.0, 0.0}; const std::vector INITIAL_EFF_JOINTS = {0.0, 0.0, 0.0}; -const auto update_rate = rclcpp::Duration::from_seconds(0.01); - bool is_same_sign_or_zero(double val1, double val2) { return val1 * val2 > 0.0 || (val1 == 0.0 && val2 == 0.0); @@ -413,7 +411,17 @@ 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(); @@ -429,9 +437,20 @@ class TrajectoryControllerTest : public ::testing::Test } } + /** + * @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)) + 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)) { From 10d6aef7df61aff487f3420a82007dcc57f4243f Mon Sep 17 00:00:00 2001 From: Christoph Froehlich Date: Mon, 27 Nov 2023 09:29:21 +0000 Subject: [PATCH 13/14] Reactivate jump tests --- .../test/test_trajectory_controller.cpp | 305 +++++++++--------- .../test/test_trajectory_controller_utils.hpp | 27 +- 2 files changed, 165 insertions(+), 167 deletions(-) diff --git a/joint_trajectory_controller/test/test_trajectory_controller.cpp b/joint_trajectory_controller/test/test_trajectory_controller.cpp index ffc4b7b4c1..7265cbda9a 100644 --- a/joint_trajectory_controller/test/test_trajectory_controller.cpp +++ b/joint_trajectory_controller/test/test_trajectory_controller.cpp @@ -1531,25 +1531,18 @@ TEST_P(TrajectoryControllerTestParameterized, test_execute_partial_traj_in_futur 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}; @@ -1562,95 +1555,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); 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); - updateControllerAsync(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]); - updateControllerAsync(rclcpp::Duration::from_seconds(0.01)); - EXPECT_GT(joint_pos_[0], joint_state_pos_[0]); - EXPECT_LT(joint_pos_[0], first_goal[0]); - updateControllerAsync(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 - updateControllerAsync(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); - updateControllerAsync(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]); - updateControllerAsync(rclcpp::Duration::from_seconds(0.01)); - EXPECT_LT(joint_pos_[0], joint_state_pos_[0]); - EXPECT_GT(joint_pos_[0], first_goal[0]); - updateControllerAsync(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 - updateControllerAsync(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}; @@ -1661,77 +1654,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); 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); - updateControllerAsync(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]); - updateControllerAsync(rclcpp::Duration::from_seconds(0.01)); - EXPECT_GT(joint_pos_[0], first_goal[0]); - EXPECT_LT(joint_pos_[0], second_goal[0]); - updateControllerAsync(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 - updateControllerAsync(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); - updateControllerAsync(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]); - updateControllerAsync(rclcpp::Duration::from_seconds(0.01)); - EXPECT_GT(joint_pos_[0], first_goal[0]); - EXPECT_LT(joint_pos_[0], second_goal[0]); - updateControllerAsync(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 - updateControllerAsync(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 diff --git a/joint_trajectory_controller/test/test_trajectory_controller_utils.hpp b/joint_trajectory_controller/test/test_trajectory_controller_utils.hpp index c3d7b75f7e..8d614a10b7 100644 --- a/joint_trajectory_controller/test/test_trajectory_controller_utils.hpp +++ b/joint_trajectory_controller/test/test_trajectory_controller_utils.hpp @@ -237,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( From 6b992289f133ec2e223e4841aafde454299e740f Mon Sep 17 00:00:00 2001 From: Christoph Froehlich Date: Wed, 15 Nov 2023 21:21:08 +0000 Subject: [PATCH 14/14] Create a function to reuse for tests --- .../test/test_trajectory_controller.cpp | 65 +------------------ .../test/test_trajectory_controller_utils.hpp | 43 +++++++++++- 2 files changed, 45 insertions(+), 63 deletions(-) diff --git a/joint_trajectory_controller/test/test_trajectory_controller.cpp b/joint_trajectory_controller/test/test_trajectory_controller.cpp index 7265cbda9a..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) diff --git a/joint_trajectory_controller/test/test_trajectory_controller_utils.hpp b/joint_trajectory_controller/test/test_trajectory_controller_utils.hpp index 8d614a10b7..3c8e252067 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" @@ -623,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_;