fix: 🐛 make force_torque_sensor_broadcaster wait for realtime_publisher (ros-controls#327)

* fix: 🐛 make force_torque_sensor_broadcaster wait for realtime_publisher loop to be ready before finishing configure
* test: ✅ check for published message in test_force_torque_sensor_broadcaster
- create a loop that checks if force_torque_sensor_broadcaster published before checking subscribed message
* refactor: ♻️ remove unnecessary includes
* test: ✅ port realtime_publisher check to imu_sensor_broadcaster and joint_state_broadcaster
* refactor: ♻️ remove unnecessary wait_for code from tests
Co-authored-by: Denis Štogl <denis@stogl.de>

Author: jaron-l

force_torque_sensor_broadcaster/test/test_force_torque_sensor_broadcaster.cpp

@@ -37,15 +37,6 @@ namespace
 {
 constexpr auto NODE_SUCCESS = controller_interface::CallbackReturn::SUCCESS;
 constexpr auto NODE_ERROR = controller_interface::CallbackReturn::ERROR;
-
-rclcpp::WaitResultKind wait_for(rclcpp::SubscriptionBase::SharedPtr subscription)
-{
-  rclcpp::WaitSet wait_set;
-  wait_set.add_subscription(subscription);
-  const auto timeout = std::chrono::seconds(20);
-  return wait_set.wait(timeout).kind();
-}
-
 }  // namespace
 
 void ForceTorqueSensorBroadcasterTest::SetUpTestCase() { rclcpp::init(0, nullptr); }
@@ -86,12 +77,21 @@ void ForceTorqueSensorBroadcasterTest::subscribe_and_get_message(
     "/test_force_torque_sensor_broadcaster/wrench", 10, subs_callback);
 
   // call update to publish the test value
-  ASSERT_EQ(
-    fts_broadcaster_->update(rclcpp::Time(0), rclcpp::Duration::from_seconds(0.01)),
-    controller_interface::return_type::OK);
-
-  // wait for message to be passed
-  ASSERT_EQ(wait_for(subscription), rclcpp::WaitResultKind::Ready);
+  // since update doesn't guarantee a published message, republish until received
+  int max_sub_check_loop_count = 5;  // max number of tries for pub/sub loop
+  rclcpp::WaitSet wait_set;          // block used to wait on message
+  wait_set.add_subscription(subscription);
+  while (max_sub_check_loop_count--)
+  {
+    fts_broadcaster_->update(rclcpp::Time(0), rclcpp::Duration::from_seconds(0.01));
+    // check if message has been received
+    if (wait_set.wait(std::chrono::milliseconds(2)).kind() == rclcpp::WaitResultKind::Ready)
+    {
+      break;
+    }
+  }
+  ASSERT_GE(max_sub_check_loop_count, 0) << "Test was unable to publish a message through "
+                                            "controller/broadcaster update loop";
 
   // take message from subscription
   rclcpp::MessageInfo msg_info;

imu_sensor_broadcaster/test/test_imu_sensor_broadcaster.cpp

@@ -38,13 +38,6 @@ namespace
 constexpr auto NODE_SUCCESS = controller_interface::CallbackReturn::SUCCESS;
 constexpr auto NODE_ERROR = controller_interface::CallbackReturn::ERROR;
 
-rclcpp::WaitResultKind wait_for(rclcpp::SubscriptionBase::SharedPtr subscription)
-{
-  rclcpp::WaitSet wait_set;
-  wait_set.add_subscription(subscription);
-  return wait_set.wait().kind();
-}
-
 }  // namespace
 
 void IMUSensorBroadcasterTest::SetUpTestCase() { rclcpp::init(0, nullptr); }
@@ -88,12 +81,21 @@ void IMUSensorBroadcasterTest::subscribe_and_get_message(sensor_msgs::msg::Imu &
     "/test_imu_sensor_broadcaster/imu", 10, subs_callback);
 
   // call update to publish the test value
