test: ✅ improve speed of joint_trajectory_controller tests

joint_trajectory_controller/src/joint_trajectory_controller.cpp

@@ -886,12 +886,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;
   }

joint_trajectory_controller/test/test_trajectory_controller.cpp

@@ -434,13 +434,13 @@ TEST_P(TrajectoryControllerTestParameterized, correct_initialization_using_param
 TEST_P(TrajectoryControllerTestParameterized, state_topic_consistency)
 {
   rclcpp::executors::SingleThreadedExecutor executor;
-  SetUpAndActivateTrajectoryController(executor, true, {});
+  SetUpAndActivateTrajectoryController(executor, true, {rclcpp::Parameter("state_publish_rate", 0.0)});
   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();
 
@@ -520,16 +520,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

joint_trajectory_controller/test/test_trajectory_controller_utils.hpp

@@ -284,6 +284,7 @@ class TrajectoryControllerTest : public ::testing::Test
         std::lock_guard<std::mutex> guard(state_mutex_);
         state_msg_ = msg;
       });
+    state_sub_wait_set_.add_subscription(state_subscriber_);
   }
 
   /// Publish trajectory msgs with multiple points
@@ -355,12 +356,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;
     }
   }
 
@@ -376,8 +395,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)
     {
@@ -400,9 +418,18 @@ class TrajectoryControllerTest : public ::testing::Test
     }
   }
 
-  std::shared_ptr<control_msgs::msg::JointTrajectoryControllerState> getState() const
+  std::shared_ptr<control_msgs::msg::JointTrajectoryControllerState> 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<std::mutex> 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);
@@ -422,6 +449,7 @@ class TrajectoryControllerTest : public ::testing::Test
     state_subscriber_;
   mutable std::mutex state_mutex_;
   std::shared_ptr<control_msgs::msg::JointTrajectoryControllerState> state_msg_;
+  rclcpp::WaitSet state_sub_wait_set_;
 
   std::vector<double> joint_pos_;
   std::vector<double> joint_vel_;