From e0d9c0a8de5e3db45f9757985bbbf5948934ee93 Mon Sep 17 00:00:00 2001 From: Jaron Lundwall <13423952+jaron-l@users.noreply.github.com> Date: Tue, 15 Nov 2022 14:31:00 +0000 Subject: [PATCH 1/2] test: :white_check_mark: update joint_trajectory_controller_tests to be faster - make updateController not require sleeps --- .../src/joint_trajectory_controller.cpp | 10 ++--- .../test/test_trajectory_controller.cpp | 13 ++---- .../test/test_trajectory_controller_utils.hpp | 44 +++++++++++++++---- 3 files changed, 44 insertions(+), 23 deletions(-) diff --git a/joint_trajectory_controller/src/joint_trajectory_controller.cpp b/joint_trajectory_controller/src/joint_trajectory_controller.cpp index f5f5190caa..e8bb3c6d82 100644 --- a/joint_trajectory_controller/src/joint_trajectory_controller.cpp +++ b/joint_trajectory_controller/src/joint_trajectory_controller.cpp @@ -974,12 +974,10 @@ void JointTrajectoryController::publish_state( const JointTrajectoryPoint & desired_state, const JointTrajectoryPoint & current_state, const JointTrajectoryPoint & state_error) { - if (state_publisher_period_.seconds() <= 0.0) - { - return; - } - - if (get_node()->now() < (last_state_publish_time_ + state_publisher_period_)) + if ( + state_publisher_period_.seconds() > 0 && + get_node()->now() < (last_state_publish_time_ + state_publisher_period_) + ) { return; } diff --git a/joint_trajectory_controller/test/test_trajectory_controller.cpp b/joint_trajectory_controller/test/test_trajectory_controller.cpp index e758dc038f..02f5c2cab4 100644 --- a/joint_trajectory_controller/test/test_trajectory_controller.cpp +++ b/joint_trajectory_controller/test/test_trajectory_controller.cpp @@ -439,13 +439,13 @@ TEST_P(TrajectoryControllerTestParameterized, correct_initialization_using_param TEST_P(TrajectoryControllerTestParameterized, state_topic_consistency) { rclcpp::executors::SingleThreadedExecutor executor; - SetUpAndActivateTrajectoryController(true, {}, &executor); + SetUpAndActivateTrajectoryController(true, {rclcpp::Parameter("state_publish_rate", 0.0)}, &executor); subscribeToState(); + ASSERT_FALSE(waitForSubscriberToBeReady()) << "subscriber was never ready"; updateController(); // Spin to receive latest state - executor.spin_some(); - auto state = getState(); + auto state = getState(executor); size_t n_joints = joint_names_.size(); @@ -525,16 +525,11 @@ void TrajectoryControllerTest::test_state_publish_rate_target(int target_msg_cou /** * @brief test_state_publish_rate Test that state publish rate matches configure rate */ -TEST_P(TrajectoryControllerTestParameterized, test_state_publish_rate) +TEST_F(TrajectoryControllerTest, test_state_publish_rate) { test_state_publish_rate_target(10); } -TEST_P(TrajectoryControllerTestParameterized, zero_state_publish_rate) -{ - test_state_publish_rate_target(0); -} - // /** // * @brief test_jumbled_joint_order Test sending trajectories with a joint order different from // * internal controller order diff --git a/joint_trajectory_controller/test/test_trajectory_controller_utils.hpp b/joint_trajectory_controller/test/test_trajectory_controller_utils.hpp index 840a624eca..dcaaa4588d 100644 --- a/joint_trajectory_controller/test/test_trajectory_controller_utils.hpp +++ b/joint_trajectory_controller/test/test_trajectory_controller_utils.hpp @@ -278,6 +278,7 @@ class TrajectoryControllerTest : public ::testing::Test std::lock_guard guard(state_mutex_); state_msg_ = msg; }); + state_sub_wait_set_.add_subscription(state_subscriber_); } /// Publish trajectory msgs with multiple points @@ -349,12 +350,30 @@ class TrajectoryControllerTest : public ::testing::Test void updateController(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; - while (clock.now() < end_time) + + rclcpp::Time now_time = rclcpp::Clock(RCL_STEADY_TIME).now(); + traj_controller_->update(now_time, wait_time); + traj_controller_->update(now_time + wait_time, wait_time); + } + + /* Returns true if timed out, else returns false */ + bool waitForSubscriberToBeReady(rclcpp::Duration timeout = rclcpp::Duration::from_seconds(.2)) + { + // mark start_time for determining timeout + rclcpp::Clock clock = rclcpp::Clock(RCL_STEADY_TIME); + rclcpp::Time start_time = clock.now(); + // sleep until subscriber has a publisher or timeout + while( + clock.now() < start_time + timeout && + state_subscriber_->get_publisher_count() < 1 + ) { - traj_controller_->update(clock.now(), clock.now() - start_time); + rclcpp::sleep_for(std::chrono::milliseconds(100)); + } + if (clock.now() > start_time + timeout) { + return true; + } else { + return false; } } @@ -370,8 +389,7 @@ 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(); + auto state_msg = getState(executor); ASSERT_TRUE(state_msg); for (size_t i = 0; i < expected_actual.positions.size(); ++i) { @@ -394,9 +412,18 @@ class TrajectoryControllerTest : public ::testing::Test } } - std::shared_ptr getState() const + std::shared_ptr getState( + rclcpp::Executor & executor + ) { + executor.spin_some(); + std::chrono::milliseconds timeout(500); + if (state_sub_wait_set_.wait(timeout).kind() == rclcpp::WaitResultKind::Ready) + { + executor.spin_some(); + } std::lock_guard guard(state_mutex_); + EXPECT_NE(state_msg_, nullptr) << "Warning: state message is null"; return state_msg_; } void test_state_publish_rate_target(int target_msg_count); @@ -416,6 +443,7 @@ class TrajectoryControllerTest : public ::testing::Test state_subscriber_; mutable std::mutex state_mutex_; std::shared_ptr state_msg_; + rclcpp::WaitSet state_sub_wait_set_; std::vector joint_pos_; std::vector joint_vel_; From b069b53d2a8053979e77eb276d9d2d9219f4f226 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Denis=20=C5=A0togl?= Date: Wed, 30 Nov 2022 14:35:31 +0100 Subject: [PATCH 2/2] Update joint_trajectory_controller/test/test_trajectory_controller_utils.hpp --- .../test/test_trajectory_controller_utils.hpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/joint_trajectory_controller/test/test_trajectory_controller_utils.hpp b/joint_trajectory_controller/test/test_trajectory_controller_utils.hpp index 9a8f74f0a0..7e16a58566 100644 --- a/joint_trajectory_controller/test/test_trajectory_controller_utils.hpp +++ b/joint_trajectory_controller/test/test_trajectory_controller_utils.hpp @@ -363,7 +363,7 @@ class TrajectoryControllerTest : public ::testing::Test } /* Returns true if timed out, else returns false */ - bool waitForSubscriberToBeReady(rclcpp::Duration timeout = rclcpp::Duration::from_seconds(.2)) + bool waitForSubscriberToBeReady(rclcpp::Duration timeout = rclcpp::Duration::from_seconds(0.2)) { // mark start_time for determining timeout rclcpp::Clock clock = rclcpp::Clock(RCL_STEADY_TIME);