From ca9a6185be6d33b07a5f265130b8991e6db312c7 Mon Sep 17 00:00:00 2001 From: dmalad Date: Tue, 28 Nov 2023 18:13:18 +0100 Subject: [PATCH] First Version --- src/image2rtsp/CMakeLists.txt | 61 ++++++ LICENSE => src/image2rtsp/LICENSE | 0 README.md => src/image2rtsp/README.md | 0 src/image2rtsp/config/parameters.yaml | 12 ++ src/image2rtsp/include/image2rtsp.hpp | 41 ++++ src/image2rtsp/include/image_encodings.h | 233 +++++++++++++++++++++ src/image2rtsp/launch/image2rtsp.launch.py | 20 ++ src/image2rtsp/package.xml | 23 ++ src/image2rtsp/python/__init__.py | 0 src/image2rtsp/python/rtsp.py | 15 ++ src/image2rtsp/src/image2rtsp.cpp | 61 ++++++ src/image2rtsp/src/video.cpp | 169 +++++++++++++++ 12 files changed, 635 insertions(+) create mode 100644 src/image2rtsp/CMakeLists.txt rename LICENSE => src/image2rtsp/LICENSE (100%) rename README.md => src/image2rtsp/README.md (100%) create mode 100644 src/image2rtsp/config/parameters.yaml create mode 100644 src/image2rtsp/include/image2rtsp.hpp create mode 100644 src/image2rtsp/include/image_encodings.h create mode 100644 src/image2rtsp/launch/image2rtsp.launch.py create mode 100644 src/image2rtsp/package.xml create mode 100644 src/image2rtsp/python/__init__.py create mode 100644 src/image2rtsp/python/rtsp.py create mode 100644 src/image2rtsp/src/image2rtsp.cpp create mode 100644 src/image2rtsp/src/video.cpp diff --git a/src/image2rtsp/CMakeLists.txt b/src/image2rtsp/CMakeLists.txt new file mode 100644 index 0000000..4f20fef --- /dev/null +++ b/src/image2rtsp/CMakeLists.txt @@ -0,0 +1,61 @@ +cmake_minimum_required(VERSION 3.5) +project(image2rtsp) + +# Default to C99 +if(NOT CMAKE_C_STANDARD) + set(CMAKE_C_STANDARD 99) +endif() + +# Default to C++14 +if(NOT CMAKE_CXX_STANDARD) + set(CMAKE_CXX_STANDARD 14) +endif() + +if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") + add_compile_options(-Wall -Wextra -Wpedantic) +endif() + +# find dependencies +find_package(ament_cmake REQUIRED) +find_package(rclcpp REQUIRED) +find_package(sensor_msgs REQUIRED) +pkg_check_modules(GST REQUIRED + gstreamer-1.0 + gstreamer-app-1.0 + gstreamer-rtsp-server-1.0 +) +file(GLOB SOURCES src/video.cpp) + +add_executable(image2rtsp src/image2rtsp.cpp ${SOURCES}) +ament_target_dependencies(image2rtsp rclcpp sensor_msgs) + +include_directories( + ${GST_INCLUDE_DIRS} + ) +target_link_libraries(image2rtsp + ${GST_LIBRARIES} + ) + +install(TARGETS + image2rtsp + DESTINATION lib/${PROJECT_NAME} + ) + +install( + DIRECTORY + config + launch + DESTINATION share/${PROJECT_NAME} + ) + +# Install Python modules +ament_python_install_package(python) + +# Install Python executables +install(PROGRAMS + python/rtsp.py + DESTINATION lib/${PROJECT_NAME} +) + + +ament_package() diff --git a/LICENSE b/src/image2rtsp/LICENSE similarity index 100% rename from LICENSE rename to src/image2rtsp/LICENSE diff --git a/README.md b/src/image2rtsp/README.md similarity index 100% rename from README.md rename to src/image2rtsp/README.md diff --git a/src/image2rtsp/config/parameters.yaml b/src/image2rtsp/config/parameters.yaml new file mode 100644 index 0000000..a5d9bac --- /dev/null +++ b/src/image2rtsp/config/parameters.yaml @@ -0,0 +1,12 @@ +/image2rtsp: + ros__parameters: + topic: "/color/image_raw" + mountpoint: "/back" + bitrate: "500" + caps: "video/x-raw,framerate=10/1,width=640,height=480" + port: "8554" + local_only: True # True = rtsp://127.0.0.1:port (The stream is accessible only from the local machine) + # False = rtsp://0.0.0.0:portAndMountpoint (The stream is accessible from the outside) + # For example, to access the stream running on the machine with IP = 192.168.20.20, + # use rtsp://192.186.20.20:portAndMountpoint + # True = rtsp://127.0.0.1:portAndMountpoint (The stream is accessible only from the local machine) \ No newline at end of file diff --git a/src/image2rtsp/include/image2rtsp.hpp b/src/image2rtsp/include/image2rtsp.hpp new file mode 100644 index 0000000..6f30dbf --- /dev/null +++ b/src/image2rtsp/include/image2rtsp.hpp @@ -0,0 +1,41 @@ +#ifndef IMAGE2RTSP_IMAGE2RTSP_HPP +#define IMAGE2RTSP_IMAGE2RTSP_HPP + +#include +#include +#include +#include +#include "sensor_msgs/msg/image.hpp" + +using namespace std; + +class Image2rtsp : public rclcpp::Node{ +public: + Image2rtsp(); + GstRTSPServer *rtsp_server; + +private: + string topic; + string mountpoint; + string bitrate; + string caps; + string port; + string pipeline; + string pipeline_head; + string pipeline_tail; + bool local_only; + GstAppSrc *appsrc; + + void video_mainloop_start(); + void rtsp_server_add_url(const char *url, const char *sPipeline, GstElement **appsrc); + void topic_callback(const sensor_msgs::msg::Image::SharedPtr msg); + GstRTSPServer *rtsp_server_create(const string &port, const bool local_only); + GstCaps *gst_caps_new_from_image(const sensor_msgs::msg::Image::SharedPtr &msg); + rclcpp::Subscription::SharedPtr subscription_; +}; + +static void media_configure(GstRTSPMediaFactory *factory, GstRTSPMedia *media, GstElement **appsrc); +static void *mainloop(void *arg); +static gboolean session_cleanup(Image2rtsp *node, rclcpp::Logger logger, gboolean ignored); + +#endif // IMAGE2RTSP_IMAGE2RTSP_HPP diff --git a/src/image2rtsp/include/image_encodings.h b/src/image2rtsp/include/image_encodings.h new file mode 100644 index 0000000..20a7f68 --- /dev/null +++ b/src/image2rtsp/include/image_encodings.h @@ -0,0 +1,233 @@ + +/********************************************************************* +* Software License Agreement (BSD License) +* +* Copyright (c) 2009, Willow Garage, Inc. +* All rights reserved. +* +* Redistribution and use in source and binary forms, with or without +* modification, are permitted provided that the following conditions +* are met: +* +* * Redistributions of source code must retain the above copyright +* notice, this list of conditions and the following disclaimer. +* * 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. +* * Neither the name of the Willow Garage 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. +*********************************************************************/ + +#ifndef SENSOR_MSGS_IMAGE_ENCODINGS_H +#define SENSOR_MSGS_IMAGE_ENCODINGS_H + +#include +#include +#include + +namespace sensor_msgs +{ + namespace image_encodings + { + const std::string RGB8 = "rgb8"; + const std::string RGBA8 = "rgba8"; + const std::string RGB16 = "rgb16"; + const std::string RGBA16 = "rgba16"; + const std::string BGR8 = "bgr8"; + const std::string BGRA8 = "bgra8"; + const std::string BGR16 = "bgr16"; + const std::string BGRA16 = "bgra16"; + const std::string MONO8="mono8"; + const std::string MONO16="mono16"; + + // OpenCV CvMat types + const std::string TYPE_8UC1="8UC1"; + const std::string TYPE_8UC2="8UC2"; + const std::string TYPE_8UC3="8UC3"; + const std::string TYPE_8UC4="8UC4"; + const std::string TYPE_8SC1="8SC1"; + const std::string TYPE_8SC2="8SC2"; + const std::string TYPE_8SC3="8SC3"; + const std::string TYPE_8SC4="8SC4"; + const std::string TYPE_16UC1="16UC1"; + const std::string TYPE_16UC2="16UC2"; + const std::string TYPE_16UC3="16UC3"; + const std::string TYPE_16UC4="16UC4"; + const std::string TYPE_16SC1="16SC1"; + const std::string TYPE_16SC2="16SC2"; + const std::string TYPE_16SC3="16SC3"; + const std::string TYPE_16SC4="16SC4"; + const std::string TYPE_32SC1="32SC1"; + const std::string TYPE_32SC2="32SC2"; + const std::string TYPE_32SC3="32SC3"; + const std::string TYPE_32SC4="32SC4"; + const std::string TYPE_32FC1="32FC1"; + const std::string TYPE_32FC2="32FC2"; + const std::string TYPE_32FC3="32FC3"; + const std::string TYPE_32FC4="32FC4"; + const std::string TYPE_64FC1="64FC1"; + const std::string TYPE_64FC2="64FC2"; + const std::string TYPE_64FC3="64FC3"; + const std::string TYPE_64FC4="64FC4"; + + // Bayer encodings + const std::string BAYER_RGGB8="bayer_rggb8"; + const std::string BAYER_BGGR8="bayer_bggr8"; + const std::string BAYER_GBRG8="bayer_gbrg8"; + const std::string BAYER_GRBG8="bayer_grbg8"; + const std::string BAYER_RGGB16="bayer_rggb16"; + const std::string BAYER_BGGR16="bayer_bggr16"; + const std::string BAYER_GBRG16="bayer_gbrg16"; + const std::string BAYER_GRBG16="bayer_grbg16"; + + // Miscellaneous + // This is the UYVY version of YUV422 codec http://www.fourcc.org/yuv.php#UYVY + // with an 8-bit depth + const std::string YUV422="yuv422"; + + // Prefixes for abstract image encodings + const std::string ABSTRACT_ENCODING_PREFIXES[] = { + "8UC", "8SC", "16UC", "16SC", "32SC", "32FC", "64FC"}; + + // Utility functions for inspecting an encoding string + static inline bool isColor(const std::string& encoding) + { + return encoding == RGB8 || encoding == BGR8 || + encoding == RGBA8 || encoding == BGRA8 || + encoding == RGB16 || encoding == BGR16 || + encoding == RGBA16 || encoding == BGRA16; + } + + static inline bool isMono(const std::string& encoding) + { + return encoding == MONO8 || encoding == MONO16; + } + + static inline bool isBayer(const std::string& encoding) + { + return encoding == BAYER_RGGB8 || encoding == BAYER_BGGR8 || + encoding == BAYER_GBRG8 || encoding == BAYER_GRBG8 || + encoding == BAYER_RGGB16 || encoding == BAYER_BGGR16 || + encoding == BAYER_GBRG16 || encoding == BAYER_GRBG16; + } + + static inline bool hasAlpha(const std::string& encoding) + { + return encoding == RGBA8 || encoding == BGRA8 || + encoding == RGBA16 || encoding == BGRA16; + } + + static inline int numChannels(const std::string& encoding) + { + // First do the common-case encodings + if (encoding == MONO8 || + encoding == MONO16) + return 1; + if (encoding == BGR8 || + encoding == RGB8 || + encoding == BGR16 || + encoding == RGB16) + return 3; + if (encoding == BGRA8 || + encoding == RGBA8 || + encoding == BGRA16 || + encoding == RGBA16) + return 4; + if (encoding == BAYER_RGGB8 || + encoding == BAYER_BGGR8 || + encoding == BAYER_GBRG8 || + encoding == BAYER_GRBG8 || + encoding == BAYER_RGGB16 || + encoding == BAYER_BGGR16 || + encoding == BAYER_GBRG16 || + encoding == BAYER_GRBG16) + return 1; + + // Now all the generic content encodings + // TODO: Rewrite with regex when ROS supports C++11 + for (size_t i=0; i < sizeof(ABSTRACT_ENCODING_PREFIXES) / sizeof(*ABSTRACT_ENCODING_PREFIXES); i++) + { + std::string prefix = ABSTRACT_ENCODING_PREFIXES[i]; + if (encoding.substr(0, prefix.size()) != prefix) + continue; + if (encoding.size() == prefix.size()) + return 1; // ex. 8UC -> 1 + int n_channel = atoi(encoding.substr(prefix.size(), + encoding.size() - prefix.size()).c_str()); // ex. 8UC5 -> 5 + if (n_channel != 0) + return n_channel; // valid encoding string + } + + if (encoding == YUV422) + return 2; + + throw std::runtime_error("Unknown encoding " + encoding); + return -1; + } + + static inline int bitDepth(const std::string& encoding) + { + if (encoding == MONO16) + return 16; + if (encoding == MONO8 || + encoding == BGR8 || + encoding == RGB8 || + encoding == BGRA8 || + encoding == RGBA8 || + encoding == BAYER_RGGB8 || + encoding == BAYER_BGGR8 || + encoding == BAYER_GBRG8 || + encoding == BAYER_GRBG8) + return 8; + + if (encoding == MONO16 || + encoding == BGR16 || + encoding == RGB16 || + encoding == BGRA16 || + encoding == RGBA16 || + encoding == BAYER_RGGB16 || + encoding == BAYER_BGGR16 || + encoding == BAYER_GBRG16 || + encoding == BAYER_GRBG16) + return 16; + + // Now all the generic content encodings + // TODO: Rewrite with regex when ROS supports C++11 + for (size_t i=0; i < sizeof(ABSTRACT_ENCODING_PREFIXES) / sizeof(*ABSTRACT_ENCODING_PREFIXES); i++) + { + std::string prefix = ABSTRACT_ENCODING_PREFIXES[i]; + if (encoding.substr(0, prefix.size()) != prefix) + continue; + if (encoding.size() == prefix.size()) + return atoi(prefix.c_str()); // ex. 8UC -> 8 + int n_channel = atoi(encoding.substr(prefix.size(), + encoding.size() - prefix.size()).c_str()); // ex. 8UC10 -> 10 + if (n_channel != 0) + return atoi(prefix.c_str()); // valid encoding string + } + + if (encoding == YUV422) + return 8; + + throw std::runtime_error("Unknown encoding " + encoding); + return -1; + } + } +} + +#endif \ No newline at end of file diff --git a/src/image2rtsp/launch/image2rtsp.launch.py b/src/image2rtsp/launch/image2rtsp.launch.py new file mode 100644 index 0000000..ffa0a25 --- /dev/null +++ b/src/image2rtsp/launch/image2rtsp.launch.py @@ -0,0 +1,20 @@ +import os +from ament_index_python.packages import get_package_share_directory +from launch import LaunchDescription +from launch_ros.actions import Node + +def generate_launch_description(): + config = os.path.join( + get_package_share_directory('image2rtsp'), + 'config', + 'parameters.yaml' + ) + + return LaunchDescription([ + Node( + package='image2rtsp', + executable='image2rtsp', + name='image2rtsp', + parameters=[config] + ) + ]) \ No newline at end of file diff --git a/src/image2rtsp/package.xml b/src/image2rtsp/package.xml new file mode 100644 index 0000000..52e6e46 --- /dev/null +++ b/src/image2rtsp/package.xml @@ -0,0 +1,23 @@ + + + + image2rtsp + 0.0.0 + topic to RTSP stream package + dmitry + Apache License 2.0 + + ament_cmake + + rclcpp + sensor_msgs + + ament_lint_auto + ament_lint_common + + ros2launch + + + ament_cmake + + diff --git a/src/image2rtsp/python/__init__.py b/src/image2rtsp/python/__init__.py new file mode 100644 index 0000000..e69de29 diff --git a/src/image2rtsp/python/rtsp.py b/src/image2rtsp/python/rtsp.py new file mode 100644 index 0000000..988d32e --- /dev/null +++ b/src/image2rtsp/python/rtsp.py @@ -0,0 +1,15 @@ +import cv2 + +cap = cv2.VideoCapture('rtsp://127.0.0.1:8554/back') + +while True: + ret, frame = cap.read() + + if ret: + cv2.imshow('RTSP Stream', frame) + + if cv2.waitKey(1) & 0xFF == ord('q'): + break + +cap.release() +cv2.destroyAllWindows() diff --git a/src/image2rtsp/src/image2rtsp.cpp b/src/image2rtsp/src/image2rtsp.cpp new file mode 100644 index 0000000..28bfc10 --- /dev/null +++ b/src/image2rtsp/src/image2rtsp.cpp @@ -0,0 +1,61 @@ +#include "rclcpp/rclcpp.hpp" +#include "sensor_msgs/msg/image.hpp" +#include +#include +#include +#include "../include/image2rtsp.hpp" + +using std::placeholders::_1; + +Image2rtsp::Image2rtsp() : Node("image2rtsp"){ + // Declare and get the parameters + this->declare_parameter("topic"); + this->declare_parameter("mountpoint"); + this->declare_parameter("bitrate"); + this->declare_parameter("caps"); + this->declare_parameter("port"); + this->declare_parameter("local_only"); + + topic = this->get_parameter("topic").as_string(); + mountpoint = this->get_parameter("mountpoint").as_string(); + bitrate = this->get_parameter("bitrate").as_string(); + caps = this->get_parameter("caps").as_string(); + port = this->get_parameter("port").as_string(); + local_only = this->get_parameter("local_only").as_bool(); + + // Check if the parameter is set, since no default value is provided + if (!this->has_parameter("topic") || !this->has_parameter("mountpoint") || !this->has_parameter("bitrate") || !this->has_parameter("caps") || !this->has_parameter("port")){ + rclcpp::shutdown(); // Shutdown the node if there are some issues with launch file + return; + } + + // Start the subscription + subscription_ = this->create_subscription(topic, 10, std::bind(&Image2rtsp::topic_callback, this, _1)); + + // Start the RTSP server + video_mainloop_start(); + rtsp_server = rtsp_server_create(port, local_only); + appsrc = NULL; + // Setup the pipeline + pipeline_head = "( appsrc name=imagesrc do-timestamp=true min-latency=0 max-latency=0 max-bytes=1000 is-live=true ! videoconvert ! videoscale ! "; + pipeline_tail = "key-int-max=30 ! video/x-h264, profile=baseline ! rtph264pay name=pay0 pt=96 )"; + pipeline = pipeline_head + caps + " ! x264enc tune=zerolatency bitrate=" + bitrate + pipeline_tail; + rtsp_server_add_url(mountpoint.c_str(), pipeline.c_str(), (GstElement **)&(appsrc)); + RCLCPP_INFO(this->get_logger(), "Stream available at rtsp://%s:%s%s", gst_rtsp_server_get_address(rtsp_server), port.c_str(), mountpoint.c_str()); +} + +int main(int argc, char *argv[]){ + rclcpp::init(argc, argv); + try{ + auto node = std::make_shared(); + if (rclcpp::ok()){ + rclcpp::spin(node); + } + }catch (const std::exception &e){ + RCLCPP_INFO(rclcpp::get_logger("image2rtsp"), "One or more required parameters are not set. Shutting down the node."); + RCLCPP_INFO(rclcpp::get_logger("image2rtsp"), "Tip: Start the node only using launch file. Also rebuild after making changes in the YAML file"); + RCLCPP_ERROR(rclcpp::get_logger("image2rtsp"), "Parameter is not set: %s", e.what()); + } + rclcpp::shutdown(); + return 0; +} diff --git a/src/image2rtsp/src/video.cpp b/src/image2rtsp/src/video.cpp new file mode 100644 index 0000000..383c426 --- /dev/null +++ b/src/image2rtsp/src/video.cpp @@ -0,0 +1,169 @@ +#include +#include +#include +#include + +#include "../include/image2rtsp.hpp" +#include "../include/image_encodings.h" + +using namespace std; + +static void *mainloop(void *arg){ + GMainLoop *loop = g_main_loop_new(NULL, FALSE); + g_main_loop_run(loop); + g_main_destroy(loop); + return NULL; +} + +void Image2rtsp::video_mainloop_start(){ + pthread_t tloop; + gst_init(NULL, NULL); + pthread_create(&tloop, NULL, &mainloop, NULL); +} + +GstRTSPServer *Image2rtsp::rtsp_server_create(const std::string &port, const bool local_only){ + GstRTSPServer *server; + + /* create a server instance */ + server = gst_rtsp_server_new(); + // char *port = (char *) port; + g_object_set(server, "service", port.c_str(), NULL); + /* attach the server to the default maincontext */ + if (local_only){ + g_object_set(server, "address", "127.0.0.1", NULL); + } + gst_rtsp_server_attach(server, NULL); + /* add a timeout for the session cleanup */ + g_timeout_add_seconds(2, (GSourceFunc)session_cleanup, this); + return server; +} + +void Image2rtsp::rtsp_server_add_url(const char *url, const char *sPipeline, GstElement **appsrc){ + GstRTSPMountPoints *mounts; + GstRTSPMediaFactory *factory; + + /* get the mount points for this server, every server has a default object + * that be used to map uri mount points to media factories */ + mounts = gst_rtsp_server_get_mount_points(rtsp_server); + + /* make a media factory for a test stream. The default media factory can use + * gst-launch syntax to create pipelines. + * any launch line works as long as it contains elements named pay%d. Each + * element with pay%d names will be a stream */ + factory = gst_rtsp_media_factory_new(); + gst_rtsp_media_factory_set_launch(factory, sPipeline); + + /* notify when our media is ready, This is called whenever someone asks for + * the media and a new pipeline is created */ + g_signal_connect(factory, "media-configure", (GCallback)media_configure, appsrc); + + gst_rtsp_media_factory_set_shared(factory, TRUE); + + /* attach the factory to the url */ + gst_rtsp_mount_points_add_factory(mounts, url, factory); + + /* don't need the ref to the mounts anymore */ + g_object_unref(mounts); +} + +static void media_configure(GstRTSPMediaFactory *factory, GstRTSPMedia *media, GstElement **appsrc){ + if(appsrc){ + GstElement *pipeline = gst_rtsp_media_get_element(media); + + *appsrc = gst_bin_get_by_name(GST_BIN(pipeline), "imagesrc"); + + /* this instructs appsrc that we will be dealing with timed buffer */ + gst_util_set_object_arg(G_OBJECT(*appsrc), "format", "time"); + + gst_object_unref(pipeline); + }else{ + guint i, n_streams; + n_streams = gst_rtsp_media_n_streams(media); + + for (i = 0; i < n_streams; i++){ + GstRTSPAddressPool *pool; + GstRTSPStream *stream; + gchar *min, *max; + + stream = gst_rtsp_media_get_stream(media, i); + + /* make a new address pool */ + pool = gst_rtsp_address_pool_new(); + + min = g_strdup_printf("224.3.0.%d", (2 * i) + 1); + max = g_strdup_printf("224.3.0.%d", (2 * i) + 2); + gst_rtsp_address_pool_add_range(pool, min, max, 5000 + (10 * i), 5010 + (10 * i), 1); + g_free(min); + g_free(max); + gst_rtsp_stream_set_address_pool(stream, pool); + g_object_unref(pool); + } + } +} + +GstCaps *Image2rtsp::gst_caps_new_from_image(const sensor_msgs::msg::Image::SharedPtr &msg){ + // http://gstreamer.freedesktop.org/data/doc/gstreamer/head/pwg/html/section-types-definitions.html + static const std::map known_formats = { + {sensor_msgs::image_encodings::RGB8, "RGB"}, + {sensor_msgs::image_encodings::RGB16, "RGB16"}, + {sensor_msgs::image_encodings::RGBA8, "RGBA"}, + {sensor_msgs::image_encodings::RGBA16, "RGBA16"}, + {sensor_msgs::image_encodings::BGR8, "BGR"}, + {sensor_msgs::image_encodings::BGR16, "BGR16"}, + {sensor_msgs::image_encodings::BGRA8, "BGRA"}, + {sensor_msgs::image_encodings::BGRA16, "BGRA16"}, + {sensor_msgs::image_encodings::MONO8, "GRAY8"}, + {sensor_msgs::image_encodings::MONO16, "GRAY16_LE"}, + }; + + if (msg->is_bigendian){ + RCLCPP_ERROR(this->get_logger(), "GST: big endian image format is not supported"); + return nullptr; + } + + auto format = known_formats.find(msg->encoding); + if (format == known_formats.end()){ + RCLCPP_ERROR(this->get_logger(), "GST: image format '%s' unknown", msg->encoding.c_str()); + return nullptr; + } + + return gst_caps_new_simple("video/x-raw", + "format", G_TYPE_STRING, format->second.c_str(), + "width", G_TYPE_INT, msg->width, + "height", G_TYPE_INT, msg->height, + "framerate", GST_TYPE_FRACTION, 10, 1, + nullptr); +} + +static gboolean session_cleanup(Image2rtsp *node, rclcpp::Logger logger, gboolean ignored){ + GstRTSPServer *server = node->rtsp_server; + GstRTSPSessionPool *pool; + int num; + + pool = gst_rtsp_server_get_session_pool(server); + num = gst_rtsp_session_pool_cleanup(pool); + g_object_unref(pool); + + if (num > 0) + { + char s[32]; + snprintf(s, 32, (char *)"Sessions cleaned: %d", num); + RCLCPP_INFO(node->get_logger(), s); + } + return TRUE; +} + +void Image2rtsp::topic_callback(const sensor_msgs::msg::Image::SharedPtr msg){ + GstBuffer *buf; + GstCaps *caps; // image properties. see return of Image2rtsp::gst_caps_new_from_image + char *gst_type, *gst_format = (char *)""; + if (appsrc != NULL){ + // Set caps from message + caps = gst_caps_new_from_image(msg); + gst_app_src_set_caps(appsrc, caps); + buf = gst_buffer_new_allocate(nullptr, msg->data.size(), nullptr); + gst_buffer_fill(buf, 0, msg->data.data(), msg->data.size()); + GST_BUFFER_FLAG_SET(buf, GST_BUFFER_FLAG_LIVE); + gst_app_src_push_buffer(appsrc, buf); + } +} \ No newline at end of file