From 38db8e9a29ed795d01909b07ba833d7983cac219 Mon Sep 17 00:00:00 2001 From: Takayuki Murooka Date: Thu, 21 Mar 2024 13:20:41 +0900 Subject: [PATCH] fix(vehicle_cmd_gate): fix publisher HZ in the unit test Signed-off-by: Takayuki Murooka --- .../src/test_filter_in_vehicle_cmd_gate_node.cpp | 14 ++++++++++++-- 1 file changed, 12 insertions(+), 2 deletions(-) diff --git a/control/vehicle_cmd_gate/test/src/test_filter_in_vehicle_cmd_gate_node.cpp b/control/vehicle_cmd_gate/test/src/test_filter_in_vehicle_cmd_gate_node.cpp index f9f3087a6b69c..8e35600314fe2 100644 --- a/control/vehicle_cmd_gate/test/src/test_filter_in_vehicle_cmd_gate_node.cpp +++ b/control/vehicle_cmd_gate/test/src/test_filter_in_vehicle_cmd_gate_node.cpp @@ -388,15 +388,25 @@ TEST_P(TestFixture, CheckFilterForSinCmd) // << ")" << std::endl; for (size_t i = 0; i < 100; ++i) { + auto start_time = std::chrono::steady_clock::now(); + const bool reset_clock = (i == 0); const auto cmd = cmd_generator_.calcSinWaveCommand(reset_clock); pub_sub_node_.publishControlCommand(cmd); pub_sub_node_.publishDefaultTopicsNoSpin(); - for (int i = 0; i < 20; ++i) { + for (int j = 0; j < 20; ++j) { rclcpp::spin_some(pub_sub_node_.get_node_base_interface()); rclcpp::spin_some(vehicle_cmd_gate_node_->get_node_base_interface()); } - std::this_thread::sleep_for(std::chrono::milliseconds{10LL}); + + auto end_time = std::chrono::steady_clock::now(); + std::chrono::milliseconds elapsed = + std::chrono::duration_cast(end_time - start_time); + + std::chrono::milliseconds sleep_duration = std::chrono::milliseconds{10} - elapsed; + if (sleep_duration.count() > 0) { + std::this_thread::sleep_for(sleep_duration); + } } std::cerr << "received cmd num = " << pub_sub_node_.cmd_received_times_.size() << std::endl;