From 05706b7b5683b26cf9277bfc0628a7ba7adbe734 Mon Sep 17 00:00:00 2001 From: Terran Gerratt Date: Tue, 18 Jun 2024 14:51:34 -0600 Subject: [PATCH 1/6] Camera: ported GstCameraPlugin from PX4 gazebo classic --- CMakeLists.txt | 21 +- include/GstCameraPlugin.hh | 71 +++++ models/gimbal_small_3d/model.sdf | 8 + src/GstCameraPlugin.cc | 481 +++++++++++++++++++++++++++++++ 4 files changed, 580 insertions(+), 1 deletion(-) create mode 100644 include/GstCameraPlugin.hh create mode 100644 src/GstCameraPlugin.cc diff --git a/CMakeLists.txt b/CMakeLists.txt index 0278b07..3ec097d 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -52,8 +52,11 @@ else() endif() # --------------------------------------------------------------------------- # -# Find RapidJSON. find_package(RapidJSON REQUIRED) +find_package(OpenCV REQUIRED) + +pkg_check_modules(GST REQUIRED gstreamer-1.0 gstreamer-app-1.0) + # --------------------------------------------------------------------------- # # Build plugin. @@ -95,6 +98,21 @@ target_link_libraries(CameraZoomPlugin PRIVATE gz-sim${GZ_SIM_VER}::gz-sim${GZ_SIM_VER} ) +add_library(GstCameraPlugin + SHARED + src/GstCameraPlugin.cc +) +target_include_directories(GstCameraPlugin PRIVATE + include + ${OpenCV_INCLUDE_DIRS} + ${GST_INCLUDE_DIRS} +) +target_link_libraries(GstCameraPlugin PRIVATE + gz-sim${GZ_SIM_VER}::gz-sim${GZ_SIM_VER} + ${OpenCV_LIBS} + ${GST_LINK_LIBRARIES} +) + # --------------------------------------------------------------------------- # # Install. @@ -103,6 +121,7 @@ install( ArduPilotPlugin ParachutePlugin CameraZoomPlugin + GstCameraPlugin DESTINATION lib/${PROJECT_NAME} ) diff --git a/include/GstCameraPlugin.hh b/include/GstCameraPlugin.hh new file mode 100644 index 0000000..981c200 --- /dev/null +++ b/include/GstCameraPlugin.hh @@ -0,0 +1,71 @@ +/* + * Copyright (C) 2012-2016 Open Source Robotics Foundation + * + * 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. + * +*/ +#ifndef GSTCAMERAPLUGIN_HH_ +#define GSTCAMERAPLUGIN_HH_ + +#include + +#include + +/** + * @class GstCameraPlugin + * A Gazebo plugin that can be attached to a camera and then streams the video data using gstreamer. + * It streams to a configurable UDP IP and UDP Port, defaults are respectively 127.0.0.1 and 5600. + * IP and Port can be configured in the SDF as well as by GZ_VIDEO_HOST_IP and GZ_VIDEO_HOST_PORT env variables + * + * Connect to the stream via command line with: + gst-launch-1.0 -v udpsrc port=5600 caps='application/x-rtp, media=(string)video, clock-rate=(int)90000, encoding-name=(string)H264' ! rtph264depay ! avdec_h264 ! videoconvert ! autovideosink sync=false + */ +namespace gz { +namespace sim { +inline namespace GZ_SIM_VERSION_NAMESPACE { +namespace systems { + +/// \brief Camera stream plugin. +class GstCameraPlugin : + public System, + public ISystemConfigure, + public ISystemPreUpdate +{ + /// \brief Destructor + public: virtual ~GstCameraPlugin(); + + /// \brief Constructor + public: GstCameraPlugin(); + + // Documentation inherited + public: void PreUpdate(const gz::sim::UpdateInfo &_info, + gz::sim::EntityComponentManager &_ecm) final; + + // Documentation inherited + public: void Configure(const Entity &_entity, + const std::shared_ptr &_sdf, + EntityComponentManager &_ecm, + EventManager &) final; + + /// \internal + /// \brief Private implementation + private: class Impl; + private: std::unique_ptr impl; +}; + +} // namespace systems +} +} // namespace sim +} // namespace gz + +#endif // GSTCAMERAPLUGIN_HH_ diff --git a/models/gimbal_small_3d/model.sdf b/models/gimbal_small_3d/model.sdf index 19b7805..9ca84da 100644 --- a/models/gimbal_small_3d/model.sdf +++ b/models/gimbal_small_3d/model.sdf @@ -210,6 +210,14 @@ 125.0 0.42514285714 + + + 127.0.0.1 + 5600 + /world/iris_runway/model/iris_with_gimbal/model/gimbal/link/pitch_link/sensor/camera/image + /enable_video_stream + diff --git a/src/GstCameraPlugin.cc b/src/GstCameraPlugin.cc new file mode 100644 index 0000000..68b78a4 --- /dev/null +++ b/src/GstCameraPlugin.cc @@ -0,0 +1,481 @@ +/* + * Copyright (C) 2012-2016 Open Source Robotics Foundation + * + * 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 "GstCameraPlugin.hh" + +#include +#include + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +namespace gz { +namespace sim { +inline namespace GZ_SIM_VERSION_NAMESPACE { +namespace systems { + +////////////////////////////////////////////////// + +class GstCameraPlugin::Impl { + public: + void initializeCamera(); + void startStreaming(); + static void *start_thread(void *); + void startGstThread(); + + void onImage(const msgs::Image &msg); + void onVideoStreamEnable(const msgs::Boolean &_msg); + void OnRenderTeardown(); + + void stopStreaming(); + void stopGstThread(); + + std::string udpHost; + int udpPort; + bool useRtmpPipeline = false; + std::string rtmpLocation; + bool useBasicPipeline = false; + bool useCuda; + std::string imageTopic; + std::string enableTopic; + + unsigned int width = 0; + unsigned int height = 0; + float rate = 5; // unused by actual pipeline since it's based on the gazebo topic rate? + + pthread_t threadId; + bool isGstMainLoopActive = false; + bool requestedStartStreaming = false; + + GMainLoop *gst_loop = nullptr; + GstElement *source = nullptr; + void createMpeg2tsPipeline(GstElement *pipeline); + void createRtmpPipeline(GstElement *pipeline); + void createGenericPipeline(GstElement *pipeline); + GstElement *createEncoder(); + + Entity parentSensor{kNullEntity}; + rendering::ScenePtr scene; + rendering::CameraPtr camera; + std::string cameraName; + std::vector connections; + transport::Node node; + + bool SensorValid(const EntityComponentManager &_ecm) const { + return nullptr != _ecm.Component(parentSensor); + } + + std::optional SensorName(const EntityComponentManager &_ecm) const { + return _ecm.ComponentData(parentSensor); + } +}; + +////////////////////////////////////////////////// + +GstCameraPlugin::GstCameraPlugin() + : impl(std::make_unique()) { +} + +GstCameraPlugin::~GstCameraPlugin() { + impl->OnRenderTeardown(); +} + +void GstCameraPlugin::Configure( + const Entity &_entity, + const std::shared_ptr &_sdf, + EntityComponentManager &_ecm, + EventManager &_eventMgr) { + impl->parentSensor = _entity; + if (!impl->SensorValid(_ecm)) { + gzerr << "GstCameraPlugin must be attached to a camera sensor. " + "Failed to initialize" + << std::endl; + return; + } + + if (auto maybeName = impl->SensorName(_ecm)) { + gzwarn << "GstCameraPlugin attached to sensor [" + << maybeName.value() << "]" << std::endl; + } else { + gzerr << "Camera sensor has invalid name" << std::endl; + return; + } + + impl->udpHost = "127.0.0.1"; + const char *host_ip = std::getenv("GZ_VIDEO_HOST_IP"); + if (host_ip) { + impl->udpHost = std::string(host_ip); + } else if (_sdf->HasElement("udpHost")) { + impl->udpHost = _sdf->Get("udpHost"); + } + + impl->udpPort = 5600; + const char *host_port = std::getenv("GZ_VIDEO_HOST_PORT"); + if (host_port) { + sscanf(host_port, "%d", &impl->udpPort); + } else if (_sdf->HasElement("udpPort")) { + impl->udpPort = _sdf->Get("udpPort"); + } + gzwarn << "Streaming video to " << impl->udpHost << ":" << impl->udpPort << std::endl; + + // uses MPEG2TS pipeline by default. RTMP and Generic are mutually exclusive with priority to RTMP + if (_sdf->HasElement("rtmpLocation")) { + impl->rtmpLocation = _sdf->Get("rtmpLocation"); + impl->useRtmpPipeline = true; + + } else if (_sdf->HasElement("useBasicPipeline")) { + impl->useBasicPipeline = _sdf->Get("useBasicPipeline"); + } + + // Use CUDA for video encoding + if (_sdf->HasElement("useCuda")) { + impl->useCuda = _sdf->Get("useCuda"); + } else { + impl->useCuda = false; + } + + impl->imageTopic = "/world/iris_runway/model/iris_with_gimbal/model/gimbal/link/pitch_link/sensor/camera/image"; + const char *image_topic = std::getenv("GZ_VIDEO_TOPIC"); + if (image_topic) { + impl->imageTopic = std::string(image_topic); + } else if (_sdf->HasElement("imageTopic")) { + impl->imageTopic = _sdf->Get("imageTopic"); + } + + impl->enableTopic = "/enable_video_stream"; + const char *enable_topic = std::getenv("GZ_ENABLE_TOPIC"); + if (enable_topic) { + impl->enableTopic = std::string(enable_topic); + } else if (_sdf->HasElement("enableTopic")) { + impl->enableTopic = _sdf->Get("enableTopic"); + } + + // subscribe to gazebo topics + impl->node.Subscribe(impl->enableTopic, &GstCameraPlugin::Impl::onVideoStreamEnable, impl.get()); + impl->node.Subscribe(impl->imageTopic, &GstCameraPlugin::Impl::onImage, impl.get()); + + // subscribe to events + impl->connections.push_back(_eventMgr.Connect( + std::bind(&GstCameraPlugin::Impl::OnRenderTeardown, impl.get()))); +} + +void GstCameraPlugin::PreUpdate(const UpdateInfo &_info, EntityComponentManager &_ecm) { + if (impl->cameraName.empty()) { + Entity cameraEntity = impl->parentSensor; + impl->cameraName = removeParentScope(scopedName(cameraEntity, _ecm, "::", false), "::"); + gzwarn << "Camera name: [" << impl->cameraName << "]" << std::endl; + } + + if (!impl->camera && !impl->cameraName.empty()) { + impl->initializeCamera(); + return; + } +} + +void GstCameraPlugin::Impl::initializeCamera() { + // Wait for render engine to be available. + if (rendering::loadedEngines().empty()) return; + + // Get scene. + if (!scene) { + scene = rendering::sceneFromFirstRenderEngine(); + } + + // Return if scene not ready or no sensors available. + if (scene == nullptr || !scene->IsInitialized() || scene->SensorCount() == 0) { + gzwarn << "No scene or camera sensors available" << std::endl; + return; + } + + // Get camera. + if (!camera) { + auto sensor = scene->SensorByName(cameraName); + if (!sensor) { + gzerr << "Unable to find sensor: [" << cameraName << "]" << std::endl; + return; + } + + camera = std::dynamic_pointer_cast(sensor); + if (!camera) { + gzerr << "[" << cameraName << "] is not a camera" << std::endl; + return; + } + } +} + +void GstCameraPlugin::Impl::startStreaming() { + if (!isGstMainLoopActive) { + pthread_create(&threadId, NULL, start_thread, this); + } +} + +void *GstCameraPlugin::Impl::start_thread(void *param) { + GstCameraPlugin::Impl *impl = (GstCameraPlugin::Impl *)param; + impl->startGstThread(); + return nullptr; +} + +void GstCameraPlugin::Impl::startGstThread() { + gst_init(nullptr, nullptr); + + gst_loop = g_main_loop_new(nullptr, FALSE); + if (!gst_loop) { + gzerr << "Create loop failed" << std::endl; + return; + } + + GstElement *pipeline = gst_pipeline_new(nullptr); + if (!pipeline) { + gzerr << "ERR: Create pipeline failed" << std::endl; + return; + } + + source = gst_element_factory_make("appsrc", nullptr); + if (useRtmpPipeline) { + createRtmpPipeline(pipeline); + } else if (useBasicPipeline) { + createGenericPipeline(pipeline); + } else { + createMpeg2tsPipeline(pipeline); + } + + // Configure source element + g_object_set(G_OBJECT(source), "caps", + gst_caps_new_simple("video/x-raw", + "format", G_TYPE_STRING, "I420", + "width", G_TYPE_INT, width, + "height", G_TYPE_INT, height, + "framerate", GST_TYPE_FRACTION, (unsigned int)rate, 1, nullptr), + "is-live", TRUE, + "do-timestamp", TRUE, + "stream-type", GST_APP_STREAM_TYPE_STREAM, + "format", GST_FORMAT_TIME, nullptr); + + gst_object_ref(source); + + // Start + auto ret = gst_element_set_state(pipeline, GST_STATE_PLAYING); + if (ret != GST_STATE_CHANGE_SUCCESS) { + gzwarn << "State change result: " << ret << std::endl; + } + + // this call blocks until the main_loop is killed + gzwarn << "starting gstreamer main loop" << std::endl; + isGstMainLoopActive = true; + g_main_loop_run(gst_loop); + isGstMainLoopActive = false; + requestedStartStreaming = false; + gzwarn << "gstreamer main loop ended" << std::endl; + + // Clean up + gst_element_set_state(pipeline, GST_STATE_NULL); + gst_object_unref(GST_OBJECT(pipeline)); + gst_object_unref(source); + g_main_loop_unref(gst_loop); + gst_loop = nullptr; + source = nullptr; +} + +void GstCameraPlugin::Impl::createRtmpPipeline(GstElement *pipeline) { + gzdbg << "Creating RTMP pipeline" << std::endl; + GstElement *queue = gst_element_factory_make("queue", nullptr); + GstElement *converter = gst_element_factory_make("videoconvert", nullptr); + GstElement *encoder = createEncoder(); + GstElement *payloader = gst_element_factory_make("flvmux", nullptr); + GstElement *sink = gst_element_factory_make("rtmpsink", nullptr); + + g_object_set(G_OBJECT(sink), "location", rtmpLocation.c_str(), nullptr); + + if (!source || !queue || !converter || !encoder || !payloader || !sink) { + gzerr << "ERR: Create elements failed" << std::endl; + return; + } + + // Connect all elements to pipeline + gst_bin_add_many(GST_BIN(pipeline), source, queue, converter, encoder, payloader, sink, nullptr); + + // Link all elements + if (gst_element_link_many(source, queue, converter, encoder, payloader, sink, nullptr) != TRUE) { + gzerr << "ERR: Link elements failed" << std::endl; + return; + } +} + +void GstCameraPlugin::Impl::createGenericPipeline(GstElement *pipeline) { + gzdbg << "Creating Generic pipeline" << std::endl; + GstElement *queue = gst_element_factory_make("queue", nullptr); + GstElement *converter = gst_element_factory_make("videoconvert", nullptr); + GstElement *encoder = createEncoder(); + GstElement *payloader = gst_element_factory_make("rtph264pay", nullptr); + GstElement *sink = gst_element_factory_make("udpsink", nullptr); + + g_object_set(G_OBJECT(sink), "host", udpHost.c_str(), "port", udpPort, nullptr); + + if (!source || !queue || !converter || !encoder || !payloader || !sink) { + gzerr << "ERR: Create elements failed" << std::endl; + return; + } + + // Connect all elements to pipeline + gst_bin_add_many(GST_BIN(pipeline), source, queue, converter, encoder, payloader, sink, nullptr); + + // Link all elements + if (gst_element_link_many(source, queue, converter, encoder, payloader, sink, nullptr) != TRUE) { + gzerr << "ERR: Link elements failed" << std::endl; + return; + } +} + +void GstCameraPlugin::Impl::createMpeg2tsPipeline(GstElement *pipeline) { + gzdbg << "Creating MPEG2TS pipeline" << std::endl; + GstElement *queue = gst_element_factory_make("queue", nullptr); + GstElement *converter = gst_element_factory_make("videoconvert", nullptr); + GstElement *encoder = createEncoder(); + GstElement *h264_parser = gst_element_factory_make("h264parse", nullptr); + GstElement *payloader = gst_element_factory_make("mpegtsmux", nullptr); + GstElement *queue_mpeg = gst_element_factory_make("queue", nullptr); + GstElement *sink = gst_element_factory_make("udpsink", nullptr); + + g_object_set(G_OBJECT(payloader), "alignment", 7, nullptr); + g_object_set(G_OBJECT(sink), "host", udpHost.c_str(), "port", udpPort, "sync", false, nullptr); + + if (!source || !queue || !converter || !encoder || !h264_parser || !payloader || !queue_mpeg || !sink) { + gzerr << "ERR: Create elements failed" << std::endl; + return; + } + + gst_bin_add_many(GST_BIN(pipeline), source, queue, converter, encoder, h264_parser, payloader, queue_mpeg, sink, nullptr); + if (gst_element_link_many(source, queue, converter, encoder, h264_parser, payloader, queue_mpeg, sink, nullptr) != TRUE) { + gzerr << "ERR: Link elements failed" << std::endl; + return; + } +} + +GstElement* GstCameraPlugin::Impl::createEncoder() { + GstElement* encoder; + if (useCuda) { + gzdbg << "Using Cuda" << std::endl; + encoder = gst_element_factory_make("nvh264enc", nullptr); + g_object_set(G_OBJECT(encoder), "bitrate", 800, "preset", 1, nullptr); + } else { + encoder = gst_element_factory_make("x264enc", nullptr); + g_object_set(G_OBJECT(encoder), "bitrate", 800, "speed-preset", 6, "tune", 4, "key-int-max", 10, nullptr); + } + return encoder; +} + +void GstCameraPlugin::Impl::onImage(const msgs::Image &msg) { + if (!requestedStartStreaming) { + requestedStartStreaming = true; + width = msg.width(); + height = msg.height(); + startStreaming(); + return; + } + + if (!isGstMainLoopActive) return; + + // Alloc buffer + const guint size = width * height * 1.5; + GstBuffer *buffer = gst_buffer_new_allocate(NULL, size, NULL); + + if (!buffer) { + gzerr << "gst_buffer_new_allocate failed" << std::endl; + return; + } + + GstMapInfo map; + + if (!gst_buffer_map(buffer, &map, GST_MAP_WRITE)) { + gzerr << "gst_buffer_map failed" << std::endl; + return; + } + + // Color Conversion from RGB to YUV + cv::Mat frame = cv::Mat(height, width, CV_8UC3); + cv::Mat frameYUV = cv::Mat(height, width, CV_8UC3); + frame.data = (uchar *)msg.data().c_str(); + + cvtColor(frame, frameYUV, cv::COLOR_RGB2YUV_I420); + memcpy(map.data, frameYUV.data, size); + gst_buffer_unmap(buffer, &map); + + GstFlowReturn ret = gst_app_src_push_buffer(GST_APP_SRC(this->source), buffer); + if (ret != GST_FLOW_OK) { + /* something wrong, stop pushing */ + gzerr << "gst_app_src_push_buffer failed" << std::endl; + g_main_loop_quit(gst_loop); + } +} + +void GstCameraPlugin::Impl::onVideoStreamEnable(const msgs::Boolean &msg) { + gzwarn << "VideoStreamEnable: " << msg.data() << std::endl; + msg.data() ? startStreaming() : stopStreaming(); +} + +void GstCameraPlugin::Impl::OnRenderTeardown() { + gzdbg << "onRenderTeardown" << std::endl; + + camera.reset(); + scene.reset(); + stopStreaming(); +} + +void GstCameraPlugin::Impl::stopStreaming() { + if (isGstMainLoopActive) { + stopGstThread(); + + pthread_join(threadId, NULL); + isGstMainLoopActive = false; + } +} + +void GstCameraPlugin::Impl::stopGstThread() { + if (gst_loop) { + g_main_loop_quit(gst_loop); + } +} + +////////////////////////////////////////////////// + +} // namespace systems +} // namespace GZ_SIM_VERSION_NAMESPACE +} // namespace sim +} // namespace gz + +GZ_ADD_PLUGIN( + gz::sim::systems::GstCameraPlugin, + gz::sim::System, + gz::sim::systems::GstCameraPlugin::ISystemConfigure, + gz::sim::systems::GstCameraPlugin::ISystemPreUpdate) + +GZ_ADD_PLUGIN_ALIAS( + gz::sim::systems::GstCameraPlugin, + "GstCameraPlugin") \ No newline at end of file From 0ced991503c81f6602cf124c5fabcfd2a8199199 Mon Sep 17 00:00:00 2001 From: Rhys Mainwaring Date: Thu, 20 Jun 2024 18:34:10 +0100 Subject: [PATCH 2/6] GstCameraPlugin: apply coding style and remove env variable options - Update class and parameter documentation. - Apply Gazebo code style (mainly removing cuddled braces). - Use snake case for SDF elements. - Remove the environment variable defaults. - Require mandatory fields are set in SDF. - Use sensor topic name as default for image topic. - Use sensor topic name as prefix for enable streaming topic. - Update class doc string. - Update gz message strings. - Ensure streaming stops / starts when requested. Signed-off-by: Rhys Mainwaring --- include/GstCameraPlugin.hh | 56 +++-- src/GstCameraPlugin.cc | 500 +++++++++++++++++++++++-------------- 2 files changed, 348 insertions(+), 208 deletions(-) diff --git a/include/GstCameraPlugin.hh b/include/GstCameraPlugin.hh index 981c200..a9875a9 100644 --- a/include/GstCameraPlugin.hh +++ b/include/GstCameraPlugin.hh @@ -1,3 +1,7 @@ +/* + * Copyright (C) 2024 ArduPilot + */ + /* * Copyright (C) 2012-2016 Open Source Robotics Foundation * @@ -13,7 +17,7 @@ * See the License for the specific language governing permissions and * limitations under the License. * -*/ + */ #ifndef GSTCAMERAPLUGIN_HH_ #define GSTCAMERAPLUGIN_HH_ @@ -21,21 +25,39 @@ #include -/** - * @class GstCameraPlugin - * A Gazebo plugin that can be attached to a camera and then streams the video data using gstreamer. - * It streams to a configurable UDP IP and UDP Port, defaults are respectively 127.0.0.1 and 5600. - * IP and Port can be configured in the SDF as well as by GZ_VIDEO_HOST_IP and GZ_VIDEO_HOST_PORT env variables - * - * Connect to the stream via command line with: - gst-launch-1.0 -v udpsrc port=5600 caps='application/x-rtp, media=(string)video, clock-rate=(int)90000, encoding-name=(string)H264' ! rtph264depay ! avdec_h264 ! videoconvert ! autovideosink sync=false - */ namespace gz { namespace sim { inline namespace GZ_SIM_VERSION_NAMESPACE { namespace systems { -/// \brief Camera stream plugin. +/// \brief Plugin to stream camera sensor data using GStreamer. +/// \class GstCameraPlugin +/// +/// A Gazebo plugin that can be attached to a camera and then streams the +/// video data using gstreamer. +/// +/// Parameters +/// the UDP host IP, defaults to 127.0.0.1 +/// the UDP port, defaults to 5600 +/// the RTMP location +/// set to true if not using +/// set to true to use CUDA (if available) +/// the camera image topic +/// the topic to enable / disable video streaming +/// +/// Start streaming +/// assumes: /camera/enable_streaming +/// +/// gz topic -t "/camera/enable_streaming" -m gz.msgs.Boolean -p "data: 1" +/// +/// Connect to the stream via command line and open an OpenGL window: +/// +/// gst-launch-1.0 -v udpsrc port=5600 caps='application/x-rtp, +/// media=(string)video, clock-rate=(int)90000, +/// encoding-name=(string)H264' +/// ! rtph264depay ! avdec_h264 ! videoconvert +/// ! autovideosink sync=false +/// class GstCameraPlugin : public System, public ISystemConfigure, @@ -57,15 +79,15 @@ class GstCameraPlugin : EntityComponentManager &_ecm, EventManager &) final; - /// \internal + /// \internal /// \brief Private implementation private: class Impl; private: std::unique_ptr impl; -}; +}; -} // namespace systems +} // namespace systems } -} // namespace sim -} // namespace gz +} // namespace sim +} // namespace gz -#endif // GSTCAMERAPLUGIN_HH_ +#endif // GSTCAMERAPLUGIN_HH_ diff --git a/src/GstCameraPlugin.cc b/src/GstCameraPlugin.cc index 68b78a4..7c7f984 100644 --- a/src/GstCameraPlugin.cc +++ b/src/GstCameraPlugin.cc @@ -20,6 +20,10 @@ #include #include +#include +#include +#include + #include #include #include @@ -31,10 +35,8 @@ #include #include #include -#include + #include -#include -#include namespace gz { namespace sim { @@ -45,65 +47,61 @@ namespace systems { class GstCameraPlugin::Impl { public: - void initializeCamera(); - void startStreaming(); - static void *start_thread(void *); - void startGstThread(); + void InitializeCamera(); + void StartStreaming(); + static void *StartThread(void *); + void StartGstThread(); - void onImage(const msgs::Image &msg); - void onVideoStreamEnable(const msgs::Boolean &_msg); + void OnImage(const msgs::Image &msg); + void OnVideoStreamEnable(const msgs::Boolean &_msg); void OnRenderTeardown(); - void stopStreaming(); - void stopGstThread(); + void StopStreaming(); + void StopGstThread(); - std::string udpHost; - int udpPort; - bool useRtmpPipeline = false; + std::string udpHost{"127.0.0.1"}; + int udpPort{5600}; + bool useRtmpPipeline{false}; std::string rtmpLocation; - bool useBasicPipeline = false; - bool useCuda; + bool useBasicPipeline{false}; + bool useCuda{false}; std::string imageTopic; std::string enableTopic; - unsigned int width = 0; - unsigned int height = 0; - float rate = 5; // unused by actual pipeline since it's based on the gazebo topic rate? + unsigned int width{0}; + unsigned int height{0}; - pthread_t threadId; - bool isGstMainLoopActive = false; - bool requestedStartStreaming = false; + // Unused by actual pipeline since it's based on the gazebo topic rate? + unsigned int rate{5}; - GMainLoop *gst_loop = nullptr; - GstElement *source = nullptr; - void createMpeg2tsPipeline(GstElement *pipeline); - void createRtmpPipeline(GstElement *pipeline); - void createGenericPipeline(GstElement *pipeline); - GstElement *createEncoder(); - - Entity parentSensor{kNullEntity}; + pthread_t threadId; + bool isGstMainLoopActive{false}; + bool requestedStartStreaming{false}; + + GMainLoop *gst_loop{nullptr}; + GstElement *source{nullptr}; + void CreateMpeg2tsPipeline(GstElement *pipeline); + void CreateRtmpPipeline(GstElement *pipeline); + void CreateGenericPipeline(GstElement *pipeline); + GstElement *CreateEncoder(); + + bool is_initialised{false}; + Sensor parentSensor; rendering::ScenePtr scene; rendering::CameraPtr camera; std::string cameraName; std::vector connections; transport::Node node; - - bool SensorValid(const EntityComponentManager &_ecm) const { - return nullptr != _ecm.Component(parentSensor); - } - - std::optional SensorName(const EntityComponentManager &_ecm) const { - return _ecm.ComponentData(parentSensor); - } }; ////////////////////////////////////////////////// - GstCameraPlugin::GstCameraPlugin() - : impl(std::make_unique()) { + : impl(std::make_unique()) +{ } -GstCameraPlugin::~GstCameraPlugin() { +GstCameraPlugin::~GstCameraPlugin() +{ impl->OnRenderTeardown(); } @@ -111,168 +109,236 @@ void GstCameraPlugin::Configure( const Entity &_entity, const std::shared_ptr &_sdf, EntityComponentManager &_ecm, - EventManager &_eventMgr) { - impl->parentSensor = _entity; - if (!impl->SensorValid(_ecm)) { - gzerr << "GstCameraPlugin must be attached to a camera sensor. " - "Failed to initialize" - << std::endl; + EventManager &_eventMgr) +{ + impl->parentSensor = Sensor(_entity); + + if (!impl->parentSensor.Valid(_ecm)) + { + gzerr << "GstCameraPlugin: must be attached to a camera sensor. " + "Failed to initialize" << std::endl; return; } - if (auto maybeName = impl->SensorName(_ecm)) { - gzwarn << "GstCameraPlugin attached to sensor [" - << maybeName.value() << "]" << std::endl; - } else { - gzerr << "Camera sensor has invalid name" << std::endl; + if (auto maybeName = impl->parentSensor.Name(_ecm)) + { + gzmsg << "GstCameraPlugin: attached to sensor [" + << maybeName.value() << "]" << std::endl; + } + else + { + gzerr << "GstCameraPlugin: camera sensor has invalid name. " + "Failed to initialize" << std::endl; return; } - impl->udpHost = "127.0.0.1"; - const char *host_ip = std::getenv("GZ_VIDEO_HOST_IP"); - if (host_ip) { - impl->udpHost = std::string(host_ip); - } else if (_sdf->HasElement("udpHost")) { - impl->udpHost = _sdf->Get("udpHost"); + if (_sdf->HasElement("udp_host")) + { + impl->udpHost = _sdf->Get("udp_host"); } - impl->udpPort = 5600; - const char *host_port = std::getenv("GZ_VIDEO_HOST_PORT"); - if (host_port) { - sscanf(host_port, "%d", &impl->udpPort); - } else if (_sdf->HasElement("udpPort")) { - impl->udpPort = _sdf->Get("udpPort"); + if (_sdf->HasElement("udp_port")) + { + impl->udpPort = _sdf->Get("udp_port"); } - gzwarn << "Streaming video to " << impl->udpHost << ":" << impl->udpPort << std::endl; + gzmsg << "GstCameraPlugin: streaming video to " + << impl->udpHost << ":" + << impl->udpPort << std::endl; - // uses MPEG2TS pipeline by default. RTMP and Generic are mutually exclusive with priority to RTMP - if (_sdf->HasElement("rtmpLocation")) { - impl->rtmpLocation = _sdf->Get("rtmpLocation"); + // uses MPEG2TS pipeline by default. RTMP and Generic are + // mutually exclusive with priority to RTMP + if (_sdf->HasElement("rtmp_location")) + { + impl->rtmpLocation = _sdf->Get("rtmp_location"); impl->useRtmpPipeline = true; - } else if (_sdf->HasElement("useBasicPipeline")) { - impl->useBasicPipeline = _sdf->Get("useBasicPipeline"); + } + else if (_sdf->HasElement("use_basic_pipeline")) + { + impl->useBasicPipeline = _sdf->Get("use_basic_pipeline"); } // Use CUDA for video encoding - if (_sdf->HasElement("useCuda")) { - impl->useCuda = _sdf->Get("useCuda"); - } else { - impl->useCuda = false; + if (_sdf->HasElement("use_cuda")) + { + impl->useCuda = _sdf->Get("use_cuda"); } - impl->imageTopic = "/world/iris_runway/model/iris_with_gimbal/model/gimbal/link/pitch_link/sensor/camera/image"; - const char *image_topic = std::getenv("GZ_VIDEO_TOPIC"); - if (image_topic) { - impl->imageTopic = std::string(image_topic); - } else if (_sdf->HasElement("imageTopic")) { - impl->imageTopic = _sdf->Get("imageTopic"); + if (_sdf->HasElement("image_topic")) + { + impl->imageTopic = _sdf->Get("image_topic"); } - impl->enableTopic = "/enable_video_stream"; - const char *enable_topic = std::getenv("GZ_ENABLE_TOPIC"); - if (enable_topic) { - impl->enableTopic = std::string(enable_topic); - } else if (_sdf->HasElement("enableTopic")) { - impl->enableTopic = _sdf->Get("enableTopic"); + if (_sdf->HasElement("enable_topic")) + { + impl->enableTopic = _sdf->Get("enable_topic"); } - // subscribe to gazebo topics - impl->node.Subscribe(impl->enableTopic, &GstCameraPlugin::Impl::onVideoStreamEnable, impl.get()); - impl->node.Subscribe(impl->imageTopic, &GstCameraPlugin::Impl::onImage, impl.get()); + //! @note subscriptions are deferred to Pre-Update as the enclosing + // sensor must be fully initialised before entity - component queries + // for topics names etc. to succeed. // subscribe to events - impl->connections.push_back(_eventMgr.Connect( - std::bind(&GstCameraPlugin::Impl::OnRenderTeardown, impl.get()))); + impl->connections.push_back( + _eventMgr.Connect( + std::bind(&GstCameraPlugin::Impl::OnRenderTeardown, impl.get()))); } -void GstCameraPlugin::PreUpdate(const UpdateInfo &_info, EntityComponentManager &_ecm) { - if (impl->cameraName.empty()) { - Entity cameraEntity = impl->parentSensor; - impl->cameraName = removeParentScope(scopedName(cameraEntity, _ecm, "::", false), "::"); - gzwarn << "Camera name: [" << impl->cameraName << "]" << std::endl; +void GstCameraPlugin::PreUpdate(const UpdateInfo &_info, + EntityComponentManager &_ecm) +{ + if (impl->cameraName.empty()) + { + Entity cameraEntity = impl->parentSensor.Entity(); + impl->cameraName = removeParentScope( + scopedName(cameraEntity, _ecm, "::", false), "::"); + gzmsg << "GstCameraPlugin: camera name [" + << impl->cameraName << "]" << std::endl; + } + + // complete initialisation deferred from Configure() + if (!impl->is_initialised) + { + if (impl->imageTopic.empty()) + { + auto maybeTopic = impl->parentSensor.Topic(_ecm); + if (!maybeTopic.has_value()) + { + return; + } + impl->imageTopic = maybeTopic.value(); + } + + if (impl->enableTopic.empty()) + { + auto maybeTopic = impl->parentSensor.Topic(_ecm); + if (!maybeTopic.has_value()) + { + return; + } + impl->enableTopic = maybeTopic.value() + "/enable_streaming"; + } + gzmsg << "GstCameraPlugin: image topic [" + << impl->imageTopic << "]" << std::endl; + gzmsg << "GstCameraPlugin: enable topic [" + << impl->enableTopic << "]" << std::endl; + + // subscribe to gazebo topics + impl->node.Subscribe(impl->imageTopic, + &GstCameraPlugin::Impl::OnImage, impl.get()); + impl->node.Subscribe(impl->enableTopic, + &GstCameraPlugin::Impl::OnVideoStreamEnable, impl.get()); + + impl->is_initialised = true; } - if (!impl->camera && !impl->cameraName.empty()) { - impl->initializeCamera(); + if (!impl->camera && !impl->cameraName.empty()) + { + impl->InitializeCamera(); return; } } -void GstCameraPlugin::Impl::initializeCamera() { +void GstCameraPlugin::Impl::InitializeCamera() +{ // Wait for render engine to be available. - if (rendering::loadedEngines().empty()) return; + if (rendering::loadedEngines().empty()) + { + return; + } // Get scene. - if (!scene) { + if (!scene) + { scene = rendering::sceneFromFirstRenderEngine(); } // Return if scene not ready or no sensors available. - if (scene == nullptr || !scene->IsInitialized() || scene->SensorCount() == 0) { - gzwarn << "No scene or camera sensors available" << std::endl; + if (scene == nullptr || !scene->IsInitialized() + || scene->SensorCount() == 0) + { + gzwarn << "GstCameraPlugin: no scene or camera sensors available" + << std::endl; return; } // Get camera. - if (!camera) { + if (!camera) + { auto sensor = scene->SensorByName(cameraName); - if (!sensor) { - gzerr << "Unable to find sensor: [" << cameraName << "]" << std::endl; + if (!sensor) + { + gzerr << "GstCameraPlugin: unable to find sensor [" + << cameraName << "]" << std::endl; return; } camera = std::dynamic_pointer_cast(sensor); - if (!camera) { - gzerr << "[" << cameraName << "] is not a camera" << std::endl; + if (!camera) + { + gzerr << "GstCameraPlugin: sensor [" + << cameraName << "] is not a camera" << std::endl; return; } } } -void GstCameraPlugin::Impl::startStreaming() { - if (!isGstMainLoopActive) { - pthread_create(&threadId, NULL, start_thread, this); +void GstCameraPlugin::Impl::StartStreaming() +{ + if (!isGstMainLoopActive) + { + pthread_create(&threadId, NULL, StartThread, this); } } -void *GstCameraPlugin::Impl::start_thread(void *param) { +void *GstCameraPlugin::Impl::StartThread(void *param) +{ GstCameraPlugin::Impl *impl = (GstCameraPlugin::Impl *)param; - impl->startGstThread(); + impl->StartGstThread(); return nullptr; } -void GstCameraPlugin::Impl::startGstThread() { +void GstCameraPlugin::Impl::StartGstThread() +{ gst_init(nullptr, nullptr); gst_loop = g_main_loop_new(nullptr, FALSE); - if (!gst_loop) { - gzerr << "Create loop failed" << std::endl; + if (!gst_loop) + { + gzerr << "GstCameraPlugin: failed to create GStreamer main loop" + << std::endl; return; } GstElement *pipeline = gst_pipeline_new(nullptr); - if (!pipeline) { - gzerr << "ERR: Create pipeline failed" << std::endl; + if (!pipeline) + { + gzerr << "GstCameraPlugin: GStreamer pipeline failed" << std::endl; return; } source = gst_element_factory_make("appsrc", nullptr); - if (useRtmpPipeline) { - createRtmpPipeline(pipeline); - } else if (useBasicPipeline) { - createGenericPipeline(pipeline); - } else { - createMpeg2tsPipeline(pipeline); + if (useRtmpPipeline) + { + CreateRtmpPipeline(pipeline); + } + else if (useBasicPipeline) + { + CreateGenericPipeline(pipeline); + } + else + { + CreateMpeg2tsPipeline(pipeline); } // Configure source element g_object_set(G_OBJECT(source), "caps", - gst_caps_new_simple("video/x-raw", - "format", G_TYPE_STRING, "I420", - "width", G_TYPE_INT, width, - "height", G_TYPE_INT, height, - "framerate", GST_TYPE_FRACTION, (unsigned int)rate, 1, nullptr), + gst_caps_new_simple("video/x-raw", + "format", G_TYPE_STRING, "I420", + "width", G_TYPE_INT, width, + "height", G_TYPE_INT, height, + "framerate", GST_TYPE_FRACTION, + this->rate, 1, nullptr), "is-live", TRUE, "do-timestamp", TRUE, "stream-type", GST_APP_STREAM_TYPE_STREAM, @@ -282,17 +348,18 @@ void GstCameraPlugin::Impl::startGstThread() { // Start auto ret = gst_element_set_state(pipeline, GST_STATE_PLAYING); - if (ret != GST_STATE_CHANGE_SUCCESS) { - gzwarn << "State change result: " << ret << std::endl; + if (ret != GST_STATE_CHANGE_SUCCESS) + { + gzmsg << "GstCameraPlugin: GStreamer element set state returned: " + << ret << std::endl; } // this call blocks until the main_loop is killed - gzwarn << "starting gstreamer main loop" << std::endl; + gzmsg << "GstCameraPlugin: starting GStreamer main loop" << std::endl; isGstMainLoopActive = true; g_main_loop_run(gst_loop); isGstMainLoopActive = false; - requestedStartStreaming = false; - gzwarn << "gstreamer main loop ended" << std::endl; + gzmsg << "GstCameraPlugin: stopping GStreamer main loop" << std::endl; // Clean up gst_element_set_state(pipeline, GST_STATE_NULL); @@ -303,100 +370,131 @@ void GstCameraPlugin::Impl::startGstThread() { source = nullptr; } -void GstCameraPlugin::Impl::createRtmpPipeline(GstElement *pipeline) { - gzdbg << "Creating RTMP pipeline" << std::endl; +void GstCameraPlugin::Impl::CreateRtmpPipeline(GstElement *pipeline) +{ + gzdbg << "GstCameraPlugin: creating RTMP pipeline" << std::endl; GstElement *queue = gst_element_factory_make("queue", nullptr); GstElement *converter = gst_element_factory_make("videoconvert", nullptr); - GstElement *encoder = createEncoder(); + GstElement *encoder = CreateEncoder(); GstElement *payloader = gst_element_factory_make("flvmux", nullptr); GstElement *sink = gst_element_factory_make("rtmpsink", nullptr); g_object_set(G_OBJECT(sink), "location", rtmpLocation.c_str(), nullptr); - if (!source || !queue || !converter || !encoder || !payloader || !sink) { - gzerr << "ERR: Create elements failed" << std::endl; + if (!source || !queue || !converter || !encoder || !payloader || !sink) + { + gzerr << "GstCameraPlugin: failed to create GStreamer elements" + << std::endl; return; } // Connect all elements to pipeline - gst_bin_add_many(GST_BIN(pipeline), source, queue, converter, encoder, payloader, sink, nullptr); + gst_bin_add_many(GST_BIN(pipeline), source, queue, converter, encoder, + payloader, sink, nullptr); // Link all elements - if (gst_element_link_many(source, queue, converter, encoder, payloader, sink, nullptr) != TRUE) { - gzerr << "ERR: Link elements failed" << std::endl; + if (gst_element_link_many(source, queue, converter, encoder, + payloader, sink, nullptr) != TRUE) + { + gzerr << "GstCameraPlugin: failed to link GStreamer elements" + << std::endl; return; } } -void GstCameraPlugin::Impl::createGenericPipeline(GstElement *pipeline) { - gzdbg << "Creating Generic pipeline" << std::endl; +void GstCameraPlugin::Impl::CreateGenericPipeline(GstElement *pipeline) +{ + gzdbg << "GstCameraPlugin: creating generic pipeline" << std::endl; GstElement *queue = gst_element_factory_make("queue", nullptr); GstElement *converter = gst_element_factory_make("videoconvert", nullptr); - GstElement *encoder = createEncoder(); + GstElement *encoder = CreateEncoder(); GstElement *payloader = gst_element_factory_make("rtph264pay", nullptr); GstElement *sink = gst_element_factory_make("udpsink", nullptr); - g_object_set(G_OBJECT(sink), "host", udpHost.c_str(), "port", udpPort, nullptr); + g_object_set(G_OBJECT(sink), "host", udpHost.c_str(), + "port", udpPort, nullptr); - if (!source || !queue || !converter || !encoder || !payloader || !sink) { - gzerr << "ERR: Create elements failed" << std::endl; + if (!source || !queue || !converter || !encoder || !payloader || !sink) + { + gzerr << "GstCameraPlugin: failed to create GStreamer elements" + << std::endl; return; } // Connect all elements to pipeline - gst_bin_add_many(GST_BIN(pipeline), source, queue, converter, encoder, payloader, sink, nullptr); + gst_bin_add_many(GST_BIN(pipeline), source, queue, converter, encoder, + payloader, sink, nullptr); // Link all elements - if (gst_element_link_many(source, queue, converter, encoder, payloader, sink, nullptr) != TRUE) { - gzerr << "ERR: Link elements failed" << std::endl; + if (gst_element_link_many(source, queue, converter, encoder, + payloader, sink, nullptr) != TRUE) + { + gzerr << "GstCameraPlugin: failed to link GStreamer elements" + << std::endl; return; } } -void GstCameraPlugin::Impl::createMpeg2tsPipeline(GstElement *pipeline) { - gzdbg << "Creating MPEG2TS pipeline" << std::endl; +void GstCameraPlugin::Impl::CreateMpeg2tsPipeline(GstElement *pipeline) +{ + gzdbg << "GstCameraPlugin: creating MPEG2TS pipeline" << std::endl; GstElement *queue = gst_element_factory_make("queue", nullptr); GstElement *converter = gst_element_factory_make("videoconvert", nullptr); - GstElement *encoder = createEncoder(); + GstElement *encoder = CreateEncoder(); GstElement *h264_parser = gst_element_factory_make("h264parse", nullptr); GstElement *payloader = gst_element_factory_make("mpegtsmux", nullptr); GstElement *queue_mpeg = gst_element_factory_make("queue", nullptr); GstElement *sink = gst_element_factory_make("udpsink", nullptr); g_object_set(G_OBJECT(payloader), "alignment", 7, nullptr); - g_object_set(G_OBJECT(sink), "host", udpHost.c_str(), "port", udpPort, "sync", false, nullptr); + g_object_set(G_OBJECT(sink), "host", udpHost.c_str(), "port", udpPort, + "sync", false, nullptr); - if (!source || !queue || !converter || !encoder || !h264_parser || !payloader || !queue_mpeg || !sink) { - gzerr << "ERR: Create elements failed" << std::endl; + if (!source || !queue || !converter || !encoder || !h264_parser + || !payloader || !queue_mpeg || !sink) + { + gzerr << "GstCameraPlugin: failed to create GStreamer elements" + << std::endl; return; } - gst_bin_add_many(GST_BIN(pipeline), source, queue, converter, encoder, h264_parser, payloader, queue_mpeg, sink, nullptr); - if (gst_element_link_many(source, queue, converter, encoder, h264_parser, payloader, queue_mpeg, sink, nullptr) != TRUE) { - gzerr << "ERR: Link elements failed" << std::endl; + gst_bin_add_many(GST_BIN(pipeline), source, queue, converter, encoder, + h264_parser, payloader, queue_mpeg, sink, nullptr); + if (gst_element_link_many(source, queue, converter, encoder, + h264_parser, payloader, queue_mpeg, sink, nullptr) != TRUE) + { + gzerr << "GstCameraPlugin: failed to link GStreamer elements" + << std::endl; return; } } -GstElement* GstCameraPlugin::Impl::createEncoder() { - GstElement* encoder; - if (useCuda) { +GstElement* GstCameraPlugin::Impl::CreateEncoder() +{ + GstElement* encoder{nullptr}; + if (useCuda) + { gzdbg << "Using Cuda" << std::endl; encoder = gst_element_factory_make("nvh264enc", nullptr); g_object_set(G_OBJECT(encoder), "bitrate", 800, "preset", 1, nullptr); - } else { + } + else + { encoder = gst_element_factory_make("x264enc", nullptr); - g_object_set(G_OBJECT(encoder), "bitrate", 800, "speed-preset", 6, "tune", 4, "key-int-max", 10, nullptr); + g_object_set(G_OBJECT(encoder), "bitrate", 800, "speed-preset", 6, + "tune", 4, "key-int-max", 10, nullptr); } return encoder; } -void GstCameraPlugin::Impl::onImage(const msgs::Image &msg) { - if (!requestedStartStreaming) { - requestedStartStreaming = true; +void GstCameraPlugin::Impl::OnImage(const msgs::Image &msg) +{ + if (requestedStartStreaming) + { width = msg.width(); height = msg.height(); - startStreaming(); + StartStreaming(); + requestedStartStreaming = false; return; } @@ -406,59 +504,79 @@ void GstCameraPlugin::Impl::onImage(const msgs::Image &msg) { const guint size = width * height * 1.5; GstBuffer *buffer = gst_buffer_new_allocate(NULL, size, NULL); - if (!buffer) { - gzerr << "gst_buffer_new_allocate failed" << std::endl; + if (!buffer) + { + gzerr << "GstCameraPlugin: gst_buffer_new_allocate failed" + << std::endl; return; } GstMapInfo map; - if (!gst_buffer_map(buffer, &map, GST_MAP_WRITE)) { - gzerr << "gst_buffer_map failed" << std::endl; + if (!gst_buffer_map(buffer, &map, GST_MAP_WRITE)) + { + gzerr << "GstCameraPlugin: gst_buffer_map failed" << std::endl; return; } // Color Conversion from RGB to YUV cv::Mat frame = cv::Mat(height, width, CV_8UC3); cv::Mat frameYUV = cv::Mat(height, width, CV_8UC3); - frame.data = (uchar *)msg.data().c_str(); + frame.data = reinterpret_cast( + const_cast(msg.data().c_str())); cvtColor(frame, frameYUV, cv::COLOR_RGB2YUV_I420); memcpy(map.data, frameYUV.data, size); gst_buffer_unmap(buffer, &map); - GstFlowReturn ret = gst_app_src_push_buffer(GST_APP_SRC(this->source), buffer); - if (ret != GST_FLOW_OK) { - /* something wrong, stop pushing */ - gzerr << "gst_app_src_push_buffer failed" << std::endl; + GstFlowReturn ret = + gst_app_src_push_buffer(GST_APP_SRC(this->source), buffer); + if (ret != GST_FLOW_OK) + { + // Something wrong, stop pushing + gzerr << "GstCameraPlugin: gst_app_src_push_buffer failed" + << std::endl; g_main_loop_quit(gst_loop); } } -void GstCameraPlugin::Impl::onVideoStreamEnable(const msgs::Boolean &msg) { - gzwarn << "VideoStreamEnable: " << msg.data() << std::endl; - msg.data() ? startStreaming() : stopStreaming(); +void GstCameraPlugin::Impl::OnVideoStreamEnable(const msgs::Boolean &msg) +{ + gzmsg << "GstCameraPlugin:: streaming: " + << (msg.data() ? "started" : "stopped") << std::endl; + if (msg.data()) + { + requestedStartStreaming = true; + } + else + { + requestedStartStreaming = false; + StopStreaming(); + } } -void GstCameraPlugin::Impl::OnRenderTeardown() { - gzdbg << "onRenderTeardown" << std::endl; - +void GstCameraPlugin::Impl::OnRenderTeardown() +{ + StopStreaming(); camera.reset(); scene.reset(); - stopStreaming(); } -void GstCameraPlugin::Impl::stopStreaming() { - if (isGstMainLoopActive) { - stopGstThread(); +void GstCameraPlugin::Impl::StopStreaming() +{ + if (isGstMainLoopActive) + { + StopGstThread(); pthread_join(threadId, NULL); isGstMainLoopActive = false; } } -void GstCameraPlugin::Impl::stopGstThread() { - if (gst_loop) { +void GstCameraPlugin::Impl::StopGstThread() +{ + if (gst_loop) + { g_main_loop_quit(gst_loop); } } @@ -478,4 +596,4 @@ GZ_ADD_PLUGIN( GZ_ADD_PLUGIN_ALIAS( gz::sim::systems::GstCameraPlugin, - "GstCameraPlugin") \ No newline at end of file + "GstCameraPlugin") From 46a9a3fce7f40bf3cbbdb5676d94fa9a9880d18f Mon Sep 17 00:00:00 2001 From: Rhys Mainwaring Date: Thu, 20 Jun 2024 20:28:49 +0100 Subject: [PATCH 3/6] GstCameraPlugin: update ci-build dependencies Signed-off-by: Rhys Mainwaring --- .github/workflows/ubuntu-build.yml | 10 +++++++++- 1 file changed, 9 insertions(+), 1 deletion(-) diff --git a/.github/workflows/ubuntu-build.yml b/.github/workflows/ubuntu-build.yml index 37a9778..a29323d 100644 --- a/.github/workflows/ubuntu-build.yml +++ b/.github/workflows/ubuntu-build.yml @@ -34,7 +34,15 @@ jobs: sudo wget https://packages.osrfoundation.org/gazebo.gpg -O /usr/share/keyrings/pkgs-osrf-archive-keyring.gpg echo "deb [arch=$(dpkg --print-architecture) signed-by=/usr/share/keyrings/pkgs-osrf-archive-keyring.gpg] http://packages.osrfoundation.org/gazebo/ubuntu-stable $(lsb_release -cs) main" | sudo tee /etc/apt/sources.list.d/gazebo-stable.list > /dev/null sudo apt update && sudo apt install --no-install-recommends -y \ - rapidjson-dev gz-harmonic + rapidjson-dev \ + libopencv-dev \ + libunwind-dev \ + libgstreamer1.0-dev \ + libgstreamer-plugins-base1.0-dev \ + gstreamer1.0-plugins-bad \ + gstreamer1.0-libav \ + gstreamer1.0-gl \ + gz-harmonic # Put ccache into github cache for faster build - name: Prepare ccache timestamp From 85ceb27e2a5f45102daea9c7458f27d8beeba09e Mon Sep 17 00:00:00 2001 From: Rhys Mainwaring Date: Thu, 20 Jun 2024 20:29:14 +0100 Subject: [PATCH 4/6] GstCameraPlugin: update dependencies in README Signed-off-by: Rhys Mainwaring --- README.md | 3 +++ 1 file changed, 3 insertions(+) diff --git a/README.md b/README.md index db0649b..6036e06 100644 --- a/README.md +++ b/README.md @@ -51,6 +51,7 @@ Manual - Gazebo Garden Dependencies: ```bash sudo apt update sudo apt install libgz-sim7-dev rapidjson-dev +sudo apt install libopencv-dev libgstreamer1.0-dev libgstreamer-plugins-base1.0-dev gstreamer1.0-plugins-bad gstreamer1.0-libav gstreamer1.0-gl ``` #### Harmonic (apt) @@ -60,6 +61,7 @@ Manual - Gazebo Harmonic Dependencies: ```bash sudo apt update sudo apt install libgz-sim8-dev rapidjson-dev +sudo apt install libopencv-dev libgstreamer1.0-dev libgstreamer-plugins-base1.0-dev gstreamer1.0-plugins-bad gstreamer1.0-libav gstreamer1.0-gl ``` #### Rosdep @@ -82,6 +84,7 @@ rosdep install --from-paths src --ignore-src -y ```bash brew update brew install rapidjson +brew install opencv gstreamer ``` Ensure the `GZ_VERSION` environment variable is set to either From cf5a21d0a5183c2f4902a998037afdf6f38e89dc Mon Sep 17 00:00:00 2001 From: Rhys Mainwaring Date: Sun, 23 Jun 2024 11:21:35 +0100 Subject: [PATCH 5/6] GstCameraPlugin: add GstCameraPlugin to all gimbals Signed-off-by: Rhys Mainwaring --- models/gimbal_small_1d/model.sdf | 9 +++++++++ models/gimbal_small_2d/model.sdf | 9 +++++++++ models/gimbal_small_3d/model.sdf | 12 ++++++------ 3 files changed, 24 insertions(+), 6 deletions(-) diff --git a/models/gimbal_small_1d/model.sdf b/models/gimbal_small_1d/model.sdf index adbe05b..f449fae 100644 --- a/models/gimbal_small_1d/model.sdf +++ b/models/gimbal_small_1d/model.sdf @@ -123,6 +123,15 @@ 1 10 1 + + + 127.0.0.1 + 5600 + true + false + + diff --git a/models/gimbal_small_2d/model.sdf b/models/gimbal_small_2d/model.sdf index 8b1cace..794e998 100644 --- a/models/gimbal_small_2d/model.sdf +++ b/models/gimbal_small_2d/model.sdf @@ -159,6 +159,15 @@ 1 10 1 + + + 127.0.0.1 + 5600 + true + false + + diff --git a/models/gimbal_small_3d/model.sdf b/models/gimbal_small_3d/model.sdf index 9ca84da..d265f7e 100644 --- a/models/gimbal_small_3d/model.sdf +++ b/models/gimbal_small_3d/model.sdf @@ -211,12 +211,12 @@ 0.42514285714 - - 127.0.0.1 - 5600 - /world/iris_runway/model/iris_with_gimbal/model/gimbal/link/pitch_link/sensor/camera/image - /enable_video_stream + + 127.0.0.1 + 5600 + true + false From e2208009d4e82d2ea8ee48b8ab113689c5bd12e7 Mon Sep 17 00:00:00 2001 From: Rhys Mainwaring Date: Mon, 24 Jun 2024 11:12:37 +0100 Subject: [PATCH 6/6] GstCameraPlugin: add usage section to README Signed-off-by: Rhys Mainwaring --- README.md | 32 ++++++++++++++++++++++++++++++++ 1 file changed, 32 insertions(+) diff --git a/README.md b/README.md index 6036e06..cedda89 100644 --- a/README.md +++ b/README.md @@ -200,6 +200,38 @@ greater than one: MANUAL> param set SIM_SPEEDUP 10 ``` +### 3. Streaming camera video + +Images from camera sensors may be streamed with GStreamer using +the `GstCameraPlugin` sensor plugin. The example gimbal models include the +plugin element: + +```xml + + 127.0.0.1 + 5600 + true + false + +``` + +The `` and `` parameters are deduced from the +topic name for the camera sensor, but may be overriden if required. + +The `gimbal.sdf` world includes a 3 degrees of freedom gimbal with a +zoomable camera. To start streaming: + +```bash +gz topic -t /world/gimbal/model/mount/model/gimbal/link/pitch_link/sensor/camera/image/enable_streaming -m gz.msgs.Boolean -p "data: 1" +``` + +Display the streamed video: + +```bash +gst-launch-1.0 -v udpsrc port=5600 caps='application/x-rtp, media=(string)video, clock-rate=(int)90000, encoding-name=(string)H264' ! rtph264depay ! avdec_h264 ! videoconvert ! autovideosink sync=false +``` + ## Models In addition to the Iris and Zephyr models included here, a selection