diff --git a/CMakeLists.txt b/CMakeLists.txt index f0adae8..06259c4 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -241,7 +241,7 @@ elseif(ROS_BUILD_TYPE STREQUAL "ament_cmake") ament_add_gtest(smoke_test ros2_foxglove_bridge/tests/smoke_test.cpp) ament_target_dependencies(smoke_test rclcpp rclcpp_components std_msgs std_srvs) - target_link_libraries(smoke_test foxglove_bridge_base) + target_link_libraries(smoke_test foxglove_bridge_component) enable_strict_compiler_warnings(smoke_test) ament_add_gtest(utils_test ros2_foxglove_bridge/tests/utils_test.cpp) diff --git a/ros2_foxglove_bridge/include/foxglove_bridge/ros2_foxglove_bridge.hpp b/ros2_foxglove_bridge/include/foxglove_bridge/ros2_foxglove_bridge.hpp index 4a09ab6..3db76bb 100644 --- a/ros2_foxglove_bridge/include/foxglove_bridge/ros2_foxglove_bridge.hpp +++ b/ros2_foxglove_bridge/include/foxglove_bridge/ros2_foxglove_bridge.hpp @@ -82,6 +82,7 @@ class FoxgloveBridge : public rclcpp::Node { std::atomic _subscribeGraphUpdates = false; bool _includeHidden = false; std::unique_ptr _fetchAssetQueue; + std::atomic _shuttingDown = false; void subscribeConnectionGraph(bool subscribe); diff --git a/ros2_foxglove_bridge/src/ros2_foxglove_bridge.cpp b/ros2_foxglove_bridge/src/ros2_foxglove_bridge.cpp index 19b2c7d..6c9d499 100644 --- a/ros2_foxglove_bridge/src/ros2_foxglove_bridge.cpp +++ b/ros2_foxglove_bridge/src/ros2_foxglove_bridge.cpp @@ -135,6 +135,7 @@ FoxgloveBridge::FoxgloveBridge(const rclcpp::NodeOptions& options) } FoxgloveBridge::~FoxgloveBridge() { + _shuttingDown = true; RCLCPP_INFO(this->get_logger(), "Shutting down %s", this->get_name()); if (_rosgraphPollThread) { _rosgraphPollThread->join(); @@ -148,7 +149,7 @@ void FoxgloveBridge::rosgraphPollThread() { updateAdvertisedServices(); auto graphEvent = this->get_graph_event(); - while (rclcpp::ok()) { + while (!_shuttingDown && rclcpp::ok()) { try { this->wait_for_graph_change(graphEvent, 200ms); bool triggered = graphEvent->check_and_clear(); diff --git a/ros2_foxglove_bridge/tests/smoke_test.cpp b/ros2_foxglove_bridge/tests/smoke_test.cpp index 0d8f83f..8112e78 100644 --- a/ros2_foxglove_bridge/tests/smoke_test.cpp +++ b/ros2_foxglove_bridge/tests/smoke_test.cpp @@ -4,11 +4,11 @@ #include #include -#include #include #include #include +#include #include #include @@ -21,7 +21,26 @@ constexpr uint8_t HELLO_WORLD_BINARY[] = {0, 1, 0, 0, 12, 0, 0, 0, constexpr auto ONE_SECOND = std::chrono::seconds(1); constexpr auto DEFAULT_TIMEOUT = std::chrono::seconds(10); -class ParameterTest : public ::testing::Test { +class TestWithExecutor : public testing::Test { +protected: + TestWithExecutor() { + this->_executorThread = std::thread([this]() { + this->executor.spin(); + }); + } + + ~TestWithExecutor() override { + this->executor.cancel(); + this->_executorThread.join(); + } + + rclcpp::executors::SingleThreadedExecutor executor; + +private: + std::thread _executorThread; +}; + +class ParameterTest : public TestWithExecutor { public: using PARAM_1_TYPE = std::string; inline static const std::string NODE_1_NAME = "node_1"; @@ -63,29 +82,24 @@ class ParameterTest : public ::testing::Test { _paramNode2->declare_parameter(PARAM_3_NAME, PARAM_3_DEFAULT_VALUE); _paramNode2->declare_parameter(PARAM_4_NAME, PARAM_4_DEFAULT_VALUE); - _executor.add_node(_paramNode1); - _executor.add_node(_paramNode2); - _executorThread = std::thread([this]() { - _executor.spin(); - }); + executor.add_node(_paramNode1); + executor.add_node(_paramNode2); _wsClient = std::make_shared>(); ASSERT_EQ(std::future_status::ready, _wsClient->connect(URI).wait_for(DEFAULT_TIMEOUT)); } void TearDown() override { - _executor.cancel(); - _executorThread.join(); + executor.remove_node(_paramNode1); + executor.remove_node(_paramNode2); } - rclcpp::executors::SingleThreadedExecutor _executor; rclcpp::Node::SharedPtr _paramNode1; rclcpp::Node::SharedPtr _paramNode2; - std::thread _executorThread; std::shared_ptr> _wsClient; }; -class ServiceTest : public ::testing::Test { +class ServiceTest : public TestWithExecutor { public: inline static const std::string SERVICE_NAME = "/foo_service"; @@ -98,26 +112,19 @@ class ServiceTest : public ::testing::Test { res->message = "hello"; res->success = req->data; }); - - _executor.add_node(_node); - _executorThread = std::thread([this]() { - _executor.spin(); - }); + executor.add_node(_node); } void TearDown() override { - _executor.cancel(); - _executorThread.join(); + executor.remove_node(_node); } - rclcpp::executors::SingleThreadedExecutor _executor; rclcpp::Node::SharedPtr _node; rclcpp::ServiceBase::SharedPtr _service; - std::thread _executorThread; std::shared_ptr> _wsClient; }; -class ExistingPublisherTest : public ::testing::Test { +class ExistingPublisherTest : public TestWithExecutor { public: inline static const std::string TOPIC_NAME = "/some_topic"; @@ -126,21 +133,15 @@ class ExistingPublisherTest : public ::testing::Test { _node = rclcpp::Node::make_shared("node"); _publisher = _node->create_publisher(TOPIC_NAME, rclcpp::SystemDefaultsQoS()); - _executor.add_node(_node); - _executorThread = std::thread([this]() { - _executor.spin(); - }); + executor.add_node(_node); } void TearDown() override { - _executor.cancel(); - _executorThread.join(); + executor.remove_node(_node); } - rclcpp::executors::SingleThreadedExecutor _executor; rclcpp::Node::SharedPtr _node; rclcpp::PublisherBase::SharedPtr _publisher; - std::thread _executorThread; }; template @@ -251,7 +252,7 @@ TEST(SmokeTest, testSubscriptionParallel) { } } -TEST(SmokeTest, testPublishing) { +TEST_F(TestWithExecutor, testPublishing) { foxglove::ClientAdvertisement advertisement; advertisement.channelId = 1; advertisement.topic = "/foo"; @@ -266,8 +267,7 @@ TEST(SmokeTest, testPublishing) { advertisement.topic, 10, [&msgPromise](std::shared_ptr msg) { msgPromise.set_value(msg->data); }); - rclcpp::executors::SingleThreadedExecutor executor; - executor.add_node(node); + this->executor.add_node(node); // Set up the client, advertise and publish the binary message auto client = std::make_shared>(); @@ -283,8 +283,8 @@ TEST(SmokeTest, testPublishing) { client->unadvertise({advertisement.channelId}); // Ensure that we have received the correct message via our ROS subscriber - const auto ret = executor.spin_until_future_complete(msgFuture, ONE_SECOND); - ASSERT_EQ(rclcpp::FutureReturnCode::SUCCESS, ret); + ASSERT_EQ(std::future_status::ready, msgFuture.wait_for(DEFAULT_TIMEOUT)); + this->executor.remove_node(node); EXPECT_EQ("hello world", msgFuture.get()); } @@ -732,34 +732,23 @@ int main(int argc, char** argv) { testing::InitGoogleTest(&argc, argv); rclcpp::init(argc, argv); - const size_t numThreads = 2; - auto executor = - rclcpp::executors::MultiThreadedExecutor::make_shared(rclcpp::ExecutorOptions{}, numThreads); - - rclcpp_components::ComponentManager componentManager(executor, "ComponentManager"); - const auto componentResources = componentManager.get_component_resources("foxglove_bridge"); - - if (componentResources.empty()) { - RCLCPP_INFO(componentManager.get_logger(), "No loadable resources found"); - return EXIT_FAILURE; - } - - auto componentFactory = componentManager.create_component_factory(componentResources.front()); + rclcpp::executors::SingleThreadedExecutor executor; rclcpp::NodeOptions nodeOptions; // Explicitly allow file:// asset URIs for testing purposes. nodeOptions.append_parameter_override("asset_uri_allowlist", std::vector({"file://.*"})); - auto node = componentFactory->create_node_instance(nodeOptions); - executor->add_node(node.get_node_base_interface()); + foxglove_bridge::FoxgloveBridge node(nodeOptions); + executor.add_node(node.get_node_base_interface()); std::thread spinnerThread([&executor]() { - executor->spin(); + executor.spin(); }); const auto testResult = RUN_ALL_TESTS(); - executor->cancel(); + + executor.cancel(); spinnerThread.join(); - rclcpp::shutdown(); + executor.remove_node(node.get_node_base_interface()); return testResult; }