Fix ROS2 initialization (#164)

* Check if rclcpp already initialized

* Add test for checking rclcpp library

* Downgrade spdlog to match version used by ROS2

* Disable test on Windows

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)

src/graph/NodesRos2.hpp

@@ -50,6 +50,7 @@ struct Ros2PublishPointsNode : Node, IPointsNodeSingleInput
 	rclcpp::Publisher<sensor_msgs::msg::PointCloud2>::SharedPtr ros2Publisher;
 	sensor_msgs::msg::PointCloud2 ros2Message;
 
+	static bool isRclcppInitializedByRGL;
 	static rclcpp::Node::SharedPtr ros2Node;
 	static std::string ros2NodeName;
 	static std::set<std::string> ros2TopicNames;

src/graph/Ros2PublishPointsNode.cpp

@@ -16,6 +16,7 @@
 #include <scene/Scene.hpp>
 #include <RGLFields.hpp>
 
+bool Ros2PublishPointsNode::isRclcppInitializedByRGL = false;
 rclcpp::Node::SharedPtr Ros2PublishPointsNode::ros2Node = nullptr;
 std::string Ros2PublishPointsNode::ros2NodeName = "RobotecGPULidar";
 std::set<std::string> 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<rclcpp::Node>(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<builtin_interfaces::msg::Time>(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;
+		}
 	}
 }
 

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

test/src/externalLibraryTest.cpp

@@ -0,0 +1,37 @@
+#include <gtest/gtest.h>
+#include <gtest/gtest-death-test.h>
+#include <utils.hpp>
+
+#if RGL_BUILD_ROS2_EXTENSION
+#include <rgl/api/extensions/ros2.h>
+#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