From 7c2aa726903e7ad6d271d46a4b5c7431b8a53cfc Mon Sep 17 00:00:00 2001 From: Jacob Dahl <37091262+dakejahl@users.noreply.github.com> Date: Sun, 9 Mar 2025 00:11:52 -0900 Subject: [PATCH] gz: add gstreamer plugin (#24475) --- .../simulation/gz_bridge/server.config | 1 + .../simulation/gz_plugins/CMakeLists.txt | 3 +- .../gz_plugins/gstreamer/CMakeLists.txt | 69 +++ .../gz_plugins/gstreamer/GstCameraSystem.cpp | 477 ++++++++++++++++++ .../gz_plugins/gstreamer/GstCameraSystem.hpp | 116 +++++ .../simulation/gz_plugins/gstreamer/README.md | 57 +++ 6 files changed, 722 insertions(+), 1 deletion(-) create mode 100644 src/modules/simulation/gz_plugins/gstreamer/CMakeLists.txt create mode 100644 src/modules/simulation/gz_plugins/gstreamer/GstCameraSystem.cpp create mode 100644 src/modules/simulation/gz_plugins/gstreamer/GstCameraSystem.hpp create mode 100644 src/modules/simulation/gz_plugins/gstreamer/README.md diff --git a/src/modules/simulation/gz_bridge/server.config b/src/modules/simulation/gz_bridge/server.config index 93bff3f442..0001356a3a 100644 --- a/src/modules/simulation/gz_bridge/server.config +++ b/src/modules/simulation/gz_bridge/server.config @@ -14,6 +14,7 @@ ogre2 + diff --git a/src/modules/simulation/gz_plugins/CMakeLists.txt b/src/modules/simulation/gz_plugins/CMakeLists.txt index 4bb3307108..8ee1bdd6ed 100644 --- a/src/modules/simulation/gz_plugins/CMakeLists.txt +++ b/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() diff --git a/src/modules/simulation/gz_plugins/gstreamer/CMakeLists.txt b/src/modules/simulation/gz_plugins/gstreamer/CMakeLists.txt new file mode 100644 index 0000000000..2c07a5d0c7 --- /dev/null +++ b/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} +) diff --git a/src/modules/simulation/gz_plugins/gstreamer/GstCameraSystem.cpp b/src/modules/simulation/gz_plugins/gstreamer/GstCameraSystem.cpp new file mode 100644 index 0000000000..9ff384e1e0 --- /dev/null +++ b/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 +#include +#include +#include + +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 &_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(_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("udpHost"); + } + + if (_sdf->HasElement("udpPort")) { + _udpPort = _sdf->Get("udpPort"); + } + + if (_sdf->HasElement("rtmpLocation")) { + _rtmpLocation = _sdf->Get("rtmpLocation"); + _useRtmp = true; + } + + if (_sdf->HasElement("useCuda")) { + _useCuda = _sdf->Get("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 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 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 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") diff --git a/src/modules/simulation/gz_plugins/gstreamer/GstCameraSystem.hpp b/src/modules/simulation/gz_plugins/gstreamer/GstCameraSystem.hpp new file mode 100644 index 0000000000..d383171117 --- /dev/null +++ b/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 +#include +#include +#include +#include +#include +#include + +#include +#include +#include +#include +#include + +#include +#include + +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 &_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 _newFrameAvailable {}; + + // GStreamer elements + GMainLoop *_gstLoop {}; + GstElement *_pipeline {}; + GstElement *_source {}; + std::thread _gstThread; + std::atomic _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 diff --git a/src/modules/simulation/gz_plugins/gstreamer/README.md b/src/modules/simulation/gz_plugins/gstreamer/README.md new file mode 100644 index 0000000000..b8b17e168a --- /dev/null +++ b/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