Use a "steady clock" when measuring time differences (#427)

* Use "steady" clocks when measuring changes in time * Check if the "publish rate" tests pass now
ros-controls · Sep 1, 2022 · fe89a54 · fe89a54
1 parent 5e6ef29
commit fe89a54
Show file tree

Hide file tree

Showing 4 changed files with 50 additions and 48 deletions.
diff --git a/joint_trajectory_controller/test/test_trajectory.cpp b/joint_trajectory_controller/test/test_trajectory.cpp
@@ -40,6 +40,7 @@ const double EPS = 1e-8;
 
 TEST(TestTrajectory, initialize_trajectory)
 {
+  auto clock = rclcpp::Clock(RCL_STEADY_TIME);
   {
     auto empty_msg = std::make_shared<trajectory_msgs::msg::JointTrajectory>();
     empty_msg->header.stamp.sec = 2;
@@ -49,7 +50,7 @@ TEST(TestTrajectory, initialize_trajectory)
 
     trajectory_msgs::msg::JointTrajectoryPoint expected_point;
     joint_trajectory_controller::TrajectoryPointConstIter start, end;
-    traj.sample(rclcpp::Clock().now(), DEFAULT_INTERPOLATION, expected_point, start, end);
+    traj.sample(clock.now(), DEFAULT_INTERPOLATION, expected_point, start, end);
 
     EXPECT_EQ(traj.end(), start);
     EXPECT_EQ(traj.end(), end);
@@ -59,13 +60,13 @@ TEST(TestTrajectory, initialize_trajectory)
     auto empty_msg = std::make_shared<trajectory_msgs::msg::JointTrajectory>();
     empty_msg->header.stamp.sec = 0;
     empty_msg->header.stamp.nanosec = 0;
-    const auto now = rclcpp::Clock().now();
+    const auto now = clock.now();
     auto traj = joint_trajectory_controller::Trajectory(empty_msg);
     const auto traj_starttime = traj.time_from_start();
 
     trajectory_msgs::msg::JointTrajectoryPoint expected_point;
     joint_trajectory_controller::TrajectoryPointConstIter start, end;
-    traj.sample(rclcpp::Clock().now(), DEFAULT_INTERPOLATION, expected_point, start, end);
+    traj.sample(clock.now(), DEFAULT_INTERPOLATION, expected_point, start, end);
 
     EXPECT_EQ(traj.end(), start);
     EXPECT_EQ(traj.end(), end);

diff --git a/joint_trajectory_controller/test/test_trajectory_actions.cpp b/joint_trajectory_controller/test/test_trajectory_actions.cpp
@@ -91,12 +91,13 @@ class TestTrajectoryActions : public TrajectoryControllerTest
       [&]()
       {
         // controller hardware cycle update loop
-        auto start_time = rclcpp::Clock().now();
+        auto clock = rclcpp::Clock(RCL_STEADY_TIME);
+        auto start_time = clock.now();
         rclcpp::Duration wait = rclcpp::Duration::from_seconds(2.0);
         auto end_time = start_time + wait;
-        while (rclcpp::Clock().now() < end_time)
+        while (clock.now() < end_time)
         {
-          traj_controller_->update(rclcpp::Clock().now(), rclcpp::Clock().now() - start_time);
+          traj_controller_->update(clock.now(), clock.now() - start_time);
         }
       });
 

diff --git a/joint_trajectory_controller/test/test_trajectory_controller.cpp b/joint_trajectory_controller/test/test_trajectory_controller.cpp
@@ -343,7 +343,6 @@ TEST_P(TrajectoryControllerTestParameterized, cleanup)
   state = traj_controller_->get_node()->cleanup();
   ASSERT_EQ(State::PRIMARY_STATE_UNCONFIGURED, state.id());
   // update for 0.25 seconds
-  const auto start_time = rclcpp::Clock().now();
   updateController(rclcpp::Duration::from_seconds(0.25));
 
   // should be home pose again
@@ -489,52 +488,52 @@ TEST_P(TrajectoryControllerTestParameterized, state_topic_consistency)
   EXPECT_TRUE(state->error.accelerations.empty() || state->error.accelerations == zeros);
 }
 
-// void TrajectoryControllerTest::test_state_publish_rate_target(int target_msg_count)
-// {
-//   rclcpp::Parameter state_publish_rate_param(
-//     "state_publish_rate", static_cast<double>(target_msg_count));
-//   rclcpp::executors::SingleThreadedExecutor executor;
-//   SetUpAndActivateTrajectoryController(true, {state_publish_rate_param}, &executor);
+void TrajectoryControllerTest::test_state_publish_rate_target(int target_msg_count)
+{
+  rclcpp::Parameter state_publish_rate_param(
+    "state_publish_rate", static_cast<double>(target_msg_count));
+  rclcpp::executors::SingleThreadedExecutor executor;
+  SetUpAndActivateTrajectoryController(true, {state_publish_rate_param}, &executor);
 
-//   auto future_handle = std::async(
-//     std::launch::async, [&executor]() -> void { executor.spin(); });
+  auto future_handle = std::async(std::launch::async, [&executor]() -> void { executor.spin(); });
 
-//   using control_msgs::msg::JointTrajectoryControllerState;
+  using control_msgs::msg::JointTrajectoryControllerState;
 
-//   const int qos_level = 10;
-//   int echo_received_counter = 0;
-//   rclcpp::Subscription<JointTrajectoryControllerState>::SharedPtr subs =
-//     traj_controller_->get_node()->create_subscription<JointTrajectoryControllerState>(
-//       controller_name_ + "/state", qos_level,
-//       [&](JointTrajectoryControllerState::UniquePtr) { ++echo_received_counter; });
+  const int qos_level = 10;
+  int echo_received_counter = 0;
+  rclcpp::Subscription<JointTrajectoryControllerState>::SharedPtr subs =
+    traj_controller_->get_node()->create_subscription<JointTrajectoryControllerState>(
+      controller_name_ + "/state", qos_level,
+      [&](JointTrajectoryControllerState::UniquePtr) { ++echo_received_counter; });
 
-//   // update for 1second
-//   const auto start_time = rclcpp::Clock().now();
-//   const rclcpp::Duration wait = rclcpp::Duration::from_seconds(1.0);
-//   const auto end_time = start_time + wait;
-//   while (rclcpp::Clock().now() < end_time)
-//   {
-//     traj_controller_->update(rclcpp::Time(0), rclcpp::Duration::from_seconds(0.01));
-//   }
+  // update for 1second
+  auto clock = rclcpp::Clock(RCL_STEADY_TIME);
+  const auto start_time = clock.now();
+  const rclcpp::Duration wait = rclcpp::Duration::from_seconds(1.0);
+  const auto end_time = start_time + wait;
+  while (clock.now() < end_time)
+  {
+    traj_controller_->update(rclcpp::Time(0), rclcpp::Duration::from_seconds(0.01));
+  }
 
-//   // We may miss the last message since time allowed is exactly the time needed
-//   EXPECT_NEAR(target_msg_count, echo_received_counter, 1);
+  // We may miss the last message since time allowed is exactly the time needed
+  EXPECT_NEAR(target_msg_count, echo_received_counter, 1);
 
-//   executor.cancel();
-// }
+  executor.cancel();
+}
 
-// /**
-//  * @brief test_state_publish_rate Test that state publish rate matches configure rate
-//  */
-// TEST_P(TrajectoryControllerTestParameterized, test_state_publish_rate)
-// {
-//   test_state_publish_rate_target(10);
-// }
+/**
+ * @brief test_state_publish_rate Test that state publish rate matches configure rate
+ */
+TEST_P(TrajectoryControllerTestParameterized, test_state_publish_rate)
+{
+  test_state_publish_rate_target(10);
+}
 
-// TEST_P(TrajectoryControllerTestParameterized, zero_state_publish_rate)
-// {
-//   test_state_publish_rate_target(0);
-// }
+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

diff --git a/joint_trajectory_controller/test/test_trajectory_controller_utils.hpp b/joint_trajectory_controller/test/test_trajectory_controller_utils.hpp
@@ -349,11 +349,12 @@ class TrajectoryControllerTest : public ::testing::Test
 
   void updateController(rclcpp::Duration wait_time = rclcpp::Duration::from_seconds(0.2))
   {
-    const auto start_time = rclcpp::Clock().now();
+    auto clock = rclcpp::Clock(RCL_STEADY_TIME);
+    const auto start_time = clock.now();
     const auto end_time = start_time + wait_time;
-    while (rclcpp::Clock().now() < end_time)
+    while (clock.now() < end_time)
     {
-      traj_controller_->update(rclcpp::Clock().now(), rclcpp::Clock().now() - start_time);
+      traj_controller_->update(clock.now(), clock.now() - start_time);
     }
   }