From 90d982f643897fec6916d5c0672ba1f994b041fa Mon Sep 17 00:00:00 2001 From: gwalck Date: Fri, 7 Jul 2023 13:00:08 +0200 Subject: [PATCH] Fixed update period computation in test (#693) (cherry picked from commit de7083e5d9178e24e24efae411cb2f228fdd09c3) --- .../test/test_trajectory_controller_utils.hpp | 6 +++++- 1 file changed, 5 insertions(+), 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 0c01eb1357..b452c30065 100644 --- a/joint_trajectory_controller/test/test_trajectory_controller_utils.hpp +++ b/joint_trajectory_controller/test/test_trajectory_controller_utils.hpp @@ -386,9 +386,13 @@ class TrajectoryControllerTest : public ::testing::Test auto clock = rclcpp::Clock(RCL_STEADY_TIME); const auto start_time = clock.now(); const auto end_time = start_time + wait_time; + auto previous_time = start_time; + while (clock.now() < end_time) { - traj_controller_->update(clock.now(), clock.now() - start_time); + auto now = clock.now(); + traj_controller_->update(now, now - previous_time); + previous_time = now; } }