add fix to the MultiInterfaceForwardCommandControllerTest

ros-controls · Jul 15, 2024 · 7d7914e · 7d7914e
1 parent 9b77e55
commit 7d7914e
Show file tree

Hide file tree

Showing 2 changed files with 25 additions and 3 deletions.
diff --git a/forward_command_controller/test/test_multi_interface_forward_command_controller.cpp b/forward_command_controller/test/test_multi_interface_forward_command_controller.cpp
@@ -41,12 +41,28 @@ using testing::SizeIs;
 
 namespace
 {
-rclcpp::WaitResultKind wait_for(rclcpp::SubscriptionBase::SharedPtr subscription)
+template <typename T>
+void wait_for_and_process_callback(
+  rclcpp::Subscription<std_msgs::msg::Float64MultiArray>::SharedPtr subscription)
 {
   rclcpp::WaitSet wait_set;
   wait_set.add_subscription(subscription);
   const auto timeout = std::chrono::seconds(10);
-  return wait_set.wait(timeout).kind();
+  const auto wait_result = wait_set.wait(timeout);
+  ASSERT_EQ(wait_result.kind(), rclcpp::WaitResultKind::Ready);
+  if (wait_result.kind() == rclcpp::WaitResultKind::Ready)
+  {
+    if (wait_result.get_wait_set().get_rcl_wait_set().subscriptions[0U])
+    {
+      T msg;
+      rclcpp::MessageInfo msg_info;
+      if (subscription->take(msg, msg_info))
+      {
+        std::shared_ptr<void> type_erased_msg = std::make_shared<T>(msg);
+        subscription->handle_message(type_erased_msg, msg_info);
+      }
+    }
+  }
 }
 }  // namespace
 
@@ -273,7 +289,8 @@ TEST_F(MultiInterfaceForwardCommandControllerTest, CommandCallbackTest)
   command_pub->publish(command_msg);
 
   // wait for command message to be passed
-  ASSERT_EQ(wait_for(controller_->joints_command_subscriber_), rclcpp::WaitResultKind::Ready);
+  wait_for_and_process_callback<std_msgs::msg::Float64MultiArray>(
+    controller_->joints_command_subscriber_);
 
   // process callbacks
   rclcpp::spin_some(controller_->get_node()->get_node_base_interface());

diff --git a/forward_command_controller/test/test_multi_interface_forward_command_controller.hpp b/forward_command_controller/test/test_multi_interface_forward_command_controller.hpp
@@ -50,6 +50,11 @@ class FriendMultiInterfaceForwardCommandController
   FRIEND_TEST(MultiInterfaceForwardCommandControllerTest, NoCommandCheckTest);
   FRIEND_TEST(MultiInterfaceForwardCommandControllerTest, CommandCallbackTest);
   FRIEND_TEST(MultiInterfaceForwardCommandControllerTest, ActivateDeactivateCommandsResetSuccess);
+
+  rclcpp::CallbackGroup::SharedPtr get_callback_group() override
+  {
+    return get_node()->create_callback_group(rclcpp::CallbackGroupType::MutuallyExclusive, false);
+  }
 };
 
 class MultiInterfaceForwardCommandControllerTest : public ::testing::Test