gz: add gstreamer plugin (#24475)

src/modules/simulation/gz_bridge/server.config

@@ -14,6 +14,7 @@
       <render_engine>ogre2</render_engine>
     </plugin>
     <plugin entity_name="*" entity_type="world" filename="libOpticalFlowSystem.so" name="custom::OpticalFlowSystem"/>
+    <plugin entity_name="*" entity_type="world" filename="libGstCameraSystem.so" name="custom::GstCameraSystem"/>
     <!-- <plugin entity_name="*" entity_type="world" filename="libTemplatePlugin.so" name="custom::TemplateSystem"/> -->
   </plugins>
 </server_config>

src/modules/simulation/gz_plugins/CMakeLists.txt

@@ -80,7 +80,8 @@ if (gz-transport${GZ_TRANSPORT_VERSION}_FOUND)
     # Add our plugins as subdirectories
     add_subdirectory(optical_flow)
     add_subdirectory(template_plugin)
+    add_subdirectory(gstreamer)
 
     # Add an alias target for each plugin
-    add_custom_target(px4_gz_plugins ALL DEPENDS OpticalFlowSystem TemplatePlugin)
+    add_custom_target(px4_gz_plugins ALL DEPENDS OpticalFlowSystem TemplatePlugin GstCameraSystem)
 endif()

src/modules/simulation/gz_plugins/gstreamer/CMakeLists.txt

@@ -0,0 +1,69 @@
+############################################################################
+#
+#   Copyright (c) 2025 PX4 Development Team. All rights reserved.
+#
+# Redistribution and use in source and binary forms, with or without
+# modification, are permitted provided that the following conditions
+# are met:
+#
+# 1. Redistributions of source code must retain the above copyright
+#    notice, this list of conditions and the following disclaimer.
+# 2. Redistributions in binary form must reproduce the above copyright
+#    notice, this list of conditions and the following disclaimer in
+#    the documentation and/or other materials provided with the
+#    distribution.
+# 3. Neither the name PX4 nor the names of its contributors may be
+#    used to endorse or promote products derived from this software
+#    without specific prior written permission.
+#
+# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+# OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+# AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+# POSSIBILITY OF SUCH DAMAGE.
+#
+############################################################################
+
+project(GstCameraSystem)
+
+# Find required packages
+find_package(OpenCV REQUIRED)
+find_package(PkgConfig REQUIRED)
+pkg_check_modules(GSTREAMER REQUIRED gstreamer-1.0)
+pkg_check_modules(GSTREAMER_APP REQUIRED gstreamer-app-1.0)
+
+add_library(${PROJECT_NAME} SHARED
+    GstCameraSystem.cpp
+)
+
+target_link_libraries(${PROJECT_NAME}
+    PUBLIC px4_gz_msgs
+    PUBLIC gz-sensors${GZ_SENSORS_VERSION}::gz-sensors${GZ_SENSORS_VERSION}
+    PUBLIC gz-plugin${GZ_PLUGIN_VERSION}::gz-plugin${GZ_PLUGIN_VERSION}
+    PUBLIC gz-sim${GZ_SIM_VERSION}::gz-sim${GZ_SIM_VERSION}
+    PUBLIC gz-transport${GZ_TRANSPORT_VERSION}::gz-transport${GZ_TRANSPORT_VERSION}
+    PUBLIC ${OpenCV_LIBS}
+    PUBLIC ${GSTREAMER_LIBRARIES}
+    PUBLIC ${GSTREAMER_APP_LIBRARIES}
+)
+
+target_include_directories(${PROJECT_NAME}
+    PUBLIC ${CMAKE_CURRENT_SOURCE_DIR}
+    PUBLIC ${CMAKE_CURRENT_BINARY_DIR}
+    PUBLIC px4_gz_msgs
+    PUBLIC ${OpenCV_INCLUDE_DIRS}
+    PUBLIC ${GSTREAMER_INCLUDE_DIRS}
+    PUBLIC ${GSTREAMER_APP_INCLUDE_DIRS}
+)
+
+target_compile_options(${PROJECT_NAME}
+    PRIVATE ${GSTREAMER_CFLAGS}
+    PRIVATE ${GSTREAMER_APP_CFLAGS}
+)

src/modules/simulation/gz_plugins/gstreamer/GstCameraSystem.cpp

@@ -0,0 +1,477 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2025 PX4 Development Team. All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in
+ *    the documentation and/or other materials provided with the
+ *    distribution.
+ * 3. Neither the name PX4 nor the names of its contributors may be
+ *    used to endorse or promote products derived from this software
+ *    without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
+#include "GstCameraSystem.hpp"
+
+#include <gz/plugin/Register.hh>
+#include <gz/common/Console.hh>
+#include <gz/transport/TopicUtils.hh>
+#include <gz/sim/Util.hh>
+
+using namespace custom;
+
+//////////////////////////////////////////////////
+GstCameraSystem::GstCameraSystem()
+{
+	// Default UDP host
+	const char *host_ip = std::getenv("PX4_VIDEO_HOST_IP");
+
+	if (host_ip) {
+		_udpHost = std::string(host_ip);
+
+	} else {
+		_udpHost = "127.0.0.1";
+	}
+
+	// Initialize gstreamer
+	static bool gstInitialized = false;
+
+	if (!gstInitialized) {
+		gst_init(nullptr, nullptr);
+		gstInitialized = true;
+	}
+
+	// Setup camera topic regex pattern
+	_cameraTopicPattern = std::regex("/world/([^/]+)/model/([^/]+)/link/([^/]+)/sensor/([^/]+)/image");
+}
+
+//////////////////////////////////////////////////
+GstCameraSystem::~GstCameraSystem()
+{
+	_running = false;
+
+	if (_gstLoop) {
+		g_main_loop_quit(_gstLoop);
+	}
+
+	if (_gstThread.joinable()) {
+		_gstThread.join();
+	}
+
+	if (_pipeline) {
+		gst_element_set_state(_pipeline, GST_STATE_NULL);
+		gst_object_unref(_pipeline);
+	}
+}
+
+//////////////////////////////////////////////////
+void GstCameraSystem::Configure(const gz::sim::Entity &_entity,
+				const std::shared_ptr<const sdf::Element> &_sdf,
+				gz::sim::EntityComponentManager &_ecm,
+				gz::sim::EventManager &/*_eventMgr*/)
+{
+	// Check if this is a world entity
+	if (!_ecm.EntityHasComponentType(_entity, gz::sim::components::World::typeId)) {
+		gzerr << "GstCameraSystem should be attached to a world entity" << std::endl;
+		return;
+	}
+
+	// Get world name
+	auto worldNameComp = _ecm.Component<gz::sim::components::Name>(_entity);
+
+	if (worldNameComp) {
+		_worldName = worldNameComp->Data();
+
+	} else {
+		gzerr << "Failed to get world name" << std::endl;
+		return;
+	}
+
+	gzdbg << "GstCameraSystem configured for world [" << _worldName << "]" << std::endl;
+
+	// Parse SDF parameters
+	if (_sdf->HasElement("udpHost")) {
+		_udpHost = _sdf->Get<std::string>("udpHost");
+	}
+
+	if (_sdf->HasElement("udpPort")) {
+		_udpPort = _sdf->Get<int>("udpPort");
+	}
+
+	if (_sdf->HasElement("rtmpLocation")) {
+		_rtmpLocation = _sdf->Get<std::string>("rtmpLocation");
+		_useRtmp = true;
+	}
+
+	if (_sdf->HasElement("useCuda")) {
+		_useCuda = _sdf->Get<bool>("useCuda");
+	}
+
+	gzdbg << "GstCameraSystem parameters:" << std::endl
+	      << "  UDP Host: " << _udpHost << std::endl
+	      << "  UDP Port: " << _udpPort << std::endl
+	      << "  Use RTMP: " << (_useRtmp ? "true" : "false") << std::endl
+	      << "  RTMP Location: " << _rtmpLocation << std::endl
+	      << "  Use CUDA: " << (_useCuda ? "true" : "false") << std::endl;
+}
+
+//////////////////////////////////////////////////
+void GstCameraSystem::PostUpdate(const gz::sim::UpdateInfo &_info,
+				 const gz::sim::EntityComponentManager &/*_ecm*/)
+{
+	if (_info.paused || _initialized) {
+		return;
+	}
+
+	// Find a camera topic
+	findCameraTopic();
+}
+
+//////////////////////////////////////////////////
+void GstCameraSystem::findCameraTopic()
+{
+	// Get all available topics
+	std::vector<std::string> topics;
+	_node.TopicList(topics);
+
+	for (const auto &topic : topics) {
+		std::smatch matches;
+
+		// Check if the topic matches our camera pattern
+		if (std::regex_search(topic, matches, _cameraTopicPattern)) {
+			std::string worldName = matches[1].str();
+
+			// Only process cameras in our world
+			if (worldName == _worldName) {
+				_cameraTopic = topic;
+
+				gzdbg << "Found camera topic: " << _cameraTopic << std::endl;
+
+				// Subscribe to first message to get camera info
+				_node.Subscribe(_cameraTopic, &GstCameraSystem::onCameraInfo, this);
+
+				_initialized = true;
+				return;
+			}
+		}
+	}
+
+	if (!_initialized) {
+		gzdbg << "No camera topics found yet, will check again next update" << std::endl;
+	}
+}
+
+//////////////////////////////////////////////////
+void GstCameraSystem::onCameraInfo(const gz::msgs::Image &msg)
+{
+	_width = msg.width();
+	_height = msg.height();
+
+	gzdbg << "Camera info: " << _width << "x" << _height << std::endl;
+
+	// Unsubscribe from the initial subscription
+	_node.Unsubscribe(_cameraTopic);
+
+	// Subscribe to actual stream with our callback
+	_node.Subscribe(_cameraTopic, &GstCameraSystem::onImage, this);
+
+	// Start GStreamer pipeline
+	_gstThread = std::thread(&GstCameraSystem::gstThreadFunc, this);
+}
+
+//////////////////////////////////////////////////
+void GstCameraSystem::onImage(const gz::msgs::Image &msg)
+{
+	if (!_running) {
+		return;
+	}
+
+	// Convert the image data to OpenCV format
+	cv::Mat frame(msg.height(), msg.width(), CV_8UC3);
+
+	// Check pixel format and convert if necessary
+	if (msg.pixel_format_type() == gz::msgs::PixelFormatType::RGB_INT8) {
+		// Copy RGB data directly
+		memcpy(frame.data, msg.data().c_str(), msg.data().size());
+
+		// Process the frame
+		std::lock_guard<std::mutex> lock(_frameMutex);
+		frame.copyTo(_currentFrame);
+		_newFrameAvailable = true;
+
+	} else {
+		gzwarn << "Unsupported pixel format: " << msg.pixel_format_type() << std::endl;
+	}
+}
+
+//////////////////////////////////////////////////
+void GstCameraSystem::gstThreadFunc()
+{
+	gzdbg << "Starting GStreamer thread" << std::endl;
+
+	_gstLoop = g_main_loop_new(nullptr, FALSE);
+
+	if (!_gstLoop) {
+		gzerr << "Failed to create GStreamer main loop" << std::endl;
+		return;
+	}
+
+	_pipeline = gst_pipeline_new(nullptr);
+
+	if (!_pipeline) {
+		gzerr << "Failed to create GStreamer pipeline" << std::endl;
+		g_main_loop_unref(_gstLoop);
+		_gstLoop = nullptr;
+		return;
+	}
+
+	// Create elements
+	_source = gst_element_factory_make("appsrc", nullptr);
+	GstElement *queue1 = gst_element_factory_make("queue", nullptr);
+	GstElement *videoRate = gst_element_factory_make("videorate", nullptr);
+	GstElement *converter = gst_element_factory_make("videoconvert", nullptr);
+	GstElement *queue2 = gst_element_factory_make("queue", nullptr);
+
+	// Configure source and queues for better buffering
+	g_object_set(G_OBJECT(queue1),
+		     "max-size-buffers", 30,
+		     "max-size-time", 0,
+		     "max-size-bytes", 0,
+		     "leaky", 2, // downstream (newer buffers)
+		     NULL);
+
+	g_object_set(G_OBJECT(queue2),
+		     "max-size-buffers", 30,
+		     "max-size-time", 0,
+		     "max-size-bytes", 0,
+		     NULL);
+
+	// Configure video rate to reduce tearing
+	g_object_set(G_OBJECT(videoRate),
+		     "max-rate", 30,
+		     "drop-only", TRUE,
+		     NULL);
+
+	GstElement *encoder;
+
+	if (_useCuda) {
+		encoder = gst_element_factory_make("nvh264enc", nullptr);
+
+		if (encoder) {
+			// Higher quality NVIDIA encoder settings
+			g_object_set(G_OBJECT(encoder),
+				     "bitrate", 4000,       // Increased bitrate for higher quality
+				     "preset", 2,           // Higher quality preset (HP)
+				     "rc-mode", 1,          // Constant bitrate mode
+				     "zerolatency", TRUE,   // Reduce latency
+				     "qp-const", 20,        // Lower QP (higher quality)
+				     NULL);
+
+		} else {
+			gzerr << "NVIDIA H.264 encoder not available, falling back to software encoder" << std::endl;
+			encoder = gst_element_factory_make("x264enc", nullptr);
+			g_object_set(G_OBJECT(encoder),
+				     "bitrate", 4000,
+				     "speed-preset", 4,     // Higher quality preset (slower)
+				     "tune", 4,             // 'zerolatency' tune option
+				     "key-int-max", 30,     // Keyframe every 30 frames (1s at 30 fps)
+				     "threads", 4,          // Use multiple threads
+				     "pass", 5,             // Quality-based VBR
+				     "quantizer", 20,       // Lower = higher quality
+				     NULL);
+		}
+
+	} else {
+		encoder = gst_element_factory_make("x264enc", nullptr);
+		g_object_set(G_OBJECT(encoder),
+			     "bitrate", 4000,
+			     "speed-preset", 4,       // Higher quality preset (slower)
+			     "tune", 4,               // 'zerolatency' tune option
+			     "key-int-max", 30,       // Keyframe every 30 frames (1s at 30 fps)
+			     "threads", 4,            // Use multiple threads
+			     "pass", 5,               // Quality-based VBR
+			     "quantizer", 20,         // Lower = higher quality
+			     NULL);
+	}
+
+	GstElement *payloader;
+	GstElement *sink;
+
+	if (_useRtmp) {
+		payloader = gst_element_factory_make("flvmux", nullptr);
+		g_object_set(G_OBJECT(payloader), "streamable", TRUE, NULL);
+		sink = gst_element_factory_make("rtmpsink", nullptr);
+		g_object_set(G_OBJECT(sink), "location", _rtmpLocation.c_str(), NULL);
+
+	} else {
+		payloader = gst_element_factory_make("rtph264pay", nullptr);
+		// Improve RTP settings for local streaming
+		g_object_set(G_OBJECT(payloader),
+			     "config-interval", 1,    // Send SPS/PPS with every I-frame
+			     "mtu", 1400,             // Large MTU for local network
+			     NULL);
+
+		sink = gst_element_factory_make("udpsink", nullptr);
+		g_object_set(G_OBJECT(sink),
+			     "host", _udpHost.c_str(),
+			     "port", _udpPort,
+			     "sync", FALSE,           // Don't sync, reduce latency
+			     "async", FALSE,          // Don't async, reduce latency
+			     NULL);
+	}
+
+	if (!_source || !queue1 || !videoRate || !converter || !queue2 || !encoder || !payloader || !sink) {
+		gzerr << "Failed to create one or more GStreamer elements" << std::endl;
+		gst_object_unref(_pipeline);
+		g_main_loop_unref(_gstLoop);
+		_pipeline = nullptr;
+		_gstLoop = nullptr;
+		return;
+	}
+
+	// Configure source
+	GstCaps *sourceCaps = gst_caps_new_simple("video/x-raw",
+			      "format", G_TYPE_STRING, "RGB",
+			      "width", G_TYPE_INT, _width,
+			      "height", G_TYPE_INT, _height,
+			      "framerate", GST_TYPE_FRACTION, (unsigned int)_rate, 1,
+			      NULL);
+
+	g_object_set(G_OBJECT(_source),
+		     "caps", sourceCaps,
+		     "is-live", TRUE,
+		     "do-timestamp", TRUE,
+		     "stream-type", GST_APP_STREAM_TYPE_STREAM,
+		     "format", GST_FORMAT_TIME,
+		     "min-latency", 0,
+		     "max-latency", 0,
+		     "emit-signals", TRUE,
+		     NULL);
+
+	gst_caps_unref(sourceCaps);
+
+	// Set caps filter after videorate to ensure consistent framerate
+	GstElement *capsFilter = gst_element_factory_make("capsfilter", nullptr);
+	GstCaps *rateCaps = gst_caps_new_simple("video/x-raw",
+						"framerate", GST_TYPE_FRACTION, (unsigned int)_rate, 1,
+						NULL);
+	g_object_set(G_OBJECT(capsFilter), "caps", rateCaps, NULL);
+	gst_caps_unref(rateCaps);
+
+	// Add elements to pipeline
+	gst_bin_add_many(GST_BIN(_pipeline), _source, queue1, videoRate, capsFilter, converter, queue2, encoder, payloader,
+			 sink, nullptr);
+
+	// Link elements
+	if (!gst_element_link_many(_source, queue1, videoRate, capsFilter, converter, queue2, encoder, payloader, sink,
+				   nullptr)) {
+		gzerr << "Failed to link GStreamer elements" << std::endl;
+		gst_object_unref(_pipeline);
+		g_main_loop_unref(_gstLoop);
+		_pipeline = nullptr;
+		_gstLoop = nullptr;
+		return;
+	}
+
+	// Start pipeline
+	if (gst_element_set_state(_pipeline, GST_STATE_PLAYING) == GST_STATE_CHANGE_FAILURE) {
+		gzerr << "Failed to set GStreamer pipeline to playing state" << std::endl;
+		gst_object_unref(_pipeline);
+		g_main_loop_unref(_gstLoop);
+		_pipeline = nullptr;
+		_gstLoop = nullptr;
+		return;
+	}
+
+	gzdbg << "GStreamer pipeline started, streaming to "
+	      << (_useRtmp ? _rtmpLocation : (_udpHost + ":" + std::to_string(_udpPort))) << std::endl;
+
+	_running = true;
+
+	// Process frames
+	while (_running) {
+		std::unique_lock<std::mutex> lock(_frameMutex);
+
+		if (_newFrameAvailable && !_currentFrame.empty()) {
+			// Push RGB data directly - we configured the caps to accept RGB
+			const guint size = _width * _height * 3; // RGB is 3 bytes per pixel
+			GstBuffer *buffer = gst_buffer_new_allocate(nullptr, size, nullptr);
+
+			if (buffer) {
+				GstMapInfo map;
+
+				if (gst_buffer_map(buffer, &map, GST_MAP_WRITE)) {
+					// Copy RGB data directly from the OpenCV Mat
+					memcpy(map.data, _currentFrame.data, size);
+					gst_buffer_unmap(buffer, &map);
+
+					// Add timing information for smoother playback
+					GstClock *clock = gst_system_clock_obtain();
+					GstClockTime timestamp = gst_clock_get_time(clock);
+					gst_object_unref(clock);
+
+					GST_BUFFER_PTS(buffer) = timestamp;
+					GST_BUFFER_DURATION(buffer) = gst_util_uint64_scale_int(1, GST_SECOND, (int)_rate);
+
+					GstFlowReturn ret = gst_app_src_push_buffer(GST_APP_SRC(_source), buffer);
+
+					if (ret != GST_FLOW_OK) {
+						gzerr << "Failed to push buffer to GStreamer pipeline: " << ret << std::endl;
+					}
+
+				} else {
+					gzerr << "Failed to map GStreamer buffer" << std::endl;
+					gst_buffer_unref(buffer);
+				}
+
+			} else {
+				gzerr << "Failed to allocate GStreamer buffer" << std::endl;
+			}
+
+			_newFrameAvailable = false;
+		}
+
+		lock.unlock();
+
+		// Sleep to prevent high CPU usage when no frames are available
+		std::this_thread::sleep_for(std::chrono::milliseconds(5));
+	}
+
+	// Cleanup
+	gst_element_set_state(_pipeline, GST_STATE_NULL);
+	gst_object_unref(_pipeline);
+	g_main_loop_unref(_gstLoop);
+	_pipeline = nullptr;
+	_gstLoop = nullptr;
+
+	gzdbg << "GStreamer thread stopped" << std::endl;
+}
+
+// Register this plugin
+GZ_ADD_PLUGIN(GstCameraSystem,
+	      gz::sim::System,
+	      GstCameraSystem::ISystemConfigure,
+	      GstCameraSystem::ISystemPostUpdate)
+
+// Add plugin alias for custom namespace
+GZ_ADD_PLUGIN_ALIAS(GstCameraSystem, "custom::GstCameraSystem")

src/modules/simulation/gz_plugins/gstreamer/GstCameraSystem.hpp

@@ -0,0 +1,116 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2025 PX4 Development Team. All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in
+ *    the documentation and/or other materials provided with the
+ *    distribution.
+ * 3. Neither the name PX4 nor the names of its contributors may be
+ *    used to endorse or promote products derived from this software
+ *    without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
+#pragma once
+
+#include <memory>
+#include <string>
+#include <thread>
+#include <mutex>
+#include <atomic>
+#include <regex>
+#include <opencv2/opencv.hpp>
+
+#include <gz/sim/System.hh>
+#include <gz/transport/Node.hh>
+#include <gz/msgs/image.pb.h>
+#include <gz/sim/components/Name.hh>
+#include <gz/sim/components/World.hh>
+
+#include <gst/gst.h>
+#include <gst/app/gstappsrc.h>
+
+namespace custom
+{
+class GstCameraSystem :
+	public gz::sim::System,
+	public gz::sim::ISystemConfigure,
+	public gz::sim::ISystemPostUpdate
+{
+public:
+	GstCameraSystem();
+	~GstCameraSystem();
+
+	void Configure(const gz::sim::Entity &_entity,
+		       const std::shared_ptr<const sdf::Element> &_sdf,
+		       gz::sim::EntityComponentManager &_ecm,
+		       gz::sim::EventManager &_eventMgr) override;
+
+	void PostUpdate(const gz::sim::UpdateInfo &_info,
+			const gz::sim::EntityComponentManager &_ecm) override;
+
+private:
+	void onImage(const gz::msgs::Image &msg);
+	void onCameraInfo(const gz::msgs::Image &msg);
+
+	// Find first camera topic in the world
+	void findCameraTopic();
+
+	void gstThreadFunc();
+
+	// Transport
+	gz::transport::Node _node;
+
+	// Image processing
+	cv::Mat _currentFrame;
+	std::mutex _frameMutex;
+	std::atomic<bool> _newFrameAvailable {};
+
+	// GStreamer elements
+	GMainLoop *_gstLoop {};
+	GstElement *_pipeline {};
+	GstElement *_source {};
+	std::thread _gstThread;
+	std::atomic<bool> _running {};
+
+	// Configuration
+	std::string _worldName;
+	std::string _udpHost;
+	int _udpPort = 5600;
+	bool _useRtmp {};
+	std::string _rtmpLocation;
+	bool _useCuda = true;
+
+	// Topic info
+	std::string _cameraTopic;
+	int _width {};
+	int _height {};
+	double _rate = 30.0;
+
+	// Topic pattern for matching camera image topics
+	std::regex _cameraTopicPattern;
+
+	// Flag to control discovery
+	bool _initialized {};
+};
+}  // namespace custom

src/modules/simulation/gz_plugins/gstreamer/README.md

@@ -0,0 +1,57 @@
+# GStreamer Camera System Plugin for Gazebo Harmonic
+
+This plugin provides GStreamer-based video streaming capabilities for cameras in Gazebo Harmonic simulation.
+
+## Features
+
+- Automatically discovers camera sensors in the simulation and selects the first.
+- Streams camera feed via UDP (RTP/H.264) or RTMP
+- Support for hardware acceleration with NVIDIA GPUs (CUDA)
+- Low-latency streaming
+
+## Prerequisites
+
+- GStreamer 1.0 with development files
+- OpenCV with development files
+- NVIDIA drivers and CUDA (optional, for hardware acceleration)
+
+## Configuration
+
+The plugin can be configured with the following parameters in the SDF model file:
+
+| Parameter | Type | Default | Description |
+|-----------|------|---------|-------------|
+| udpHost | string | 127.0.0.1 | Destination IP address for UDP streaming |
+| udpPort | int | 5600 | Destination port for UDP streaming |
+| rtmpLocation | string | | RTMP URL for streaming. If provided, RTMP will be used instead of UDP |
+| useCuda | bool | true | Enable NVIDIA hardware acceleration |
+
+## Viewing Streams
+
+### UDP Stream
+
+For UDP streams, you can use GStreamer to view the video feed:
+
+```bash
+gst-launch-1.0 udpsrc port=5600 \
+    ! application/x-rtp,encoding-name=H264,payload=96 \
+    ! rtph264depay \
+    ! avdec_h264 \
+    ! videoconvert \
+    ! autovideosink
+```
+
+### RTMP Stream
+
+For RTMP streams, you can use any RTMP-compatible player, such as:
+
+- VLC: Media > Open Network Stream > rtmp://your-rtmp-url
+- ffplay: `ffplay rtmp://your-rtmp-url`
+
+## Environment Variables
+
+- `PX4_VIDEO_HOST_IP`: Can be set to override the default UDP destination IP
+
+## License
+
+BSD 3-Clause