diff --git a/topic_tools/test/test_demux.cpp b/topic_tools/test/test_demux.cpp index 47ec9837..ad2300b7 100644 --- a/topic_tools/test/test_demux.cpp +++ b/topic_tools/test/test_demux.cpp @@ -56,8 +56,11 @@ class DemuxTest : public TestTopicToolMultiPub } } - auto result = srv_client_->async_send_request(request); - rclcpp::spin_some(target_node_); + auto future = srv_client_->async_send_request(request); + executor_->spin_node_all(target_node_, std::chrono::nanoseconds(0)); + const auto ret = executor_->spin_until_future_complete(future, service_call_timeout_); + ASSERT_EQ(ret, rclcpp::FutureReturnCode::SUCCESS); + ASSERT_TRUE(future.get()->success); } void publish_and_check(std::string msg_content) @@ -66,6 +69,7 @@ class DemuxTest : public TestTopicToolMultiPub } private: + const std::chrono::seconds service_call_timeout_{10}; rclcpp::Client::SharedPtr srv_client_; std::shared_ptr target_node_; }; diff --git a/topic_tools/test/test_mux.cpp b/topic_tools/test/test_mux.cpp index 76033689..17a0f02a 100644 --- a/topic_tools/test/test_mux.cpp +++ b/topic_tools/test/test_mux.cpp @@ -61,8 +61,11 @@ class MuxTest : public TestTopicToolMultiSub } } - auto result = srv_client_->async_send_request(request); - rclcpp::spin_some(target_node_); + auto future = srv_client_->async_send_request(request); + executor_->spin_node_all(target_node_, std::chrono::nanoseconds(0)); + const auto ret = executor_->spin_until_future_complete(future, service_call_timeout_); + ASSERT_EQ(ret, rclcpp::FutureReturnCode::SUCCESS); + ASSERT_TRUE(future.get()->success); } void publish_and_check( @@ -73,6 +76,7 @@ class MuxTest : public TestTopicToolMultiSub } private: + const std::chrono::seconds service_call_timeout_{10}; rclcpp::Client::SharedPtr srv_client_; std::shared_ptr target_node_; }; diff --git a/topic_tools/test/test_topic_tool.hpp b/topic_tools/test/test_topic_tool.hpp index da59b8bf..54f9790c 100644 --- a/topic_tools/test/test_topic_tool.hpp +++ b/topic_tools/test/test_topic_tool.hpp @@ -71,6 +71,8 @@ class TestTopicToolSingleSub : public TestTopicTool const std::string test_name = ::testing::UnitTest::GetInstance()->current_test_info()->name(); test_node_ = rclcpp::Node::make_shared(test_name); + executor_ = std::make_shared(); + executor_->add_node(test_node_); target_input_topic_ = "/" + test_name + "/input"; target_output_topic_ = "/" + test_name + "/output"; subscription_ = test_node_->create_subscription( @@ -94,8 +96,8 @@ class TestTopicToolSingleSub : public TestTopicTool auto message = std_msgs::msg::String(); message.data = msg_content; publisher_->publish(message); - rclcpp::spin_some(target_node); - rclcpp::spin_some(test_node_); + executor_->spin_node_all(target_node, std::chrono::nanoseconds(0)); + executor_->spin_some(); } std::string get_target_input_topic() @@ -112,6 +114,7 @@ class TestTopicToolSingleSub : public TestTopicTool std::shared_ptr test_node_; rclcpp::Subscription::SharedPtr subscription_; rclcpp::Publisher::SharedPtr publisher_; + std::shared_ptr executor_; std::string target_input_topic_; std::string target_output_topic_; }; @@ -125,6 +128,8 @@ class TestTopicToolMultiSub : public TestTopicTool const std::string test_name = ::testing::UnitTest::GetInstance()->current_test_info()->name(); test_node_ = rclcpp::Node::make_shared(test_name); + executor_ = std::make_shared(); + executor_->add_node(test_node_); target_input_topic_prefix_ = "/" + test_name + "/input"; target_output_topic_ = "/" + test_name + "/output"; subscription_ = test_node_->create_subscription( @@ -155,8 +160,8 @@ class TestTopicToolMultiSub : public TestTopicTool message.data = msg_content; assert(publisher_index < num_target_input_topics_); publishers_[publisher_index]->publish(message); - rclcpp::spin_some(target_node); - rclcpp::spin_some(test_node_); + executor_->spin_node_all(target_node, std::chrono::nanoseconds(0)); + executor_->spin_some(); } std::vector get_target_input_topics() @@ -175,6 +180,7 @@ class TestTopicToolMultiSub : public TestTopicTool protected: std::shared_ptr test_node_; + std::shared_ptr executor_; private: rclcpp::Subscription::SharedPtr subscription_; @@ -193,6 +199,8 @@ class TestTopicToolMultiPub : public TestTopicTool using std::placeholders::_1; const std::string test_name = ::testing::UnitTest::GetInstance()->current_test_info()->name(); test_node_ = rclcpp::Node::make_shared(test_name); + executor_ = std::make_shared(); + executor_->add_node(test_node_); target_input_topic_ = "/" + test_name + "/input"; target_output_topic_prefix_ = "/" + test_name + "/output"; publisher_ = test_node_->create_publisher(target_input_topic_, 10); @@ -219,8 +227,8 @@ class TestTopicToolMultiPub : public TestTopicTool auto message = std_msgs::msg::String(); message.data = msg_content; publisher_->publish(message); - rclcpp::spin_some(target_node); - rclcpp::spin_some(test_node_); + executor_->spin_node_all(target_node, std::chrono::nanoseconds(0)); + executor_->spin_some(); } std::vector get_target_output_topics() @@ -236,6 +244,7 @@ class TestTopicToolMultiPub : public TestTopicTool protected: std::shared_ptr test_node_; + std::shared_ptr executor_; private: rclcpp::Publisher::SharedPtr publisher_;