diff --git a/external/CMakeLists.txt b/external/CMakeLists.txt index 0ccab08b..57182e14 100644 --- a/external/CMakeLists.txt +++ b/external/CMakeLists.txt @@ -3,7 +3,7 @@ include(FetchContent) FetchContent_Declare( spdlog GIT_REPOSITORY https://github.com/gabime/spdlog.git - GIT_TAG v1.10.0 + GIT_TAG v1.9.2 GIT_SHALLOW TRUE ) FetchContent_MakeAvailable(spdlog) diff --git a/src/graph/NodesRos2.hpp b/src/graph/NodesRos2.hpp index 2e4f63c3..8a8eaf3a 100644 --- a/src/graph/NodesRos2.hpp +++ b/src/graph/NodesRos2.hpp @@ -50,6 +50,7 @@ struct Ros2PublishPointsNode : Node, IPointsNodeSingleInput rclcpp::Publisher::SharedPtr ros2Publisher; sensor_msgs::msg::PointCloud2 ros2Message; + static bool isRclcppInitializedByRGL; static rclcpp::Node::SharedPtr ros2Node; static std::string ros2NodeName; static std::set ros2TopicNames; diff --git a/src/graph/Ros2PublishPointsNode.cpp b/src/graph/Ros2PublishPointsNode.cpp index 8da094d9..da7bd01d 100644 --- a/src/graph/Ros2PublishPointsNode.cpp +++ b/src/graph/Ros2PublishPointsNode.cpp @@ -16,6 +16,7 @@ #include #include +bool Ros2PublishPointsNode::isRclcppInitializedByRGL = false; rclcpp::Node::SharedPtr Ros2PublishPointsNode::ros2Node = nullptr; std::string Ros2PublishPointsNode::ros2NodeName = "RobotecGPULidar"; std::set Ros2PublishPointsNode::ros2TopicNames = {}; @@ -26,10 +27,14 @@ void Ros2PublishPointsNode::setParameters( rgl_qos_policy_durability_t qosDurability, rgl_qos_policy_history_t qosHistory, int32_t qosHistoryDepth) { - if (ros2Node.get() == nullptr) { - static const char *args[] = { "--ros-args", "--disable-external-lib-logs" }; - rclcpp::init(2, args); + // Check if rclcpp initialized + if (!rclcpp::ok()) { + rclcpp::init(0, nullptr); + isRclcppInitializedByRGL = true; + } + // Create ROS2 node + if (ros2Node.get() == nullptr) { ros2Node = std::make_shared(ros2NodeName); } @@ -76,6 +81,9 @@ void Ros2PublishPointsNode::schedule(cudaStream_t stream) ros2Message.header.stamp = Scene::defaultInstance()->getTime().has_value() ? Scene::defaultInstance()->getTime()->asRos2Msg() : static_cast(ros2Node->get_clock()->now()); + if (!rclcpp::ok()) { + throw std::runtime_error("Unable to publish a message because ROS2 has been shut down."); + } ros2Publisher->publish(ros2Message); } @@ -85,8 +93,12 @@ Ros2PublishPointsNode::~Ros2PublishPointsNode() ros2Publisher.reset(); if (ros2TopicNames.empty()) { - rclcpp::shutdown(); ros2Node.reset(); + + if (isRclcppInitializedByRGL) { + rclcpp::shutdown(); + isRclcppInitializedByRGL = false; + } } } diff --git a/test/CMakeLists.txt b/test/CMakeLists.txt index 3f43c55c..fa5bb408 100644 --- a/test/CMakeLists.txt +++ b/test/CMakeLists.txt @@ -1,6 +1,7 @@ cmake_minimum_required(VERSION 3.16) set(RGL_TEST_FILES + src/externalLibraryTest.cpp src/graphTest.cpp src/apiReadmeExample.cpp src/gaussianStressTest.cpp diff --git a/test/src/externalLibraryTest.cpp b/test/src/externalLibraryTest.cpp new file mode 100644 index 00000000..73106d96 --- /dev/null +++ b/test/src/externalLibraryTest.cpp @@ -0,0 +1,37 @@ +#include +#include +#include + +#if RGL_BUILD_ROS2_EXTENSION +#include +#endif + +class ExternalLibraryTest : public RGLTest {}; + +// TODO (msz-rai): Make it work on Windows. +// Currently, there is a bug when destroying Optix on Windows which causes non-zero exit code. +#if RGL_BUILD_ROS2_EXTENSION && __GNUC__ +/** + * rclcpp library (part of the ROS2 extension) depends on spdlog library. + * RGL also uses spdlog for logging purposes. + * Using RGL with rclcpp causes an error (segmentation fault) + * when the version of spdlog with which libraries (RGL and rclcpp) were built differs. + * + * This test checks if rclcpp initializes and shuts down properly. + * rclcpp is initialized when creating the first `rgl_node_points_ros2_publish` node, + * and shut down when destroying the last of the `rgl_node_points_ros2_publish` nodes. + */ +TEST_F(ExternalLibraryTest, RclcppInitializeAndShutDownProperly) +{ + ::testing::GTEST_FLAG(death_test_style) = "threadsafe"; + ASSERT_EXIT({ + rgl_node_t ros2pub = nullptr; + rgl_node_points_ros2_publish(&ros2pub, "rglTopic", "rglFrame"); + rgl_cleanup(); + exit(0); + }, ::testing::ExitedWithCode(0), "") + << "Test for rclcpp (de)initialization failed. " + << "It is probably caused by a mismatched version of the spdlog library for RGL and ROS2. " + << "Check its compatibility."; +} +#endif