Implement Ros2PublishRadarScanNode with tests

extensions/ros2/CMakeLists.txt

@@ -12,6 +12,7 @@ target_sources(RobotecGPULidar PRIVATE
     src/api/apiRos2.cpp
     src/graph/Ros2PublishPointsNode.cpp
     src/graph/Ros2PublishPointVelocityMarkersNode.cpp
+    src/graph/Ros2PublishRadarScanNode.cpp
 )
 
 target_include_directories(RobotecGPULidar PUBLIC

extensions/ros2/include/rgl/api/extensions/ros2.h

@@ -87,3 +87,23 @@ RGL_API rgl_status_t rgl_node_points_ros2_publish_with_qos(rgl_node_t* node, con
                                                            rgl_qos_policy_reliability_t qos_reliability,
                                                            rgl_qos_policy_durability_t qos_durability,
                                                            rgl_qos_policy_history_t qos_history, int32_t qos_history_depth);
+
+/**
+ * Creates or modifies Ros2PublishRadarScanNode.
+ * The node publishes a RadarScan message to the ROS2 topic using specified Quality of Service settings.
+ * The message header stamp gets time from the raytraced scene. If the scene has no time, header will get the actual time.
+ * Graph input: point cloud
+ * Graph output: point cloud
+ * @param node If (*node) == nullptr, a new node will be created. Otherwise, (*node) will be modified.
+ * @param topic_name Topic name to publish on.
+ * @param frame_id Frame this data is associated with.
+ * @param qos_reliability QoS reliability policy.
+ * @param qos_durability QoS durability policy.
+ * @param qos_history QoS history policy.
+ * @param qos_history_depth QoS history depth. If history policy is KEEP_ALL, depth is ignored but must always be non-negative.
+ *
+ */
+RGL_API rgl_status_t rgl_node_publish_ros2_radarscan(rgl_node_t* node, const char* topic_name, const char* frame_id,
+                                                     rgl_qos_policy_reliability_t qos_reliability,
+                                                     rgl_qos_policy_durability_t qos_durability,
+                                                     rgl_qos_policy_history_t qos_history, int32_t qos_history_depth);

extensions/ros2/src/api/apiRos2.cpp

@@ -53,7 +53,7 @@ RGL_API rgl_status_t rgl_node_points_ros2_publish(rgl_node_t* node, const char*
 
 void TapeRos2::tape_node_points_ros2_publish(const YAML::Node& yamlNode, PlaybackState& state)
 {
-	size_t nodeId = yamlNode[0].as<TapeAPIObjectID>();
+	auto nodeId = yamlNode[0].as<TapeAPIObjectID>();
 	rgl_node_t node = state.nodes.contains(nodeId) ? state.nodes[nodeId] : nullptr;
 	rgl_node_points_ros2_publish(&node, yamlNode[1].as<std::string>().c_str(), yamlNode[2].as<std::string>().c_str());
 	state.nodes.insert(std::make_pair(nodeId, node));
@@ -83,12 +83,38 @@ RGL_API rgl_status_t rgl_node_points_ros2_publish_with_qos(rgl_node_t* node, con
 
 void TapeRos2::tape_node_points_ros2_publish_with_qos(const YAML::Node& yamlNode, PlaybackState& state)
 {
-	size_t nodeId = yamlNode[0].as<TapeAPIObjectID>();
+	auto nodeId = yamlNode[0].as<TapeAPIObjectID>();
 	rgl_node_t node = state.nodes.contains(nodeId) ? state.nodes[nodeId] : nullptr;
 	rgl_node_points_ros2_publish_with_qos(&node, yamlNode[1].as<std::string>().c_str(), yamlNode[2].as<std::string>().c_str(),
 	                                      (rgl_qos_policy_reliability_t) yamlNode[3].as<int>(),
 	                                      (rgl_qos_policy_durability_t) yamlNode[4].as<int>(),
 	                                      (rgl_qos_policy_history_t) yamlNode[5].as<int>(), yamlNode[6].as<int32_t>());
 	state.nodes.insert(std::make_pair(nodeId, node));
 }
+
+RGL_API rgl_status_t rgl_node_publish_ros2_radarscan(rgl_node_t* node, const char* topic_name, const char* frame_id,
+                                                     rgl_qos_policy_reliability_t qos_reliability,
+                                                     rgl_qos_policy_durability_t qos_durability,
+                                                     rgl_qos_policy_history_t qos_history, int32_t qos_history_depth)
+{
+	auto status = rglSafeCall([&]() {
+		RGL_DEBUG("rgl_node_publish_ros2_radarscan(node={}, topic_name={}, frame_id={})", repr(node), topic_name, frame_id);
+		CHECK_ARG(topic_name != nullptr);
+		CHECK_ARG(topic_name[0] != '\0');
+		CHECK_ARG(frame_id != nullptr);
+		CHECK_ARG(frame_id[0] != '\0');
+
+		createOrUpdateNode<Ros2PublishRadarScanNode>(node, topic_name, frame_id);
+	});
+	TAPE_HOOK(node, topic_name, frame_id);
+	return status;
+}
+
+void TapeRos2::rgl_node_publish_ros2_radarscan(const YAML::Node& yamlNode, PlaybackState& state)
+{
+	auto nodeId = yamlNode[0].as<TapeAPIObjectID>();
+	rgl_node_t node = state.nodes.contains(nodeId) ? state.nodes[nodeId] : nullptr;
+	rgl_node_points_ros2_publish(&node, yamlNode[1].as<std::string>().c_str(), yamlNode[2].as<std::string>().c_str());
+	state.nodes.insert(std::make_pair(nodeId, node));
+}
 }

extensions/ros2/src/graph/NodesRos2.hpp

@@ -83,17 +83,18 @@ struct Ros2PublishRadarScanNode : IPointsNodeSingleInput
 	void setParameters(const char* topicName, const char* frameId,
 	                   rgl_qos_policy_reliability_t qosReliability = QOS_POLICY_RELIABILITY_SYSTEM_DEFAULT,
 	                   rgl_qos_policy_durability_t qosDurability = QOS_POLICY_DURABILITY_SYSTEM_DEFAULT,
-	                   rgl_qos_policy_history_t qosHistory = QOS_POLICY_HISTORY_SYSTEM_DEFAULT, int32_t qosHistoryDepth = 10)
-	{}
+	                   rgl_qos_policy_history_t qosHistory = QOS_POLICY_HISTORY_SYSTEM_DEFAULT, int32_t qosHistoryDepth = 10);
 	std::vector<rgl_field_t> getRequiredFieldList() const override
 	{
 		return {DISTANCE_F32, AZIMUTH_F32, ELEVATION_F32, RADIAL_SPEED_F32};
 	}
-	void validateImpl() override {}
-	void enqueueExecImpl() override {}
+	void validateImpl() override;
+	void enqueueExecImpl() override;
 
 private:
 	radar_msgs::msg::RadarScan ros2Message;
 	rclcpp::Publisher<radar_msgs::msg::RadarScan>::SharedPtr ros2Publisher;
+	DeviceAsyncArray<char>::Ptr formattedData = DeviceAsyncArray<char>::create(arrayMgr);
 	std::shared_ptr<Ros2InitGuard> ros2InitGuard;
+	GPUFieldDescBuilder fieldDescBuilder;
 };

extensions/ros2/src/graph/Ros2PublishRadarScanNode.cpp

@@ -0,0 +1,56 @@
+// Copyright 2024 Robotec.AI
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#include <graph/NodesRos2.hpp>
+#include <scene/Scene.hpp>
+
+void Ros2PublishRadarScanNode::setParameters(const char* topicName, const char* frameId,
+                                             rgl_qos_policy_reliability_t qosReliability,
+                                             rgl_qos_policy_durability_t qosDurability, rgl_qos_policy_history_t qosHistory,
+                                             int32_t qosHistoryDepth)
+{
+	ros2InitGuard = Ros2InitGuard::acquire();
+
+	ros2Message.header.frame_id = frameId;
+
+	auto qos = rclcpp::QoS(qosHistoryDepth);
+	qos.reliability(static_cast<rmw_qos_reliability_policy_t>(qosReliability));
+	qos.durability(static_cast<rmw_qos_durability_policy_t>(qosDurability));
+	qos.history(static_cast<rmw_qos_history_policy_t>(qosHistory));
+	ros2Publisher = ros2InitGuard->createUniquePublisher<radar_msgs::msg::RadarScan>(topicName, qos);
+}
+
+void Ros2PublishRadarScanNode::validateImpl()
+{
+	IPointsNodeSingleInput::validateImpl();
+	if (input->getHeight() != 1) {
+		throw InvalidPipeline("ROS2 radar publish supports unorganized pointclouds only");
+	}
+}
+
+void Ros2PublishRadarScanNode::enqueueExecImpl()
+{
+	ros2Message.header.stamp = Scene::instance().getTime().has_value() ?
+	                               Scene::instance().getTime().value().asRos2Msg() :
+	                               static_cast<builtin_interfaces::msg::Time>(ros2InitGuard->getNode().get_clock()->now());
+	std::vector<rgl_field_t> fields = this->getRequiredFieldList();
+	fields.push_back(PADDING_32); // Placeholder for amplitude
+	FormatPointsNode::formatAsync(formattedData, input, fields, fieldDescBuilder);
+	ros2Message.returns.resize(input->getPointCount());
+	CHECK_CUDA(cudaMemcpyAsync(ros2Message.returns.data(), formattedData->getReadPtr(),
+	                           formattedData->getSizeOf() * formattedData->getCount(), cudaMemcpyDeviceToHost,
+	                           formattedData->getStream()->getHandle()));
+	CHECK_CUDA(cudaStreamSynchronize(formattedData->getStream()->getHandle()));
+	ros2Publisher->publish(ros2Message);
+}

extensions/ros2/src/tape/TapeRos2.hpp

@@ -21,12 +21,14 @@ class TapeRos2
 {
 	static void tape_node_points_ros2_publish(const YAML::Node& yamlNode, PlaybackState& state);
 	static void tape_node_points_ros2_publish_with_qos(const YAML::Node& yamlNode, PlaybackState& state);
+	static void rgl_node_publish_ros2_radarscan(const YAML::Node& yamlNode, PlaybackState& state);
 
 	// Called once in the translation unit
 	static inline bool autoExtendTapeFunctions = std::invoke([]() {
 		std::map<std::string, TapeFunction> tapeFunctions = {
 		    TAPE_CALL_MAPPING("rgl_node_points_ros2_publish", TapeRos2::tape_node_points_ros2_publish),
 		    TAPE_CALL_MAPPING("rgl_node_points_ros2_publish_with_qos", TapeRos2::tape_node_points_ros2_publish_with_qos),
+		    TAPE_CALL_MAPPING("rgl_node_publish_ros2_radarscan", TapeRos2::rgl_node_publish_ros2_radarscan),
 		};
 		TapePlayer::extendTapeFunctions(tapeFunctions);
 		return true;

test/CMakeLists.txt

@@ -48,6 +48,12 @@ set(RGL_TEST_FILES
     src/scene/incidentAngleTest.cpp
 )
 
+if (RGL_BUILD_ROS2_EXTENSION)
+    list(APPEND RGL_TEST_FILES
+        src/graph/nodes/Ros2PublishRadarScanNodeTest.cpp
+    )
+endif()
+
 # Only Linux
 if ((NOT WIN32))
 #    list(APPEND RGL_TEST_FILES
@@ -90,12 +96,15 @@ if (RGL_BUILD_ROS2_EXTENSION)
     # This allows accessing Nodes directly (as opposed to through RGL-API)
     # It is needed by EntityVelocity test, which makes a use of non-public Ros2 node.
     find_package(rclcpp REQUIRED)
+    find_package(radar_msgs REQUIRED)
+    list(REMOVE_DUPLICATES radar_msgs_LIBRARIES)
     target_include_directories(RobotecGPULidar_test PRIVATE
         ${CMAKE_SOURCE_DIR}/extensions/ros2/src
         ${rclcpp_INCLUDE_DIRS}
     )
     target_link_libraries(RobotecGPULidar_test PRIVATE
         ${rclcpp_LIBRARIES}
+        ${radar_msgs_LIBRARIES}
     )
 endif()
 

test/src/TapeTest.cpp

@@ -262,14 +262,15 @@ TEST_F(TapeTest, RecordPlayAllCalls)
 	EXPECT_RGL_SUCCESS(rgl_node_points_radar_postprocess(&radarPostprocess, 1.0f, 0.5f));
 
 #if RGL_BUILD_ROS2_EXTENSION
-	rgl_node_t ros2pub = nullptr;
-	EXPECT_RGL_SUCCESS(rgl_node_points_ros2_publish(&ros2pub, "pointcloud", "rgl"));
+	rgl_node_t pointcloud2Pub = nullptr, radarscanPub = nullptr;
+	EXPECT_RGL_SUCCESS(rgl_node_points_ros2_publish(&pointcloud2Pub, "pointcloud", "rgl"));
 
 	rgl_node_t ros2pubqos = nullptr;
 	rgl_qos_policy_reliability_t qos_r = QOS_POLICY_RELIABILITY_BEST_EFFORT;
 	rgl_qos_policy_durability_t qos_d = QOS_POLICY_DURABILITY_VOLATILE;
 	rgl_qos_policy_history_t qos_h = QOS_POLICY_HISTORY_KEEP_LAST;
 	EXPECT_RGL_SUCCESS(rgl_node_points_ros2_publish_with_qos(&ros2pubqos, "pointcloud_ex", "rgl", qos_r, qos_d, qos_h, 10));
+	EXPECT_RGL_SUCCESS(rgl_node_publish_ros2_radarscan(&radarscanPub, "radarscan", "rgl", qos_r, qos_d, qos_h, 10));
 #endif
 
 	rgl_node_t noiseAngularRay = nullptr;

test/src/graph/nodes/Ros2PublishRadarScanNodeTest.cpp

@@ -0,0 +1,73 @@
+#include <helpers/commonHelpers.hpp>
+#include <helpers/sceneHelpers.hpp>
+#include <helpers/graphHelpers.hpp>
+#include <helpers/testPointCloud.hpp>
+
+#include <graph/NodesRos2.hpp>
+
+#include <rclcpp/rclcpp.hpp>
+
+// TODO: replace with API calls
+#include <api/apiCommon.hpp>
+
+TEST(Ros2PublishRadarScanNodeTest, should_receive_sent_data)
+{
+	const auto POINT_COUNT = 5;
+	const auto TOPIC_NAME = "rgl_test_radar_scan";
+	const auto FRAME_ID = "rgl_test_frame_id";
+	const auto NODE_NAME = "rgl_test_node";
+	const auto WAIT_TIME_SECS = 1;
+	const auto MESSAGE_REPEATS = 3;
+	TestPointCloud input({DISTANCE_F32, AZIMUTH_F32, ELEVATION_F32, RADIAL_SPEED_F32}, POINT_COUNT);
+
+	// Create nodes
+	rgl_node_t inputNode = input.createUsePointsNode(), radarScanNode = nullptr;
+	ASSERT_RGL_SUCCESS(
+	    rgl_node_publish_ros2_radarscan(&radarScanNode, TOPIC_NAME, FRAME_ID, QOS_POLICY_RELIABILITY_SYSTEM_DEFAULT,
+	                                    QOS_POLICY_DURABILITY_SYSTEM_DEFAULT, QOS_POLICY_HISTORY_SYSTEM_DEFAULT, 0));
+
+	// Connect nodes
+	ASSERT_RGL_SUCCESS(rgl_graph_node_add_child(inputNode, radarScanNode));
+
+	// Synchronization primitives
+	std::atomic<int> messageCount = 0;
+
+	// Create ROS2 subscriber to receive RadarScan messages on topic "radar_scan"
+	auto node = std::make_shared<rclcpp::Node>(NODE_NAME, rclcpp::NodeOptions{});
+	auto qos = rclcpp::QoS(10);
+	qos.reliability(static_cast<rmw_qos_reliability_policy_t>(QOS_POLICY_RELIABILITY_SYSTEM_DEFAULT));
+	qos.durability(static_cast<rmw_qos_durability_policy_t>(QOS_POLICY_DURABILITY_SYSTEM_DEFAULT));
+	qos.history(static_cast<rmw_qos_history_policy_t>(QOS_POLICY_HISTORY_SYSTEM_DEFAULT));
+	auto subscriber = node->create_subscription<radar_msgs::msg::RadarScan>(
+	    TOPIC_NAME, qos, [&](const radar_msgs::msg::RadarScan::SharedPtr msg) {
+		    EXPECT_EQ(msg->returns.size(), POINT_COUNT);
+		    EXPECT_EQ(msg->header.frame_id, FRAME_ID);
+		    EXPECT_NE(msg->header.stamp.sec + msg->header.stamp.nanosec, 0);
+
+		    for (int i = 0; i < POINT_COUNT; ++i) {
+			    EXPECT_EQ(msg->returns[i].range, input.getFieldValue<DISTANCE_F32>(i));
+			    EXPECT_EQ(msg->returns[i].azimuth, input.getFieldValue<AZIMUTH_F32>(i));
+			    EXPECT_EQ(msg->returns[i].elevation, input.getFieldValue<ELEVATION_F32>(i));
+			    EXPECT_EQ(msg->returns[i].doppler_velocity, input.getFieldValue<RADIAL_SPEED_F32>(i));
+		    }
+		    ++messageCount;
+	    });
+
+	// Run
+	for (int i = 0; i < MESSAGE_REPEATS; ++i) {
+		ASSERT_RGL_SUCCESS(rgl_graph_run(inputNode));
+	}
+
+	// Wait for messages
+	{
+		auto start = std::chrono::steady_clock::now();
+		do {
+			rclcpp::spin_some(node);
+			std::this_thread::sleep_for(std::chrono::milliseconds(10));
+		} while (messageCount != MESSAGE_REPEATS &&
+		         std::chrono::steady_clock::now() - start < std::chrono::seconds(WAIT_TIME_SECS));
+		ASSERT_EQ(messageCount, MESSAGE_REPEATS);
+	}
+
+	rclcpp::shutdown();
+}