-  ASSERT_EQ(
-    imu_broadcaster_->update(rclcpp::Time(0), rclcpp::Duration::from_seconds(0.01)),
-    controller_interface::return_type::OK);
-
-  // wait for message to be passed
-  ASSERT_EQ(wait_for(subscription), rclcpp::WaitResultKind::Ready);
+  // since update doesn't guarantee a published message, republish until received
+  int max_sub_check_loop_count = 5;  // max number of tries for pub/sub loop
+  rclcpp::WaitSet wait_set;          // block used to wait on message
+  wait_set.add_subscription(subscription);
+  while (max_sub_check_loop_count--)
+  {
+    imu_broadcaster_->update(rclcpp::Time(0), rclcpp::Duration::from_seconds(0.01));
+    // check if message has been received
+    if (wait_set.wait(std::chrono::milliseconds(2)).kind() == rclcpp::WaitResultKind::Ready)
+    {
+      break;
+    }
+  }
+  ASSERT_GE(max_sub_check_loop_count, 0) << "Test was unable to publish a message through "
+                                            "controller/broadcaster update loop";
 
   // take message from subscription
   rclcpp::MessageInfo msg_info;

joint_state_broadcaster/test/test_joint_state_broadcaster.cpp

@@ -45,14 +45,6 @@ namespace
 {
 constexpr auto NODE_SUCCESS = controller_interface::CallbackReturn::SUCCESS;
 constexpr auto NODE_ERROR = controller_interface::CallbackReturn::ERROR;
-
-rclcpp::WaitResultKind wait_for(rclcpp::SubscriptionBase::SharedPtr subscription)
-{
-  rclcpp::WaitSet wait_set;
-  wait_set.add_subscription(subscription);
-  const auto timeout = std::chrono::seconds(10);
-  return wait_set.wait(timeout).kind();
-}
 }  // namespace
 
 void JointStateBroadcasterTest::SetUpTestCase() { rclcpp::init(0, nullptr); }
@@ -606,12 +598,22 @@ void JointStateBroadcasterTest::test_published_joint_state_message(const std::st
   auto subscription =
     test_node.create_subscription<sensor_msgs::msg::JointState>(topic, 10, subs_callback);
 
-  ASSERT_EQ(
-    state_broadcaster_->update(rclcpp::Time(0), rclcpp::Duration::from_seconds(0.01)),
-    controller_interface::return_type::OK);
-
-  // wait for message to be passed
-  ASSERT_EQ(wait_for(subscription), rclcpp::WaitResultKind::Ready);
+  // call update to publish the test value
+  // since update doesn't guarantee a published message, republish until received
+  int max_sub_check_loop_count = 5;  // max number of tries for pub/sub loop
+  rclcpp::WaitSet wait_set;          // block used to wait on message
+  wait_set.add_subscription(subscription);
+  while (max_sub_check_loop_count--)
+  {
+    state_broadcaster_->update(rclcpp::Time(0), rclcpp::Duration::from_seconds(0.01));
+    // check if message has been received
+    if (wait_set.wait(std::chrono::milliseconds(2)).kind() == rclcpp::WaitResultKind::Ready)
+    {
+      break;
+    }
+  }
+  ASSERT_GE(max_sub_check_loop_count, 0) << "Test was unable to publish a message through "
+                                            "controller/broadcaster update loop";
 
   // take message from subscription
   sensor_msgs::msg::JointState joint_state_msg;
@@ -658,12 +660,22 @@ void JointStateBroadcasterTest::test_published_dynamic_joint_state_message(
   auto subscription =
     test_node.create_subscription<control_msgs::msg::DynamicJointState>(topic, 10, subs_callback);
 
-  ASSERT_EQ(
-    state_broadcaster_->update(rclcpp::Time(0), rclcpp::Duration::from_seconds(0.01)),
-    controller_interface::return_type::OK);
-
-  // wait for message to be passed
-  ASSERT_EQ(wait_for(subscription), rclcpp::WaitResultKind::Ready);
+  // call update to publish the test value
+  // since update doesn't guarantee a published message, republish until received
+  int max_sub_check_loop_count = 5;  // max number of tries for pub/sub loop
+  rclcpp::WaitSet wait_set;          // block used to wait on message
+  wait_set.add_subscription(subscription);
+  while (max_sub_check_loop_count--)
+  {
+    state_broadcaster_->update(rclcpp::Time(0), rclcpp::Duration::from_seconds(0.01));
+    // check if message has been received
+    if (wait_set.wait(std::chrono::milliseconds(2)).kind() == rclcpp::WaitResultKind::Ready)
+    {
+      break;
+    }
+  }
+  ASSERT_GE(max_sub_check_loop_count, 0) << "Test was unable to publish a message through "
+                                            "controller/broadcaster update loop";
 
   // take message from subscription
   control_msgs::msg::DynamicJointState dynamic_joint_state_msg;