Implement Ros2PublishLaserScanNode with tests

extensions/ros2/CMakeLists.txt

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

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

@@ -106,3 +106,15 @@ RGL_API rgl_status_t rgl_node_publish_ros2_radarscan(rgl_node_t* node, const cha
                                                      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 Ros2PublishLaserScanNode.
+ * The node publishes a LaserScan message to the ROS2 topic using default 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.
+ */
+RGL_API rgl_status_t rgl_node_publish_ros2_laserscan(rgl_node_t* node, const char* topic_name, const char* frame_id);

extensions/ros2/src/api/apiRos2.cpp

@@ -125,4 +125,27 @@ void TapeRos2::tape_node_publish_ros2_radarscan(const YAML::Node& yamlNode, Play
 	                                (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_laserscan(rgl_node_t* node, const char* topic_name, const char* frame_id)
+{
+	auto status = rglSafeCall([&]() {
+		RGL_DEBUG("rgl_node_publish_ros2_laserscan(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<Ros2PublishLaserScanNode>(node, topic_name, frame_id);
+	});
+	TAPE_HOOK(node, topic_name, frame_id);
+	return status;
+}
+
+void TapeRos2::tape_node_publish_ros2_laserscan(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_publish_ros2_laserscan(&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

@@ -26,6 +26,7 @@
 #include <visualization_msgs/msg/marker_array.hpp>
 #include <Ros2InitGuard.hpp>
 #include <radar_msgs/msg/radar_scan.hpp>
+#include <sensor_msgs/msg/laser_scan.hpp>
 
 struct Ros2Node : IPointsNodeSingleInput
 {
@@ -122,3 +123,31 @@ struct Ros2PublishRadarScanNode : Ros2Node
 	DeviceAsyncArray<char>::Ptr formattedData = DeviceAsyncArray<char>::create(arrayMgr);
 	GPUFieldDescBuilder fieldDescBuilder;
 };
+
+struct Ros2PublishLaserScanNode : Ros2Node
+{
+	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);
+
+	std::vector<rgl_field_t> getRequiredFieldList() const override
+	{
+		return {AZIMUTH_F32, DISTANCE_F32, TIME_STAMP_F64, INTENSITY_F32};
+	}
+
+	// Ros2Node
+	void ros2ValidateImpl() override;
+	void ros2EnqueueExecImpl() override;
+
+private:
+	DeviceAsyncArray<char>::Ptr inputFmtData = DeviceAsyncArray<char>::create(arrayMgr);
+
+	sensor_msgs::msg::LaserScan ros2Message;
+	rclcpp::Publisher<sensor_msgs::msg::LaserScan>::SharedPtr ros2Publisher;
+
+	HostPinnedArray<Field<AZIMUTH_F32>::type>::Ptr angles = HostPinnedArray<Field<AZIMUTH_F32>::type>::create();
+	HostPinnedArray<Field<TIME_STAMP_F64>::type>::Ptr times = HostPinnedArray<Field<TIME_STAMP_F64>::type>::create();
+	HostPinnedArray<Field<DISTANCE_F32>::type>::Ptr ranges = HostPinnedArray<Field<DISTANCE_F32>::type>::create();
+	HostPinnedArray<Field<INTENSITY_F32>::type>::Ptr intensities = HostPinnedArray<Field<INTENSITY_F32>::type>::create();
+};

extensions/ros2/src/graph/Ros2PublishLaserScanNode.cpp

@@ -0,0 +1,94 @@
+// 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>
+#include <RGLFields.hpp>
+
+void Ros2PublishLaserScanNode::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)
+{
+	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<sensor_msgs::msg::LaserScan>(topicName, qos);
+}
+
+void Ros2PublishLaserScanNode::ros2ValidateImpl()
+{
+	if (input->getHeight() != 1) {
+		throw InvalidPipeline("ROS2 publish supports unorganized pointclouds only");
+	}
+}
+
+void Ros2PublishLaserScanNode::ros2EnqueueExecImpl()
+{
+	const auto currentTime = Scene::instance().getTime();
+	ros2Message.header.stamp = currentTime.has_value() ?
+	                               currentTime.value().asRos2Msg() :
+	                               static_cast<builtin_interfaces::msg::Time>(ros2InitGuard->getNode().get_clock()->now());
+
+	const size_t pointCount = input->getPointCount();
+
+	angles->copyFrom(input->getFieldDataTyped<AZIMUTH_F32>());
+	auto minAng = angles->at(0);
+	auto maxAng = minAng;
+	for (size_t i = 1; i < pointCount; ++i) {
+		if (angles->at(i) < minAng) {
+			minAng = angles->at(i);
+		}
+		if (angles->at(i) > maxAng) {
+			maxAng = angles->at(i);
+		}
+	}
+	ros2Message.angle_min = minAng;
+	ros2Message.angle_max = maxAng;
+	ros2Message.angle_increment = angles->at(1) - angles->at(0);
+
+	times->copyFrom(input->getFieldDataTyped<TIME_STAMP_F64>());
+	ros2Message.time_increment = static_cast<float>(times->at(1) - times->at(0));
+	ros2Message.scan_time = static_cast<float>(times->at(pointCount - 1) - times->at(0));
+
+	ranges->copyFrom(input->getFieldDataTyped<DISTANCE_F32>());
+	ros2Message.ranges.resize(input->getPointCount());
+	size_t size = pointCount * Field<DISTANCE_F32>::size;
+	CHECK_CUDA(cudaMemcpyAsync(ros2Message.ranges.data(), ranges->getRawReadPtr(), size, cudaMemcpyDefault, getStreamHandle()));
+	CHECK_CUDA(cudaStreamSynchronize(getStreamHandle()));
+
+	auto minRange = ranges->at(0);
+	auto maxRange = minRange;
+	for (size_t i = 1; i < pointCount; ++i) {
+		if (ranges->at(i) < minRange) {
+			minRange = ranges->at(i);
+		}
+		if (ranges->at(i) > maxRange) {
+			maxRange = ranges->at(i);
+		}
+	}
+	ros2Message.range_min = minRange;
+	ros2Message.range_max = maxRange;
+
+	intensities->copyFrom(input->getFieldDataTyped<INTENSITY_F32>());
+	ros2Message.intensities.resize(pointCount);
+	size = pointCount * Field<INTENSITY_F32>::size;
+	CHECK_CUDA(cudaMemcpyAsync(ros2Message.intensities.data(), intensities->getRawReadPtr(), size, cudaMemcpyDefault,
+	                           getStreamHandle()));
+	CHECK_CUDA(cudaStreamSynchronize(getStreamHandle()));
+
+	ros2Publisher->publish(ros2Message);
+}

extensions/ros2/src/graph/Ros2PublishPointsNode.cpp

@@ -62,7 +62,6 @@ void Ros2PublishPointsNode::ros2EnqueueExecImpl()
 	ros2Publisher->publish(ros2Message);
 }
 
-
 void Ros2PublishPointsNode::updateRos2Message(const std::vector<rgl_field_t>& fields, bool isDense)
 {
 	ros2Message.fields.clear();

extensions/ros2/src/tape/TapeRos2.hpp

@@ -22,13 +22,15 @@ 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 tape_node_publish_ros2_radarscan(const YAML::Node& yamlNode, PlaybackState& state);
+	static void tape_node_publish_ros2_laserscan(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::tape_node_publish_ros2_radarscan),
+		    TAPE_CALL_MAPPING("tape_node_publish_ros2_laserscan", TapeRos2::tape_node_publish_ros2_laserscan)
 		};
 		TapePlayer::extendTapeFunctions(tapeFunctions);
 		return true;

test/CMakeLists.txt

@@ -55,6 +55,7 @@ if (RGL_BUILD_ROS2_EXTENSION)
         src/graph/Ros2NodesTest.cpp
         src/graph/nodes/Ros2PublishRadarScanNodeTest.cpp
         src/graph/nodes/Ros2PublishPointsNodeTest.cpp
+        src/graph/nodes/Ros2PublishLaserScanNodeTest.cpp
         src/scene/radarTest.cpp
     )
 endif()
@@ -108,14 +109,17 @@ if (RGL_BUILD_ROS2_EXTENSION)
     # It is needed by EntityVelocity test, which makes a use of non-public Ros2 node.
     find_package(rclcpp REQUIRED)
     find_package(radar_msgs REQUIRED)
+    find_package(sensor_msgs REQUIRED)
     list(REMOVE_DUPLICATES radar_msgs_LIBRARIES)
+    list(REMOVE_DUPLICATES sensor_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}
+        ${sensor_msgs_LIBRARIES}
     )
 endif()
 

test/src/graph/nodes/Ros2PublishLaserScanNodeTest.cpp

@@ -0,0 +1,116 @@
+#include <helpers/commonHelpers.hpp>
+#include <helpers/graphHelpers.hpp>
+#include <helpers/testPointCloud.hpp>
+
+#include <rgl/api/extensions/ros2.h>
+
+#include <rclcpp/rclcpp.hpp>
+#include <sensor_msgs/msg/laser_scan.hpp>
+
+class Ros2PublishLaserScanNodeTest : public RGLTest
+{};
+
+TEST_F(Ros2PublishLaserScanNodeTest, should_throw_invalid_pipeline_when_ros2_shutdown_laserscan)
+{
+	std::vector<rgl_field_t> fields{AZIMUTH_F32, DISTANCE_F32, TIME_STAMP_F64, INTENSITY_F32};
+	TestPointCloud pointCloud(fields, 10);
+	rgl_node_t points = pointCloud.createUsePointsNode();
+
+	rgl_node_t yield = nullptr;
+	EXPECT_RGL_SUCCESS(rgl_node_points_yield(&yield, fields.data(), fields.size()));
+
+	rgl_node_t ros2pub = nullptr;
+
+	EXPECT_RGL_SUCCESS(rgl_node_publish_ros2_laserscan(&ros2pub, "laserscan", "rglFrame"));
+
+	EXPECT_RGL_SUCCESS(rgl_graph_node_add_child(points, yield));
+	EXPECT_RGL_SUCCESS(rgl_graph_node_add_child(yield, ros2pub));
+
+	EXPECT_RGL_SUCCESS(rgl_graph_run(points));
+
+	rclcpp::shutdown();
+	EXPECT_RGL_INVALID_PIPELINE(rgl_graph_run(points), "Unable to execute Ros2Node because ROS2 has been shut down.");
+}
+
+TEST_F(Ros2PublishLaserScanNodeTest, should_receive_sent_data)
+{
+	const auto POINT_COUNT = 5;
+	const auto TOPIC_NAME = "rgl_test_laser_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;
+	const auto START_ANGLE = 0.8f;
+	const auto ANGLE_INCREMENT = 0.11f;
+	const auto TIME_INCREMENT = 0.22f;
+
+	TestPointCloud input({AZIMUTH_F32, DISTANCE_F32, TIME_STAMP_F64, INTENSITY_F32}, POINT_COUNT);
+
+	// Expected data
+	const auto expectedAngles = generateFieldValues(
+	    POINT_COUNT, std::function<Field<AZIMUTH_F32>::type(int)>(
+	                     [START_ANGLE, ANGLE_INCREMENT](int i) { return START_ANGLE + i * ANGLE_INCREMENT; }));
+	input.setFieldValues<AZIMUTH_F32>(expectedAngles);
+	const auto expectedTimes = generateFieldValues(POINT_COUNT, std::function<Field<TIME_STAMP_F64>::type(int)>(
+	                                                          [TIME_INCREMENT](int i) { return i * TIME_INCREMENT; }));
+	input.setFieldValues<TIME_STAMP_F64>(expectedTimes);
+	const auto expectedRanges = input.getFieldValues<DISTANCE_F32>();
+	const auto expectedIntensities = input.getFieldValues<INTENSITY_F32>();
+	const auto [minRange, maxRange] = std::minmax_element(expectedRanges.begin(), expectedRanges.end());
+	const auto expectedScanTime = expectedTimes.at(expectedTimes.size() - 1) - expectedTimes.at(0);
+
+	// Create nodes
+	rgl_node_t inputNode = input.createUsePointsNode(), laserScanNode = nullptr;
+	ASSERT_RGL_SUCCESS(rgl_node_publish_ros2_laserscan(&laserScanNode, TOPIC_NAME, FRAME_ID));
+
+	// Connect nodes
+	ASSERT_RGL_SUCCESS(rgl_graph_node_add_child(inputNode, laserScanNode));
+
+	// Synchronization primitives
+	std::atomic<int> messageCount = 0;
+
+
+	// Create ROS2 subscriber to receive LaserScan messages on topic "laser_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<sensor_msgs::msg::LaserScan>(
+	    TOPIC_NAME, qos, [&](const sensor_msgs::msg::LaserScan::ConstSharedPtr msg) {
+		    EXPECT_EQ(msg->header.frame_id, FRAME_ID);
+		    EXPECT_NE(msg->header.stamp.sec + msg->header.stamp.nanosec, 0);
+
+			EXPECT_NEAR(msg->angle_min, START_ANGLE, EPSILON_F);
+			EXPECT_NEAR(msg->angle_max, START_ANGLE + (POINT_COUNT - 1) * ANGLE_INCREMENT, EPSILON_F);
+			EXPECT_NEAR(msg->range_min, *minRange, EPSILON_F);
+			EXPECT_NEAR(msg->range_max, *maxRange, EPSILON_F);
+			EXPECT_NEAR(msg->angle_increment, ANGLE_INCREMENT, EPSILON_F);
+			EXPECT_NEAR(msg->time_increment, TIME_INCREMENT, EPSILON_F);
+			EXPECT_NEAR(msg->scan_time, expectedScanTime, EPSILON_F);
+
+		    ASSERT_EQ(msg->ranges.size(), POINT_COUNT);
+		    ASSERT_EQ(msg->intensities.size(), POINT_COUNT);
+		    for (int i = 0; i < POINT_COUNT; ++i) {
+				EXPECT_NEAR(msg->ranges[i], expectedRanges[i], EPSILON_F);
+				EXPECT_NEAR(msg->intensities[i], expectedIntensities[i], EPSILON_F);
+		    }
+		    ++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);
+	}
+}