@@ -294,7 +294,12 @@ class PubSubNode : public rclcpp::Node
294
294
const auto max_lat_acc_lim = *std::max_element (lat_acc_lim.begin (), lat_acc_lim.end ());
295
295
const auto max_lat_jerk_lim = *std::max_element (lat_jerk_lim.begin (), lat_jerk_lim.end ());
296
296
297
+ // This test is designed to verify that the filter is applied correctly. However, if topic
298
+ // communication is delayed, the allowable range of change in the command values between the
299
+ // sender and receiver of the topic will vary, making the results dependent on processing time.
300
+ // We define the allowable error margin to account for this.
297
301
constexpr auto threshold_scale = 1.1 ;
302
+
298
303
// Output command must be smaller than maximum limit.
299
304
// TODO(Horibe): check for each velocity range.
300
305
if (std::abs (lon_vel) > 0.01 ) {
@@ -433,7 +438,7 @@ TEST_P(TestFixture, CheckFilterForSinCmd)
433
438
// << cmd_generator_.p_.steering.freq << " * dt + " << cmd_generator_.p_.steering.bias
434
439
// << ")" << std::endl;
435
440
436
- for (size_t i = 0 ; i < 100 ; ++i) {
441
+ for (size_t i = 0 ; i < 30 ; ++i) {
437
442
auto start_time = std::chrono::steady_clock::now ();
438
443
439
444
const bool reset_clock = (i == 0 );
@@ -449,7 +454,13 @@ TEST_P(TestFixture, CheckFilterForSinCmd)
449
454
std::chrono::milliseconds elapsed =
450
455
std::chrono::duration_cast<std::chrono::milliseconds>(end_time - start_time);
451
456
452
- std::chrono::milliseconds sleep_duration = std::chrono::milliseconds{10 } - elapsed;
457
+ // The value determines the period of the topic. The filter logic of vehicle_cmd_gate depends on
458
+ // real-time, and if the time from publishing to subscribing becomes too long, this test will
459
+ // fail (the test specification itself should be improved). To prevent processing bottlenecks,
460
+ // please set this value appropriately. It is set to 30ms because it occasionally fails at 10ms.
461
+ constexpr int running_ms = 30 ;
462
+
463
+ std::chrono::milliseconds sleep_duration = std::chrono::milliseconds{running_ms} - elapsed;
453
464
if (sleep_duration.count () > 0 ) {
454
465
std::this_thread::sleep_for (sleep_duration);
455
466
}
0 commit comments